#include "Localisation.h"
#include "Strategie_prise_cerises.h"
#include "Strategie.h"
#include "Trajet.h"
#include "math.h"

#define DEGREE_EN_RADIAN  (M_PI / 180.)

void Homologation(uint32_t step_ms){
    static enum etat_strategie_t{
        STRATEGIE_INIT,
        APPROCHE_CERISE_1_A,
        APPROCHE_CERISE_1_B,
        ATTRAPE_CERISE_1,
        APPROCHE_PANIER_1_A,
        APPROCHE_PANIER_1_B,
        CALAGE_PANIER_1,
        STRATEGIE_FIN
    }etat_strategie=STRATEGIE_INIT;

    enum etat_action_t etat_action;
    enum etat_trajet_t etat_trajet;

    struct trajectoire_t trajectoire;

    switch(etat_strategie){
        case STRATEGIE_INIT:
            Localisation_set(775., 109., -60. * DEGREE_EN_RADIAN);
            etat_strategie = APPROCHE_CERISE_1_A;
            break;

        case APPROCHE_CERISE_1_A:
            Trajet_config(250, 500);
            Trajectoire_droite(&trajectoire,775, 109, 857, 156, -60. * DEGREE_EN_RADIAN, +30. * DEGREE_EN_RADIAN);
            Trajet_debut_trajectoire(trajectoire);
            etat_strategie = APPROCHE_CERISE_1_B;
            break;

        case APPROCHE_CERISE_1_B:
            etat_trajet = Trajet_avance(step_ms/1000.);
            if(etat_trajet == TRAJET_TERMINE){
                etat_strategie = ATTRAPE_CERISE_1;
            }
            break;
        
        case ATTRAPE_CERISE_1:
            etat_action = cerise_attraper_bordure(LONGER_VERS_C, step_ms);
            if(etat_action == ACTION_TERMINEE){
                etat_strategie = APPROCHE_PANIER_1_A;
            }
            break;

        case APPROCHE_PANIER_1_A:
            Trajectoire_bezier(&trajectoire,Localisation_get().x_mm, Localisation_get().y_mm, 
                                485, Localisation_get().y_mm,
                                465, 857,
                                465,2830,
                                +30. * DEGREE_EN_RADIAN, +120. * DEGREE_EN_RADIAN);
            Trajet_debut_trajectoire(trajectoire);
            etat_strategie = APPROCHE_PANIER_1_B;
            break;
        
        case APPROCHE_PANIER_1_B:
            etat_trajet = Trajet_avance(step_ms/1000.);
            if(etat_trajet == TRAJET_TERMINE){
                etat_strategie = ATTRAPE_CERISE_1;
            }
            break;



        case STRATEGIE_FIN:
            break;
    }
}