#include "Strategie_2024.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
};

int ordre_groupe_pot[6];

unsigned int get_groupe_pot(enum couleur_t couleur){
    if(couleur == COULEUR_BLEU){
        return GROUPE_POT_B1;
    }
    return GROUPE_POT_B2;
}
enum zone_plante_t get_zone_plante(enum couleur_t couleur){
    enum zone_plante_t ordre_groupe_plante_bleu[6] = { ZONE_PLANTE_3, ZONE_PLANTE_2, ZONE_PLANTE_4, ZONE_PLANTE_5, ZONE_PLANTE_6, ZONE_PLANTE_1};
    enum zone_plante_t ordre_groupe_plante_jaune[6] = { ZONE_PLANTE_5, ZONE_PLANTE_6, ZONE_PLANTE_4, ZONE_PLANTE_3, ZONE_PLANTE_2, ZONE_PLANTE_1};
    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;
}

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;
    enum evitement_t evitement;
    enum etat_action_t etat_action=ACTION_EN_COURS;
    static int tempo_ms;

    static enum {
        TAP_CALAGE,
        TAP_POT,
        TAP_PANNEAUX_SOLAIRES,
        TAP_ALLER_PLANTE,
        TAP_PLANTE_ORIENTATION,
        TAP_PLANTE_ATTRAPE_1,
        TAP_PLANTE_ATTRAPE_2,
        TAP_ECHANGE_POT,
        TAP_PLANTE_ATTRAPE_3,
        TAP_PLANTE_ATTRAPE_4,
        TAP_RENTRE,
        TAP_RENTRE_RECALE,
        TAP_DEPOSE_0,
        TAP_DEPOSE_1,
        TAP_DEPOSE_2,
        TAP_DEPOSE_3,
        TAP_RECHARGE,
        TAP_FINI
    } etat_test = TAP_CALAGE, next_etat_test;


    switch(etat_test){
        case TAP_CALAGE:
            if(Strategie_calage_debut_manuel(couleur, _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_PANNEAUX_SOLAIRES;
            }
            break;

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

        case TAP_ALLER_PLANTE:
            if(couleur == COULEUR_BLEU){
                pos_x = 940;
            }else{
                pos_x = 3000 - 940;
            }
            if(Strategie_aller_a(pos_x, 300, 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_1;
            }
            break;

        case TAP_PLANTE_ATTRAPE_1:
            etat_action = Strat_2024_plante_dans_pot(_step_ms, PLANTE_BRAS_1, get_zone_plante(couleur));
            if( etat_action == ACTION_TERMINEE){
                etat_test=TAP_PLANTE_ATTRAPE_2;
                etat_action = ACTION_EN_COURS;
            }else if( etat_action == ACTION_ECHEC){
                etat_test=TAP_RENTRE;
                etat_action = ACTION_EN_COURS;
            }
            break;
        
        case TAP_PLANTE_ATTRAPE_2:
            etat_action = Strat_2024_plante_dans_pot(_step_ms, PLANTE_BRAS_6, get_zone_plante(couleur));
            if( etat_action == ACTION_TERMINEE){
                etat_test=TAP_ECHANGE_POT;
                etat_action = ACTION_EN_COURS;
            }else if( etat_action == ACTION_ECHEC){
                etat_test=TAP_RENTRE;
                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_3;
            }
            break;
        
        case TAP_PLANTE_ATTRAPE_3:
            etat_action = Strat_2024_plante_dans_pot(_step_ms, PLANTE_BRAS_1, get_zone_plante(couleur));
            if( etat_action == ACTION_TERMINEE){
                etat_test=TAP_PLANTE_ATTRAPE_4;
                etat_action = ACTION_EN_COURS;
            }else if( etat_action == ACTION_ECHEC){
                etat_test=TAP_RENTRE;
                etat_action = ACTION_EN_COURS;
            }
            break;

        case TAP_PLANTE_ATTRAPE_4:
            etat_action = Strat_2024_plante_dans_pot(_step_ms, PLANTE_BRAS_1, get_zone_plante(couleur));
            if( etat_action == ACTION_TERMINEE){
                etat_test=TAP_RENTRE;
                etat_action = ACTION_EN_COURS;
            }else if( etat_action == ACTION_ECHEC){
                etat_test=TAP_RENTRE;
                etat_action = ACTION_EN_COURS;
            }
            break;

        case TAP_RENTRE:
            if(couleur == COULEUR_BLEU){
                angle_destination = 60 * DEGRE_EN_RADIAN;
                pos_x = 350;
            }else{
                angle_destination = 60 * DEGRE_EN_RADIAN;
                pos_x = 3000-350;
            }
            
            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_debut(couleur, step_ms);
            if(etat_action == ACTION_TERMINEE || etat_action == ACTION_ECHEC){
                etat_test=TAP_DEPOSE_0;
            }
            break;

        case TAP_DEPOSE_0:
            if(couleur == COULEUR_BLEU){
                pos_x = 300;
                angle_destination = 15 * DEGRE_EN_RADIAN;
            }else{
                pos_x = 3000-300;
                angle_destination = (90-15) * DEGRE_EN_RADIAN;
            }
            
            Trajet_config(TRAJECT_CONFIG_AVANCE_ET_TOURNE);
            if(Strategie_aller_a_puis_tourner(pos_x, 300, angle_destination, EVITEMENT_PAUSE_DEVANT_OBSTACLE, _step_ms) == ACTION_TERMINEE){
                etat_test=TAP_DEPOSE_1;
            }
            break;
        
        case TAP_DEPOSE_1:
            commande_vitesse_stop();
            if(Strat_2024_depose_pot(MASQUE_POT_1 | MASQUE_POT_6, _step_ms)== ACTION_TERMINEE){
                etat_test=TAP_DEPOSE_2;
            }
            break;

        case TAP_DEPOSE_2:
            if(couleur == COULEUR_BLEU){
                angle_destination = (180+15) * DEGRE_EN_RADIAN;
                pos_x = 450;
            }else{
                angle_destination = (90-(180+15)) * DEGRE_EN_RADIAN;
                pos_x = 3000-450;
            }
            Trajet_config(TRAJECT_CONFIG_AVANCE_ET_TOURNE);
            if(Strategie_aller_a_puis_tourner(pos_x, 450, angle_destination, EVITEMENT_PAUSE_DEVANT_OBSTACLE, _step_ms) == ACTION_TERMINEE){

                etat_test=TAP_DEPOSE_3;
            }
            break;

        case TAP_DEPOSE_3:
            if(Strat_2024_depose_pot(MASQUE_POT_3 | MASQUE_POT_4, _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;

    static enum {
        ZONE_HAUTE,
        ZONE_OPPOSEE,
    } etat_rentre_charge = ZONE_OPPOSEE;

    Trajet_config(TRAJECT_CONFIG_AVANCE_ET_TOURNE);

    switch (etat_rentre_charge){
        case ZONE_OPPOSEE:
            if(couleur == COULEUR_BLEU){
                etat_action = Strategie_aller_a(2700, 1000, EVITEMENT_ARRET_DEVANT_OBSTACLE, step_ms);
            }else{
                etat_action = Strategie_aller_a(300, 1000, EVITEMENT_ARRET_DEVANT_OBSTACLE, step_ms);
            }
            if(etat_action == ACTION_ECHEC){
                etat_rentre_charge = ZONE_HAUTE;
            }else if(etat_action == ACTION_TERMINEE){
                return ACTION_TERMINEE;
            }
            break;

        case ZONE_HAUTE:
            if(couleur == COULEUR_BLEU){
                etat_action = Strategie_aller_a(300, 1700, EVITEMENT_ARRET_DEVANT_OBSTACLE, step_ms);
            }else{
                etat_action = Strategie_aller_a(2700, 1700, EVITEMENT_ARRET_DEVANT_OBSTACLE, step_ms);
            }
            if(etat_action == ACTION_ECHEC){
                etat_rentre_charge = ZONE_OPPOSEE;
            }else if(etat_action == ACTION_TERMINEE){
                return ACTION_TERMINEE;
            }
            break;
    }
    return ACTION_EN_COURS;
}