#include "hardware/gpio.h"
#include "i2c_annexe.h"
#include "Asser_Position.h"
#include "Commande_vitesse.h"
#include "Geometrie_robot.h"
#include "Localisation.h"
#include "Monitoring.h"
#include "Moteurs.h"
#include "Score.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

#define NB_OBJECTIFS 4

struct objectif_t objectifs[NB_OBJECTIFS];
struct objectif_t * objectif_courant;

uint32_t Score_cerises_dans_robot=0;

void Strategie_set_cerise_dans_robot(uint32_t nb_cerises){
    Score_cerises_dans_robot = nb_cerises;
}

uint32_t Strategie_get_cerise_dans_robot(void){
    return Score_cerises_dans_robot;
}



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

enum etat_action_t lance_balles_dans_panier(enum couleur_t couleur, uint32_t step_ms, uint32_t nb_cerises);
enum etat_action_t lance_balles(uint32_t step_ms, uint32_t nb_cerises);



int Robot_est_dans_quart_haut_gauche();
int Robot_est_dans_quart_haut_droit();
int Robot_est_dans_zone_cerise_haut(enum couleur_t couleur);
int Robot_est_dans_zone_cerise_gauche();
int Robot_est_dans_zone_cerise_droite();
enum etat_action_t Strategie_aller_cerises_laterales_proches(enum couleur_t couleur, uint32_t step_ms);
enum etat_action_t Strategie_aller_cerises_laterales_opposees(enum couleur_t couleur, uint32_t step_ms);

enum etat_action_t Strategie_calage(enum couleur_t couleur);


void Strategie(enum couleur_t couleur, uint32_t step_ms, uint32_t temps_ms){
    const uint32_t temps_pre_fin_match = (97000 - 10000);
    static bool pre_fin_match_active=false;
    float angle_fin;
    float recal_pos_x, recal_pos_y;
    float point_x, point_y;
    enum longer_direction_t longer_direction;
    enum etat_action_t etat_action;
    enum etat_trajet_t etat_trajet;

    struct trajectoire_t trajectoire;

    static enum etat_strategie_t {
        STRATEGIE_INIT,
        ALLER_CERISE_BAS,
        ATTRAPER_CERISE_BAS,
        ALLER_CERISE_HAUT,
        ATTRAPER_CERISE_HAUT,
        ALLER_CERISE_GAUCHE,
        ATTRAPER_CERISE_GAUCHE,
        ALLER_CERISE_DROITE,
        ATTRAPER_CERISE_DROITE,
        ALLER_PANIER,
        LANCER_PANIER,
        RETOUR_MAISON,
        DECISION_STRATEGIE,
        STRATEGIE_FIN
    }etat_strategie;

    if(Monitoring_get_erreur_critique()){
        etat_strategie=STRATEGIE_FIN;
    }

    if(temps_ms > temps_pre_fin_match && pre_fin_match_active == false){
        if(etat_strategie != LANCER_PANIER){
            etat_strategie = RETOUR_MAISON;
        }
        pre_fin_match_active=true;
    }

    switch(etat_strategie){
        case STRATEGIE_INIT:
            if(Strategie_calage_debut(COULEUR_BLEU, step_ms) == ACTION_TERMINEE){
                etat_strategie = STRATEGIE_FIN;
            }
            break;
        
        case RETOUR_MAISON:
            if(Strategie_pieds_dans_plat(couleur, step_ms) == ACTION_TERMINEE){
                etat_strategie=STRATEGIE_FIN;
            }
            break;


        case STRATEGIE_FIN:
            pre_fin_match_active=true;
            commande_vitesse_stop();
            i2c_annexe_couleur_balise(0xE0, 0x0FFF);
            break;
    }
}



/// Fonction de localisation approximatives pour la stratégie.

/// @brief Renvoi 1 si le robot est dans le quart supérieur gauche du terrain
/// @return 1 si oui, 0 si non.
int Robot_est_dans_quart_haut_gauche(){
    if(Localisation_get().x_mm < 1000 && Localisation_get().y_mm > 1500){
        return 1;
    }
    return 0;
}

/// @brief Renvoi 1 si le robot est dans le quart supérieur droit du terrain
/// @return 1 si oui, 0 si non.
int Robot_est_dans_quart_haut_droit(){
    if(Localisation_get().x_mm > 1000 && Localisation_get().y_mm > 1500){
        return 1;
    }
    return 0;
}

int Robot_est_dans_zone_cerise_gauche(){
    if(Localisation_get().x_mm  < 630 && 
    Localisation_get().y_mm >1000 && Localisation_get().y_mm < 2000){
        return 1;
    }
    return 0;

}

