#include <stdio.h>
#include "pico/multicore.h"

#include "QEI.h"
#include "Moteurs.h"
#include "Asser_Moteurs.h"
#include "Localisation.h"
#include "i2c_maitre.h"
#include "i2c_annexe.h"
#include "Robot_config.h"
#include "gyro.h"
#include "Temps.h"

#include "Test_gyro.h"


#define TEST_TIMEOUT_US 10000000

int test_QIE(void);
int test_QIE_mm(void);
int test_asser_moteur(void);
int test_vitesse_moteur(enum t_moteur moteur);
int test_moteurs(void);
void affiche_localisation();
int test_localisation();


// Mode test : renvoie 0 pour quitter le mode test
int mode_test_unitaire(){
    static int iteration = 2;
    printf("Appuyez sur une touche pour entrer en mode test :\n");
    printf("A - pour les codeurs\n");
    printf("B - pour les codeurs (somme en mm)\n");
    printf("C - pour les moteurs\n");
    printf("D - pour asser_moteurs (rotation)\n");
    printf("E - pour le gyroscope\n");
    printf("F - pour la localisation\n");
    
    
    stdio_flush();
    int rep = getchar_timeout_us(TEST_TIMEOUT_US);
    stdio_flush();
    switch (rep)
    {
    case 'a':
    case 'A':
        while(test_QIE());
        break;
    case 'b':
    case 'B':
        while(test_QIE_mm());
        break;
        
    case 'C':
    case 'c':
        while(test_moteurs());
        break;

    case 'D':
    case 'd':
        while(test_asser_moteur());
        break;

    case 'E':
    case 'e':
        while(test_gyro());
        break;
        
    
    case 'F':
    case 'f':
        while(test_localisation());
        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_QIE(){
    int lettre;
    printf("Affichage des QEI :\nAppuyez sur une touche pour quitter\n");
    QEI_init();
    do{
        QEI_update();
        printf("Codeur A : %d (%3.2f mm), codeur B : %d (%3.2f mm), codeur C : %d (%3.2f mm)\n", 
            QEI_get(QEI_A_NAME), QEI_get_mm(QEI_A_NAME),
            QEI_get(QEI_B_NAME), QEI_get_mm(QEI_B_NAME),
            QEI_get(QEI_C_NAME), QEI_get_mm(QEI_C_NAME));
        printf(">CodeurA:%.3f\n>CodeurB:%.3f\n>CodeurC:%.3f\n", QEI_get_mm(QEI_A_NAME), QEI_get_mm(QEI_B_NAME), QEI_get_mm(QEI_C_NAME));
        sleep_ms(100);

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

int test_QIE_mm(){
    int lettre;
    printf("Affichage des QEI :\nAppuyez sur une touche pour quitter\n");
    QEI_init();
    float a_mm=0, b_mm=0, c_mm=0;
    do{
        QEI_update();
        a_mm += QEI_get_mm(QEI_A_NAME);
        b_mm += QEI_get_mm(QEI_B_NAME);
        c_mm += QEI_get_mm(QEI_C_NAME);
        printf("Codeur A : %3.2f mm, codeur B : %3.2f mm, codeur C : %3.2f mm\n", a_mm, b_mm, c_mm);
        printf(">CodeurA:%.3f\n>CodeurB:%.3f\n>CodeurC:%.3f\n", a_mm, b_mm, c_mm);
        sleep_ms(100);

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

}

int test_moteurs(){
    int lettre_moteur;
    Moteur_Init();

    printf("Indiquez le moteurs à tester (A, B ou C):\n");
    do{
         lettre_moteur = getchar_timeout_us(TEST_TIMEOUT_US);
         stdio_flush();
    }while(lettre_moteur == PICO_ERROR_TIMEOUT);
    printf("Moteur choisi : %c %d %x\n", lettre_moteur, lettre_moteur, lettre_moteur);

    switch (lettre_moteur)
    {
    case 'A':
    case 'a':
        while(test_vitesse_moteur(MOTEUR_A));
        break;

    case 'B':
    case 'b':
        while(test_vitesse_moteur(MOTEUR_B));
        break;

    case 'C':
    case 'c':
        while(test_vitesse_moteur(MOTEUR_C));
        break;
    
    case 'Q':
    case 'q':
        return 0;
        break;
    
    default:
        break;
    }
    
    return 1;
}

int test_vitesse_moteur(enum t_moteur moteur){
    printf("Vitesse souhaitée :\n0 - 0%%\n1 - 10%%\n2 - 20%%\n...\n9 - 90%%\nA - 100%%\n");

    int vitesse_moteur;
    do{ 
        vitesse_moteur = getchar_timeout_us(0);
        stdio_flush();

    }while(vitesse_moteur == PICO_ERROR_TIMEOUT);
    
    switch (vitesse_moteur)
    {
    case '0':
    case '1':
    case '2':
    case '3':
    case '4':
    case '5':
    case '6':
    case '7':
    case '8':
    case '9':
        printf("Vitesse choisie : %c0%%\n", vitesse_moteur);
        Moteur_SetVitesse(moteur, (vitesse_moteur - '0') * 32767.0 / 10.);
        break;

    case 'A':
    case 'a':
        printf("Vitesse choisie : 100%%\n");
        Moteur_SetVitesse(moteur, (int16_t) 32766.0);
        break;

    case 'b':
    case 'B':
        printf("Vitesse choisie : -50%%\n");
        Moteur_SetVitesse(moteur, (int16_t) -32766.0/2);
        break;

    case 'q':
    case 'Q':
        return 0;
        break;

    default:
        break;
    }
    return 1;
}

void test_asser_moteur_printf(){
    int _step_ms = 1;
    while(1){
        printf(">Vitesse_A:%.0f\n>Vitesse_B:%.0f\n>Vitesse_C:%.0f\n", AsserMoteur_getVitesse_mm_s(MOTEUR_A, _step_ms),
            AsserMoteur_getVitesse_mm_s(MOTEUR_B, _step_ms), AsserMoteur_getVitesse_mm_s(MOTEUR_C, _step_ms));
        
        //sleep_ms(5);
    }
}

int test_asser_moteur(){
    int lettre;
    int _step_ms = 1;
    AsserMoteur_Init();
    printf("Asservissement des moteurs :\nAppuyez sur une touche pour quitter\n");
    AsserMoteur_setConsigne_mm_s(MOTEUR_A, 100);
    AsserMoteur_setConsigne_mm_s(MOTEUR_B, 100);
    AsserMoteur_setConsigne_mm_s(MOTEUR_C, 100);
    multicore_launch_core1(test_asser_moteur_printf);
    do{
        QEI_update();
        AsserMoteur_Gestion(_step_ms);
        sleep_ms(_step_ms);
        //printf("Vitesse A : %d, codeur B : %d, codeur C : %d\n", QEI_get(QEI_A_NAME), QEI_get(QEI_B_NAME), QEI_get(QEI_C_NAME));
        //printf("Vitesse A : %.0f, vitesse B : %.0f, vitesse C : %.0f\n", AsserMoteur_getVitesse_mm_s(MOTEUR_A, _step_ms),
        //    AsserMoteur_getVitesse_mm_s(MOTEUR_B, _step_ms), AsserMoteur_getVitesse_mm_s(MOTEUR_C, _step_ms));
        lettre = getchar_timeout_us(0);
    }while(lettre == PICO_ERROR_TIMEOUT);
    Moteur_SetVitesse(MOTEUR_A, 0);
    Moteur_SetVitesse(MOTEUR_B, 0);
    Moteur_SetVitesse(MOTEUR_C, 0);
    multicore_reset_core1();
    return 0;
}

void affiche_localisation(){
    struct position_t position;
    while(1){
        position = Localisation_get();
        printf(">X:%f\n>Y:%f\n>angle:%f\n", position.x_mm, position.y_mm, position.angle_radian *180. / 3.141592654);
        printf(">v_bat:%2.2f\n", i2c_annexe_get_tension_batterie());
    }
}


int test_localisation(){
    int lettre;
    int moteurs = 0;
    struct position_t position;
    uint32_t temps_ms;
    uint32_t _step_ms_gyro = 2, _step_ms=1, tempo_moteur=0;
    uint32_t m_gyro = 0;
    int16_t vitesse;
    
    Localisation_init();

    printf("A - Sans gyroscope\n");
    printf("B - Avec Gyroscope\n");
    printf("C - Avec Gyroscope + moteurs\n");
    do{
         lettre = getchar_timeout_us(TEST_TIMEOUT_US);
         stdio_flush();
    }while(lettre == PICO_ERROR_TIMEOUT);

    switch(lettre){
        case 'A':
        case 'a':
            set_position_avec_gyroscope(0);
            printf("Sans gyroscope\n");
            break;
        case 'c':
        case 'C':
            moteurs = 1;
            i2c_maitre_init();
        case 'B':
        case 'b':
            set_position_avec_gyroscope(1);
            printf("Avec gyroscope, initialisation...\n");
            m_gyro=1;
            Gyro_Init();
            break;
        default:
            return 0;
    }

    temps_ms = Temps_get_temps_ms();

    multicore_launch_core1(affiche_localisation);

    vitesse = (int16_t) 32766.0;
    
    printf("Affichage de la position du robot.\nAppuyez sur une touche pour quitter\n");
    do{
        i2c_gestion(i2c0);
        i2c_annexe_gestion();
        while(temps_ms == Temps_get_temps_ms());
        QEI_update();
        if(m_gyro){
            if(temps_ms % _step_ms_gyro == 0){
                Gyro_Read(_step_ms_gyro);
            }
        }
        Localisation_gestion();
        position = Localisation_get();
        if(moteurs){
            tempo_moteur++;
            if(tempo_moteur > 1000){
                tempo_moteur = 0;
                vitesse = -vitesse;
                Moteur_SetVitesse(MOTEUR_A ,vitesse);
                Moteur_SetVitesse(MOTEUR_B ,vitesse);
                Moteur_SetVitesse(MOTEUR_C ,vitesse);
            }
        }
        

        lettre = getchar_timeout_us(0);

        temps_ms += _step_ms;
    }while(lettre == PICO_ERROR_TIMEOUT || lettre == 0);

    multicore_reset_core1();

    return 0;
    
}