#include <stdio.h>
#include "pico/stdio.h"
#include "pico/error.h"
#include "pico/multicore.h"
#include "hardware/i2c.h"
#include "Commande_vitesse.h"
#include "i2c_annexe.h"
#include "i2c_maitre.h"
#include "Evitement.h"
#include "Geometrie.h"
#include "Geometrie_robot.h"
#include "Asser_Moteurs.h"
#include "Localisation.h"
#include "Robot_config.h"
#include "Strategie_2024_plante.h"
#include "Strategie_2024_pots.h"
#include "Strategie.h"
#include "Trajectoire.h"
#include "QEI.h"
#include "gyro.h"
#include "Moteurs.h"


int test_calcul_position_pot(void);
int test_calage_debut(void);
int test_attrape_pot(void);
int test_aller_a_plante(void);
int test_attrape_plante(void);
int test_aller_zone_plante();
int test_pseudo_homologation(void);
int test_attrape_1_pot(void);
int test_echange_pot(void);
void affichage_test_strategie_2024(void);

int test_strategie_2024(){
    printf("A - Position groupes pot.\n");
    printf("B - Calage debut.\n");
    printf("C - Attrape pot.\n");
    printf("D - Aller  a plante.\n");
    printf("E - Attrape plante.\n");
    printf("F - Aller zone plante.\n");
    printf("G - Pseudo homologation.\n");
    printf("H - reglage pots.\n");
    printf("I - echange pots.\n");
    printf("J - Calage fin.\n");
    
    int lettre;
    do{
        lettre = getchar_timeout_us(0);
    }while(lettre == PICO_ERROR_TIMEOUT || lettre == 0);
    switch(lettre){
        case 'a':
        case 'A':
            while(test_calcul_position_pot());
            break;

        case 'b':
        case 'B':
            while(test_calage_debut());
            break;

        case 'c':
        case 'C':
            while(test_attrape_pot());
            break;

        case 'd':
        case 'D':
            while(test_aller_a_plante());
            break;

        case 'e':
        case 'E':
            while(test_attrape_plante());
            break;

        case 'f':
        case 'F':
            while(test_aller_zone_plante());
            break;

        case 'g':
        case 'G':
            while(test_pseudo_homologation());
            break;

        case 'h':
        case 'H':
            while(test_attrape_1_pot());
            break;

        case 'i':
        case 'I':
            while(test_echange_pot());
            break;

            
            

        case 'q':
        case 'Q':
            return 0;

    }
}


void print_position(struct position_t position){
    printf("x_mm: %.2f, y_mm: %.2f, angle: %.2f\n", position.x_mm, position.y_mm, position.angle_radian/DEGRE_EN_RADIAN);
}

int test_calcul_position_pot(){
    printf("\ngroupe: GROUPE_POT_B1, pot: 5 \n");
    print_position(groupe_pot_get_pot(GROUPE_POT_B1, POT_5));

    printf("\ngroupe: GROUPE_POT_B2, pot: 1 \n");
    print_position(groupe_pot_get_pot(GROUPE_POT_B2, POT_1));

    printf("\ngroupe: GROUPE_POT_L1, pot: 1 \n");
    print_position(groupe_pot_get_pot(GROUPE_POT_L1, POT_1));

    printf("\ngroupe: GROUPE_POT_L2, pot: 1 \n");
    print_position(groupe_pot_get_pot(GROUPE_POT_L2, POT_1));

    printf("\ngroupe: GROUPE_POT_R1, pot: 1 \n");
    print_position(groupe_pot_get_pot(GROUPE_POT_R1, POT_1));

    printf("\ngroupe: GROUPE_POT_R2, pot: 1 \n");
    print_position(groupe_pot_get_pot(GROUPE_POT_R2, POT_1));

    printf("\ngroupe: GROUPE_POT_L1, pot: 1 \n");
    print_position(groupe_pot_get_pot(GROUPE_POT_L1, POT_1));

    printf("\ngroupe: GROUPE_POT_L1, pot: 2 \n");
    print_position(groupe_pot_get_pot(GROUPE_POT_L1, POT_2));

    printf("\ngroupe: GROUPE_POT_L1, pot: 3 \n");
    print_position(groupe_pot_get_pot(GROUPE_POT_L1, POT_3));

    printf("\ngroupe: GROUPE_POT_L1, pot: 4 \n");
    print_position(groupe_pot_get_pot(GROUPE_POT_L1, POT_4));

    printf("\ngroupe: GROUPE_POT_L1, pot: 5 \n");
    print_position(groupe_pot_get_pot(GROUPE_POT_L1, POT_5));

    return 0;

}


