#include "Holonome2023.h"
#include "Strategie_2024.h"
#include "Demonstration.h"

#define TEST_TIMEOUT_US 10000000

#define CAPTEUR_POUR_ATTENTE 6
#define CAPTEUR_POUR_ATTENTE_DEVANT 6
#define CAPTEUR_POUR_ATTENTE_GAUCHE 9
#define CAPTEUR_POUR_ATTENTE_DROIT 3
#define CAPTEUR_POUR_ATTENTE_ARRIERE 0


int Demonstration_init(void);
enum etat_action_t Demonstration_calage();
enum etat_action_t Demonstration_rectangle(int avance_x_mm, int avance_y_mm);
enum etat_action_t Demonstration_avance_puis_tourne(int avance_x_mm, int avance_y_mm, float angle_degrees);
enum etat_action_t Demonstration_tourne(float angle_degrees);
enum etat_action_t Demonstration_bezier();
enum etat_action_t Demonstration_attente();
enum etat_action_t Demonstration_leve_bras(uint32_t bras);
enum etat_action_t  Demonstration_baisse_bras(void);
enum etat_action_t Demonstration_attrape_plante();
enum etat_action_t Demonstration_attente_capteur(int capteur);
void Demonstration_menu_balise(void);
void Demonstration_prise_plante(void);
void Demonstration_actionneurs(void);

uint32_t temps_ms_demo = 0, temps_ms_old;

void demo_affiche_localisation(){
    struct position_t position;
    while(1){
        position = Localisation_get();
        printf(">X:%f\n>Y:%f\n>angle:%f\n", position.x_mm, position.y_mm, position.angle_radian *180. / 3.141592654);
        printf(">v_bat:%2.2f\n", i2c_annexe_get_tension_batterie() / 10.);
        printf(">capteur:%d\n", Balise_VL53L1X_get_capteur_cm(CAPTEUR_POUR_ATTENTE));
        sleep_ms(50);
    }
}

int Demonstration_menu(void){
    static int iteration = 2;
    int rep;
    printf("Mode demo - init\n");
    set_position_avec_gyroscope(false);
    Demonstration_init();
    while(1){
        do{
            printf("A - Calage dans l'angle\n");
            printf("B - Trajets unitaires - Rectangle\n");
            printf("C - Trajets unitaires - Droit puis tourne\n");
            printf("D - Trajets unitaires - Bezier\n");
            printf("E - Test attente\n");
            /*printf("C - Trajets enchaines - manuels\n");
            printf("D - Trajets enchaines - auto\n");
            printf("E - Asservissement angulaire\n");*/
            printf("F - Rotation et actionneurs\n");
            printf("G - Prise plante\n");
            printf("M - Menu Balise\n");
            printf("Z - Sem-automatique\n");
            printf("Q - Quitter\n");
            rep = getchar_timeout_us(TEST_TIMEOUT_US);
        }while(rep == 0 || rep == PICO_ERROR_TIMEOUT);

        switch (rep)
        {
        case 'a':
        case 'A':
            Demonstration_calage();
            break;

        case 'b':
        case 'B':
            printf("Demonstration rectangle\n");
            Demonstration_rectangle(550, 1000);
            printf("Recalage\n");
            Demonstration_calage();
            break;

        case 'c':
        case 'C':
            Demonstration_avance_puis_tourne(300, 1000, 720.);
            break;

        case 'd':
        case 'D':
            Demonstration_bezier();
            break;

        case 'e':
        case 'E':
            printf("Début attente\n");
            Demonstration_attente();
            printf("Fin attente\n");
            break;

        case 'f':
        case 'F':
            while(1){
                printf("Demo actionneur\n");
                Demonstration_actionneurs();
            }
            break;

        case 'g':
        case 'G':
            printf("Début prise plante\n");
            Demonstration_prise_plante();
            printf("Fin prise plante\n");
            break;

        case 'm':
        case 'M':
            printf("Menu balise\n");
            Demonstration_menu_balise();
            printf("Fin menu balise\n");
            break;

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

        case 'z':
        case 'Z':
            printf("Demo semi-auto\n");
            Demonstration_semiauto();
            break;

        }
    }
    return 1;
}

