#include "Strategie_2024.h"
#include "Localisation.h"
#include "Geometrie_robot.h"
#include "pico/stdlib.h"
#include "Balise_VL53L1X.h"
#include "Temps.h"
#include "Trajectoire.h"
#include "Trajet.h"
#include "i2c_annexe.h"

enum etat_groupe_pot_t{
    GROUPE_POT_DISPO,
    GROUPE_POT_ECHEC
};

enum etat_groupe_plante_t{
    GROUPE_PLANTE_DISPO,
    GROUPE_PLANTE_ECHEC
};

enum etat_groupe_pot_t etat_groupe_pot[6]={
    GROUPE_POT_DISPO,
    GROUPE_POT_DISPO,
    GROUPE_POT_DISPO,
    GROUPE_POT_DISPO,
    GROUPE_POT_DISPO,
    GROUPE_POT_DISPO
};

enum etat_groupe_plante_t etat_groupe_plante[6]={
    GROUPE_PLANTE_DISPO,
    GROUPE_PLANTE_DISPO,
    GROUPE_PLANTE_DISPO,
    GROUPE_PLANTE_DISPO,
    GROUPE_PLANTE_DISPO,
    GROUPE_PLANTE_DISPO
};

struct position_t approche_groupe_plante_bleu[6]={
    {.x_mm= 0, .y_mm=0, .angle_radian = 0},
    {.x_mm= 580, .y_mm=1530, .angle_radian = 0},
    {.x_mm= 580, .y_mm=530, .angle_radian = 0},
    {.x_mm= 1220, .y_mm=360, .angle_radian = 0}, 
    {.x_mm= 3000 - 580, .y_mm=530, .angle_radian = 0},
    {.x_mm= 3000 - 580, .y_mm=1530, .angle_radian = 0},
};

struct position_t approche_groupe_plante_jaune[6]={
    {.x_mm= 0, .y_mm=0, .angle_radian = 0},
    {.x_mm= 580, .y_mm=1530, .angle_radian = 0},
    {.x_mm= 580, .y_mm=530, .angle_radian = 0},
    {.x_mm= 3000 - 1220, .y_mm=360, .angle_radian = 0}, 
    {.x_mm= 3000 - 580, .y_mm=530, .angle_radian = 0},
    {.x_mm= 3000 - 580, .y_mm=1530, .angle_radian = 0},
};


int ordre_groupe_pot[6];

unsigned int get_groupe_pot(enum couleur_t couleur){
    if(couleur == COULEUR_BLEU){
        return GROUPE_POT_L1;
    }
    return GROUPE_POT_R1;
}
enum zone_plante_t get_zone_plante(enum couleur_t couleur){
    enum zone_plante_t ordre_groupe_plante_bleu[6] = { ZONE_PLANTE_2, ZONE_PLANTE_3, ZONE_PLANTE_4, ZONE_PLANTE_AUCUNE, ZONE_PLANTE_AUCUNE, ZONE_PLANTE_AUCUNE};
    enum zone_plante_t ordre_groupe_plante_jaune[6] = { ZONE_PLANTE_6, ZONE_PLANTE_5, ZONE_PLANTE_4, ZONE_PLANTE_AUCUNE, ZONE_PLANTE_AUCUNE, ZONE_PLANTE_AUCUNE};
    enum zone_plante_t *ordre_groupe_plante;
    int i;
    if(couleur == COULEUR_BLEU){
        ordre_groupe_plante = ordre_groupe_plante_bleu;
    }else{
        ordre_groupe_plante = ordre_groupe_plante_jaune;
    }
    for(i=0; i<6; i++){
        if(etat_groupe_plante[ordre_groupe_plante[i]] == GROUPE_PLANTE_DISPO){
            return ordre_groupe_plante[i];
        }
    }
    return ZONE_PLANTE_AUCUNE;
}

struct position_t get_position_approche_zone_plante(enum couleur_t couleur, enum zone_plante_t zone_plante){
    if(couleur == COULEUR_BLEU){
        return approche_groupe_plante_bleu[zone_plante];
    }
    return approche_groupe_plante_jaune[zone_plante];
}

