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

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

const uint LED_PIN = 25;
const uint LED_PIN_ROUGE = 28;
const uint LED_PIN_NE_PAS_UTILISER = 22;

uint temps_cycle;


#define V_INIT -999.0
#define TEST_TIMEOUT_US 10000000

int mode_test();

int main() {
    bi_decl(bi_program_description("This is a test binary."));
    bi_decl(bi_1pin_with_name(LED_PIN, "On-board LED"));
    float vitesse_filtre_x=V_INIT, vitesse_filtre_y=V_INIT, vitesse_filtre_z=V_INIT;
    struct t_angle_gyro_float angle_gyro;

    uint32_t temps_ms = 0, temps_ms_old;
    uint32_t temps_us_debut_cycle;
    uint32_t score=0;

    stdio_init_all();

    gpio_init(LED_PIN);
    gpio_set_dir(LED_PIN, GPIO_OUT);
    gpio_put(LED_PIN, 1);

    gpio_init(LED_PIN_ROUGE);
    gpio_set_dir(LED_PIN_ROUGE, GPIO_OUT);
    gpio_put(LED_PIN_ROUGE, 0);

    gpio_init(COULEUR);
    gpio_init(TIRETTE);
    gpio_set_dir(COULEUR, GPIO_IN);
    gpio_set_dir(TIRETTE, GPIO_IN);


    // Il faut neutraliser cette broche qui pourrait interférer avec 
    // la lecture des codeurs. (problème sur la carte électrique)...
    gpio_init(LED_PIN_NE_PAS_UTILISER);
    gpio_set_dir(LED_PIN_NE_PAS_UTILISER, GPIO_IN);

    sleep_ms(3000);
    Servomoteur_Init();
    //puts("Debut");
    //spi_test();

    //while(1);
    Temps_init();
    Moteur_Init();
    QEI_init();
    AsserMoteur_Init();
    Localisation_init();

    //while(mode_test());
    i2c_maitre_init();
    Trajet_init();

    set_position_avec_gyroscope(1);
    if(get_position_avec_gyroscope()){
        Gyro_Init();
    }

    multicore_launch_core1(Monitoring_display);

    temps_ms = Temps_get_temps_ms();
    temps_ms_old = temps_ms;
    while (1) { 
        static enum {
            MATCH_ATTENTE_TIRETTE,
            MATCH_EN_COURS,
            MATCH_ARRET_EN_COURS,
            MATCH_TERMINEE
        } statu_match=MATCH_ATTENTE_TIRETTE;
        uint16_t _step_ms_gyro = 2;
        const uint16_t _step_ms = 1;
        static uint32_t timer_match_ms=0;
        static enum couleur_t couleur;

        // Surveillance du temps d'execution
        temps_cycle_check();

        i2c_gestion(i2c0);
        i2c_annexe_gestion();
        Balise_VL53L1X_gestion();
        
        
        if(temps_ms != Temps_get_temps_ms()){
            timer_match_ms = timer_match_ms + _step_ms;
            temps_ms = Temps_get_temps_ms();
            QEI_update();
            Localisation_gestion();
            AsserMoteur_Gestion(_step_ms);

            // Routine à 2 ms
            if(temps_ms % _step_ms_gyro == 0){
                if(get_position_avec_gyroscope()){
                    Gyro_Read(_step_ms_gyro);
                }
            }

            switch(statu_match){
                case MATCH_ATTENTE_TIRETTE:
                    if(lire_couleur() == COULEUR_VERT){
                        // Tout vert
                        i2c_annexe_couleur_balise(0b00011100, 0x0FFF);
                    }else{
                        // Tout bleu
                        i2c_annexe_couleur_balise(0b00000011, 0x0FFF);
                    }
                    if(attente_tirette() == 0){
                        statu_match = MATCH_EN_COURS;
                        couleur = lire_couleur();
                        timer_match_ms=0;
                        i2c_annexe_couleur_balise(0, 0x00);
                        score=5;
                        i2c_annexe_envoie_score(score);
                    }
                    break;

                case MATCH_EN_COURS:
                    if (timer_match_ms > 98000){
                        printf("MATCH_ARRET_EN_COURS\n");
                        statu_match = MATCH_ARRET_EN_COURS;
                    }
                    Strategie(couleur, _step_ms);
                    break;
                
                case MATCH_ARRET_EN_COURS:
                    commande_vitesse_stop();
                    i2c_annexe_active_deguisement();
                    if (timer_match_ms > 100000){
                        statu_match = MATCH_TERMINEE;
                    }
                    
                    break;

                case MATCH_TERMINEE:
                    Moteur_Stop();
                    while(1);
                    break;
            }
        }
    }
}