#include "hardware/gpio.h"
#include "i2c_annexe.h"
#include "Asser_Position.h"
#include "Balise_VL53L1X.h"
#include "Commande_vitesse.h"
#include "Geometrie_robot.h"
#include "Localisation.h"
#include "Moteurs.h"
#include "Strategie_prise_cerises.h"
#include "Strategie.h"
#include "Trajet.h"
#include "math.h"

#define SEUIL_RECAL_DIST_MM 75
#define SEUIL_RECAL_ANGLE_RADIAN (5 * DEGRE_EN_RADIAN)
#define DISTANCE_OBSTACLE_CM 50
#define DISTANCE_PAS_OBSTACLE_MM 2000

// TODO: Peut-être à remetttre en variable locale après
double distance_obstacle;

enum etat_action_t calage_angle(enum longer_direction_t longer_direction, double x_mm, double y_mm, double angle_radian);
enum etat_action_t lance_balles(uint32_t step_ms);

enum etat_strategie_t etat_strategie=STRATEGIE_INIT;

void Homologation(uint32_t step_ms){
    

    enum etat_action_t etat_action;
    enum etat_trajet_t etat_trajet;

    struct trajectoire_t trajectoire;

    switch(etat_strategie){
        case STRATEGIE_INIT:
            Localisation_set(775., 109., (-60.+CORR_ANGLE_DEPART_DEGREE) * DEGRE_EN_RADIAN);
            etat_strategie = ATTENTE_TIRETTE;
            break;

        case ATTENTE_TIRETTE:
            if(attente_tirette() == 0){
                etat_strategie = APPROCHE_CERISE_1_A;
            }
            break;

        case APPROCHE_CERISE_1_A:
            Trajet_config(250, 500);
            Trajectoire_droite(&trajectoire,775, 109, 857, 156, -60. * DEGRE_EN_RADIAN, 30. * DEGRE_EN_RADIAN);
            if(parcourt_trajet_simple_sans_evitement(trajectoire, step_ms) == TRAJET_TERMINE){
                etat_strategie = ATTRAPE_CERISE_1;
            }
            break;
        
        case ATTRAPE_CERISE_1:
            etat_action = cerise_attraper_bordure(LONGER_VERS_C, step_ms, 1000-15-PETIT_RAYON_ROBOT_MM);
            if(etat_action == ACTION_TERMINEE){
                etat_strategie = APPROCHE_PANIER_1;
            }
            break;

        case APPROCHE_PANIER_1:
            Trajet_config(500, 500);
            Trajectoire_bezier(&trajectoire,Localisation_get().x_mm, Localisation_get().y_mm, 
                                485, Localisation_get().y_mm,
                                465, 857,
                                465,2830,
                                +30. * DEGRE_EN_RADIAN, +120. * DEGRE_EN_RADIAN);

            if(parcourt_trajet_simple(trajectoire, step_ms) == ACTION_TERMINEE){
                etat_strategie = CALAGE_PANIER_1;
            }
            break;

        case APPROCHE_PANIER_2:
            Trajectoire_droite(&trajectoire,Localisation_get().x_mm, Localisation_get().y_mm, 
                                265,2830,
                                +120. * DEGRE_EN_RADIAN, +120. * DEGRE_EN_RADIAN);

            if(parcourt_trajet_simple(trajectoire, step_ms) == ACTION_TERMINEE){
                etat_strategie = CALAGE_PANIER_1;
            }

            break;

        case CALAGE_PANIER_1:
            if(calage_angle(LONGER_VERS_A, RAYON_ROBOT, 3000 - (RAYON_ROBOT/(RACINE_DE_3/2.)), 120. *DEGRE_EN_RADIAN) == ACTION_TERMINEE){
                etat_strategie = RECULE_PANIER;
            }
            break;
        
        case RECULE_PANIER:
            Trajet_config(250, 500);
            Trajectoire_droite(&trajectoire,Localisation_get().x_mm, Localisation_get().y_mm, 
                                180, 3000 - (RAYON_ROBOT/(RACINE_DE_3/2)) - 80,
                                120. * DEGRE_EN_RADIAN, +270. * DEGRE_EN_RADIAN);

            if(parcourt_trajet_simple_sans_evitement(trajectoire, step_ms) == ACTION_TERMINEE){
                etat_strategie = LANCE_DANS_PANIER;
            }
            break;

        case LANCE_DANS_PANIER:
            Asser_Position_maintien();
            if(lance_balles(step_ms) == ACTION_TERMINEE){
                etat_strategie = STRATEGIE_FIN;
            }
            break;

        case APPROCHE_CERISE_2:
            Trajet_config(250, 500);
            Trajectoire_droite(&trajectoire,Localisation_get().x_mm, Localisation_get().y_mm, 
                                830, 3000 - 156,
                                Localisation_get().angle_radian, 30. * DEGRE_EN_RADIAN);

            if(parcourt_trajet_simple_sans_evitement(trajectoire, step_ms) == ACTION_TERMINEE){
                etat_strategie = STRATEGIE_FIN;
            }
            break;

        case ATTRAPPE_CERISE_2:
            etat_action = cerise_attraper_bordure(LONGER_VERS_A, step_ms, 1000-15-PETIT_RAYON_ROBOT_MM);
            if(etat_action == ACTION_TERMINEE){
                etat_strategie = APPROCHE_PANIER_2;
            }
            break;


        case STRATEGIE_FIN:
            i2c_annexe_desactive_propulseur();
            commande_vitesse_stop();
            break;
    }
}


