#include "i2c_annexe.h"
#include "Localisation.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 {
        GATEAU_PROCHE_PRE_POUSSE,
        GATEAU_PROCHE_POUSSE,
        GATEAU_PROCHE_RELACHE,
    } etat_pousse_proche;

    struct trajectoire_t trajectoire;

    float angle_fin, point_x, point_y;

    switch(etat_pousse_proche){
        case GATEAU_PROCHE_PRE_POUSSE:
            if(couleur == COULEUR_BLEU){
                angle_fin = 90. * DEGRE_EN_RADIAN;
                point_x = 310;
            }else{
                angle_fin = -135. * DEGRE_EN_RADIAN;
                point_x = 1655;
            }
            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 - 360,
                     Localisation_get().angle_radian, angle_fin);

            if(parcourt_trajet_simple(trajectoire, step_ms) == TRAJET_TERMINE){
                etat_pousse_proche = GATEAU_PROCHE_POUSSE;
                i2c_annexe_deplie_bras();
            }
            break;

        case GATEAU_PROCHE_POUSSE:
            if(couleur == COULEUR_BLEU){
                angle_fin = 90. * DEGRE_EN_RADIAN;
                point_x = 390;
            }else{
                angle_fin = -135. * DEGRE_EN_RADIAN;
                point_x = 1585;
            }
            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 - 1665,
                     Localisation_get().angle_radian, angle_fin);

            if(parcourt_trajet_simple(trajectoire, step_ms) == TRAJET_TERMINE){
                etat_pousse_proche = GATEAU_PROCHE_POUSSE;
                i2c_annexe_plie_bras();
            }
            break;

        case GATEAU_PROCHE_RELACHE:
    }

}