#include "Deplacement.h"
#include "Trajet.h"
#include "Evitement.h"
#include "Geometrie.h"

float distance_obstacle_mm;

static enum {
        PARCOURS_INIT,
        PARCOURS_AVANCE,
    } etat_parcourt=PARCOURS_INIT;

/// @brief Reçoit la distance de l'obstacle et l'envoi à la fonction trajet
/// si nécessaire
/// @param _distance_mm 
void Deplacement_set_distance_obstacle(float _distance_mm){
    distance_obstacle_mm = _distance_mm;
}

void Deplacement_interrompre_trajet(void){
    etat_parcourt=PARCOURS_INIT;
}

enum etat_trajet_t Deplacement_parcourir_trajet(struct trajectoire_t trajectoire, uint32_t step_ms, enum evitement_t evitement){
    enum etat_trajet_t etat_action = TRAJET_EN_COURS;
    enum etat_trajet_t etat_trajet;
    float angle_avancement;
    static bool trajet_inverse = false;
    
    

    switch (etat_parcourt){
        case PARCOURS_INIT:
            Trajet_debut_trajectoire(trajectoire);
            etat_parcourt = PARCOURS_AVANCE;
            break;
        
        case PARCOURS_AVANCE:
            if(evitement != EVITEMENT_SANS_EVITEMENT){
                angle_avancement = Trajet_get_orientation_avance();
                Trajet_set_obstacle_mm(distance_obstacle_mm);
            }

            if(Evitement_get_statu() == OBSTACLE_CONFIRME || Evitement_get_statu() == OBSTACLE_NON_CONFIRME){
                switch(evitement){
                    case EVITEMENT_SANS_EVITEMENT:
                        //printf("Evitement lors trajet EVITEMENT_SANS_EVITEMENT: ERREUR\n");
                        break;
                    case EVITEMENT_PAUSE_DEVANT_OBSTACLE:
                        // Rien à faire ici
                        break;
                    
                    case EVITEMENT_ARRET_DEVANT_OBSTACLE:
                        etat_parcourt = PARCOURS_INIT;
                        return TRAJET_ECHEC;

                    case EVITEMENT_RETOUR_SI_OBSTABLE:
                        trajet_inverse = !trajet_inverse;
                        Trajet_inverse();
                        break;

                    case EVITEMENT_CONTOURNEMENT: // TODO
                        break;
                        
                }

            }

            etat_trajet = Trajet_avance(step_ms/1000.);
            if(etat_trajet == TRAJET_TERMINE){
                if(trajet_inverse){
                    etat_action = TRAJET_ECHEC;
                }else{
                    etat_action = TRAJET_TERMINE;
                }
                etat_parcourt = PARCOURS_INIT;
            }
            break;
    }

    return etat_action;

}