/**
 * @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 STEP0_PIN 6
#define STEP1_PIN 7
#define DIR0_PIN 8
#define DIR1_PIN 9

int main() {
/*
    gpio_init(ENABLE0_PIN);
    gpio_set_dir(ENABLE0_PIN, GPIO_OUT);
    gpio_put(ENABLE0_PIN, 0);

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

    //gpio_init(STEP0_PIN);
    //gpio_set_dir(STEP0_PIN, GPIO_OUT);
    //gpio_put(STEP0_PIN, 0);

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

    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);


    PIO pio_core = pio0;
    int SM_DIV0 = 0;
    int SM_COUNT0 = 2;

    uint pulser_offset = pio_add_program(pio_core, &pulser_program);

    pio_sm_config c_div0 = pulser_program_get_default_config(pulser_offset);
    pio_sm_config c_count0 = pulser_program_get_default_config(pulser_offset);
    
    pio_gpio_init(pio_core, STEP0_PIN);
    pio_gpio_init(pio_core, STEP1_PIN);
    
    pio_sm_set_consecutive_pindirs(pio_core, SM_COUNT0, STEP0_PIN, 1, true);
    sm_config_set_set_pins(&c_count0, STEP0_PIN, 1);

    pio_sm_init(pio_core, SM_DIV0, pulser_offset, &c_div0);
    pio_sm_init(pio_core, SM_COUNT0, pulser_offset + 6, &c_count0);

    const double target_freq_khz = 500;
    const double clock_div = ((double)(SYS_CLK_KHZ))/((double)(target_freq_khz));
    pio_sm_set_clkdiv(pio_core, SM_DIV0, clock_div);
    pio_sm_set_clkdiv(pio_core, SM_COUNT0, 1.0);

    pio_sm_set_enabled(pio_core, SM_DIV0, true);
    pio_sm_set_enabled(pio_core, SM_COUNT0, true);
    pio_sm_put_blocking(pio_core, SM_DIV0, 0);

    pio_sm_put_blocking(pio_core, SM_COUNT0, 0);

    // freq = base / 6 + n cyles
    // n = (base/freq) - 6

    const double double_div_freq_khz = 0.5;
    const uint32_t cycles = (target_freq_khz/double_div_freq_khz) - 6;
    pio_sm_put_blocking(pio_core, SM_DIV0, cycles);
*/

    TMC2209_step_dual step_driver = TMC2209_step_dual(STEP0_PIN, STEP1_PIN, DIR0_PIN, DIR1_PIN, ENABLE0_PIN, ENABLE1_PIN, pio0);
    step_driver.set_conf0(5000, 0);
    step_driver.set_conf1(10000, 0);

    step_driver.pulse1(UINT32_MAX - 10);
    sleep_ms(5);
    step_driver.pulse0(UINT32_MAX - 10);
    step_driver.pulse1(5000);
    step_driver.pulse0(1);
    step_driver.wait1(3);
    step_driver.pulse0(10);
    step_driver.pulse1(1);
    //step_driver.pulse0(5);

    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(100);

    volatile uint32_t res1 = uart_driver1.read(0x00);
    volatile uint32_t res0 = uart_driver0.read(0x00);

    //TMC2209_Registers registers(uart_driver);
    //registers.read_all_state();

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

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

    return 0;
}