/**
 * @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 "pico/stdio.h"
#include "pico/stdlib.h"
#include "pico/time.h"
#include "hardware/uart.h"
#include <stdio.h>

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

#include "CoreXY.hpp"

#include "FreeRTOSConfig.h"
#include "FreeRTOS.h"
#include "task.h"

#define LED_PIN 25

#define RGB1_B_PIN 21
#define RGB1_G_PIN 22

#define RGB2_R_PIN 26
#define RGB2_G_PIN 27
#define RGB2_B_PIN 28

#define BOARD_LED1_PIN 14
#define BOARD_LED2_PIN 15

#define ENABLE0_PIN 2
#define ENABLE1_PIN 3

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

enum SYSTEM_STATE {
    STARTUP,
    HOMEING_FIRST_RUN,
    HOMEING_BACKOFF,
    HOMEING_SECOND_RUN,
    IDLE
};

SYSTEM_STATE system_state = STARTUP;

/*
void homeMotors0(TMC2209_step_dual * driver) {

    driver->set_conf0(10000, 1);
    driver->pulse0(10000);
    driver->wait0(0);

    driver->set_conf0(10000, 0);
    driver->pulse0(100000);
    while(!gpio_get(LIMIT2_PIN)) {
        sleep_ms(1);
    }
    driver->pulse0(0);
    sleep_ms(500);

    // Second Run
    driver->set_conf0(5000, 1);
    driver->pulse0(200);
    driver->wait0(0);

    driver->set_conf0(500, 0);
    driver->pulse0(5000);
    while(!gpio_get(LIMIT2_PIN)) {
        sleep_ms(1);
    }
    driver->pulse0(0);
    sleep_ms(1);
}

void homeMotors1(TMC2209_step_dual * driver) {

    driver->set_conf1(10000, 1);
    driver->pulse1(5000);
    driver->wait1(0);

    driver->set_conf1(10000, 0);
    driver->pulse1(100000);
    while(!gpio_get(LIMIT3_PIN)) {
        sleep_ms(1);
    }
    driver->pulse1(0);
    sleep_ms(500);

    // Second Run
    driver->set_conf1(5000, 1);
    driver->pulse1(200);
    driver->wait1(0);

    driver->set_conf1(500, 0);
    driver->pulse1(5000);
    while(!gpio_get(LIMIT3_PIN)) {
        sleep_ms(1);
    }
    driver->pulse1(0);
    sleep_ms(1);
}

void task_home0(void *pvParameters) {
    TMC2209_step_dual * driver = (TMC2209_step_dual *)pvParameters;
    vTaskDelay(500);
    homeMotors0(driver);
}

void task_home1(void *pvParameters) {
    TMC2209_step_dual * driver = (TMC2209_step_dual *)pvParameters;
    homeMotors1(driver);
}
*/




int main() {
    stdio_init_all();

    printf("Hello, World!\n");

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

    CoreXYMaschine machine = CoreXYMaschine(&uart_driver0, &uart_driver1, &step_driver);
    
    volatile uint8_t a = uart_driver0.read(REG_ADDR_CHOPPER_CHOPCONF);
    volatile uint8_t b = uart_driver1.read(REG_ADDR_CHOPPER_CHOPCONF);

    machine.runTasks();

/*
    gpio_init(LIMIT0_PIN);
    gpio_set_dir(LIMIT0_PIN, GPIO_IN);
    gpio_pull_up(LIMIT0_PIN);
    gpio_set_input_hysteresis_enabled(LIMIT0_PIN, true);

    gpio_init(LIMIT1_PIN);
    gpio_set_dir(LIMIT1_PIN, GPIO_IN);
    gpio_pull_up(LIMIT1_PIN);
    gpio_set_input_hysteresis_enabled(LIMIT1_PIN, true);

    gpio_init(LIMIT2_PIN);
    gpio_set_dir(LIMIT2_PIN, GPIO_IN);
    gpio_pull_up(LIMIT2_PIN);
    gpio_set_input_hysteresis_enabled(LIMIT2_PIN, true);

    gpio_init(LIMIT3_PIN);
    gpio_set_dir(LIMIT3_PIN, GPIO_IN);
    gpio_pull_up(LIMIT3_PIN);
    gpio_set_input_hysteresis_enabled(LIMIT3_PIN, true);

    gpio_set_irq_enabled_with_callback(
        LIMIT0_PIN, 
        GPIO_IRQ_EDGE_RISE, 
        true, 
        isr_LimitSwitch
    );
    gpio_set_irq_enabled_with_callback(
        LIMIT1_PIN, 
        GPIO_IRQ_EDGE_RISE, 
        true, 
        isr_LimitSwitch
    );
    gpio_set_irq_enabled_with_callback(
        LIMIT2_PIN, 
        GPIO_IRQ_EDGE_RISE, 
        true, 
        isr_LimitSwitch
    );
    gpio_set_irq_enabled_with_callback(
        LIMIT3_PIN, 
        GPIO_IRQ_EDGE_RISE, 
        true, 
        isr_LimitSwitch
    );

    gpio_init(RGB2_B_PIN);
    gpio_init(RGB2_G_PIN);
    gpio_init(RGB2_R_PIN);
    gpio_set_dir(RGB2_B_PIN, GPIO_OUT);
    gpio_set_dir(RGB2_G_PIN, GPIO_OUT);
    gpio_set_dir(RGB2_R_PIN, GPIO_OUT);

    gpio_init(RGB1_B_PIN);
    gpio_init(RGB1_G_PIN);
    gpio_set_dir(RGB1_B_PIN, GPIO_OUT);
    gpio_set_dir(RGB1_G_PIN, GPIO_OUT);

    gpio_put(RGB1_B_PIN, 1);
    gpio_put(RGB1_G_PIN, 1);
    gpio_put(RGB2_B_PIN, 1);
    gpio_put(RGB2_G_PIN, 1);
    gpio_put(RGB2_R_PIN, 1);


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

    //step_driver.set_conf0(10000, 0);
    //step_driver.pulse0(1000000);
    homeMotors0(&step_driver);
    printf("Home0 done\n");
    homeMotors1(&step_driver);
    printf("Home1 done\n");
*/
    //xTaskCreate(task_home0, "Home0", 1024, &step_driver, 5, NULL);
    //xTaskCreate(task_home1, "Home1", 1024, &step_driver, 5, NULL);

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

        if(gpio_get(LIMIT1_PIN)) {
            gpio_put(RGB1_B_PIN, 0);
        } else {
            gpio_put(RGB1_B_PIN, 1);
        }

        if(gpio_get(LIMIT3_PIN)) {
            gpio_put(RGB1_G_PIN, 0);
        } else {
            gpio_put(RGB1_G_PIN, 1);
        }

        if(gpio_get(LIMIT0_PIN)) {
            gpio_put(RGB2_B_PIN, 0);
        } else {
            gpio_put(RGB2_B_PIN, 1);
        }

        if(gpio_get(LIMIT2_PIN)) {
            gpio_put(RGB2_G_PIN, 0);
        } else {
            gpio_put(RGB2_G_PIN, 1);
        }
    }
    */

    return 0;
}