int Robot_est_dans_zone_cerise_droite(){
    if(Localisation_get().x_mm  > 2000 - 630 && 
    Localisation_get().y_mm >1000 && Localisation_get().y_mm < 2000){
        return 1;
    }
    return 0;

}

int Robot_est_dans_zone_cerise_haut(enum couleur_t couleur){
    if(couleur == COULEUR_BLEU){
        if(Localisation_get().y_mm  > 2500 && Localisation_get().x_mm < 1000){
            return 1;
        }
    }else{
        if(Localisation_get().y_mm  > 2500 && Localisation_get().x_mm > 1000){
            return 1;
        }
    }
    return 0;
}


int Robot_est_dans_zone_depose_panier(enum couleur_t couleur){
    float x_mm, y_mm;
    x_mm = Localisation_get().x_mm;
    y_mm = Localisation_get().y_mm;
    if(couleur == COULEUR_BLEU){
        // Zone 1
        if(x_mm < 550 && y_mm > 2450){
            return 1;
        }
    }else{
        // Zone 1
        if(x_mm > 2000 - 550 && y_mm > 2450){
            return 1;
        }
    }
    return 0;
}

/// @brief Renvoi 1 si le robot intersect une zone de dépose
/// @param  couleur du robot
/// @return 1 si oui, 0 si non.
int Robot_est_dans_zone_depose(enum couleur_t couleur){
    float x_mm, y_mm;
    x_mm = Localisation_get().x_mm;
    y_mm = Localisation_get().y_mm;
    if(couleur == COULEUR_BLEU){
        // Zone 1
        if(x_mm < 550 && y_mm > 2450){
            return 1;
        }
        // Zone 2
        if(x_mm < 550 && y_mm > 800 && y_mm < 1450){
            return 1;
        }
        // Zone 3
        if(x_mm > 400 && x_mm < 1050 && y_mm < 550){
            return 1;
        }
        // Zone 4
        if(x_mm > 1450 && y_mm < 550){
            return 1;
        }
        // Zone 5
        if(x_mm > 1450 && y_mm > 1550 && y_mm < 2200){
            return 1;
        }
        return 0;

    }else{
        // VERT
        // Zone 1
        if(x_mm > (2000 - 550) && y_mm > 2450){
            return 1;
        }
        // Zone 2
        if(x_mm > (2000 - 550) && y_mm > 800 && y_mm < 1450){
            return 1;
        }
        // Zone 3
        if(x_mm < (2000 - 400) && x_mm > 2000 - 1050 && y_mm < 550){
            return 1;
        }
        // Zone 4
        if(x_mm < (2000 - 1450) && y_mm < 550){
            return 1;
        }
        // Zone 5
        if(x_mm < (2000 - 1450) && y_mm > 1550 && y_mm < 2200){
            return 1;
        }
        return 0;
    }
}


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

/// @brief Renvoi COULEUR_JAUNE ou COULEUR_BLEU
enum couleur_t lire_couleur(void){
    if (gpio_get(COULEUR))
        return COULEUR_JAUNE;
    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;
    }
}

enum etat_action_t Strategie_aller_a(float pos_x, float pos_y, uint32_t step_ms){
    struct trajectoire_t trajectoire;

    Trajectoire_droite(&trajectoire, Localisation_get().x_mm, Localisation_get().y_mm, 
        pos_x, pos_y,
        Localisation_get().angle_radian, Localisation_get().angle_radian);

    return Strategie_parcourir_trajet(trajectoire, step_ms, EVITEMENT_ARRET_DEVANT_OBSTACLE);
}

enum etat_action_t Strategie_tourner_et_aller_a(float pos_x, float pos_y, float angle_radian, enum evitement_t evitement,uint32_t step_ms){
    struct trajectoire_t trajectoire;

    Trajectoire_droite(&trajectoire, Localisation_get().x_mm, Localisation_get().y_mm, 
        pos_x, pos_y,
        Localisation_get().angle_radian, Geometrie_get_angle_optimal(Localisation_get().angle_radian,angle_radian));

    return Strategie_parcourir_trajet(trajectoire, step_ms, evitement);
}

/// @brief Avance puis tourne
/// @param pos_x 
/// @param pos_y 
/// @param angle_radian 
/// @param  
/// @param step_ms 
/// @return 
enum etat_action_t Strategie_aller_a_puis_tourner(float pos_x, float pos_y, float angle_radian, enum evitement_t evitement,uint32_t step_ms){
    struct trajectoire_t trajectoire;
    static enum {
        AAPT_ALLER,
        AAPT_TOURNER,
    }etat_aller_a_puis_tourner=AAPT_ALLER;