int test_calage_debut(){
    int lettre, _step_ms = 1, temps_ms=0, _step_ms_gyro=2,temps_ms_init;
    struct trajectoire_t trajectoire;
    enum evitement_t evitement;
    enum etat_action_t etat_action;
    printf("test_calage_debut\n");


    i2c_maitre_init();
    Trajet_init();
    Balise_VL53L1X_init();
    Localisation_set(250,250,0);

    
    set_position_avec_gyroscope(0);
    if(get_position_avec_gyroscope()){
        printf("Init gyroscope\n");
        Gyro_Init();
    }

    stdio_flush();
    Trajet_config(100, 500);

    multicore_launch_core1(affichage_test_strategie_2024);


    temps_ms = Temps_get_temps_ms();
    temps_ms_init = temps_ms;
    do{
        i2c_gestion(i2c0);
        i2c_annexe_gestion();
        Balise_VL53L1X_gestion();
        
        // Routines à 1 ms
        if(temps_ms != Temps_get_temps_ms()){
            temps_ms = Temps_get_temps_ms();
            QEI_update();
            Localisation_gestion();
            AsserMoteur_Gestion(_step_ms);
            Evitement_gestion(_step_ms);

            // Routine à 2 ms
            if(temps_ms % _step_ms_gyro == 0){
                if(get_position_avec_gyroscope()){
                    Gyro_Read(_step_ms_gyro);
                }
            }

            etat_action = Strategie_calage_bas(COULEUR_BLEU, _step_ms);

        }
        lettre = getchar_timeout_us(0);
    //}while((lettre == PICO_ERROR_TIMEOUT) || (lettre == 0));
    }while(etat_action == ACTION_EN_COURS);
    Moteur_Stop();
    
    return 0;

}

int test_attrape_plante(){
    int lettre, _step_ms = 1, temps_ms=0, _step_ms_gyro=2,temps_ms_init;
    struct trajectoire_t trajectoire;
    enum evitement_t evitement;
    enum etat_action_t etat_action=ACTION_EN_COURS;

    printf("test_aller_a_plante\n");

    i2c_maitre_init();
    Trajet_init();
    Balise_VL53L1X_init();

    
    set_position_avec_gyroscope(0);
    if(get_position_avec_gyroscope()){
        printf("Init gyroscope\n");
        Gyro_Init();
    }

    stdio_flush();

    multicore_launch_core1(affichage_test_strategie_2024);


    temps_ms = Temps_get_temps_ms();
    temps_ms_init = temps_ms;
    do{
        i2c_gestion(i2c0);
        i2c_annexe_gestion();
        Balise_VL53L1X_gestion();
        
        // Routines à 1 ms
        if(temps_ms != Temps_get_temps_ms()){
            temps_ms = Temps_get_temps_ms();
            QEI_update();
            Localisation_gestion();
            AsserMoteur_Gestion(_step_ms);
            Evitement_gestion(_step_ms);

            // Routine à 2 ms
            if(temps_ms % _step_ms_gyro == 0){
                if(get_position_avec_gyroscope()){
                    Gyro_Read(_step_ms_gyro);
                }
            }

            etat_action = Strat_2024_plante_dans_pot(_step_ms, PLANTE_BRAS_6, ZONE_PLANTE_3);


        }
        lettre = getchar_timeout_us(0);
    //}while((lettre == PICO_ERROR_TIMEOUT) || (lettre == 0));
    }while(etat_action == ACTION_EN_COURS);
    Moteur_Stop();
    
    return 0;

}

