#include "pico/stdlib.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(){
    static uint32_t temps_obstacle;
    switch(evitement_statu){
        case PAS_D_OBSTACLE:
            if(Trajet_get_bloque() == 1){
                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 = ARRET_DEVANT_OBSTACLE;
            }
            if(Trajet_get_bloque() == 1){
                evitement_statu = PAS_D_OBSTACLE;
            }
            break;
        case ARRET_DEVANT_OBSTACLE:
            if(Trajet_get_bloque() == 1){
                evitement_statu = PAS_D_OBSTACLE;
            }
            break;
    }
}

enum evitement_statu_t Evitement_get_statu(){
    return evitement_statu;
}