#include "pico/multicore.h"
#include "stdio.h"
#include "hardware/i2c.h"
#include "math.h"

#include "Asser_Moteurs.h"
#include "i2c_annexe.h"
#include "i2c_maitre.h"
#include "gyro.h"
#include "Localisation.h"
#include "QEI.h"
#include "Robot_config.h"
#include "Strategie.h"
#include "Temps.h"
#include "Trajet.h"
#include "Trajectoire.h"
#include "Test.h"

int test_accostage(void);
int test_longe(void);
int test_homologation(void);

void affichage_test_strategie(){
    uint32_t temps;
    
    while(true){
        temps = time_us_32()/1000;
        printf(">contacteur_butee_A:%ld:%d\n", temps, i2c_annexe_get_contacteur_butee_A());
        printf(">contacteur_butee_C:%ld:%d\n", temps, i2c_annexe_get_contacteur_butee_C());
        printf(">contacteur_longer_A:%ld:%d\n", temps, i2c_annexe_get_contacteur_longer_A());
        printf(">contacteur_longer_C:%ld:%d\n", temps, i2c_annexe_get_contacteur_longer_C());
        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/M_PI*180.);

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

        sleep_ms(100);
    }
}


int test_strategie(){
    printf("L - longer.\n");
    printf("A - Accoster.\n");
    printf("H - Homologation.\n");
    int lettre;
    do{
        lettre = getchar_timeout_us(0);
    }while(lettre == PICO_ERROR_TIMEOUT || lettre == 0);
    switch(lettre){
        case 'a':
        case 'A':
            while(test_accostage());
            break;

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

        case 'l':
        case 'L':
            while(test_longe());
            break;

        

        case 'q':
        case 'Q':
            return 0;
        default:
            return 1;
    }
}

int test_homologation(){
    int lettre, _step_ms = 1, temps_ms=0, _step_ms_gyro=2,temps_ms_init;
    printf("Homologation\n");

    i2c_maitre_init();
    Trajet_init();
    //printf("Init gyroscope\n");
    //Gyro_Init();

    stdio_flush();

    //set_position_avec_gyroscope(1);

    multicore_launch_core1(affichage_test_strategie);

    temps_ms = Temps_get_temps_ms();
    temps_ms_init = temps_ms;
    do{
        i2c_gestion(i2c0);
        i2c_annexe_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);

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

            Homologation(_step_ms);

        }
        lettre = getchar_timeout_us(0);
    //}while((lettre == PICO_ERROR_TIMEOUT) || (lettre == 0));
    }while(1);
    printf("STRATEGIE_LOOP_2\n");
    printf("Lettre : %d; %c\n", lettre, lettre);
    
    if(lettre == 'q' && lettre == 'Q'){
        return 0;
    }
    return 0;

}


int test_longe(){
    int lettre, _step_ms = 1, temps_ms=0, _step_ms_gyro=2,temps_ms_init;

    i2c_maitre_init();
    Trajet_init();
    //printf("Init gyroscope\n");
    //Gyro_Init();

    stdio_flush();

    //set_position_avec_gyroscope(1);

    temps_ms = Temps_get_temps_ms();
    temps_ms_init = temps_ms;
    do{
        i2c_gestion(i2c0);
        i2c_annexe_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);

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

            if(temps_ms > temps_ms_init + 200){
                if(cerise_longer_bord(LONGER_VERS_A) == ACTION_TERMINEE){
                    printf("Accostage_terminee\n");
                }
            }

        }
        lettre = getchar_timeout_us(0);
    }while((lettre == PICO_ERROR_TIMEOUT) || (lettre == 0));
    printf("Lettre : %d; %c\n", lettre, lettre);
    
    if(lettre == 'q' && lettre == 'Q'){
        return 0;
    }
    return 1;

}


int test_accostage(){
    int lettre, _step_ms = 1, temps_ms=0, _step_ms_gyro=2,temps_ms_init;

    i2c_maitre_init();
    Trajet_init();
    //printf("Init gyroscope\n");
    //Gyro_Init();

    stdio_flush();

    //set_position_avec_gyroscope(1);

    temps_ms = Temps_get_temps_ms();
    temps_ms_init = temps_ms;
    do{
        i2c_gestion(i2c0);
        i2c_annexe_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);

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

            if(temps_ms > temps_ms_init + 200){
                if(cerise_accostage() == ACTION_TERMINEE){
                    printf("Accostage_terminee\n");
                }
            }

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

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