int test_aller_a_plante(){
    int lettre, _step_ms = 1, temps_ms=0, _step_ms_gyro=2,temps_ms_init;
    struct trajectoire_t trajectoire;
    enum evitement_t evitement;
    enum etat_action_t etat_action=ACTION_EN_COURS;

    printf("test_aller_a_plante\n");

    i2c_maitre_init();
    Trajet_init();
    Balise_VL53L1X_init();

    
    set_position_avec_gyroscope(0);
    if(get_position_avec_gyroscope()){
        printf("Init gyroscope\n");
        Gyro_Init();
    }

    stdio_flush();

    multicore_launch_core1(affichage_test_strategie_2024);


    temps_ms = Temps_get_temps_ms();
    temps_ms_init = temps_ms;
    do{
        i2c_gestion(i2c0);
        i2c_annexe_gestion();
        Balise_VL53L1X_gestion();
        
        // Routines à 1 ms
        if(temps_ms != Temps_get_temps_ms()){
            temps_ms = Temps_get_temps_ms();
            QEI_update();
            Localisation_gestion();
            AsserMoteur_Gestion(_step_ms);
            Evitement_gestion(_step_ms);

            // Routine à 2 ms
            if(temps_ms % _step_ms_gyro == 0){
                if(get_position_avec_gyroscope()){
                    Gyro_Read(_step_ms_gyro);
                }
            }

            etat_action = Strat_2024_aller_a_plante(ZONE_PLANTE_AUCUNE);


        }
        lettre = getchar_timeout_us(0);
    //}while((lettre == PICO_ERROR_TIMEOUT) || (lettre == 0));
    }while(etat_action == ACTION_EN_COURS);
    Moteur_Stop();
    
    return 0;

}



int test_aller_zone_plante(){
    int lettre, _step_ms = 1, temps_ms=0, _step_ms_gyro=2,temps_ms_init;
    struct trajectoire_t trajectoire;
    enum evitement_t evitement;
    enum etat_action_t etat_action=ACTION_EN_COURS;

    printf("test_aller_a_plante\n");

    i2c_maitre_init();
    Trajet_init();
    Balise_VL53L1X_init();

    
    set_position_avec_gyroscope(0);
    if(get_position_avec_gyroscope()){
        printf("Init gyroscope\n");
        Gyro_Init();
    }

    stdio_flush();

    multicore_launch_core1(affichage_test_strategie_2024);

    Localisation_set(800, 200, 0);


    temps_ms = Temps_get_temps_ms();
    temps_ms_init = temps_ms;
    do{
        i2c_gestion(i2c0);
        i2c_annexe_gestion();
        Balise_VL53L1X_gestion();
        
        // Routines à 1 ms
        if(temps_ms != Temps_get_temps_ms()){
            temps_ms = Temps_get_temps_ms();
            QEI_update();
            Localisation_gestion();
            AsserMoteur_Gestion(_step_ms);
            Evitement_gestion(_step_ms);

            // Routine à 2 ms
            if(temps_ms % _step_ms_gyro == 0){
                if(get_position_avec_gyroscope()){
                    Gyro_Read(_step_ms_gyro);
                }
            }

            etat_action = Strat_2024_aller_zone_plante(ZONE_PLANTE_3, _step_ms);


        }
        lettre = getchar_timeout_us(0);
    //}while((lettre == PICO_ERROR_TIMEOUT) || (lettre == 0));
    }while(etat_action == ACTION_EN_COURS);
    Moteur_Stop();
    
    return 0;

}


