#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;
    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_PLANTE_ORIENTATION,
        TAP_PLANTE_ATTRAPE_1,
        TAP_PLANTE_ATTRAPE_2,
        TAP_ECHANGE_POT,
        TAP_PLANTE_ATTRAPE_3,
        TAP_PLANTE_ATTRAPE_4,
        TAP_RENTRE,
        TAP_DEPOSE_1,
        TAP_DEPOSE_2,
        TAP_DEPOSE_3,
        TAP_FINI
    } etat_test = TAP_CALAGE;


    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_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 = 15 * DEGRE_EN_RADIAN;
            }else{
                angle_destination = (90-15) * DEGRE_EN_RADIAN;
            }
            
            Trajet_config(TRAJECT_CONFIG_AVANCE_ET_TOURNE);
            if(Strategie_tourner_et_aller_a(300, 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;
            }else{
                angle_destination = (90-(180+15)) * DEGRE_EN_RADIAN;
            }
            Trajet_config(TRAJECT_CONFIG_AVANCE_ET_TOURNE);
            if(Strategie_tourner_et_aller_a(450, 450, angle_destination, EVITEMENT_PAUSE_DEVANT_OBSTACLE, _step_ms) == ACTION_TERMINEE){
                etat_test=TAP_DEPOSE_3;
            }
            break;

        case TAP_DEPOSE_3:
            commande_vitesse_stop();
            if(Strat_2024_depose_pot(MASQUE_POT_3 | MASQUE_POT_4, _step_ms)== ACTION_TERMINEE){
                etat_test=TAP_FINI;
            }
            break;

        case TAP_FINI:
            break;


    }
}