/// @brief Active le propulseur, ouvre la porte, attend qql secondes.
/// @param step_ms : pas de temps.
/// @return ACTION_EN_COURS ou ACTION_TERMINEE
enum etat_action_t lance_balles(uint32_t step_ms){
    enum etat_action_t etat_action = ACTION_EN_COURS;
    static uint32_t tempo_ms;
    static uint32_t nb_iteration;

    static enum{
        LANCE_PROPULSEUR_ON,
        LANCE_TEMPO_PROP_ON,
        LANCE_PORTE_OUVERTE,
    } etat_lance_balle = LANCE_PROPULSEUR_ON;

    switch(etat_lance_balle){
        case LANCE_PROPULSEUR_ON:
            i2c_annexe_active_propulseur();
            tempo_ms = 1000;
            nb_iteration=0;
            etat_lance_balle = LANCE_TEMPO_PROP_ON;
            break;
        
        case LANCE_TEMPO_PROP_ON:
            if (tempo_ms < step_ms){
                nb_iteration++;
                if(nb_iteration > 10){
                    etat_action = ACTION_TERMINEE;
                    etat_lance_balle = LANCE_PROPULSEUR_ON;
                    i2c_annexe_desactive_propulseur();
                }else{
                    etat_lance_balle = LANCE_PORTE_OUVERTE;
                    tempo_ms = 300;
                }
                
                i2c_annexe_ouvre_porte();
                
            }else{
                tempo_ms -= step_ms;
            }
            break;
        
        case LANCE_PORTE_OUVERTE:
            if (tempo_ms < step_ms){
                i2c_annexe_ferme_porte();
                etat_lance_balle = LANCE_TEMPO_PROP_ON;
                tempo_ms = 450;
                
            }else{
                tempo_ms -= step_ms;
            }
            break;

    }
    return etat_action;
}

/// @brief Envoie le robot se caler dans l'angle en face de lui, recale la localisation
enum etat_action_t calage_angle(enum longer_direction_t longer_direction, double x_mm, double y_mm, double angle_radian){
    enum etat_action_t etat_action = ACTION_EN_COURS;
    struct position_t position;

