#include <stdio.h>
#include "pico/stdlib.h"
#include "hardware/uart.h"
#include "hardware/irq.h" // Inclure le fichier d'en-tête pour les interruptions
#include "Tmc_uart.h"
#include "Tmc2209.h"

#include "hardware/gpio.h"
#include "unistd.h"


TMC_UART uart_instance;
TMC2209 Tmc2209;



void test_movement(){
    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);
        sleep_us(1);
        int stg = get_stallguard(&Tmc2209);
        printf("Stg : %d \n",stg);
    }

    uint32_t start_time = time_us_32();
    uint32_t current_time = start_time;
    
    while (current_time - start_time < 10000000) { // 10 secondes en microsecondes
        int stg = get_stallguard(&Tmc2209);
        printf("Stg : %d \n",stg);
        current_time = time_us_32();

    }



    for (int i = max; i >= 100; i -= 100) {
        set_vactual(&Tmc2209,i*256);
        sleep_us(1);
        int stg = get_stallguard(&Tmc2209);
        printf("Stg : %d \n",stg);
    }
    set_vactual(&Tmc2209,0);

    // sleep_ms(10000);
    gpio_put(LED_PIN, 1);

}


void setup(){
    clear_general_stat(&Tmc2209);
    // sleep_ms(2000);
    set_voltage_sense(&Tmc2209, false);
    // sleep_ms(2000);
    set_current_flow(&Tmc2209, 100, 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);
}

void test_direction(){
    int direction = get_direction_shart(&Tmc2209);
    printf("Direction 1 : %d\n", direction);
    sleep_ms(1000);

    set_direction_shart(&Tmc2209,false);
    sleep_ms(1000);

    direction = get_direction_shart(&Tmc2209);
    printf("Direction 2 (false): %d\n", direction);
    sleep_ms(1000);

    set_direction_shart(&Tmc2209,true);
    sleep_ms(1000);

    direction = get_direction_shart(&Tmc2209);
    printf("Direction 3 (true): %d\n", direction);
}


void test_ustep(){
    int32_t resolution = get_microstepping_resolution(&Tmc2209);
    printf("Microstepping resolution begin : %ld\n", resolution);
    sleep_ms(1000);
    set_microstepping_resolution(&Tmc2209,256);
    sleep_ms(1000);
    resolution = get_microstepping_resolution(&Tmc2209);
    printf("Microstepping resolution after new go to 1/256: %ld\n", resolution);
    sleep_ms(1000);
    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));
}

void test_iscale(){
    int iscale = get_iscale_analog(&Tmc2209);
    printf("Iscale 1 : %d\n", iscale);
    sleep_ms(1000);
    set_iscale_analog(&Tmc2209,false);
    sleep_ms(1000);
    iscale = get_iscale_analog(&Tmc2209);
    printf("Iscale 2 (false): %d\n", iscale);
    sleep_ms(1000);
    set_iscale_analog(&Tmc2209,true);
    sleep_ms(1000);
    iscale = get_iscale_analog(&Tmc2209);
    printf("Iscale 3 (true): %d\n", iscale);
}

void test_interpolation(){
    int interpolation = get_interpolation(&Tmc2209);
    printf("Interpolation 1 : %d\n", interpolation);
    sleep_ms(1000);
    set_interpolation(&Tmc2209,false);
    sleep_ms(1000);
    interpolation = get_interpolation(&Tmc2209);
    printf("Interpolation 2 (false): %d\n", interpolation);
    sleep_ms(1000);
    set_interpolation(&Tmc2209,true);
    sleep_ms(1000);
    interpolation = get_interpolation(&Tmc2209);
    printf("Interpolation 3 (true): %d\n", interpolation);
}

void test_internal_resistor_sense(){
    int resistor = get_internal_resistor_sense(&Tmc2209);
    printf("Internal_resistor_sense 1 : %d\n", resistor);
    sleep_ms(1000);
    set_internal_resistor_sense(&Tmc2209,false);
    sleep_ms(1000);
    resistor = get_internal_resistor_sense(&Tmc2209);
    printf("Internal_resistor_sense 2 (false): %d\n", resistor);
    sleep_ms(1000);
    set_internal_resistor_sense(&Tmc2209,true);
    sleep_ms(1000);
    resistor = get_internal_resistor_sense(&Tmc2209);
    printf("Internal_resistor_sense 3 (true): %d\n", resistor);
}

void test_spread_cycle(){
    int spread_cycle = get_spread_cycle(&Tmc2209);
    printf("Spread_cycle 1 : %d\n", spread_cycle);
    sleep_ms(1000);
    set_internal_resistor_sense(&Tmc2209,false);
    sleep_ms(1000);
    spread_cycle = get_spread_cycle(&Tmc2209);
    printf("Spread_cycle 2 (false): %d\n", spread_cycle);
    sleep_ms(1000);
    set_internal_resistor_sense(&Tmc2209,true);
    sleep_ms(1000);
    spread_cycle = get_spread_cycle(&Tmc2209);
    printf("Spread_cycle 3 (true): %d\n", spread_cycle);
}


int main() {
    stdio_init_all();


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

    
    sleep_ms(4000);


    // clear_general_stat(&uart_instance);
    printf("UART initialisé avec succès\n");

    // driver_status(&Tmc2209);
    // general_config(&Tmc2209);
    // general_stat(&Tmc2209);

    setup();
    
    // test_iscale();
    // test_interpolation();
    // test_internal_resistor_sense();
    // test_spread_cycle();

    // test_direction();

    // test_ustep();

    test_movement();


    return 0;
}