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

#include "QEI.h"
#include "Moteurs.h"
#include "Asser_Moteurs.h"
#include "Geometrie_robot.h"
#include "Commande_vitesse.h"

int test_avance(void);
int test_cde_vitesse(void);
int test_cde_vitesse_rotation(void);
int test_cde_rotation_ref_robot(float centre_x_mm, float centre_y_mm);
int test_cde_vitesse_rectangle(void);
int test_cde_vitesse_cercle(void);

#define TEST_TIMEOUT_US 10000000

// Mode test : renvoie 0 pour quitter le mode test
int mode_test_deplacement(){
    static int iteration = 2;
    printf("Appuyez sur une touche pour entrer en mode test :\n");
    printf("A - AsserMoteur - pour avancer selon Y\n");
    printf("B - AsserMoteur - Commande en vitesse...\n");
    
    
    stdio_flush();
    int rep = getchar_timeout_us(TEST_TIMEOUT_US);
    stdio_flush();
    switch (rep)
    {
    case 'a':
    case 'A':
        while(test_avance());
        break;
    case 'b':
    case 'B':
        while(test_cde_vitesse());
        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;
    
}

/// @brief Avance doucement (<10cm/s) selon l'axe X
/// @param  
/// @return 
int test_avance(void){
    int lettre;
    int _step_ms = 1;
    AsserMoteur_Init();
    AsserMoteur_setConsigne_mm_s(MOTEUR_A, -100);
    AsserMoteur_setConsigne_mm_s(MOTEUR_B, 100);
    AsserMoteur_setConsigne_mm_s(MOTEUR_C, 0);
    
    do{
        QEI_update();
        AsserMoteur_Gestion(_step_ms);
        sleep_ms(_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);
    return 0;
}

int test_cde_vitesse(){
    printf("A - Commande en vitesse - rectangle\n");
    printf("B - Commande en vitesse - cercle\n");
    printf("C - Commande en vitesse - rotation pure\n");
    printf("D - Commande en vitesse - rotation par rapport à un point (contacteur longer A)\n");
    printf("E - Commande en vitesse - rotation par rapport à un point (contacteur longer C)\n");

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

    }while(lettre == PICO_ERROR_TIMEOUT || lettre == '\0');

    switch(lettre){
        case 'a':
        case 'A':
            while(test_cde_vitesse_rectangle());
            break;
        
        case 'b':
        case 'B':
            while(test_cde_vitesse_cercle());
            break;
        
        case 'c':
        case 'C':
            while(test_cde_vitesse_rotation());
            break;
        
        case 'd':
        case 'D':
            while(test_cde_rotation_ref_robot(RAYON_ROBOT, 0));
            break;

        case 'e':
        case 'E':
            while(test_cde_rotation_ref_robot(RAYON_ROBOT/2, -RAYON_ROBOT* RACINE_DE_3/2));
            break;
    }
}

int test_cde_vitesse_rotation(){
    int lettre, _step_ms = 1;
    float vitesse =90.0/2 * 3.14159 /180.0;
    AsserMoteur_Init();
    printf("Rotation du robot sur lui-même en 8 secondes\nVitesse : %f rad/s\n", vitesse);

    commande_vitesse(0, 0, vitesse);
    do{
        QEI_update();
        AsserMoteur_Gestion(_step_ms);
        sleep_ms(_step_ms);
        lettre = getchar_timeout_us(0);
    }while(lettre == PICO_ERROR_TIMEOUT);
    Moteur_Stop();

    return 0;
}


int test_cde_rotation_ref_robot(float centre_x_mm, float centre_y_mm){
    int lettre, _step_ms = 1;
    float vitesse =90.0/4 * 3.14159 /180.0;
    AsserMoteur_Init();
    printf("Rotation du robot par rapport au point (Rayon, O)\nVitesse : %f rad/s\n", vitesse);

    commande_rotation(vitesse, centre_x_mm, centre_y_mm);
    do{
        QEI_update();
        AsserMoteur_Gestion(_step_ms);
        sleep_ms(_step_ms);
        lettre = getchar_timeout_us(0);
    }while(lettre == PICO_ERROR_TIMEOUT);
    Moteur_Stop();

    return 0;
}

int test_cde_vitesse_rectangle(){
    int lettre, _step_ms = 1, temps_ms=0;
    AsserMoteur_Init();

    printf("déplacement en rectangle du robot : 500x200 mm, 100 mm/s\n");
    do{
        QEI_update();
        AsserMoteur_Gestion(_step_ms);

        if(temps_ms < 5000){
            commande_vitesse(0, 100, 0);
        }else if(temps_ms < 7000){
            commande_vitesse(-100, 0, 0);
        }else if(temps_ms < 12000){
            commande_vitesse(0, -100, 0);
        }else if(temps_ms < 14000){
            commande_vitesse(100, 0, 0);
        }else{
            temps_ms = 0;
        }
        
        sleep_ms(_step_ms);
        temps_ms += _step_ms;
        lettre = getchar_timeout_us(0);
    }while(lettre == PICO_ERROR_TIMEOUT);
    Moteur_Stop();

    return 0;
}

int test_cde_vitesse_cercle(){
    int lettre, _step_ms = 1, temps_ms=0;
    AsserMoteur_Init();
    printf("déplacement en cercle du robot : 100 mm/s\n");
    do{
        QEI_update();
        AsserMoteur_Gestion(_step_ms);
        commande_vitesse(cos((float)temps_ms / 1000.) * 200.0, sin((float)temps_ms /1000.) * 200.0, 0);
        temps_ms += _step_ms;
        sleep_ms(_step_ms);
        

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

    return 0;
}