#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"

enum etat_action_t active_panneau_solaire(float pos_x, uint32_t step_ms);

/// @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_TOURNE_PANNEAU_1;


    switch (etat_tourne_panneaux)
    {
        case TP_TOURNE_PANNEAU_1:
            if(couleur == COULEUR_BLEU){
                etat_action = Strategie_aller_a(315, 170, EVITEMENT_SANS_EVITEMENT, step_ms);
            }else{
                etat_action = Strategie_aller_a(3000 - 305, 170, EVITEMENT_SANS_EVITEMENT, step_ms);
            }
            if (etat_action == ACTION_TERMINEE){
                etat_tourne_panneaux = TP_TOURNE_PANNEAU_2;
            }
            break;

        case TP_TOURNE_PANNEAU_2:
            if(couleur == COULEUR_BLEU){
                etat_action = active_panneau_solaire(562, step_ms);
            }else{
                etat_action = active_panneau_solaire(3000 - 558, step_ms);
            }
            if (etat_action == ACTION_TERMINEE){
                etat_tourne_panneaux = TP_TOURNE_PANNEAU_3;
            }else if(etat_action == ACTION_ECHEC){
                etat_tourne_panneaux = TP_DEGAGEMENT;
            }
            break;

        case TP_TOURNE_PANNEAU_3:
            if(couleur == COULEUR_BLEU){
                etat_action = active_panneau_solaire(795, step_ms);
            }else{
                etat_action = active_panneau_solaire(3000 - 802, step_ms);
            }
            if (etat_action == ACTION_TERMINEE){
                etat_tourne_panneaux = TP_DEGAGEMENT;
            }else if(etat_action == ACTION_ECHEC){
                etat_tourne_panneaux = TP_DEGAGEMENT;
            }
            break;

        case TP_DEGAGEMENT:
            if(Localisation_get().y_mm < 360){
                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, 360, EVITEMENT_PAUSE_DEVANT_OBSTACLE, step_ms);
            break;
    }
    return ACTION_EN_COURS;
}


enum etat_action_t active_panneau_solaire(float pos_x, uint32_t step_ms){
    struct trajectoire_t trajectoire;
    enum etat_action_t etat_action;
    Trajectoire_bezier(&trajectoire, Localisation_get().x_mm, Localisation_get().y_mm,
         Localisation_get().x_mm, 400, pos_x, 400, pos_x, 140, 
         Localisation_get().angle_radian, 
         Geometrie_get_angle_optimal(Localisation_get().angle_radian, -30 * DEGRE_EN_RADIAN));

    etat_action = Strategie_parcourir_trajet(trajectoire, step_ms, EVITEMENT_ARRET_DEVANT_OBSTACLE);
    return etat_action;
}