    avance_puis_longe_bordure(longer_direction);
    if( ((longer_direction == LONGER_VERS_A) && (i2c_annexe_get_contacteur_butee_A() == CONTACTEUR_ACTIF) ) ||
        ((longer_direction == LONGER_VERS_C) && (i2c_annexe_get_contacteur_butee_C() == CONTACTEUR_ACTIF) ) ){
        etat_action = ACTION_TERMINEE;

        position = Localisation_get();
        if(fabs(position.x_mm - x_mm) < SEUIL_RECAL_DIST_MM){
            Localisation_set_x(x_mm);
        }
        if(fabs(position.y_mm - y_mm) < SEUIL_RECAL_DIST_MM){
            Localisation_set_y(y_mm);
        }
        if(fabs(position.angle_radian - angle_radian) < SEUIL_RECAL_ANGLE_RADIAN){
            Localisation_set_angle(angle_radian);
        }
    }
    return etat_action;
}

enum etat_action_t parcourt_trajet_simple(struct trajectoire_t trajectoire, uint32_t step_ms){
    enum etat_action_t etat_action = ACTION_EN_COURS;
    enum etat_trajet_t etat_trajet;
    double angle_avancement;
    
    static enum {
        PARCOURS_INIT,
        PARCOURS_AVANCE,
    } etat_parcourt=PARCOURS_INIT;

    switch (etat_parcourt){
        case PARCOURS_INIT:
            Trajet_debut_trajectoire(trajectoire);
            etat_parcourt = PARCOURS_AVANCE;
            break;
        
        case PARCOURS_AVANCE:
            angle_avancement = Trajet_get_orientation_avance();
            distance_obstacle = Balise_VL53L1X_get_distance_obstacle_mm(angle_avancement);
            Trajet_set_obstacle_mm(distance_obstacle);
            
            etat_trajet = Trajet_avance(step_ms/1000.);
            if(etat_trajet == TRAJET_TERMINE){
                Trajet_set_obstacle_mm(DISTANCE_INVALIDE);
                etat_action = ACTION_TERMINEE;
                etat_parcourt = PARCOURS_INIT;
            }
            break;
    }

    return etat_action;
}

enum etat_action_t parcourt_trajet_simple_sans_evitement(struct trajectoire_t trajectoire, uint32_t step_ms){
    enum etat_action_t etat_action = ACTION_EN_COURS;
    enum etat_trajet_t etat_trajet;
    static enum {
        PARCOURS_INIT,
        PARCOURS_AVANCE,
    } etat_parcourt=PARCOURS_INIT;

    switch (etat_parcourt){
        case PARCOURS_INIT:
            Trajet_debut_trajectoire(trajectoire);
            Trajet_set_obstacle_mm(distance_pas_obstacle);
            etat_parcourt = PARCOURS_AVANCE;
            break;
        
        case PARCOURS_AVANCE:
            etat_trajet = Trajet_avance(step_ms/1000.);
            if(etat_trajet == TRAJET_TERMINE){
                etat_action = ACTION_TERMINEE;
                etat_parcourt = PARCOURS_INIT;
            }
            break;
    }

    return etat_action;
}

/// @brief Renvoi 1 si on doit attendre le déclenchement de la tirette
uint attente_tirette(void){
    return !gpio_get(TIRETTE);
}

/// @brief Renvoi COULEUR_VERT ou COULEUR_BLEU
enum couleur_t lire_couleur(void){
    if (gpio_get(COULEUR))
        return COULEUR_VERT;
    return COULEUR_BLEU;

}

/// @brief Décremente la temps de step_ms, renvoie 1 si la temporisation est écoulée
/// @param tempo_ms 
/// @param step_ms 
/// @return 1 si la temporisation est écoulée, 0 sinon.
int temporisation_terminee(uint32_t * tempo_ms, uint32_t step_ms){
    if(*tempo_ms < step_ms){
        return 1;
    }else{
        *tempo_ms -= step_ms;
        return 0;
    }
}