int Demonstration_init(){
    
    Holonome2023_init();
    temps_ms_demo = Temps_get_temps_ms();
    temps_ms_old = temps_ms_demo;   
    multicore_launch_core1(demo_affiche_localisation);
}

void Demonstration_semiauto(){
    Demonstration_calage();
    Demonstration_attente();

    while(true){
        Demonstration_rectangle(550, 1000);
        Demonstration_calage();
        Demonstration_attente();

        Demonstration_avance_puis_tourne(300, 1000, 720.);
        Demonstration_calage();
        Demonstration_attente();

        Demonstration_bezier();
        Demonstration_calage();
        Demonstration_attente();
    }
}

void Demonstration_auto(){
    Demonstration_calage();
    Demonstration_attente();

    while(true){
        Demonstration_rectangle(550, 1000);
        Demonstration_calage();

        Demonstration_avance_puis_tourne(300, 1000, 720.);
        Demonstration_calage();

        Demonstration_bezier();
        Demonstration_calage();
        
        Demonstration_attente();
    }
}

void Demonstration_actionneurs(){
    Demonstration_attente();
    Demonstration_attrape_plante();
    Demonstration_tourne(30);
    Demonstration_leve_bras(0);
    Demonstration_tourne(-60);
    Demonstration_leve_bras(5);
    Demonstration_tourne(120);
    Demonstration_leve_bras(1);
    Demonstration_tourne(-180);
    Demonstration_leve_bras(4);
    Demonstration_tourne(240);
    Demonstration_leve_bras(2);
    Demonstration_tourne(-300);
    Demonstration_leve_bras(3);
    Demonstration_tourne(150);
    Demonstration_baisse_bras();
}

void Demonstration_prise_plante(){
    static int32_t m_temps_ms, m_timer;
    enum etat_action_t etat_action = ACTION_EN_COURS;
    enum etat_action_t etat_action_step;
    static enum {
        INIT,
        TIENT_POT,
        LEVE_POT,
        ATTRAPE_PLANTE_1,
        ATTRAPE_PLANTE_2,
        RETOUR_MAISON
    } etat_prise_plante = INIT;
    while(etat_action == ACTION_EN_COURS){
        Holonome_cyclique(PARAM_DEFAULT);
        if(m_temps_ms != Temps_get_temps_ms()){
            m_temps_ms = Temps_get_temps_ms();

            switch (etat_prise_plante){
                case INIT:
                    m_timer = 3000;
                    etat_prise_plante = TIENT_POT;
                    etat_action = ACTION_EN_COURS;

                case TIENT_POT:
                    m_timer--;
                    i2c_annexe_actionneur_pot(0, BRAS_POT_SOL, DOIGT_TIENT);
                    i2c_annexe_actionneur_pot(5, BRAS_POT_SOL, DOIGT_TIENT);
                    if(m_timer <= 0){
                        etat_prise_plante = LEVE_POT;
                        m_timer = 1000;
                    }
                    break;
                case LEVE_POT:
                    m_timer--;
                    i2c_annexe_actionneur_pot(0, BRAS_LEVITE, DOIGT_TIENT);
                    i2c_annexe_actionneur_pot(5, BRAS_LEVITE, DOIGT_TIENT);
                    if(m_timer <= 0){
                        etat_prise_plante = ATTRAPE_PLANTE_1;
                    }
                    break;
                case ATTRAPE_PLANTE_1:
                    etat_action_step = Strat_2024_plante_dans_pot(1, PLANTE_BRAS_1, ZONE_PLANTE_1);
                    if(etat_action_step == ACTION_TERMINEE){
                        etat_prise_plante = ATTRAPE_PLANTE_2;
                    }
                    break;
                case ATTRAPE_PLANTE_2:
                    etat_action_step = Strat_2024_plante_dans_pot(1, PLANTE_BRAS_6, ZONE_PLANTE_1);
                    if(etat_action_step == ACTION_TERMINEE){
                        etat_prise_plante = RETOUR_MAISON;
                    }
                    break;
                case RETOUR_MAISON:{
                    etat_action = Strategie_aller_a(0, 0, EVITEMENT_SANS_EVITEMENT, 1);
                    if(etat_action == ACTION_TERMINEE){
                        etat_prise_plante = INIT;
                    }
                }

            }
        }
    }

    Moteur_Stop();
}

