#include <stdio.h>
#include "pico/stdio.h"
#include "pico/error.h"
#include "pico/multicore.h"
#include "hardware/i2c.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_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 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");
    
    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 '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_debut(COULEUR_BLEU, _step_ms);

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


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

            etat_action = Strat_2024_attrape_pot(GROUPE_POT_B1, _step_ms);

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

}


void affichage_test_strategie_2024(){
    uint32_t temps;
    
    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(">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());
        
        sleep_ms(100);
    }
}