void Strategie_2024(enum couleur_t couleur, uint32_t step_ms, uint32_t temps_ms){
    int lettre, _step_ms = 1, _step_ms_gyro=2,temps_ms_init;
    float angle_destination, pos_x;
    struct trajectoire_t trajectoire;
    struct position_t position;
    enum evitement_t evitement;
    enum etat_action_t etat_action=ACTION_EN_COURS;
    static int tempo_ms;
    static int nb_plante_ok = 0;
    static int bras_depose = PLANTE_BRAS_1;
    static int depose_en_cours = 0;
    static int pre_fin_match=0;

    static enum {
        TAP_CALAGE,
        TAP_PASSAGE_JAUNE,
        TAP_POT,
        TAP_PANNEAUX_SOLAIRES,
        TAP_ALLER_PLANTE,
        TAP_PLANTE_ORIENTATION,
        TAP_PLANTE_ATTRAPE,
        TAP_ECHANGE_POT,
        TAP_PLANTE_TERMINEE_ECHEC,
        TAP_RENTRE,
        TAP_RENTRE_RECALE,
        TAP_DEPOSE_0,
        TAP_DEPOSE_1,
        TAP_PANNEAU_SOLAIRE_1,
        TAP_DEPOSE_2,
        TAP_DEPOSE_3,
        TAP_RECHARGE,
        TAP_FINI
    } etat_test = TAP_CALAGE;
    
    if(temps_ms > 80000 && depose_en_cours == 0){
        etat_test = TAP_RENTRE;
        pre_fin_match = 1;
    }


    switch(etat_test){
        case TAP_CALAGE:
            if(Strategie_calage_debut_manuel(couleur, _step_ms) == ACTION_TERMINEE){
                if(couleur == COULEUR_BLEU){
                    etat_test=TAP_POT;
                }else{
                    etat_test=TAP_PASSAGE_JAUNE;
                }    
            }
            break;
        
        case TAP_PASSAGE_JAUNE:
            if(Strategie_aller_a(2600, 1600, EVITEMENT_PAUSE_DEVANT_OBSTACLE, _step_ms) == ACTION_TERMINEE){
                etat_test=TAP_POT;
            }
            break;

        case TAP_POT:
            if(Strat_2024_attrape_pot(get_groupe_pot(couleur), _step_ms) == ACTION_TERMINEE){
                etat_test=TAP_ALLER_PLANTE;
            }
            break;

        case TAP_ALLER_PLANTE:
            position = get_position_approche_zone_plante(couleur, get_zone_plante(couleur));
            Trajet_config(TRAJECT_CONFIG_AVANCE_DROIT);
            if(Strategie_aller_a(position.x_mm, position.y_mm, EVITEMENT_PAUSE_DEVANT_OBSTACLE, step_ms) == ACTION_TERMINEE){
                etat_test=TAP_PLANTE_ORIENTATION;
            }
            break;

        case TAP_PLANTE_ORIENTATION:
            if(Strat_2024_aller_zone_plante(get_zone_plante(couleur), _step_ms) == ACTION_TERMINEE){
                etat_test=TAP_PLANTE_ATTRAPE;
            }
            break;

        case TAP_PLANTE_ATTRAPE:
            etat_action = Strat_2024_plante_dans_pot(_step_ms, bras_depose, get_zone_plante(couleur));
            if( etat_action == ACTION_TERMINEE){
                etat_test=TAP_PLANTE_ATTRAPE;
                nb_plante_ok++;
                if(bras_depose == PLANTE_BRAS_1){
                    bras_depose = PLANTE_BRAS_6;
                }else{
                    bras_depose = PLANTE_BRAS_1;
                }
                if(nb_plante_ok == 2){
                    etat_test = TAP_ECHANGE_POT;
                }
                if(nb_plante_ok == 4){
                    etat_test = TAP_RENTRE;
                }
                etat_action = ACTION_EN_COURS;
            }else if( etat_action == ACTION_ECHEC){
                etat_test=TAP_PLANTE_TERMINEE_ECHEC;
                etat_action = ACTION_EN_COURS;
            }
            break;

        case TAP_ECHANGE_POT:
            if(Strat_2024_echange_pot_avant_arriere(_step_ms) == ACTION_TERMINEE){
                etat_test=TAP_PLANTE_ATTRAPE;
            }
            break;

        case TAP_PLANTE_TERMINEE_ECHEC:
            // On note que la zone plante actuelle est "occupée/epuisée"
            etat_groupe_plante[get_zone_plante(couleur)] = GROUPE_PLANTE_ECHEC;
            // On va à la zone suivante ou on rentre
            if(get_zone_plante(couleur) == ZONE_PLANTE_AUCUNE){
                etat_test=TAP_RENTRE;
            }else{
                etat_test=TAP_ALLER_PLANTE;
            }
            break;

        case TAP_RENTRE:
            depose_en_cours = 1;
            if(couleur == COULEUR_BLEU){
                angle_destination = 60 * DEGRE_EN_RADIAN;
                pos_x = 450;
            }else{
                angle_destination = 60 * DEGRE_EN_RADIAN;
                pos_x = 3000-450;
            }
            
            Trajet_config(TRAJECT_CONFIG_AVANCE_ET_TOURNE);
            if(Strategie_tourner_et_aller_a(pos_x, 300, angle_destination, EVITEMENT_PAUSE_DEVANT_OBSTACLE, _step_ms) == ACTION_TERMINEE){
                etat_test=TAP_RENTRE_RECALE;
            }
            break;

        case TAP_RENTRE_RECALE:
            etat_action = Strategie_calage_bas(couleur, step_ms);
            if(etat_action == ACTION_TERMINEE || etat_action == ACTION_ECHEC){
                etat_test=TAP_DEPOSE_0;
            }
            break;

        case TAP_DEPOSE_0:
            angle_destination = -30 * DEGRE_EN_RADIAN;
            if(couleur == COULEUR_BLEU){
                pos_x = 300;
                
            }else{
                pos_x = 3000-300;
            }
            
            Trajet_config(TRAJECT_CONFIG_AVANCE_ET_TOURNE);
            if(Strategie_aller_a_puis_tourner(pos_x, 280, angle_destination, EVITEMENT_PAUSE_DEVANT_OBSTACLE, _step_ms) == ACTION_TERMINEE){
                i2c_annexe_actionneur_pot(POT_5, BRAS_LEVITE, DOIGT_TIENT);
                etat_test=TAP_DEPOSE_1;
            }
            break;
        
        case TAP_DEPOSE_1:
            commande_vitesse_stop();
            if(couleur == COULEUR_BLEU){
                if(Strat_2024_depose_pot(MASQUE_POT_1 | MASQUE_POT_2 | MASQUE_POT_6, _step_ms)== ACTION_TERMINEE){
                    etat_test=TAP_PANNEAUX_SOLAIRES;
                    Trajet_config(TRAJECT_CONFIG_AVANCE_ET_TOURNE);
                }
            }else{
                if(Strat_2024_depose_pot(MASQUE_POT_3 | MASQUE_POT_2 | MASQUE_POT_4, _step_ms)== ACTION_TERMINEE){
                    etat_test=TAP_PANNEAUX_SOLAIRES;
                    Trajet_config(TRAJECT_CONFIG_AVANCE_ET_TOURNE);
                }
            }
            break;

        case TAP_PANNEAUX_SOLAIRES:
            if(Strat_2024_tourner_panneaux(couleur, step_ms) == ACTION_TERMINEE){
                etat_test=TAP_DEPOSE_2;
            }
            break;

        case TAP_DEPOSE_2:
            angle_destination = 150 * DEGRE_EN_RADIAN;
            if(couleur == COULEUR_BLEU){
                pos_x = 550;
            }else{
                pos_x = 3000-550;
            }
            Trajet_config(TRAJECT_CONFIG_AVANCE_ET_TOURNE);
            if(Strategie_aller_a_puis_tourner(pos_x, 280, angle_destination, EVITEMENT_PAUSE_DEVANT_OBSTACLE, _step_ms) == ACTION_TERMINEE){
                etat_test=TAP_DEPOSE_3;
            }
            break;

        case TAP_DEPOSE_3:
            commande_vitesse_stop();
            if(couleur == COULEUR_BLEU){
                if(Strat_2024_depose_pot(MASQUE_POT_3 | MASQUE_POT_4, _step_ms)== ACTION_TERMINEE){
                    etat_test=TAP_RECHARGE;
                }
            }else{
                if(Strat_2024_depose_pot(MASQUE_POT_1 | MASQUE_POT_6, _step_ms)== ACTION_TERMINEE){
                    etat_test=TAP_RECHARGE;
                }
            }
            break;
        
        case TAP_RECHARGE:
            if(rentre_recharge(couleur, step_ms) == ACTION_TERMINEE){
                etat_test=TAP_FINI;
            }
            break;

        case TAP_FINI:
            commande_vitesse_stop();
            break;


    }
}