    switch(etat_aller_a_puis_tourner){
        case AAPT_ALLER:
            Trajet_config(TRAJECT_CONFIG_AVANCE_ET_TOURNE);
            Trajectoire_droite(&trajectoire, Localisation_get().x_mm, Localisation_get().y_mm, 
                pos_x, pos_y, Localisation_get().angle_radian, Localisation_get().angle_radian);
            if(Strategie_parcourir_trajet(trajectoire, step_ms, evitement) == ACTION_TERMINEE){
                etat_aller_a_puis_tourner = AAPT_TOURNER;
            }
            break;
        case AAPT_TOURNER:
            Trajet_config(TRAJECT_CONFIG_ROTATION_PURE);
            Trajectoire_rotation(&trajectoire, Localisation_get().x_mm, Localisation_get().y_mm,
            Localisation_get().angle_radian,
            Geometrie_get_angle_optimal(Localisation_get().angle_radian,angle_radian));

            if(Strategie_parcourir_trajet(trajectoire, step_ms, EVITEMENT_SANS_EVITEMENT) == ACTION_TERMINEE){
                etat_aller_a_puis_tourner = AAPT_ALLER;
                return ACTION_TERMINEE;
            }
            break;

    }

    return ACTION_EN_COURS;
}

enum etat_action_t Strategie_tourner_a(float angle_radian,uint32_t step_ms){
    struct trajectoire_t trajectoire;

    Trajet_config(TRAJECT_CONFIG_ROTATION_PURE);
    Trajectoire_rotation(&trajectoire, Localisation_get().x_mm, Localisation_get().y_mm,
    Localisation_get().angle_radian,
    Geometrie_get_angle_optimal(Localisation_get().angle_radian,angle_radian));

    return Strategie_parcourir_trajet(trajectoire, step_ms, EVITEMENT_SANS_EVITEMENT);
}


enum etat_action_t Strategie_pieds_dans_plat(enum couleur_t couleur, uint32_t step_ms){
    static struct objectif_t objectifs_plats[5], *objectif_plat_courant=NULL;
    enum etat_action_t etat_action;
    //Trajet_config(500,500); //750, 500 => Ne marche pas

    if(objectif_plat_courant==NULL){
        objectif_plat_courant = &objectifs_plats[0];
        struct objectif_t objectif_1 = { .priorite = 1, .etat = A_FAIRE, .cible = ZONE_1};
        struct objectif_t objectif_2 = { .priorite = 3, .etat = A_FAIRE, .cible = ZONE_2};
        struct objectif_t objectif_3 = { .priorite = 5, .etat = A_FAIRE, .cible = ZONE_3};
        struct objectif_t objectif_4 = { .priorite = 2, .etat = A_FAIRE, .cible = ZONE_4};
        struct objectif_t objectif_5 = { .priorite = 4, .etat = A_FAIRE, .cible = ZONE_5};
        objectifs_plats[0] = objectif_1;
        objectifs_plats[1] = objectif_2;
        objectifs_plats[2] = objectif_3;
        objectifs_plats[3] = objectif_4;
        objectifs_plats[4] = objectif_5;
    }

    etat_action = Strategie_pieds_dans_plat_trajet(objectif_plat_courant, couleur, step_ms);
    switch(etat_action){
        case ACTION_TERMINEE:
            return ACTION_TERMINEE;
        case ACTION_EN_COURS:
            return ACTION_EN_COURS;
        case ACTION_ECHEC:
            // 1. Marquer comme bloqué l'objectif en cours
            objectif_plat_courant->etat = BLOQUE;
            // 2. Si tous les objectifs sont bloqués, les marquer tous comme A_FAIRE
            uint8_t non_bloque = 0;
            for(uint i=0 ; i<5 ; i++){
                if(objectifs_plats[i].etat == A_FAIRE){
                    non_bloque=1;
                }
            }
            if(!non_bloque){
                for(uint i=0 ; i<5 ; i++){
                    if (objectifs_plats[i].etat == BLOQUE){
                        objectifs_plats[i].etat = A_FAIRE;
                    }
                }
            }
            // 3. Trouver le prochain objectif (priorité la plus basse + etat A_FAIRE)
            // Si notre objectif est FAIT, on prend le premier objectif "A_FAIRE"
            // Si notre objectif est A_FAIRE, on prend le nouvel objectif que si sa priorité est plus basse.
            for(uint i=0; i < 5; i++){
                if(objectif_plat_courant->etat == BLOQUE && objectifs_plats[i].etat == A_FAIRE){
                    objectif_plat_courant = &(objectifs_plats[i]);
                }else if(objectif_plat_courant->etat == A_FAIRE && objectifs_plats[i].etat == A_FAIRE){
                    if(objectif_plat_courant->priorite > objectifs_plats[i].priorite){
                        objectif_plat_courant = &(objectifs_plats[i]);
                    }

                } 
            }
            return ACTION_EN_COURS;
    }



}