void Balise_cli_orange_maintenance(int nb_cli){
    int32_t temps_court_ms = 150, temps_long_ms = 700;
    static int32_t temps_ms_led_cli, timer_led_ms = 0;
    static int32_t m_nb_cli;
    static enum etat_led_cli_orange_maintenance_t{
        CLI_ON_COURT,
        CLI_OFF_COURT,
        CLI_OFF_LONG,
    }etat_led_cli_orange_maintenance=CLI_OFF_LONG;
    // Toutes les 1 ms.
    if(temps_ms_led_cli != Temps_get_temps_ms()){
        temps_ms_led_cli = Temps_get_temps_ms();

        timer_led_ms--;

        switch(etat_led_cli_orange_maintenance){
            case CLI_ON_COURT:
                if(timer_led_ms<= 0){
                    i2c_annexe_couleur_balise(0, 0xFFFF);
                    timer_led_ms = temps_court_ms;
                    m_nb_cli--;
                    if(m_nb_cli){
                        etat_led_cli_orange_maintenance=CLI_OFF_COURT;
                        timer_led_ms = temps_court_ms;
                    }else{
                        etat_led_cli_orange_maintenance=CLI_OFF_LONG;
                        timer_led_ms = temps_long_ms;
                    }
                }
                break;
            case CLI_OFF_COURT:
                if(timer_led_ms<= 0){
                    i2c_annexe_couleur_balise(0b11101000, 0xFFFF);
                    timer_led_ms = temps_court_ms;
                    etat_led_cli_orange_maintenance=CLI_ON_COURT;
                }
                break;
            case CLI_OFF_LONG:
                m_nb_cli = nb_cli;
                if(timer_led_ms<= 0){
                    i2c_annexe_couleur_balise(0b11101000, 0xFFFF);
                    timer_led_ms = temps_court_ms;
                    etat_led_cli_orange_maintenance=CLI_ON_COURT;
                }
                break;
        }
   
    }
}

void Balise_pulse_vert(){
    static int32_t temps_ms_led_cli, timer_led_ms = 800;
    int32_t timer_led_max_ms =800;
    int32_t vert_led, vert_led_max = 0b111;
    static enum etat_led_pulse_vert_t{
        PULSE_RISE,
        PULSE_FALL,
    }etat_led_pulse_vert=PULSE_FALL;
    // Toutes les 1 ms.
    if(temps_ms_led_cli != Temps_get_temps_ms()){
        temps_ms_led_cli = Temps_get_temps_ms();

        timer_led_ms--;

        switch(etat_led_pulse_vert){
            case PULSE_RISE:
                vert_led = (vert_led_max-1)  - (vert_led_max-1) * timer_led_ms / timer_led_max_ms + 1;
                vert_led = vert_led & 0b111;
                i2c_annexe_couleur_balise(vert_led << 2, 0xFFFF);
                if(timer_led_ms<= 0){
                    i2c_annexe_couleur_balise(0, 0xFFFF);
                    timer_led_ms = timer_led_max_ms;
                    etat_led_pulse_vert=PULSE_FALL;
                }
                break;
            case PULSE_FALL:
                vert_led = (vert_led_max-1) * timer_led_ms / timer_led_max_ms + 1;
                vert_led = vert_led & 0b111;
                i2c_annexe_couleur_balise(vert_led << 2, 0xFFFF);
                if(timer_led_ms<= 0){
                    timer_led_ms = timer_led_max_ms;
                    etat_led_pulse_vert=PULSE_RISE;
                }
                break;
        }
   
    }
}

