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

#include "Asser_Moteurs.h"
#include "Balise_VL53L1X.h"
#include "Evitement.h"
#include "i2c_annexe.h"
#include "i2c_maitre.h"
#include "gyro.h"
#include "Localisation.h"
#include "Monitoring.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_panier(void);
int test_homologation(void);
int test_evitement(void);
int test_tirette_et_couleur();
int test_cerise_laterales_droite(void);
int test_cerise_laterales_gauche(void);
void affichage_test_evitement();

void affichage_test_strategie(){
    uint32_t temps;
    
    while(true){
        temps = time_us_32()/1000;
        temps_cycle_display();
        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);

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

        /*switch(etat_strategie){
            case STRATEGIE_INIT: printf(">etat_strat:STRATEGIE_INIT|t\n"); break;
            case APPROCHE_CERISE_1_A: printf(">etat_strat:APPROCHE_CERISE_1_A|t\n");break;
            case APPROCHE_CERISE_1_B: printf(">etat_strat:APPROCHE_CERISE_1_B|t\n");break;
            case ATTRAPE_CERISE_1: printf(">etat_strat:ATTRAPE_CERISE_1|t\n");break;
            case APPROCHE_PANIER_1: printf(">etat_strat:APPROCHE_PANIER_1|t\n");break;
            case CALAGE_PANIER_1: printf(">etat_strat:CALAGE_PANIER_1|t\n");break;
            case RECULE_PANIER: printf(">etat_strat:RECULE_PANIER|t\n");break;
            case LANCE_DANS_PANIER: printf(">etat_strat:LANCE_DANS_PANIER|t\n");break;
            case STRATEGIE_FIN: printf(">etat_strat:STRATEGIE_FIN|t\n");break;
        }*/

        sleep_ms(100);
    }
}


int test_strategie(){
    printf("C - Couleur et tirette.\n");
    printf("E - Evitement\n");
    
    int lettre;
    do{
        lettre = getchar_timeout_us(0);
    }while(lettre == PICO_ERROR_TIMEOUT || lettre == 0);
    switch(lettre){
        case 'c':
        case 'C':
            while(test_tirette_et_couleur());
            break;

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

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



void affichage_test_evitement(){
    while(1){
        for(uint8_t capteur=0; capteur<12; capteur++){
            printf(">c%d:%d\n",capteur, Balise_VL53L1X_get_capteur_cm(capteur));
        }
        printf(">distance_minimale:%f\n",Trajet_get_obstacle_mm());
    }
}

int test_evitement(){
    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("Evitement\n");

    printf("A - Sans evitement.\n");
    printf("B - Pause devant obstacle.\n");
    printf("C - Arrêt devant obstacle.\n");
    printf("D - Retour si obstacle.\n");
    printf("E - Contournement.\n");
    do{
        lettre = getchar_timeout_us(0);
    }while(lettre == PICO_ERROR_TIMEOUT || lettre == 0);
    switch(lettre){
        case 'a':
        case 'A':evitement=EVITEMENT_SANS_EVITEMENT;break;
        case 'b':
        case 'B':evitement=EVITEMENT_PAUSE_DEVANT_OBSTACLE;break;
        case 'c':
        case 'C':evitement=EVITEMENT_ARRET_DEVANT_OBSTACLE;break;
        case 'd':
        case 'D':evitement=EVITEMENT_RETOUR_SI_OBSTABLE;break;
        case 'e':
        case 'E':evitement=EVITEMENT_CONTOURNEMENT;break;
    }

    i2c_maitre_init();
    Trajet_init();
    Balise_VL53L1X_init();
    Localisation_set(200,200,0);
    //printf("Init gyroscope\n");
    set_position_avec_gyroscope(0);
    if(get_position_avec_gyroscope()){
        Gyro_Init();
    }

    stdio_flush();
    Trajet_config(100, 500);

    multicore_launch_core1(affichage_test_evitement);

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

            Trajectoire_droite(&trajectoire,Localisation_get().x_mm, Localisation_get().y_mm, 
                                1000,200,
                                0, 0); // Angles

            etat_action = Strategie_parcourir_trajet(trajectoire, _step_ms, evitement);

        }
        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);
    
    if(lettre == 'q' && lettre == 'Q'){
        return 0;
    }
    return 0;

}



int test_tirette_et_couleur(){
    int lettre;
    uint couleur, tirette;
    enum couleur_t couleur_old;
    couleur_old = COULEUR_INCONNUE;
    printf("Tirette et couleur\n");
    i2c_maitre_init();

    stdio_flush();

    tirette= attente_tirette();
    do{
        i2c_gestion(i2c0);
        i2c_annexe_gestion();

        printf(">tirette:%d\n", attente_tirette());
        if(lire_couleur() == COULEUR_JAUNE){
            printf(">couleur:Jaune|t\n");
        }else{
            printf(">couleur:Bleu|t\n");
        }

        if(attente_tirette()){
            if(couleur_old != lire_couleur() || tirette != attente_tirette()){
                tirette = attente_tirette();
                couleur_old = lire_couleur();
                if(couleur_old == COULEUR_JAUNE){
                    // Tout Jaune
                    i2c_annexe_couleur_balise(0b11111100, 0x0FFF);
                }else{
                    // Tout bleu
                    i2c_annexe_couleur_balise(0b00000011, 0x0FFF);
                }
            }
        }else{
            if(tirette != attente_tirette()){
                tirette = attente_tirette();
                // Tout libre
                i2c_annexe_couleur_balise(0, 0x00);
            }
        }

        sleep_ms(10);
    
        lettre = getchar_timeout_us(0);
    }while((lettre == PICO_ERROR_TIMEOUT) || (lettre == 0));
    

}