#include <stdio.h>
#include "pico/multicore.h"
#include "pico/stdlib.h"
#include "pico/stdio.h"
#include "hardware/gpio.h"
#include "hardware/i2c.h"
#include "pico/binary_info.h"
#include "math.h"
#include "Test.h"

#include "APDS_9960.h"
#include "gyro.h"
#include "Asser_Moteurs.h"
#include "Asser_Position.h"
#include "Balise_VL53L1X.h"
#include "Commande_vitesse.h"
#include "Demonstration.h"
#include "Geometrie_robot.h"
#include "i2c_annexe.h"
#include "i2c_maitre.h"
#include "Localisation.h"
#include "Log.h"
#include "Moteurs.h"
#include "Monitoring.h"
#include "QEI.h"
#include "Robot_config.h"
#include "Servomoteur.h"
#include "spi_nb.h"
#include "Temps.h"
#include "Trajectoire.h"
#include "Trajet.h"

#include "Test_gyro.h"
#include "Test_i2c.h"
#include "Test_log.h"
#include "Test_strategie.h"
#include "Test_strategie_2024.h"
#include "Tests_unitaires.h"
#include "Tests_deplacement.h"
#include "Test.h"

#define V_INIT -999.0
#define TEST_TIMEOUT_US 10000000

int test_APDS9960(void);
int test_asser_position_avance(void);
int test_asser_position_avance_et_tourne(int);
int test_transition_gyro_pas_gyro(void);
void affiche_localisation(void);
int test_capteurs_balise(void);
int test_geometrie(void);
int test_angle_balise(void);


// Mode test : renvoie 0 pour quitter le mode test
int mode_test(){
    static int iteration = 2;
    printf("Appuyez sur une touche pour entrer en mode test :\n");
    printf("A - Tests unitaires...\n");
    printf("B - Tests deplacement...\n");
    printf("E - Strategie...\n");
    printf("F - Strategie 2024...\n");
    printf("G - Lecture des capteurs\n");
    printf("H - Asser Position - avance\n");
    printf("I - Asser Position - avance et tourne (gyro)\n");
    printf("J - Asser Position - avance et tourne (sans gyro)\n");
    printf("N - Fonctions geometrique\n");
    printf("O - Analyse obstacle\n");
    printf("Q - Asser Position - transition Gyro -> Pas gyro\n");
    printf("R - Test des logs...\n");
    printf("T - Trajectoire\n");
    printf("U - Tests i2c...\n");
    printf("V - APDS_9960\n");
    printf("W - Endurance aller retour\n");
    printf("Z - Codes de démonstration\n");
    stdio_flush();
    int rep = getchar_timeout_us(TEST_TIMEOUT_US);
    stdio_flush();
    switch (rep)
    {
    case 'a':
    case 'A':
        while(mode_test_unitaire());
        break;
    
    case 'b':
    case 'B':
        while(mode_test_deplacement());
        break;

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

    case 'F':
    case 'f':
        while(test_strategie_2024());
        break;

    case 'G':
    case 'g':
        while(test_capteurs_balise());
        break;

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

    case 'I':
    case 'i':
        while(test_asser_position_avance_et_tourne(1));
        break;
    
    case 'J':
    case 'j':
        while(test_asser_position_avance_et_tourne(0));
        break;


    case 'N':
    case 'n':
        while(test_geometrie());
        break;

    case 'O':
    case 'o':
        while(test_angle_balise());
        break;

    case 'Q':
    case 'q':
        while(test_transition_gyro_pas_gyro());
        break;
    
    case 'R':
    case 'r':
        while(test_log());
        break;

    case 'U':
    case 'u':
        while(test_i2c());
        break;

    case 'V':
    case 'v':
        while(test_APDS9960());
        break;
    
    case 'Z':
    case 'z':
        while(Demonstration_menu());
        break;

    case PICO_ERROR_TIMEOUT:
        iteration--;        
        if(iteration == 0){
            //printf("Sortie du mode test\n");
            //return 0;
        }

    default:
        printf("Commande inconnue\n");
        break;
    }
    return 1;
    
}

int test_continue_test(){
    int lettre;
    //printf("q pour quitter, une autre touche pour un nouveau test.\n");
    do{
        lettre = getchar_timeout_us(0);
    }while(lettre == PICO_ERROR_TIMEOUT || lettre == 0);
    switch(lettre){
        case 'q':
        case 'Q':
            return 0;
        default:
            return 1;
    }

}

int test_capteurs_balise(void){
    printf("Test de la balise\n");
    i2c_maitre_init();
    Localisation_set(0,0,0);
    Balise_VL53L1X_init();
    
    while(true){
        uint8_t min_distance;
        i2c_gestion(i2c0);
        i2c_annexe_gestion();
        Balise_VL53L1X_gestion();

        min_distance = Balise_VL53L1X_get_min_distance();
        printf(">min_distance:%d\n",min_distance);
        for(uint8_t capteur=0; capteur<12; capteur++){
            printf(">c%d:%d\n",capteur, Balise_VL53L1X_get_capteur_cm(capteur));
        }
        sleep_ms(20);
    }

}