void Demonstration_menu_balise(void){
    int nb_cli=2;
    while(1){
        Holonome_cyclique(PARAM_NO_MOTORS);
        Balise_cli_orange_maintenance(nb_cli);
        if(Demonstration_attente_capteur(CAPTEUR_POUR_ATTENTE_DROIT) == ACTION_TERMINEE){
            nb_cli++;
            printf(">nb_cli%d\n",nb_cli);
        }
        if(Demonstration_attente_capteur(CAPTEUR_POUR_ATTENTE_GAUCHE) == ACTION_TERMINEE){
            nb_cli--;
            printf(">nb_cli%d\n",nb_cli);
        }
        if(Demonstration_attente_capteur(CAPTEUR_POUR_ATTENTE_DEVANT) == ACTION_TERMINEE){
            return;
        }
        if(Demonstration_attente_capteur(CAPTEUR_POUR_ATTENTE_ARRIERE) == ACTION_TERMINEE){
            switch(nb_cli){
                case 2:
                    Demonstration_actionneurs();
                    break;
                case 3:
                    Demonstration_prise_plante();
                    break;
            }
        }

    }
}

/// @brief Comme la fonction demonstration attente, mais en choisissant le capteur. Fonction non-bloquante
/// @param capteur: numéro du capteur, 0 à l'arrière, 3 à droite, 6 devant, 9, à gauche
/// @return ACTION_TERMINEE si nous avons un signal pour ce capteur
enum etat_action_t Demonstration_attente_capteur(int capteur){
    static enum {
        ATTENTE_DETECTION,
        DETECTION_PROCHE,
        FIN_ATTENTE
    } etat_attente[12] = {ATTENTE_DETECTION,ATTENTE_DETECTION,ATTENTE_DETECTION,ATTENTE_DETECTION,ATTENTE_DETECTION,
    ATTENTE_DETECTION,ATTENTE_DETECTION,ATTENTE_DETECTION,ATTENTE_DETECTION,ATTENTE_DETECTION,ATTENTE_DETECTION,ATTENTE_DETECTION};
    static uint32_t temps_debut_tempo[12];
    uint32_t duree_tempo_ms = 50;
    
    switch(etat_attente[capteur]){
        case ATTENTE_DETECTION:
            if(Balise_VL53L1X_get_capteur_cm(capteur) < 15 && Balise_VL53L1X_get_capteur_cm(capteur) > 1){
                /// Sans obstacle, le capteur peut renvoyer 0;
                etat_attente[capteur]=DETECTION_PROCHE;
                temps_debut_tempo[capteur] = time_us_32();
            }
            break;
        
        case DETECTION_PROCHE:
            if(Balise_VL53L1X_get_capteur_cm(capteur) > 15 || Balise_VL53L1X_get_capteur_cm(capteur) < 1){
                // On a perdu la detection avant le temps écoulé
                etat_attente[capteur]=ATTENTE_DETECTION;
            }
            if((temps_debut_tempo[capteur] + (duree_tempo_ms * 1000)) < time_us_32()){
                // temps écoulé
                etat_attente[capteur]=FIN_ATTENTE;
            }
            break;

        case FIN_ATTENTE:
            if(Balise_VL53L1X_get_capteur_cm(capteur) > 15 || Balise_VL53L1X_get_capteur_cm(capteur) < 1){
                // On a perdu la detection après le temps écoulé
                etat_attente[capteur]=ATTENTE_DETECTION;
                return ACTION_TERMINEE;
            }
            break;
            
    }
    return ACTION_EN_COURS;
    //sleep_ms(20);
    

}

enum etat_action_t Demonstration_attente(){
    enum {
        ATTENTE_DETECTION,
        DETECTION_PROCHE,
        FIN_ATTENTE
    } etat_attente = ATTENTE_DETECTION;
    uint32_t temps_debut_tempo, duree_tempo_ms = 50;

    while(true){
        Holonome_cyclique(PARAM_NO_MOTORS);
        Balise_cli_orange_maintenance(2);
        
        switch(etat_attente){
            case ATTENTE_DETECTION:
                if(Balise_VL53L1X_get_capteur_cm(CAPTEUR_POUR_ATTENTE) < 15 && Balise_VL53L1X_get_capteur_cm(CAPTEUR_POUR_ATTENTE) > 1){
                    /// Sans obstacle, le capteur peut renvoyer 0;
                    etat_attente=DETECTION_PROCHE;
                    temps_debut_tempo = time_us_32();
                }
                break;
            
            case DETECTION_PROCHE:
                if(Balise_VL53L1X_get_capteur_cm(CAPTEUR_POUR_ATTENTE) > 15 || Balise_VL53L1X_get_capteur_cm(CAPTEUR_POUR_ATTENTE) < 1){
                    // On a perdu la detection avant le temps écoulé
                    etat_attente=ATTENTE_DETECTION;
                }
                if((temps_debut_tempo + (duree_tempo_ms * 1000)) < time_us_32()){
                    // temps écoulé
                    etat_attente=FIN_ATTENTE;
                }
                break;

            case FIN_ATTENTE:
                if(Balise_VL53L1X_get_capteur_cm(CAPTEUR_POUR_ATTENTE) > 15 || Balise_VL53L1X_get_capteur_cm(CAPTEUR_POUR_ATTENTE) < 1){
                    // On a perdu la detection après le temps écoulé
                    etat_attente=ATTENTE_DETECTION;
                    return ACTION_TERMINEE;
                }
                break;
                
        }
        //sleep_ms(20);
    }
    

}