int test_attrape_pot(){
    int lettre, _step_ms = 1, temps_ms=0, _step_ms_gyro=2,temps_ms_init;
    struct trajectoire_t trajectoire;
    enum evitement_t evitement;
    enum etat_action_t etat_action=ACTION_EN_COURS;

    enum {
        TAP_CALAGE,
        TAP_POT
    } etat_test = TAP_CALAGE;

    printf("test_attrape_pot\n");


    i2c_maitre_init();
    Trajet_init();
    Balise_VL53L1X_init();
    Localisation_set(250, 250, (45. * DEGRE_EN_RADIAN) - ANGLE_PINCE);

    
    set_position_avec_gyroscope(1);
    if(get_position_avec_gyroscope()){
        printf("Init gyroscope\n");
        Gyro_Init();
    }

    stdio_flush();
    Trajet_config(TRAJECT_CONFIG_STD);

    multicore_launch_core1(affichage_test_strategie_2024);


    temps_ms = Temps_get_temps_ms();
    temps_ms_init = temps_ms;
    do{
        i2c_gestion(i2c0);
        i2c_annexe_gestion();
        Balise_VL53L1X_gestion();
        
        // Routines à 1 ms
        if(temps_ms != Temps_get_temps_ms()){
            temps_ms = Temps_get_temps_ms();
            QEI_update();
            Localisation_gestion();
            AsserMoteur_Gestion(_step_ms);
            Evitement_gestion(_step_ms);

            // Routine à 2 ms
            if(temps_ms % _step_ms_gyro == 0){
                if(get_position_avec_gyroscope()){
                    Gyro_Read(_step_ms_gyro);
                }
            }
            switch(etat_test){
                case TAP_CALAGE:
                    if(Strategie_calage_debut_manuel(COULEUR_BLEU, _step_ms) == ACTION_TERMINEE){
                        etat_test=TAP_POT;
                    }
                    break;
                case TAP_POT:
                    etat_action = Strat_2024_attrape_pot(GROUPE_POT_B1, _step_ms);
                    break;
            }

        }
        lettre = getchar_timeout_us(0);
    //}while((lettre == PICO_ERROR_TIMEOUT) || (lettre == 0));
    }while(etat_action == ACTION_EN_COURS);
    printf("STRATEGIE_LOOP_2\n");
    printf("Lettre : %d; %c\n", lettre, lettre);

    while(1){Moteur_Stop();}
    
    if(lettre == 'q' && lettre == 'Q'){
        return 0;
    }
    return 0;

}

