#include "pico/stdlib.h"
#include "Asser_Moteurs.h"
#include "Evitement.h"
#include "Trajet.h"

// 250 ms
#define TEMPS_VALIDE_OBSTACLE_US 250000

enum evitement_statu_t evitement_statu=PAS_D_OBSTACLE;

void Evitement_gestion(int step_ms){
    static uint32_t temps_obstacle;
    switch(evitement_statu){
        case PAS_D_OBSTACLE:
            if(Trajet_get_bloque() == 1 && AsserMoteur_RobotImmobile(step_ms)){
                evitement_statu = OBSTACLE_NON_CONFIRME;
                temps_obstacle = time_us_32();
            }
            break;
        case OBSTACLE_NON_CONFIRME:
            if(time_us_32() - temps_obstacle >  TEMPS_VALIDE_OBSTACLE_US){
                evitement_statu = OBSTACLE_CONFIRME;
            }
            if(!Trajet_get_bloque()){
                evitement_statu = PAS_D_OBSTACLE;
            }
            break;
        case OBSTACLE_CONFIRME:
            if(!Trajet_get_bloque()){
                evitement_statu = PAS_D_OBSTACLE;
            }
            break;
    }
}

enum evitement_statu_t Evitement_get_statu(){
    return evitement_statu;
}