#include "pico/stdlib.h"
#include "Balise_VL53L1X.h"
#include "Temps.h"
#include "Trajectoire.h"
#include "Trajet.h"


#ifndef STRATEGIE_H
#define STRATEGIE_H

#define COULEUR 15
#define TIRETTE 14
//#define CORR_ANGLE_DEPART_DEGREE  (-1.145)
#define CORR_ANGLE_DEPART_DEGREE  (0)

enum etat_action_t{
    ACTION_EN_COURS,
    ACTION_TERMINEE,
    ACTION_ECHEC
};

enum longer_direction_t{
    LONGER_VERS_A,
    LONGER_VERS_C
};

enum couleur_t{
    COULEUR_BLEU=0,
    COULEUR_JAUNE,
    COULEUR_INCONNUE,
};

enum evitement_t{
    EVITEMENT_SANS_EVITEMENT,
    EVITEMENT_PAUSE_DEVANT_OBSTACLE,
    EVITEMENT_ARRET_DEVANT_OBSTACLE,
    EVITEMENT_RETOUR_SI_OBSTABLE,
    EVITEMENT_CONTOURNEMENT
};

struct objectif_t{
    int priorite;
    enum {A_FAIRE, EN_COURS, BLOQUE, FAIT} etat;
    enum {CERISE_BAS, CERISE_HAUT, CERISE_GAUCHE, CERISE_DROITE, 
        ZONE_1, ZONE_2, ZONE_3, ZONE_4, ZONE_5} cible;
};

enum etat_action_t cerise_accostage(void);
enum etat_action_t avance_puis_longe_bordure(enum longer_direction_t longer_direction);
enum etat_action_t parcourt_trajet_simple(struct trajectoire_t trajectoire, uint32_t step_ms);
enum etat_action_t parcourt_trajet_simple_sans_evitement(struct trajectoire_t trajectoire, uint32_t step_ms);

void Homologation(uint32_t step_ms);
enum couleur_t lire_couleur(void);
uint attente_tirette(void);
void Strategie(enum couleur_t couleur, uint32_t step_ms, uint32_t temps_ms);

int temporisation_terminee(uint32_t * tempo_ms, uint32_t step_ms);
enum etat_action_t  Strategie_aller_panier(enum couleur_t couleur, uint32_t step_ms);
enum etat_action_t Strategie_parcourir_trajet(struct trajectoire_t trajectoire, uint32_t step_ms, enum evitement_t evitement);
int Robot_est_dans_zone_depose(enum couleur_t couleur);
int Robot_est_dans_zone_depose_panier(enum couleur_t couleur);
enum etat_action_t Strategie_preparation();

enum etat_action_t Strategie_pieds_dans_plat_trajet(struct objectif_t *objectif_plat_courant, enum couleur_t couleur, uint32_t step_ms);
enum etat_action_t Strategie_pieds_dans_plat(enum couleur_t couleur, uint32_t step_ms);

enum etat_action_t Strategie_calage_bas(enum couleur_t couleur, uint32_t step_ms);
enum etat_action_t Strategie_calage_debut_manuel(enum couleur_t couleur, uint32_t step_ms);
enum etat_action_t Strategie_aller_a(float pos_x, float pos_y, enum evitement_t evitement, uint32_t step_ms);
enum etat_action_t Strategie_tourner_et_aller_a(float pos_x, float pos_y, float angle_radian, enum evitement_t evitement,uint32_t step_ms);
enum etat_action_t Strategie_aller_a_puis_tourner(float pos_x, float pos_y, float angle_radian, enum evitement_t evitement,uint32_t step_ms);
enum etat_action_t Strategie_tourner_a(float angle_radian,uint32_t step_ms);

extern float distance_obstacle;

// STRATEGIE_H
#endif