int test_pseudo_homologation(){
    int lettre, _step_ms = 1, temps_ms=0, _step_ms_gyro=2,temps_ms_init;
    struct trajectoire_t trajectoire;
    enum evitement_t evitement;
    enum etat_action_t etat_action=ACTION_EN_COURS;
    static int tempo_ms;

    enum {
        TAP_CALAGE,
        TAP_POT,
        TAP_PLANTE_ORIENTATION,
        TAP_PLANTE_ATTRAPE_1,
        TAP_PLANTE_ATTRAPE_2,
        TAP_ECHANGE_POT,
        TAP_PLANTE_ATTRAPE_3,
        TAP_PLANTE_ATTRAPE_4,
        TAP_RENTRE,
        TAP_DEPOSE
    } etat_test = TAP_CALAGE;

    printf("test_pseudo_homologation\n");


    i2c_maitre_init();
    Trajet_init();
    Balise_VL53L1X_init();
    Localisation_set(250, 250, (45. * DEGRE_EN_RADIAN) - ANGLE_PINCE);

    
    set_position_avec_gyroscope(1);
    if(get_position_avec_gyroscope()){
        printf("Init gyroscope\n");
        Gyro_Init();
    }

    stdio_flush();
    Trajet_config(TRAJECT_CONFIG_STD);

    multicore_launch_core1(affichage_test_strategie_2024);


    temps_ms = Temps_get_temps_ms();
    temps_ms_init = temps_ms;
    do{
        i2c_gestion(i2c0);
        i2c_annexe_gestion();
        Balise_VL53L1X_gestion();
        
        // Routines à 1 ms
        if(temps_ms != Temps_get_temps_ms()){
            temps_ms = Temps_get_temps_ms();
            QEI_update();
            Localisation_gestion();
            AsserMoteur_Gestion(_step_ms);
            Evitement_gestion(_step_ms);

            // Routine à 2 ms
            if(temps_ms % _step_ms_gyro == 0){
                if(get_position_avec_gyroscope()){
                    Gyro_Read(_step_ms_gyro);
                }
            }
            switch(etat_test){
                case TAP_CALAGE:
                    if(Strategie_calage_debut_manuel(COULEUR_BLEU, _step_ms) == ACTION_TERMINEE){
                        etat_test=TAP_POT;
                    }
                    break;
                case TAP_POT:
                    if(Strat_2024_attrape_pot(GROUPE_POT_B1, _step_ms) == ACTION_TERMINEE){
                        etat_test=TAP_PLANTE_ORIENTATION;
                    }
                    break;

                case TAP_PLANTE_ORIENTATION:
                    if(Strat_2024_aller_zone_plante(ZONE_PLANTE_3, _step_ms) == ACTION_TERMINEE){
                        etat_test=TAP_PLANTE_ATTRAPE_1;
                    }
                    break;

                case TAP_PLANTE_ATTRAPE_1:
                    etat_action = Strat_2024_plante_dans_pot(_step_ms, PLANTE_BRAS_1, ZONE_PLANTE_3);
                    if( etat_action == ACTION_TERMINEE){
                        etat_test=TAP_PLANTE_ATTRAPE_2;
                        etat_action = ACTION_EN_COURS;
                    }else if( etat_action == ACTION_ECHEC){
                        etat_test=TAP_RENTRE;
                        etat_action = ACTION_EN_COURS;
                    }
                    break;
                
                case TAP_PLANTE_ATTRAPE_2:
                    etat_action = Strat_2024_plante_dans_pot(_step_ms, PLANTE_BRAS_6, ZONE_PLANTE_3);
                    if( etat_action == ACTION_TERMINEE){
                        etat_test=TAP_ECHANGE_POT;
                        etat_action = ACTION_EN_COURS;
                    }else if( etat_action == ACTION_ECHEC){
                        etat_test=TAP_RENTRE;
                        etat_action = ACTION_EN_COURS;
                    }
                    break;

                case TAP_ECHANGE_POT:
                    if(Strat_2024_echange_pot_avant_arriere(_step_ms) == ACTION_TERMINEE){
                        etat_test=TAP_PLANTE_ATTRAPE_3;
                    }
                    break;
                
                case TAP_PLANTE_ATTRAPE_3:
                    etat_action = Strat_2024_plante_dans_pot(_step_ms, PLANTE_BRAS_1, ZONE_PLANTE_3);
                    if( etat_action == ACTION_TERMINEE){
                        etat_test=TAP_PLANTE_ATTRAPE_4;
                        etat_action = ACTION_EN_COURS;
                    }else if( etat_action == ACTION_ECHEC){
                        etat_test=TAP_RENTRE;
                        etat_action = ACTION_EN_COURS;
                    }
                    break;

                case TAP_PLANTE_ATTRAPE_4:
                    etat_action = Strat_2024_plante_dans_pot(_step_ms, PLANTE_BRAS_1, ZONE_PLANTE_3);
                    if( etat_action == ACTION_TERMINEE){
                        etat_test=TAP_RENTRE;
                        etat_action = ACTION_EN_COURS;
                    }else if( etat_action == ACTION_ECHEC){
                        etat_test=TAP_RENTRE;
                        etat_action = ACTION_EN_COURS;
                    }
                    break;

                case TAP_RENTRE:
                    float angle_destination;
                    angle_destination = 15 * DEGRE_EN_RADIAN;
                    Trajet_config(TRAJECT_CONFIG_AVANCE_ET_TOURNE);
                    if(Strategie_tourner_et_aller_a(450, 450, angle_destination, EVITEMENT_PAUSE_DEVANT_OBSTACLE, _step_ms) == ACTION_TERMINEE){
                        etat_test=TAP_DEPOSE;
                        i2c_annexe_actionneur_pot(0, BRAS_POT_SOL, DOIGT_TIENT);
                        i2c_annexe_actionneur_pot(5, BRAS_POT_SOL, DOIGT_TIENT);
                        tempo_ms=500;
                    }
                    break;
                
                case TAP_DEPOSE:
                    tempo_ms--;
                    commande_vitesse_stop();
                    if(tempo_ms<= 0){
                        i2c_annexe_actionneur_pot(0, BRAS_POT_SOL, DOIGT_LACHE);
                        i2c_annexe_actionneur_pot(5, BRAS_POT_SOL, DOIGT_LACHE);
                    }
                    break;

            }

        }
        lettre = getchar_timeout_us(0);
    //}while((lettre == PICO_ERROR_TIMEOUT) || (lettre == 0));
    }while(etat_action == ACTION_EN_COURS);
    printf("STRATEGIE_LOOP_2\n");
    printf("Lettre : %d; %c\n", lettre, lettre);

    while(1){Moteur_Stop();}
    
    if(lettre == 'q' && lettre == 'Q'){
        return 0;
    }
    return 0;

}