enum etat_action_t Demonstration_calage(){
    enum {
        CALAGE,
        DECALAGE,
        CALAGE_TERMINE
    } etat_calage = CALAGE;
    while(true){
        Holonome_cyclique(PARAM_DEFAULT);
        struct trajectoire_t trajectoire;

        // Toutes les 1 ms.
        if(temps_ms_demo != Temps_get_temps_ms()){
            temps_ms_demo = Temps_get_temps_ms();

        
            switch (etat_calage)
            {
            case CALAGE:
                // TODO: Appeler la nouvelle fonction de prise de référentiel
                /*if(calage_angle(LONGER_VERS_C, RAYON_ROBOT, PETIT_RAYON_ROBOT_MM, -60 * DEGRE_EN_RADIAN) == ACTION_TERMINEE){
                    etat_calage = DECALAGE;
                }*/
                break;

            case DECALAGE:
                Trajectoire_droite(&trajectoire, Localisation_get().x_mm, Localisation_get().y_mm, 
                    RAYON_ROBOT + 150, PETIT_RAYON_ROBOT_MM + 150,
                    Localisation_get().angle_radian, Localisation_get().angle_radian);
                if(Strategie_parcourir_trajet(trajectoire, 1, EVITEMENT_SANS_EVITEMENT) == ACTION_TERMINEE){
                    etat_calage = CALAGE_TERMINE;
                }
                break;

            
            case CALAGE_TERMINE:
                etat_calage = CALAGE;
                Moteur_Stop();
                return ACTION_TERMINEE;
                
            
            default:
                break;
            }
        }
        
    }
    return ACTION_ECHEC;
}


/// @brief Deplacement suivant un rectangle.
/// @param avance_x_mm : Distance en X (conseillé 620)
/// @param avance_y_mm : Distance en Y (conseillé 1000)
/// @return ACTION_TERMINEE
enum etat_action_t Demonstration_rectangle(int avance_x_mm, int avance_y_mm){
    enum {
        AVANCE_Y,
        AVANCE_X,
        RECULE_Y,
        RECULE_X,
        RECTANGLE_TERMINE
    } etat_rectangle = AVANCE_Y;
    while(true){
        struct trajectoire_t trajectoire;

        Holonome_cyclique(PARAM_DEFAULT);

        // Toutes les 1 ms.
        if(temps_ms_demo != Temps_get_temps_ms()){
            temps_ms_demo = Temps_get_temps_ms();

        
            switch (etat_rectangle)
            {
            case AVANCE_Y:
                Trajectoire_droite(&trajectoire, Localisation_get().x_mm, Localisation_get().y_mm, 
                    Localisation_get().x_mm, Localisation_get().y_mm + avance_y_mm,
                    Localisation_get().angle_radian, Localisation_get().angle_radian);
                if(Strategie_parcourir_trajet(trajectoire, 1, EVITEMENT_SANS_EVITEMENT) == ACTION_TERMINEE){
                    etat_rectangle = AVANCE_X;
                }
                break;
            
            case AVANCE_X:
                Trajectoire_droite(&trajectoire, Localisation_get().x_mm, Localisation_get().y_mm, 
                    Localisation_get().x_mm + avance_x_mm, Localisation_get().y_mm,
                    Localisation_get().angle_radian, Localisation_get().angle_radian);
                if(Strategie_parcourir_trajet(trajectoire, 1, EVITEMENT_SANS_EVITEMENT) == ACTION_TERMINEE){
                    etat_rectangle = RECULE_Y;
                }
                break;

            case RECULE_Y:
                Trajectoire_droite(&trajectoire, Localisation_get().x_mm, Localisation_get().y_mm, 
                    Localisation_get().x_mm, Localisation_get().y_mm - avance_y_mm,
                    Localisation_get().angle_radian, Localisation_get().angle_radian);
                if(Strategie_parcourir_trajet(trajectoire, 1, EVITEMENT_SANS_EVITEMENT) == ACTION_TERMINEE){
                    etat_rectangle = RECULE_X;
                }
                break;
            
            case RECULE_X:
                Trajectoire_droite(&trajectoire, Localisation_get().x_mm, Localisation_get().y_mm, 
                    Localisation_get().x_mm - avance_x_mm, Localisation_get().y_mm,
                    Localisation_get().angle_radian, Localisation_get().angle_radian);
                if(Strategie_parcourir_trajet(trajectoire, 1, EVITEMENT_SANS_EVITEMENT) == ACTION_TERMINEE){
                    etat_rectangle = RECTANGLE_TERMINE;
                }
                break;
            
            case RECTANGLE_TERMINE:
                etat_rectangle = AVANCE_Y;
                Moteur_Stop();
                return ACTION_TERMINEE;
            
            default:
                break;
            }
        }
        
    }
    return 0;
}

