#include <stdio.h>
#include "pico/stdlib.h"
#include "pico/time.h"

#include "hardware/uart.h"
#include "hardware/irq.h" // Inclure le fichier d'en-tête pour les interruptions
#include "hardware/gpio.h"
#include "hardware/i2c.h"

#include "Tmc_uart.h"
#include "Tmc2209.h"

#include "AS5600L.h"

#include "unistd.h"

#define I2C_PORT i2c0
#define I2C_SDA_PIN 8
#define I2C_SCL_PIN 9


int main() {
    stdio_init_all();

    TMC_UART uart_instance;
    TMC2209 Tmc2209;
    AS5600L as5600l;

    // Initialisation de l'UART avec les paramètres par défaut
    
    tmc_Uart_Init(&uart_instance, uart1, -1, -1, -1);

    TMC2209_Init(&Tmc2209,&uart_instance);
    as560x_init(&as5600l,I2C_PORT,100000,I2C_SDA_PIN,I2C_SCL_PIN);

    int abs = 0;
    abs = update_pos;
    sleep_ms(4000);



    // driver_status(&uart_instance);
    // general_config(&uart_instance);

    // clear_general_stat(&uart_instance);
    printf("UART initialisé avec succès\n");
    int32_t resolution = get_microstepping_resolution(&Tmc2209);
    printf("Microstepping resolution begin : %ld\n", resolution);
    sleep_ms(10);
    set_microstepping_resolution(&Tmc2209,256);
    sleep_ms(10);
    resolution = get_microstepping_resolution(&Tmc2209);
    printf("Microstepping resolution after new go to 1/256: %ld\n", resolution);
    sleep_ms(10);
    // set_microstepping_resolution(&Tmc2209,16);
    // sleep_ms(10);
    // resolution = get_microstepping_resolution(&Tmc2209);
    // printf("Microstepping resolution after renew go to 1/16: %ld\n", resolution);
    // printf(get_microstepping_resolution(&Tmc2209));
    // clear_general_stat(&Tmc2209);
    // sleep_ms(2000);
    // set_voltage_sense(&Tmc2209, false);
    // sleep_ms(2000);

    // set_current_flow(&Tmc2209, 1800, 0.5, 10, 5.4);

    // sleep_ms(2000);
    // set_iscale_analog(&Tmc2209, false);
    // sleep_ms(2000);
    // set_interpolation(&Tmc2209, true);
    // sleep_ms(2000);
    // set_internal_resistor_sense(&Tmc2209, false);
    // sleep_ms(2000);
    // set_spread_cycle(&Tmc2209, true);
    // sleep_ms(2000);
    // set_microstepping_resolution(&Tmc2209,256);
    // sleep_ms(2000);
    // set_microstep_resolution_regselect(&Tmc2209, true);
    // int32_t resolution = get_microstepping_resolution(&Tmc2209);
    // printf("Microstepping resolution begin : %ld\n", resolution);
    const uint LED_PIN = 7;
    gpio_init(LED_PIN);
    gpio_set_dir(LED_PIN, GPIO_OUT);
    gpio_put(LED_PIN, 0);

    sleep_ms(1000);
    int max = 1600;
    for (int i = 100; i <= max; i += 100) {
        set_vactual(&Tmc2209,i*256);
        abs = update_pos(&as5600l);
        printf("Position absolue : %d\n",abs);
    }

    uint32_t start_time = time_us_32();
    uint32_t current_time = start_time;

    while (current_time - start_time < 10000000) { // 10 secondes en microsecondes
        abs = update_pos(&as5600l);
        printf("Position absolue : %d\n", abs);
        
        // Mise à jour du temps actuel
        current_time = time_us_32();
    }

    for (int i = max; i >= 100; i -= 100) {
        set_vactual(&Tmc2209,i*256);
        abs = update_pos(&as5600l);
        printf("Position absolue : %d\n",abs);
    }
    set_vactual(&Tmc2209,0);

    sleep_ms(10000);
    gpio_put(LED_PIN, 1);
    // int32_t resolution;
    // general_config(&Tmc2209);

    // while (true) {

    // }
            

        // for (int i = 1000; i <= 200000; i += 10) {
        //     set_vactual(&Tmc2209, i);
        //     sleep_ms(2);
        // }

        // for (int i = 20000; i >= 10000; i -= 10) {
        //     set_vactual(&Tmc2209, i);
        //     sleep_ms(2);
        // }

        // sleep_ms(1000);
        // printf("end\n");
    //     // read_reg(&Tmc2209,0x6C);
    //     printf(driver_status(&Tmc2209));
    //     sleep_ms(1000);

    //     // char data[] = "Hello, UART!\n";
    //     // for (int i = 0; data[i] != '\0'; ++i) {
    //     //     TMC_UART_Write(&Tmc2209, data[i]);
    //     // }
    // }
    
    return 0;
}