int test_APDS9960(){
    int lettre;

    printf("Initialisation\n");
    APDS9960_Init();
    printf("Lecture...\n");
    /*
    do{
        APDS9960_Lire();
        lettre = getchar_timeout_us(0);
        stdio_flush();
    }while(lettre == PICO_ERROR_TIMEOUT);*/
    while(1){
        APDS9960_Lire();
        sleep_ms(100);
    }
    return 1;
}


/// @brief Avance droit 100 mm/s en tournant sur lui-même (1rad/s)
/// @param m_gyro : 1 pour utiliser le gyroscope, 0 sans
/// @param propulseur : 1 pour activer le propulseur toutes les secondes
/// @return 
int test_transition_gyro_pas_gyro(void){
    int lettre, _step_ms = 1, _step_ms_gyro = 2, step_gyro=2, propulseur_on=0;
    uint32_t temps_ms = 0, temps_ms_init = 0, temps_ms_old, tempo_prop=0;
    struct position_t position_consigne;

    position_consigne.angle_radian = 0;
    position_consigne.x_mm = 0;
    position_consigne.y_mm = 0;
    
    printf("Le robot tourne sur lui-même, transition sans gyro @ t=5s\n");

    printf("Init gyroscope\n");
    Gyro_Init();
    printf("C'est parti !\n");
    stdio_flush();

    set_position_avec_gyroscope(1);
    temps_ms = Temps_get_temps_ms();
    temps_ms_old = temps_ms;
    temps_ms_init = temps_ms;

    multicore_launch_core1(affiche_localisation);
    do{
        
        while(temps_ms == Temps_get_temps_ms());
        
        temps_ms = Temps_get_temps_ms();
        temps_ms_old = temps_ms;

        QEI_update();
        if(get_position_avec_gyroscope()){
            if(temps_ms % _step_ms_gyro == 0){
                Gyro_Read(_step_ms_gyro);
            }
        }

        if(temps_ms - temps_ms_init > 5000){
            set_position_avec_gyroscope(0);
        }
        Localisation_gestion();
        AsserMoteur_Gestion(_step_ms);
        
        position_consigne.angle_radian = (float) (temps_ms - temps_ms_init) /1000.;

        Asser_Position(position_consigne);

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

    printf("lettre : %c, %d\n", lettre, lettre);

    return 0;
}


/// @brief Avance droit 100 mm/s en tournant sur lui-même (1rad/s)
/// @param m_gyro : 1 pour utiliser le gyroscope, 0 sans
/// @param propulseur : 1 pour activer le propulseur toutes les secondes
/// @return 
int test_asser_position_avance_et_tourne(int m_gyro){
    int lettre, _step_ms = 1, _step_ms_gyro = 2, step_gyro=2, propulseur_on=0;
    uint32_t temps_ms = 0, temps_ms_init = 0, temps_ms_old, tempo_prop=0;
    struct position_t position_consigne;

    position_consigne.angle_radian = 0;
    position_consigne.x_mm = 0;
    position_consigne.y_mm = 0;
    
    printf("Le robot avance à 100 mm/s\n");
    if(m_gyro){
        printf("Init gyroscope\n");
        Gyro_Init();
        printf("C'est parti !\n");
    }
    stdio_flush();

    set_position_avec_gyroscope(m_gyro);
    temps_ms = Temps_get_temps_ms();
    temps_ms_old = temps_ms;
    temps_ms_init = temps_ms;

    multicore_launch_core1(affiche_localisation);
    do{
        
        while(temps_ms == Temps_get_temps_ms()){

        }

        temps_ms = Temps_get_temps_ms();
        temps_ms_old = temps_ms;

        QEI_update();
        if(temps_ms % _step_ms_gyro == 0){
            Gyro_Read(_step_ms_gyro);
        }
        Localisation_gestion();
        AsserMoteur_Gestion(_step_ms);
        
        position_consigne.angle_radian = (float) (temps_ms - temps_ms_init) /1000.;

        Asser_Position(position_consigne);

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

    printf("lettre : %c, %d\n", lettre, lettre);

    return 0;
}

int test_asser_position_avance(){
    int lettre, _step_ms = 1, temps_ms=0;
    struct position_t position;

    position.angle_radian = 0;
    position.x_mm = 0;
    position.y_mm = 0;
    
    printf("Le robot avance à 100 mm/s\n");
    do{
        QEI_update();
        Localisation_gestion();
        AsserMoteur_Gestion(_step_ms);
        
        if(temps_ms < 5000){
            position.y_mm = (float) temps_ms * 100. / 1000.;
        }else if(temps_ms < 10000){ 
            position.y_mm = 1000 - (float) temps_ms * 100. / 1000.;
        }else{
            temps_ms = 0;
        }

        Asser_Position(position);
        temps_ms += _step_ms;
        sleep_ms(_step_ms);

        lettre = getchar_timeout_us(0);
    }while(lettre == PICO_ERROR_TIMEOUT);

    return 0;
}




int test_geometrie(){
    float angle = 270, angle_min, angle_max;
    printf("Normalise %f° : %f°\n", angle, Geometrie_get_angle_normalisee(angle*DEGRE_EN_RADIAN)/DEGRE_EN_RADIAN);
    angle = 180;
    printf("Normalise %f° : %f°\n", angle, Geometrie_get_angle_normalisee(angle*DEGRE_EN_RADIAN)/DEGRE_EN_RADIAN);
    angle = 181;
    printf("Normalise %f° : %f°\n", angle, Geometrie_get_angle_normalisee(angle*DEGRE_EN_RADIAN)/DEGRE_EN_RADIAN);
    angle = 179;
    printf("Normalise %f° : %f°\n", angle, Geometrie_get_angle_normalisee(angle*DEGRE_EN_RADIAN)/DEGRE_EN_RADIAN);


    angle_min = -100;
    angle_max = -80;
    angle = -90;
    printf("Anglee %f° compris entre %f° et %f° : %d\n", angle, angle_min, angle_max, 
        Geometrie_compare_angle(angle*DEGRE_EN_RADIAN, angle_min*DEGRE_EN_RADIAN, angle_max*DEGRE_EN_RADIAN));
    angle = 90;
    printf("Anglee %f° compris entre %f° et %f° : %d\n", angle, angle_max, angle_min, 
        Geometrie_compare_angle(angle*DEGRE_EN_RADIAN, angle_min*DEGRE_EN_RADIAN, angle_max*DEGRE_EN_RADIAN));
    angle = -120;
    printf("Anglee %f° compris entre %f° et %f° : %d\n", angle, angle_max, angle_min, 
        Geometrie_compare_angle(angle*DEGRE_EN_RADIAN, angle_min*DEGRE_EN_RADIAN, angle_max*DEGRE_EN_RADIAN));



    angle_min = 178;
    angle_max = 182;
    angle = 179;
    printf("Anglee %f° compris entre %f° et %f° : %d\n", angle, angle_min, angle_max, 
        Geometrie_compare_angle(angle*DEGRE_EN_RADIAN, angle_min*DEGRE_EN_RADIAN, angle_max*DEGRE_EN_RADIAN));
    angle = 177;
    printf("Anglee %f° compris entre %f° et %f° : %d\n", angle, angle_max, angle_min, 
        Geometrie_compare_angle(angle*DEGRE_EN_RADIAN, angle_min*DEGRE_EN_RADIAN, angle_max*DEGRE_EN_RADIAN));
    angle = 183;
    printf("Anglee %f° compris entre %f° et %f° : %d\n", angle, angle_max, angle_min, 
        Geometrie_compare_angle(angle*DEGRE_EN_RADIAN, angle_min*DEGRE_EN_RADIAN, angle_max*DEGRE_EN_RADIAN));

    return 0;
}

void affiche_monitoring(){
    float distance;
    while(1){
        temps_cycle_display();
        printf(">distance:%f\n",Trajet_get_obstacle_mm());
        sleep_ms(100);
    }
}


int test_angle_balise(void){
    int lettre;
    float distance, angle=3.1281;

    enum {
        TEST_BLEU,
        ATTENTE1,
        TEST_VERT,
        ATTENTE2,
        TEST_RESET,
        ATTENTE3,
        TEST_ANGLE
    }etat_test_led=TEST_BLEU;

    i2c_maitre_init();
    Balise_VL53L1X_init();
    Localisation_set(1000,1500,0);

    multicore_launch_core1(affiche_monitoring);
    uint temps_ms, timer_ms=1000;

    temps_ms = Temps_get_temps_ms();
    
    do{
        temps_cycle_check();

        i2c_gestion(i2c0);
        i2c_annexe_gestion();
        Balise_VL53L1X_gestion();
        if(temps_ms != Temps_get_temps_ms()){
            temps_ms = Temps_get_temps_ms();

            switch(etat_test_led){
                case TEST_BLEU:
                    i2c_annexe_couleur_balise(0b00011100, 0x0FFF);
                    timer_ms--;
                    if(timer_ms<2){
                        etat_test_led=TEST_VERT;
                        timer_ms=1000;
                    }
                    break;
                
                case TEST_VERT:
                    i2c_annexe_couleur_balise(0b00000011, 0x0FFF);
                    timer_ms--;
                    if(timer_ms<2){
                        etat_test_led=TEST_RESET;
                        timer_ms=10000;
                    }
                    break;

                case TEST_RESET:
                    i2c_annexe_couleur_balise(0, 0x00);
                    timer_ms--;
                    if(timer_ms<2){
                        etat_test_led=TEST_ANGLE;
                        timer_ms=0;
                    }
                    break;

                case TEST_ANGLE:
                    timer_ms++;
                    angle=(float)timer_ms / 1000.;
                    Trajet_set_obstacle_mm(Balise_VL53L1X_get_distance_obstacle_mm(angle));
                    break;
                
            }
        }

        lettre = getchar_timeout_us(0);
    }while(lettre == PICO_ERROR_TIMEOUT || lettre == 0);
        
    return 0;


}