/// @brief Deplacement suivant une droite, rotation sur lui-même du robot une fois arrivée à destination,
/// puis retour en ligne droite
/// @param avance_x_mm : Distance en X (conseillé 300)
/// @param avance_y_mm : Distance en Y (conseillé 1000)
/// @param angle_degrees : Rotation du robot sur lui-même
/// @return ACTION_TERMINEE
enum etat_action_t Demonstration_avance_puis_tourne(int avance_x_mm, int avance_y_mm, float angle_degrees){
    enum {
        APT_AVANCE,
        APT_TOURNE,
        APT_RECULE,
        APT_TERMINE
    } etat_avance_puis_tourne = APT_AVANCE;
    int pos_x_init_mm, pos_y_init_mm;
    while(true){
        struct trajectoire_t trajectoire;

        Holonome_cyclique(PARAM_DEFAULT);

        // Toutes les 1 ms.
        if(temps_ms_demo != Temps_get_temps_ms()){
            temps_ms_demo = Temps_get_temps_ms();

        
            switch (etat_avance_puis_tourne)
            {
            case APT_AVANCE:
                pos_x_init_mm = Localisation_get().x_mm;
                pos_y_init_mm = Localisation_get().y_mm;
                Trajectoire_droite(&trajectoire, pos_x_init_mm, pos_y_init_mm, 
                    pos_x_init_mm + avance_x_mm, pos_y_init_mm + avance_y_mm,
                    Localisation_get().angle_radian, Localisation_get().angle_radian);
                if(Strategie_parcourir_trajet(trajectoire, 1, EVITEMENT_SANS_EVITEMENT) == ACTION_TERMINEE){
                    etat_avance_puis_tourne = APT_TOURNE;
                }
                break;
            
            case APT_TOURNE:
                Trajectoire_rotation(&trajectoire, Localisation_get().x_mm, Localisation_get().y_mm, 
                    Localisation_get().angle_radian, Localisation_get().angle_radian + (angle_degrees * DEGRE_EN_RADIAN) );
                Trajet_config(TRAJECT_CONFIG_ROTATION_PURE);
                if(Strategie_parcourir_trajet(trajectoire, 1, EVITEMENT_SANS_EVITEMENT) == ACTION_TERMINEE){
                    etat_avance_puis_tourne = APT_RECULE;
                    Trajet_config(TRAJECT_CONFIG_STD);
                }
                break;

            case APT_RECULE:
                Trajectoire_droite(&trajectoire, Localisation_get().x_mm, Localisation_get().y_mm, 
                    Localisation_get().x_mm - avance_x_mm, Localisation_get().y_mm - avance_y_mm,
                    Localisation_get().angle_radian, Localisation_get().angle_radian);
                if(Strategie_parcourir_trajet(trajectoire, 1, EVITEMENT_SANS_EVITEMENT) == ACTION_TERMINEE){
                    etat_avance_puis_tourne = APT_TERMINE;
                }
                break;
            
            case APT_TERMINE:
                etat_avance_puis_tourne = APT_AVANCE;
                Moteur_Stop();
                return ACTION_TERMINEE;
            
            default:
                break;
            }
        }
        
    }
    return ACTION_ECHEC;
}