enum etat_action_t Strategie_pieds_dans_plat_trajet(struct objectif_t *objectif_plat_courant, enum couleur_t couleur, uint32_t step_ms){
    float pos_x;
    float pos_y;

    switch (objectif_plat_courant->cible){
        case ZONE_1:
            pos_y = 2775;
            if (couleur == COULEUR_BLEU){
                pos_x = 250;
            }else{
                pos_x = 2000 - 250;
            }
            return Strategie_aller_a(pos_x, pos_y, step_ms);

        case ZONE_2:
            pos_y = 1125;
            if (couleur == COULEUR_BLEU){
                pos_x = 250;
            }else{
                pos_x = 2000 - 250;
            }
            return Strategie_aller_a(pos_x, pos_y, step_ms);

        case ZONE_3:
            pos_y = 250;
            if (couleur == COULEUR_BLEU){
                pos_x = 725;
            }else{
                pos_x = 2000 - 725;
            }
            return Strategie_aller_a(pos_x, pos_y, step_ms);

        case ZONE_4:
            pos_y = 250;
            if (couleur == COULEUR_BLEU){
                pos_x = 2000 - 250;
            }else{
                pos_x = 250;
            }
            return Strategie_aller_a(pos_x, pos_y, step_ms);

        case ZONE_5:
            pos_y = 1850;
            if (couleur == COULEUR_BLEU){
                pos_x = 2000 - 250;
            }else{
                pos_x = 250;
            }
            return Strategie_aller_a(pos_x, pos_y, step_ms);
    }
}

enum etat_action_t Strategie_calage_debut(enum couleur_t couleur, uint32_t step_ms){
    // 1 Envoyer la commande pour détecter la bordure
    // 2 Si la valeur de la bordure est valide, lire l'angle et la distance.
    //      Recaler la distance Y
    // 3 Se tourner du bon angle (en fonction de la couleur et de l'angle lu)
    // 4 Si la valeur de la bordure est valide, lire l'angle et la distance.
    // 5 Se positionner à (250, 250), PINCE orientée à 45°.
    static enum{
        CD_ENVOI_CDE_BORDURE,
        CD_LECTURE_BORDURE_Y,
        CD_ROTATION_VERS_X,
        CD_LECTURE_BORDURE_X,
        CD_ALLER_POSITION_INIT,
        CD_ROTATION_POSITION_INIT,
    }etat_calage_debut=CD_ENVOI_CDE_BORDURE;
    enum validite_vl53l8_t validite;
    struct trajectoire_t trajectoire;
    enum etat_action_t etat_action;

    float angle, distance;

