/**
 * @file main.c
 * @author AlexanderHD27
 * @brief Main file of the RPi Pico Firmware for the Oscilloscope-Renderer
 * @version 0.1
 * @date 2024-02-15
 * 
 * @copyright Copyright (c) 2024
 * 
 */

#include "tmc2209/tmc2209.hpp"
#include "pico/stdio.h"
#include "hardware/uart.h"

#include "pulser.pio.h"
#include "tmc2209/step.hpp"

#define LED_PIN 25

#define ENABLE0_PIN 2
#define ENABLE1_PIN 3

#define LIMIT0_PIN 10
#define LIMIT1_PIN 11

#define STEP0_PIN 6
#define STEP1_PIN 7
#define DIR0_PIN 8
#define DIR1_PIN 9

int main() {
    gpio_init(DIR0_PIN);
    gpio_set_dir(DIR0_PIN, GPIO_OUT);
    gpio_put(DIR0_PIN, 0);

    gpio_init(DIR1_PIN);
    gpio_set_dir(DIR1_PIN, GPIO_OUT);
    gpio_put(DIR1_PIN, 0);

    for(int i=0; i<100; i++) {
        gpio_put(DIR0_PIN, 1);
        gpio_put(DIR1_PIN, 1);
        sleep_ms(2);
        gpio_put(DIR0_PIN, 0);
        gpio_put(DIR1_PIN, 0);
        sleep_ms(2);
    }


    TMC2209_step_dual step_driver = TMC2209_step_dual(
        STEP0_PIN, STEP1_PIN, 
        DIR0_PIN, DIR1_PIN, 
        ENABLE0_PIN, ENABLE1_PIN, 
        pio0
    );
    TMC2209_UART uart_driver0 = TMC2209_UART(uart0, 0, 19200, 0, 1);
    TMC2209_UART uart_driver1 = TMC2209_UART(uart1, 0, 19200, 4, 5);
    uart_driver0.init();
    uart_driver1.init();

    sleep_ms(5);

    
    step_driver.set_conf1(10000, 0);


    gpio_init(LIMIT0_PIN);
    gpio_set_dir(LIMIT0_PIN, GPIO_IN);
    gpio_pull_up(LIMIT0_PIN);

    gpio_init(LIMIT1_PIN);
    gpio_set_dir(LIMIT1_PIN, GPIO_IN);
    gpio_pull_up(LIMIT1_PIN);
    
    step_driver.set_conf0(10000, 1);
    step_driver.pulse0(10000);
    step_driver.wait0(0);

    step_driver.set_conf0(10000, 0);
    step_driver.pulse0(100000);
    while(!gpio_get(LIMIT1_PIN)) {
        sleep_ms(1);
    }
    step_driver.pulse0(0);
    sleep_ms(500);

    step_driver.set_conf0(10000, 1);
    step_driver.pulse0(200);
    step_driver.wait0(0);

    step_driver.set_conf0(500, 0);
    step_driver.pulse0(5000);
    while(!gpio_get(LIMIT1_PIN)) {
        sleep_ms(1);
    }
    step_driver.pulse0(0);
    sleep_ms(1);

    gpio_init(LED_PIN);
    gpio_set_dir(LED_PIN, GPIO_OUT);

    while (true) {
        sleep_ms(50);
        gpio_put(LED_PIN, 0);
        sleep_ms(50);
        gpio_put(LED_PIN, 1);
    }
    

    return 0;
}