#include "i2c_annexe.h"
#include "Localisation.h"
#include "Monitoring.h"
#include "Strategie.h"
#include "Trajectoire.h"
#include "Trajet.h"
#include "Geometrie.h"

enum etat_action_t Gateau_pousse_proche(enum couleur_t couleur, uint32_t step_ms){
    static enum {
        ALLER_1,
        ALLER_2,
        ALLER_3
    } etat=ALLER_1;
    float point_x, point_y;
    float angle_fin;
    struct trajectoire_t trajectoire;
    
    
    switch(etat){
        case ALLER_1:
            if(couleur == COULEUR_BLEU){
                angle_fin = 90. * DEGRE_EN_RADIAN;
                point_x = 310;
            }else{
                angle_fin = 205. * DEGRE_EN_RADIAN;
                point_x = 1700;
            }
            angle_fin = Geometrie_get_angle_optimal(Localisation_get().angle_radian, angle_fin);

            Trajet_config(100, 250);
            Trajectoire_droite(&trajectoire, Localisation_get().x_mm, Localisation_get().y_mm, point_x, 3000 - 380,
                        Localisation_get().angle_radian, angle_fin);

            if(parcourt_trajet_simple(trajectoire, step_ms) == ACTION_TERMINEE){
                etat=ALLER_2;
                i2c_annexe_deplie_bras();
            }
            break;
        
        case ALLER_2:
            if(couleur == COULEUR_BLEU){
                angle_fin = 90. * DEGRE_EN_RADIAN;
                point_x = 310;
            }else{
                angle_fin = 205. * DEGRE_EN_RADIAN;
                point_x = 1700;
            }
            angle_fin = Geometrie_get_angle_optimal(Localisation_get().angle_radian, angle_fin);

            Trajet_config(250, 250);
            Trajectoire_droite(&trajectoire, Localisation_get().x_mm, Localisation_get().y_mm, point_x, 3000 - 1700,
                        Localisation_get().angle_radian, angle_fin);

            if(parcourt_trajet_simple(trajectoire, step_ms) == ACTION_TERMINEE){
                i2c_annexe_plie_bras();
                etat=ALLER_3;
            }
            break;
        
        case ALLER_3:
            if(couleur == COULEUR_BLEU){
                angle_fin = -150. * DEGRE_EN_RADIAN;
                point_x = 225;
            }else{
                angle_fin = 30. * DEGRE_EN_RADIAN;
                point_x = 1775;
            }
            angle_fin = Geometrie_get_angle_optimal(Localisation_get().angle_radian, angle_fin);

            Trajet_config(250, 250);
            Trajectoire_droite(&trajectoire, Localisation_get().x_mm, Localisation_get().y_mm, point_x, 3000 - 1600,
                        Localisation_get().angle_radian, angle_fin);

            if(parcourt_trajet_simple(trajectoire, step_ms) == ACTION_TERMINEE){
                etat=ALLER_1;
                return ACTION_TERMINEE;
            }
            break;

    }
    return ACTION_EN_COURS;

}