extern float abscisse;
void affichage_test_strategie_2024(){
    uint32_t temps;
    enum validite_vl53l8_t validite_vl53l8;
    float angle, distance;
    
    while(true){
        temps = time_us_32()/1000;
        //temps_cycle_display();
        printf(">V_consigne_A:%ld:%f\n>V_consigne_B:%ld:%f\n>V_consigne_C:%ld:%f\n", temps, AsserMoteur_getConsigne_mm_s(MOTEUR_A), temps, AsserMoteur_getConsigne_mm_s(MOTEUR_B), temps, AsserMoteur_getConsigne_mm_s(MOTEUR_C));
        printf(">pos_x:%ld:%f\n", temps, Localisation_get().x_mm);
        printf(">pos_y:%ld:%f\n", temps, Localisation_get().y_mm);
        printf(">pos_angle:%ld:%f\n", temps, (Localisation_get().angle_radian + ANGLE_PINCE)/ DEGRE_EN_RADIAN );

        printf(">c_pos_x:%ld:%f\n", temps, Trajet_get_consigne().x_mm);
        printf(">c_pos_y:%ld:%f\n", temps, Trajet_get_consigne().y_mm);
        printf(">c_pos_angle:%ld:%f\n", temps, (Trajet_get_consigne().angle_radian+ ANGLE_PINCE) / DEGRE_EN_RADIAN);

        printf(">abscisse:%ld:%f\n", temps, abscisse);

        printf(">tirette:%ld:%d\n", temps, attente_tirette());

        printf(">distance_obstacle:%ld:%f\n", temps, Trajet_get_obstacle_mm());
        printf(">angle_avance:%ld:%f\n", temps, Trajet_get_orientation_avance());

        i2c_annexe_get_VL53L8(&validite_vl53l8, &angle, &distance);
        printf(">v:%d\n", validite_vl53l8);
        switch(validite_vl53l8){
            case VL53L8_BORDURE:
                printf(">b_angle:%.2f\n>b_distance:%.2f\n", angle, distance);
                break;
            case VL53L8_PLANTE:
                printf(">p_angle:%.2f\n>p_distance:%.2f\n", angle, distance);
                break;
            case VL53L8_DISTANCE_LOIN:
                printf("\n>v_distance:%.2f\n", distance);
                break;
        }
        
        sleep_ms(10);
    }
}

