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

#define TEST_TIMEOUT_US 10000000

int Demonstration_calage();

uint32_t temps_ms_demo = 0, temps_ms_old;

int Demonstration_menu(void){
    static int iteration = 2;
    printf("Mode demo\n");
    printf("A - Calage dans l'angle\n");
    printf("B - Trajets unitaires\n");
    printf("C - Trajets enchaines - manuels\n");
    printf("D - Trajets enchaines - auto\n");
    printf("E - Asservissement angulaire\n");

    printf("Q - Quitter\n");

    stdio_flush();
    int rep = getchar_timeout_us(TEST_TIMEOUT_US);
    switch (rep)
    {
    case 'a':
    case 'A':
        while(Demonstration_calage());
        break;

    case 'b':
    case 'B':
        break;

    }
    return 1;
}

int Demonstration_init(){
    
    Holonome2023_init();

    temps_ms_demo = Temps_get_temps_ms();
    temps_ms_old = temps_ms_demo;   
}


int Demonstration_calage(){
    Demonstration_init();
    enum {
        CALAGE,
        CALAGE_TERMINE
    } etat_calage = CALAGE;
    while(true){
        Holonome_cyclique();

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

        
            switch (etat_calage)
            {
            case CALAGE:
                if(calage_angle(LONGER_VERS_C, RAYON_ROBOT, PETIT_RAYON_ROBOT_MM, -60 * DEGRE_EN_RADIAN) == ACTION_TERMINEE){
                    etat_calage = CALAGE_TERMINE;

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