/// @brief Rotation du robot sur lui-même du robot 
/// @param angle_degrees : Rotation du robot sur lui-même
/// @return ACTION_TERMINEE
enum etat_action_t Demonstration_tourne(float angle_degrees){
    enum {
        DEMO_TOURNE,
        DEMO_TOURNE_TERMINE
    } etat_avance_puis_tourne = DEMO_TOURNE;
    int pos_x_init_mm, pos_y_init_mm;
    while(true){
        struct trajectoire_t trajectoire;

        Holonome_cyclique(PARAM_DEFAULT);

        // Toutes les 1 ms.
        if(temps_ms_demo != Temps_get_temps_ms()){
            temps_ms_demo = Temps_get_temps_ms();

        
            switch (etat_avance_puis_tourne)
            {
            
            case DEMO_TOURNE:
                Trajectoire_rotation(&trajectoire, Localisation_get().x_mm, Localisation_get().y_mm, 
                    Localisation_get().angle_radian, Localisation_get().angle_radian + (angle_degrees * DEGRE_EN_RADIAN) );
                Trajet_config(TRAJECT_CONFIG_ROTATION_PURE);
                if(Strategie_parcourir_trajet(trajectoire, 1, EVITEMENT_SANS_EVITEMENT) == ACTION_TERMINEE){
                    etat_avance_puis_tourne = DEMO_TOURNE_TERMINE;
                    Trajet_config(TRAJECT_CONFIG_STD);
                }
                break;
            
            case DEMO_TOURNE_TERMINE:
                etat_avance_puis_tourne = DEMO_TOURNE;
                Moteur_Stop();
                return ACTION_TERMINEE;
            
            default:
                break;
            }
        }
        
    }
    return ACTION_ECHEC;
}

/// @brief Déplacement suivant deux courbes de Bézier. Recommandé pour une démo sur une planche de 1m x 1,5m
/// @return ACTION_TERMINEE
enum etat_action_t Demonstration_bezier(){
    enum {
        BEZIER_ALLER,
        BEZIER_RETOUR,
        BEZIER_TERMINE
    } etat_bezier = BEZIER_ALLER;
    while(true){
        struct trajectoire_t trajectoire;
        static int pos_x_init_mm, pos_y_init_mm;

        Holonome_cyclique(PARAM_DEFAULT);
        Trajet_config(200, 200);

        // Toutes les 1 ms.
        if(temps_ms_demo != Temps_get_temps_ms()){
            temps_ms_demo = Temps_get_temps_ms();

        
            switch (etat_bezier)
            {
            case BEZIER_ALLER:
                Trajectoire_bezier(&trajectoire, Localisation_get().x_mm, Localisation_get().y_mm, 
                    1386, Localisation_get().y_mm,
                    -576, 1500 - 276,
                    545,  1500 - 276,
                    Localisation_get().angle_radian, Localisation_get().angle_radian);
                if(Strategie_parcourir_trajet(trajectoire, 1, EVITEMENT_SANS_EVITEMENT) == ACTION_TERMINEE){
                    etat_bezier = BEZIER_RETOUR;
                }
                break;
            
            case BEZIER_RETOUR:
                Trajectoire_bezier(&trajectoire, 
                    Localisation_get().x_mm, Localisation_get().y_mm, 
                    1391, Localisation_get().y_mm,
                    -76, 1500 - 788,
                    242,  1500 - 1289,
                    Localisation_get().angle_radian, Localisation_get().angle_radian);
                if(Strategie_parcourir_trajet(trajectoire, 1, EVITEMENT_SANS_EVITEMENT) == ACTION_TERMINEE){
                    etat_bezier = BEZIER_TERMINE;
                }
                break;
            
            case BEZIER_TERMINE:
                etat_bezier = BEZIER_ALLER;
                Trajet_config(TRAJECT_CONFIG_STD);
                return ACTION_TERMINEE;
            
            default:
                break;
            }
        }
        
    }
    return ACTION_ECHEC;
}

