#include "Strategie.h"
#include "Trajet.h"
#include "Evitement.h"
#include "Geometrie.h"
#include "Balise_VL53L1X.h"

enum etat_action_t Strategie_parcourir_trajet(){

}


enum etat_action_t parcourt_trajet_simple(struct trajectoire_t trajectoire, uint32_t step_ms){
    enum etat_action_t etat_action = ACTION_EN_COURS;
    enum etat_trajet_t etat_trajet;
    float angle_avancement;
    
    static enum {
        PARCOURS_INIT,
        PARCOURS_AVANCE,
    } etat_parcourt=PARCOURS_INIT;

    switch (etat_parcourt){
        case PARCOURS_INIT:
            Trajet_debut_trajectoire(trajectoire);
            etat_parcourt = PARCOURS_AVANCE;
            break;
        
        case PARCOURS_AVANCE:
            angle_avancement = Trajet_get_orientation_avance();
            distance_obstacle = Balise_VL53L1X_get_distance_obstacle_mm(angle_avancement);
            Trajet_set_obstacle_mm(distance_obstacle);

            etat_trajet = Trajet_avance(step_ms/1000.);
            if(etat_trajet == TRAJET_TERMINE){
                Trajet_set_obstacle_mm(DISTANCE_INVALIDE);
                etat_action = ACTION_TERMINEE;
                etat_parcourt = PARCOURS_INIT;
            }
            break;
    }

    return etat_action;
}

enum etat_action_t parcourt_trajet_simple_sans_evitement(struct trajectoire_t trajectoire, uint32_t step_ms){
    enum etat_action_t etat_action = ACTION_EN_COURS;
    enum etat_trajet_t etat_trajet;
    static enum {
        PARCOURS_INIT,
        PARCOURS_AVANCE,
    } etat_parcourt=PARCOURS_INIT;

    switch (etat_parcourt){
        case PARCOURS_INIT:
            Trajet_debut_trajectoire(trajectoire);
            Trajet_set_obstacle_mm(distance_pas_obstacle);
            etat_parcourt = PARCOURS_AVANCE;
            break;
        
        case PARCOURS_AVANCE:
            etat_trajet = Trajet_avance(step_ms/1000.);
            if(etat_trajet == TRAJET_TERMINE){
                etat_action = ACTION_TERMINEE;
                etat_parcourt = PARCOURS_INIT;
            }
            break;
    }

    return etat_action;
}