enum etat_action_t rentre_recharge(enum couleur_t couleur, int step_ms){
    struct trajectoire_t trajectoire;
    enum etat_action_t etat_action;
    enum validite_vl53l8_t validite;
    float angle, distance, pos_x;

    static int tempo_ms;

    static enum {
        RR_ORIENTE,
        RR_RECALE,
        RR_DEPLACE,
    } etat_rentre_charge = RR_DEPLACE;

    Trajet_config(TRAJECT_CONFIG_AVANCE_ET_TOURNE);

    switch (etat_rentre_charge){
        case RR_ORIENTE:
            if(Strategie_tourner_a(60 * DEGRE_EN_RADIAN, step_ms) == ACTION_TERMINEE){
                i2c_annexe_set_mode_VL53L8(VL53L8_BORDURE);
                commande_vitesse_stop();
                tempo_ms = 2000;
                etat_rentre_charge = RR_RECALE;
            }
            break;

        case RR_RECALE:
            tempo_ms--;
            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);
                etat_rentre_charge = RR_DEPLACE;
            }
            if(tempo_ms <= 0){
                etat_rentre_charge =RR_DEPLACE;
            }
            break;

        case RR_DEPLACE:
            if(couleur == COULEUR_BLEU){
                pos_x = Localisation_get().x_mm + 30;
            }else{
                pos_x = Localisation_get().x_mm - 30;
            }
            if( Strategie_aller_a(pos_x , 150, EVITEMENT_SANS_EVITEMENT, step_ms) == ACTION_TERMINEE){
                etat_rentre_charge =RR_ORIENTE;
                return ACTION_TERMINEE;
            }
            break;
    }
    return ACTION_EN_COURS;
}