/// @brief Fonction abandonnée - pour le réglage des bras un par un 
/// @param  
/// @return 
int test_attrape_1_pot(void){
    int lettre, _step_ms = 1, temps_ms=0, _step_ms_gyro=2,temps_ms_init;
    struct trajectoire_t trajectoire;
    enum evitement_t evitement;
    enum etat_action_t etat_action=ACTION_EN_COURS;
    int32_t tempo_ms;

    enum {
        TAP_CALAGE,
        TAP_APPROCHE_POT,
        TAP_TOURNE_POT,
        TAP_ATTRAPE_POT,
        TAP_ATTRAPE_TEMPO
    } etat_test = TAP_CALAGE;

    printf("test_attrape_1_pot\n");


    i2c_maitre_init();
    Trajet_init();
    Balise_VL53L1X_init();
    Localisation_set(250, 250, (45. * DEGRE_EN_RADIAN) - ANGLE_PINCE);

    
    set_position_avec_gyroscope(1);
    if(get_position_avec_gyroscope()){
        printf("Init gyroscope\n");
        Gyro_Init();
    }

    stdio_flush();
    Trajet_config(TRAJECT_CONFIG_STD);

    multicore_launch_core1(affichage_test_strategie_2024);

    float angle_bras[6] =
    {
        180 * DEGRE_EN_RADIAN,
        120 * DEGRE_EN_RADIAN,
        60 * DEGRE_EN_RADIAN,
        0,
        -60 * DEGRE_EN_RADIAN,
        -120 * DEGRE_EN_RADIAN
    };

    float angle_bras_correction[6] =
    {
        11 * DEGRE_EN_RADIAN,
        0 * DEGRE_EN_RADIAN,
        0 * DEGRE_EN_RADIAN,
        0,
        0 * DEGRE_EN_RADIAN,
        13 * DEGRE_EN_RADIAN
    };


    temps_ms = Temps_get_temps_ms();
    temps_ms_init = temps_ms;
    static struct position_t position_pot, position_approche_pot, position_attrape_pot;
    position_pot = groupe_pot_get_pot(GROUPE_POT_B1, POT_5);
    position_approche_pot = Geometrie_deplace(position_pot, -DISTANCE_APPROCHE_POT_MM);
    position_attrape_pot = Geometrie_deplace(position_pot, -DISTANCE_ATTRAPE_POT_MM);

    int bras = BRAS_1;

    do{
        i2c_gestion(i2c0);
        i2c_annexe_gestion();
        Balise_VL53L1X_gestion();
        
        // Routines à 1 ms
        if(temps_ms != Temps_get_temps_ms()){
            temps_ms = Temps_get_temps_ms();
            QEI_update();
            Localisation_gestion();
            AsserMoteur_Gestion(_step_ms);
            Evitement_gestion(_step_ms);

            // Routine à 2 ms
            if(temps_ms % _step_ms_gyro == 0){
                if(get_position_avec_gyroscope()){
                    Gyro_Read(_step_ms_gyro);
                }
            }
            switch(etat_test){
                case TAP_CALAGE:
                    if(Strategie_calage_bas(COULEUR_BLEU, _step_ms) == ACTION_TERMINEE){
                        etat_test=TAP_APPROCHE_POT;
                    }
                    break;
                case TAP_APPROCHE_POT:
                    Trajet_config(TRAJECT_CONFIG_AVANCE_ET_TOURNE);
                    if(Strategie_aller_a(position_approche_pot.x_mm, position_approche_pot.y_mm, EVITEMENT_PAUSE_DEVANT_OBSTACLE, _step_ms) == ACTION_TERMINEE){
                        etat_test=TAP_TOURNE_POT;
                    }
                    break;

                case TAP_TOURNE_POT:
                    if( Strategie_tourner_a(position_approche_pot.angle_radian - angle_bras[bras] + angle_bras_correction[bras], _step_ms) == ACTION_TERMINEE){
                        etat_test=TAP_ATTRAPE_POT;
                    }
                    break;
                
                case TAP_ATTRAPE_POT:
                    Trajet_config(TRAJECT_CONFIG_AVANCE_ET_TOURNE);
                    i2c_annexe_actionneur_pot(bras, BRAS_POT_SOL, DOIGT_TIENT);
                    if( Strategie_tourner_et_aller_a(
                            position_attrape_pot.x_mm, position_attrape_pot.y_mm, position_attrape_pot.angle_radian - angle_bras[bras] + angle_bras_correction[bras],
                            EVITEMENT_SANS_EVITEMENT, _step_ms) == ACTION_TERMINEE){
                        i2c_annexe_actionneur_pot(bras, BRAS_HAUT, DOIGT_TIENT);
                        tempo_ms = 2000;
                        etat_test = TAP_ATTRAPE_TEMPO;
                    }
                    break;
                
                case TAP_ATTRAPE_TEMPO:
                    tempo_ms--;
                    if(tempo_ms < 0){
                        etat_action = ACTION_TERMINEE;
                    }
                    break;
            }

        }
        lettre = getchar_timeout_us(0);
    //}while((lettre == PICO_ERROR_TIMEOUT) || (lettre == 0));
    }while(etat_action == ACTION_EN_COURS);
    printf("STRATEGIE_LOOP_2\n");
    printf("Lettre : %d; %c\n", lettre, lettre);

    while(1){Moteur_Stop();}
    
    if(lettre == 'q' && lettre == 'Q'){
        return 0;
    }
    return 0;

}