    switch(etat_calage_debut){
        case CD_ENVOI_CDE_BORDURE:
            i2c_annexe_set_mode_VL53L8(VL53L8_BORDURE);
            etat_calage_debut = CD_LECTURE_BORDURE_Y;
            break;
        
        case CD_LECTURE_BORDURE_Y:
            
            i2c_annexe_get_VL53L8(&validite, &angle, &distance);
            if(validite == VL53L8_BORDURE){
                i2c_annexe_set_mode_VL53L8(VL53L8_INVALIDE);
                commande_vitesse_stop();
                Localisation_set_y(distance + DISTANCE_CENTRE_CAPTEUR);
                Localisation_set_angle((-90. * DEGRE_EN_RADIAN) - ANGLE_PINCE + angle);
                etat_calage_debut = CD_ROTATION_VERS_X;
            }
            break;

        case CD_ROTATION_VERS_X:
            Trajet_config(TRAJECT_CONFIG_ROTATION_PURE);
            if(couleur == COULEUR_BLEU){
                Trajectoire_rotation(&trajectoire, Localisation_get().x_mm, Localisation_get().y_mm, Localisation_get().angle_radian,
                    (-180 * DEGRE_EN_RADIAN) - ANGLE_PINCE);
            }else{
                Trajectoire_rotation(&trajectoire, Localisation_get().x_mm, Localisation_get().y_mm, Localisation_get().angle_radian,
                    (0 * DEGRE_EN_RADIAN) - ANGLE_PINCE);
            }
            if(Strategie_parcourir_trajet(trajectoire, step_ms, EVITEMENT_SANS_EVITEMENT) == ACTION_TERMINEE){
                i2c_annexe_set_mode_VL53L8(VL53L8_BORDURE);
                etat_calage_debut = CD_LECTURE_BORDURE_X;
            }
            break;

        case CD_LECTURE_BORDURE_X:
            i2c_annexe_get_VL53L8(&validite, &angle, &distance);
            if(validite == VL53L8_BORDURE){
                i2c_annexe_set_mode_VL53L8(VL53L8_INVALIDE);
                commande_vitesse_stop();
                if(couleur == COULEUR_BLEU){
                    Localisation_set_x(distance + DISTANCE_CENTRE_CAPTEUR);
                    Localisation_set_angle((-180. * DEGRE_EN_RADIAN) - ANGLE_PINCE + angle);
                }else{
                    Localisation_set_x(3000 - (distance + DISTANCE_CENTRE_CAPTEUR));
                    Localisation_set_angle((0. * DEGRE_EN_RADIAN) - ANGLE_PINCE + angle);
                }
                
                etat_calage_debut = CD_ALLER_POSITION_INIT;
            }
            break;

        case CD_ALLER_POSITION_INIT:
            Trajet_config(TRAJECT_CONFIG_AVANCE_ET_TOURNE);
            if(couleur == COULEUR_BLEU){
                etat_action = Strategie_tourner_et_aller_a(225, 225, Localisation_get().angle_radian, EVITEMENT_SANS_EVITEMENT, step_ms);
            }else{
                etat_action = Strategie_tourner_et_aller_a(3000 - 225, 225, Localisation_get().angle_radian, EVITEMENT_SANS_EVITEMENT, step_ms);
            }
            if(etat_action == ACTION_TERMINEE){
                etat_calage_debut = CD_ROTATION_POSITION_INIT;
            }
            break;
        case CD_ROTATION_POSITION_INIT:
            Trajet_config(TRAJECT_CONFIG_ROTATION_PURE);
            /*Trajectoire_rotation(&trajectoire, Localisation_get().x_mm, Localisation_get().y_mm, Localisation_get().angle_radian,
                (45. * DEGRE_EN_RADIAN) - ANGLE_PINCE);*/
            Trajectoire_rotation(&trajectoire, Localisation_get().x_mm, Localisation_get().y_mm, Localisation_get().angle_radian,
                0);
            return Strategie_parcourir_trajet(trajectoire, step_ms, EVITEMENT_SANS_EVITEMENT);
            //etat_calage_debut = CD_ENVOI_CDE_BORDURE;
    }

    return ACTION_EN_COURS;
}


enum etat_action_t Strategie_calage_debut_manuel(enum couleur_t couleur, uint32_t step_ms){
    // Le robot est positionné avec une cale à 70 du bord
    // Si l'angle avec la bordure est négatif : bleu, sinon jaune
    // On en déduit X
    static enum{
        CD_ENVOI_CDE_BORDURE,
        CD_LECTURE_BORDURE_Y,
        CD_ROTATION_VERS_X,
        CD_LECTURE_BORDURE_X,
        CD_ALLER_POSITION_INIT,
        CD_ROTATION_POSITION_INIT,
    }etat_calage_debut=CD_ENVOI_CDE_BORDURE;
    enum validite_vl53l8_t validite;
    struct trajectoire_t trajectoire;
    enum etat_action_t etat_action;

    float angle, distance;

    switch(etat_calage_debut){
        case CD_ENVOI_CDE_BORDURE:
            i2c_annexe_set_mode_VL53L8(VL53L8_BORDURE);
            etat_calage_debut = CD_LECTURE_BORDURE_X;
            break;
        
        case CD_LECTURE_BORDURE_X:
            i2c_annexe_get_VL53L8(&validite, &angle, &distance);
            if(validite == VL53L8_BORDURE){
                i2c_annexe_set_mode_VL53L8(VL53L8_INVALIDE);
                commande_vitesse_stop();
                Localisation_set_y(215.);
                if(angle < 0){
                    Localisation_set_x(distance + DISTANCE_CENTRE_CAPTEUR);
                    Localisation_set_angle((-180. * DEGRE_EN_RADIAN) - ANGLE_PINCE + angle);
                    Localisation_set_angle((-60. * DEGRE_EN_RADIAN));
                }else{
                    Localisation_set_x(3000 - (distance + DISTANCE_CENTRE_CAPTEUR));
                    Localisation_set_angle((0. * DEGRE_EN_RADIAN) - ANGLE_PINCE + angle);
                }
                
                etat_calage_debut = CD_ALLER_POSITION_INIT;
                return ACTION_TERMINEE;
            }
            break;
    }

    return ACTION_EN_COURS;
}