/// @brief Leve le bras, et agite le doigt
/// @param bras 
enum etat_action_t  Demonstration_leve_bras(uint32_t bras){
    static enum{
        LB_LEVE_BRAS,
        LB_TEMPO_BRAS,
        LB_DOIGT_ATTRAPE_1,
        LB_DOIGT_LACHE,
        LB_DOIGT_ATTRAPE_2,
    } DLB_status;
    int timer_ms;
    while(true){
        Holonome_cyclique(PARAM_NO_MOTORS);
        if(temps_ms_demo != Temps_get_temps_ms()){
            temps_ms_demo = Temps_get_temps_ms();
            timer_ms--;

            switch(DLB_status){
                case LB_LEVE_BRAS:
                    i2c_annexe_actionneur_pot(bras, BRAS_HAUT, DOIGT_LACHE);
                    DLB_status = LB_TEMPO_BRAS;
                    timer_ms = 250;
                    break;

                case LB_TEMPO_BRAS:
                    if(timer_ms <= 0){
                        DLB_status = LB_DOIGT_ATTRAPE_1;
                        timer_ms=150;
                    }
                    break;

                case LB_DOIGT_ATTRAPE_1:
                    i2c_annexe_actionneur_pot(bras, BRAS_HAUT, DOIGT_TIENT);
                    if(timer_ms <= 0){
                        DLB_status = LB_DOIGT_LACHE;
                        timer_ms=200;
                    }
                    break;

                case LB_DOIGT_LACHE:
                    i2c_annexe_actionneur_pot(bras, BRAS_HAUT, DOIGT_LACHE);
                    if(timer_ms <= 0){
                        DLB_status = LB_DOIGT_ATTRAPE_2;
                        timer_ms=200;
                    }
                    break;

                case LB_DOIGT_ATTRAPE_2:
                    i2c_annexe_actionneur_pot(bras, BRAS_HAUT, DOIGT_TIENT);
                    if(timer_ms <= 0){
                        DLB_status = LB_LEVE_BRAS;
                        return ACTION_TERMINEE;
                    }
                    break;

            }
        }
        
        
    }
}
enum etat_action_t  Demonstration_attrape_plante(void){
    int32_t timer_ms;
    enum {
        DAP_ENVOI,
        DAP_ATTENTE
    } dap_status=DAP_ENVOI;
    while(true){
        Holonome_cyclique(PARAM_NO_MOTORS);
        if(temps_ms_demo != Temps_get_temps_ms()){
            temps_ms_demo = Temps_get_temps_ms();
            timer_ms--;

            switch(dap_status){
                case DAP_ENVOI:
                    i2c_annexe_attrape_plante(PLANTE_BRAS_1);
                    dap_status = DAP_ATTENTE;
                    timer_ms=5000;
                    break;

                case DAP_ATTENTE:
                    if (timer_ms <= 0){
                        dap_status=DAP_ENVOI;
                        return ACTION_TERMINEE;
                    }
                    break;            
            }
            
        }
    }
}

enum etat_action_t  Demonstration_baisse_bras(void){
    static int32_t timer_ms;
    enum {
        DBB_ENVOI,
        DBB_ATTENTE
    } dbb_status=DBB_ENVOI;
    while(true){
        Holonome_cyclique(PARAM_NO_MOTORS);
        if(temps_ms_demo != Temps_get_temps_ms()){
            temps_ms_demo = Temps_get_temps_ms();
            timer_ms--;

            switch(dbb_status){
                case DBB_ENVOI:
                    for(int bras=0; bras < 6; bras++){
                        i2c_annexe_actionneur_pot(bras, BRAS_PLIE, DOIGT_LACHE);
                    }
                    dbb_status = DBB_ATTENTE;
                    timer_ms=500;
                    break;

                case DBB_ATTENTE:
                    if (timer_ms <= 0){
                        dbb_status = DBB_ENVOI;
                        return ACTION_TERMINEE;
                    }
                    break;            
            }
            
        }
    }
}