int test_echange_pot(){
    int lettre, _step_ms = 1, temps_ms=0, _step_ms_gyro=2,temps_ms_init;
    struct trajectoire_t trajectoire;
    enum evitement_t evitement;
    enum etat_action_t etat_action=ACTION_EN_COURS;
    static int tempo_ms;

    static enum {
        TEP_INIT,
        TEP_ACTION,
    } etat_echange_pot = TEP_INIT;

    printf("test_echange_pot\n");

    i2c_maitre_init();
    Trajet_init();
    Balise_VL53L1X_init();

    
    set_position_avec_gyroscope(1);
    if(get_position_avec_gyroscope()){
        printf("Init gyroscope\n");
        Gyro_Init();
    }

    stdio_flush();

    multicore_launch_core1(affichage_test_strategie_2024);


    temps_ms = Temps_get_temps_ms();
    temps_ms_init = temps_ms;
    do{
        i2c_gestion(i2c0);
        i2c_annexe_gestion();
        Balise_VL53L1X_gestion();
        
        // Routines à 1 ms
        if(temps_ms != Temps_get_temps_ms()){
            temps_ms = Temps_get_temps_ms();
            QEI_update();
            Localisation_gestion();
            AsserMoteur_Gestion(_step_ms);
            Evitement_gestion(_step_ms);

            // Routine à 2 ms
            if(temps_ms % _step_ms_gyro == 0){
                if(get_position_avec_gyroscope()){
                    Gyro_Read(_step_ms_gyro);
                }
            }

            switch(etat_echange_pot){
                case TEP_INIT:
                    i2c_annexe_actionneur_pot(0, BRAS_POT_SOL, DOIGT_TIENT);
                    i2c_annexe_actionneur_pot(1, BRAS_HAUT, DOIGT_TIENT);
                    i2c_annexe_actionneur_pot(2, BRAS_POT_SOL, DOIGT_TIENT);
                    i2c_annexe_actionneur_pot(3, BRAS_POT_SOL, DOIGT_TIENT);
                    i2c_annexe_actionneur_pot(4, BRAS_HAUT, DOIGT_TIENT);
                    i2c_annexe_actionneur_pot(5, BRAS_POT_SOL, DOIGT_TIENT);
                    tempo_ms=3000;
                    etat_echange_pot = TEP_ACTION;
                    break;

                case TEP_ACTION:
                    tempo_ms--;
                    if (tempo_ms <= 0)
                    {
                        etat_action = Strat_2024_echange_pot_avant_arriere(_step_ms);
                    }
                    break;


            }

        }
        lettre = getchar_timeout_us(0);
    //}while((lettre == PICO_ERROR_TIMEOUT) || (lettre == 0));
    }while(etat_action == ACTION_EN_COURS);
    Moteur_Stop();
    
    return 0;

}