#include "math.h"
#include "Strategie.h"
#include "Geometrie_robot.h"
#include "Commande_vitesse.h"
#include "Strategie_2024_pots.h"
#include "i2c_annexe.h"
#include "Localisation.h"
#include "Score.h"

/// @brief Fonction demande au robot d'aller activer les panneaux solaires
enum etat_action_t Strat_2024_tourner_panneaux(enum couleur_t couleur, uint32_t step_ms){
    enum etat_action_t etat_action;
    struct trajectoire_t trajectoire;
    static int tempo_ms;

    static enum {
        TP_APPROCHE_PANNEAU_1, // Position et orientation pour attaquer le panneau 1, bras 2 qui pousse
        TP_TOURNE_PANNEAU_1,
        TP_TOURNE_PANNEAU_2,
        TP_TOURNE_PANNEAU_3,
        TP_TOURNE_PANNEAU_4,
        TP_TOURNE_PANNEAU_5,
        TP_TOURNE_PANNEAU_6,
        TP_DEGAGEMENT,
        TP_DEGAGEMENT_ACTIF
    } etat_tourne_panneaux = TP_APPROCHE_PANNEAU_1;


    switch (etat_tourne_panneaux)
    {
        case TP_APPROCHE_PANNEAU_1:
            Trajet_config(TRAJECT_CONFIG_AVANCE_ET_TOURNE);
            if(couleur == COULEUR_BLEU){
                etat_action = Strategie_tourner_et_aller_a(
                    323, 300, (150. *DEGRE_EN_RADIAN),
                    EVITEMENT_ARRET_DEVANT_OBSTACLE, step_ms);
            }else{
                etat_action = Strategie_tourner_et_aller_a(
                    3000 - 323, 300, (150. *DEGRE_EN_RADIAN),
                    EVITEMENT_ARRET_DEVANT_OBSTACLE, step_ms);
            }
            if (etat_action == ACTION_TERMINEE){
                etat_tourne_panneaux = TP_TOURNE_PANNEAU_1;
            }else if(etat_action == ACTION_ECHEC){
                return ACTION_TERMINEE;
            }
            break;

        case TP_TOURNE_PANNEAU_1:
            if(couleur == COULEUR_BLEU){
                etat_action = Strategie_tourner_et_aller_a(
                    323, 200, (150. *DEGRE_EN_RADIAN), EVITEMENT_PAUSE_DEVANT_OBSTACLE, step_ms);
            }else{
                etat_action = Strategie_tourner_et_aller_a(
                    3000 - 323, 300, (150. *DEGRE_EN_RADIAN), EVITEMENT_PAUSE_DEVANT_OBSTACLE, step_ms);
            }
            if (etat_action == ACTION_TERMINEE){
                Score_ajout_panneau(1);
                etat_tourne_panneaux = TP_TOURNE_PANNEAU_2;
            }
            break;

        case TP_TOURNE_PANNEAU_2:
            if(couleur == COULEUR_BLEU){
                Trajectoire_bezier(&trajectoire, 323, 200, 405, 310, 548, 400, 548, 200, (150. *DEGRE_EN_RADIAN), (150. *DEGRE_EN_RADIAN));
            }else{
                Trajectoire_bezier(&trajectoire, 3000-323, 200, 3000-405, 310, 3000-548, 400, 3000-548, 200, (150. *DEGRE_EN_RADIAN), (150. *DEGRE_EN_RADIAN));
            }
            etat_action =Strategie_parcourir_trajet(trajectoire, step_ms, EVITEMENT_ARRET_DEVANT_OBSTACLE);
            if (etat_action == ACTION_TERMINEE){
                Score_ajout_panneau(1);
                etat_tourne_panneaux = TP_DEGAGEMENT;
            }else if(etat_action == ACTION_ECHEC){
                etat_tourne_panneaux = TP_DEGAGEMENT;
            }
            break;

        case TP_TOURNE_PANNEAU_3:
            if(couleur == COULEUR_BLEU){
                Trajectoire_bezier(&trajectoire, 548, 200, 630, 310, 773, 400, 773, 200, (150. *DEGRE_EN_RADIAN), (150. *DEGRE_EN_RADIAN));
            }else{
                Trajectoire_bezier(&trajectoire, 3000-548, 200, 3000-630, 310, 3000-773, 400, 3000-773, 200, (150. *DEGRE_EN_RADIAN), (150. *DEGRE_EN_RADIAN));
            }
            etat_action =Strategie_parcourir_trajet(trajectoire, step_ms, EVITEMENT_ARRET_DEVANT_OBSTACLE);
            if (etat_action == ACTION_TERMINEE){
                Score_ajout_panneau(1);
                etat_tourne_panneaux = TP_TOURNE_PANNEAU_4;
            }else if(etat_action == ACTION_ECHEC){
                etat_tourne_panneaux = TP_DEGAGEMENT;
            }
            break;

        case TP_TOURNE_PANNEAU_4:
            if(couleur == COULEUR_BLEU){
                Trajectoire_bezier(&trajectoire, 773, 200, 855, 310, 1323, 400, 1323, 200, (150. *DEGRE_EN_RADIAN), (150. *DEGRE_EN_RADIAN));
            }else{
                Trajectoire_bezier(&trajectoire, 3000-773, 200, 3000-855, 310, 3000-1323, 400, 3000-1323, 200, (150. *DEGRE_EN_RADIAN), (150. *DEGRE_EN_RADIAN));
            }
            etat_action =Strategie_parcourir_trajet(trajectoire, step_ms, EVITEMENT_ARRET_DEVANT_OBSTACLE);
            if (etat_action == ACTION_TERMINEE){
                Score_ajout_panneau(1);
                etat_tourne_panneaux = TP_TOURNE_PANNEAU_5;
            }else if(etat_action == ACTION_ECHEC){
                etat_tourne_panneaux = TP_DEGAGEMENT;
            }
            break;

        case TP_TOURNE_PANNEAU_5:
            if(couleur == COULEUR_BLEU){
                Trajectoire_bezier(&trajectoire, 1323, 200, 1410, 310, 1547, 400, 1547, 200, (150. *DEGRE_EN_RADIAN), (150. *DEGRE_EN_RADIAN));
            }else{
                Trajectoire_bezier(&trajectoire, 3000-1323, 200, 3000-1410, 310, 3000-1547, 400, 3000-1547, 200, (150. *DEGRE_EN_RADIAN), (150. *DEGRE_EN_RADIAN));
            }
            etat_action =Strategie_parcourir_trajet(trajectoire, step_ms, EVITEMENT_ARRET_DEVANT_OBSTACLE);
            if (etat_action == ACTION_TERMINEE){
                Score_ajout_panneau(1);
                etat_tourne_panneaux = TP_TOURNE_PANNEAU_6;
            }else if(etat_action == ACTION_ECHEC){
                etat_tourne_panneaux = TP_DEGAGEMENT;
            }
            break;
        
        case TP_TOURNE_PANNEAU_6:
            if(couleur == COULEUR_BLEU){
                Trajectoire_bezier(&trajectoire, 1547, 200, 1630, 310, 1771, 400, 1771, 200, (150. *DEGRE_EN_RADIAN), (150. *DEGRE_EN_RADIAN));
            }else{
                Trajectoire_bezier(&trajectoire, 3000-1547, 200, 3000-1630, 310, 3000-1771, 400, 3000-1771, 200, (150. *DEGRE_EN_RADIAN), (150. *DEGRE_EN_RADIAN));
            }
            etat_action =Strategie_parcourir_trajet(trajectoire, step_ms, EVITEMENT_ARRET_DEVANT_OBSTACLE);
            if (etat_action == ACTION_TERMINEE){
                Score_ajout_panneau(1);
                etat_tourne_panneaux = TP_TOURNE_PANNEAU_6;
            }else if(etat_action == ACTION_ECHEC){
                etat_tourne_panneaux = TP_DEGAGEMENT;
            }
            break;

        case TP_DEGAGEMENT:
            if(Localisation_get().y_mm < 250){
                etat_tourne_panneaux = TP_DEGAGEMENT_ACTIF;
            }else{
                commande_vitesse_stop();
                return ACTION_TERMINEE;
            }
            break;

        case TP_DEGAGEMENT_ACTIF:
                return Strategie_aller_a(Localisation_get().x_mm, 250, EVITEMENT_PAUSE_DEVANT_OBSTACLE, step_ms);
            break;
    }
    return ACTION_EN_COURS;
}