#include <M5Core2.h>
#include <Wire.h>
#include <WiFi.h>
#include <math.h>

#include "Com_chassis.h"
#include "Com_detection_adversaire.h"
#include "Com_gradins.h"
#include "ServerWeb.h"


#define WIRE Wire

#define Rad_Deg 57.2957795130823

#define MOUVEMEMENT_ROTATION 0x10
#define MOUVEMEMENT_TRANSLATION 0x8
#define MOUVEMENT_FINI 0x4
#define MOUVEMENT_EN_COURS 0x2
#define MOUVEMENT_INTERRUPTION 0x1

#define VITESSE_STANDARD 1000
#define ACCELERATION_STANDARD 500

#define COULEUR_BLEU 0
#define COULEUR_JAUNE 1

#define GRADIN_PRECISION_ANGLE_RAD 0.009
#define GRADIN_PRECISION_X_MM 10

#define gst_server;

extern const char *ssid;
extern const char *password;


struct triangulation_reception_t {
  int pos_x_mm, pos_y_mm;
  float angle_rad;
  bool validite;
};

// adresse du >Pi qui gere le serveur : 192.168.99.100
IPAddress local_IP(192, 168, 99, 101);
IPAddress gateway(192, 168, 99, 1);
IPAddress subnet(255, 255, 255, 0);
// const char* ssid = "metrofocus_etage";
// const char* password = "19282915001";
// IPAddress local_IP(192, 168, 1, 99);
// IPAddress gateway(192, 168, 1, 1);
// IPAddress subnet(255, 255, 255, 0);

const unsigned char configuration_match_jaune = 0;
const unsigned char configuration_match_bleu = 1;

const int16_t I2C_MASTER = 0x42;
const int16_t I2C_SLAVE_trian = 0x30;
uint8_t test = 32;
uint32_t i = 0;
int16_t cmd_chassi_x = 0;
int16_t xA =0;
int16_t cmd_chassi_y = 0;
int16_t yA =0;
int16_t rot = 0;
int16_t MemCmd_A = 0;
int16_t Cmd_Angle = 0;
int16_t vit = 0;
int16_t acc = 0;
int16_t MemCmd_X = 800;
int16_t MemCmd_Y = 800;
int16_t mem_x = 0;   // faire une memoire et travailler avec
int16_t mem_y = 0;
bool corrige = false;
int coef_mvt = 39;       // coef mise en mm de la commande de mouvement

uint8_t Mot[] = {0,0,0,0,0,0,0,0,0,0,0,0}; /*{0,0,0,0,205,206,207,208,209,210,211};*/

bool Mvt_finit = 0;
int toto = 0;

int direction = 1;
int nbr_essai = 0;

int Position_actuelle_X, Position_actuelle_Y, MemPosition_X, MemPosition_Y, X_futur, Y_futur, compar_X, compar_Y, Angle_Robot_DEG_int; // Coordonée X Y de la triangulation
float Angle_Robot_RAD =0;
int PosLimNeg = -100000;
int PosLimPos = 100000;
bool lec_Balise_1 =0;
bool lec_Balise_2 =0;
bool lec_Balise_3 =0;
bool lec_Calcul_ok =0;
uint8_t error =0;   // erreur de lecture I2C
bool Err_Tri_com =0;
bool Err_Chassi_com=0;
bool Err_Cha_send=0;
const char* ErreurListe[] = {"......", "Triang", "Chassi", "ChaRaz", "TriRaz", "ChSend"};
int IndexErr = 0;

int index_Maitre = 0;
//enum Step {Initial, MemCmdMvt, MemActPos, Verif_Chassis_Rdy, Send_Chassis, Verif_Chassis_CmdOk, Verif_Fin_Mvt, ComparPos};
//Step stp;
bool Mvt_tolerance_OK =false;
bool Balises_OK = 0;
int tolerance_position =100;
float tolerance_orientation =0.03; // 2°


char* tableau[] = {"Lecture serveur", "Prise position", "Verif mvmt end ou cmd", "Compar position", "Deplacement absolu", "Approche gradin"};
char* statu[] = {"/..","./.","../"};
int index_statu=0;

enum etat_action_t{
  ACTION_EN_COURS,
  ACTION_TERMINEE,
  ACTION_ECHEC
};

void setup() {
  // put your setup code here, to run once:
  Serial.begin(115200);
  M5.begin();  //Initialize M5Core2
  M5.Lcd.setTextSize(2);
  M5.Lcd.print("Bonjour\nEcran\n\nRecherche Wifi");
  Initialisation_wifi();
  Web_init();
  WIRE.begin();

  M5.Lcd.clear(); 
  M5.Lcd.setCursor(0, 10);
  M5.Lcd.print("Scan Triangulation en cours");
  //scan_I2C_bus();
  //int compteur = 0;  
  //while(compteur < 3){   //triangulation calcul valide ************** a prendre sur I2c)
  //  Scan_Triangulation();   //Prise de la position actuel
  //  delay(50);
  //  if (Balises_OK && error == 0 && Xr > 0 && Yr > 0) compteur ++;
  //}
  // MemPosition_X = 800;
  // MemPosition_Y = 800;
  struct detect_adv_reception_t detect_adv_reception;
  struct chassis_reception_t chassis_reception;
  int i=0;
  /*
  while(1){
    
    Detect_adv_lire(&detect_adv_reception);
    char chaine[100];
    sprintf(chaine, "Distance: %d cm\n Distance: %d cm\nNb com I2C:%d", detect_adv_reception.distance_cm[0],
          detect_adv_reception.distance_cm[11], i++);
    if(i%100 == 0){
      affiche_msg("Scan_Detect_adversaire", chaine);
    }

    Scan_chassis(&chassis_reception);          //Verif de la fin de mouvement remonté par le chassis 
  }*/

  affichage_standard_init();

  //M5.Lcd.setCursor(10, 200);M5.Lcd.print("cmd :");
}

void loop() {
  static char wait = 0;
  char erreur;
  static int64_t time;
  struct chassis_reception_t chassis_reception;

  gestion_match();
  affichage_resultats();
  delay(10);
}

void affichage_resultats() {
  index_statu ++;
  if(index_statu >=3){index_statu =0;}
  // affichage des resultats  .........................
  M5.Lcd.setCursor(75, 0);M5.Lcd.print(MemPosition_X);M5.Lcd.print("   ");  M5.Lcd.setCursor(200, 0);M5.Lcd.print(X_futur);M5.Lcd.print("   ");     
  M5.Lcd.setCursor(75, 20);M5.Lcd.print(MemPosition_Y);M5.Lcd.print("   ");  M5.Lcd.setCursor(200, 20);M5.Lcd.print(Y_futur);M5.Lcd.print("   ");
  M5.Lcd.setCursor(75, 40);M5.Lcd.print(Position_actuelle_X);M5.Lcd.print("   ");
  M5.Lcd.setCursor(75, 60);M5.Lcd.print(Position_actuelle_Y);M5.Lcd.print("   ");
  M5.Lcd.setCursor(220, 80);M5.Lcd.print(ErreurListe[IndexErr]);M5.Lcd.print("   ");
  M5.Lcd.setCursor(220, 100);M5.Lcd.print(lec_Balise_1);M5.Lcd.print(lec_Balise_2);M5.Lcd.print(lec_Balise_3);M5.Lcd.print(lec_Calcul_ok);M5.Lcd.print("   ");
  M5.Lcd.setCursor(150, 120);M5.Lcd.print(Angle_Robot_RAD);M5.Lcd.print("   ");
  M5.Lcd.setCursor(90, 140);M5.Lcd.print(compar_X);M5.Lcd.print("   ");
  M5.Lcd.setCursor(90, 160);M5.Lcd.print(compar_Y);M5.Lcd.print("   ");
  M5.Lcd.setCursor(70, 180);M5.Lcd.print(index_Maitre);M5.Lcd.print(", ");M5.Lcd.print(tableau[index_Maitre]);M5.Lcd.print("     ");
  //M5.Lcd.setCursor(90, 200);M5.Lcd.print(x);M5.Lcd.print("   ");M5.Lcd.print(y);M5.Lcd.print("   ");M5.Lcd.print(rot);M5.Lcd.print("   ");
  M5.Lcd.setCursor(50, 220);M5.Lcd.print(statu[index_statu]);M5.Lcd.print("     ");M5.Lcd.print(index_statu);
}

void Initialisation_wifi(){
  //Initialisation wifi
  //------------WIFI------------
  //Initialisation wifi
  //WiFi.config(local_IP, gateway, subnet);
  WiFi.begin(ssid, password);
  int test_wifi = 0;
  while (WiFi.status() != WL_CONNECTED){
    delay(300);
    M5.Lcd.print(".");
    test_wifi ++;
    if (test_wifi > 10) break;
  }
 if (WiFi.status() == WL_CONNECTED) {
    
    Serial.println("Server started");
    delay(500);
    M5.Lcd.clear();
    M5.Lcd.setCursor(10,10);
    M5.Lcd.print("Connecte au reseau    ;-)");
    M5.Lcd.setCursor(10,30);
    M5.Lcd.print(ssid); 
    M5.Lcd.setCursor(10,50);
    M5.Lcd.print(WiFi.localIP());
    delay(3000);
    return;
  }
  // le routeur riombotique n'a pas été trouvé - création d'un point d'accès
  WiFi.config(local_IP, gateway, subnet);
  WiFi.mode(WIFI_OFF);
  if (!WiFi.softAP("cerveau", "ilestsecret")) {
    log_e("Soft AP creation failed.");
    while(1);
  }
  IPAddress myIP = WiFi.softAPIP();
  M5.Lcd.clear();
  M5.Lcd.setCursor(10,10);
  M5.Lcd.print("Creation Hotspot    ;-)");
  M5.Lcd.setCursor(10,30);
  M5.Lcd.print("cerveau 192.168.4.1"); 
  M5.Lcd.setCursor(10,50);
  M5.Lcd.print("mdp : ilestsecret"); 

  delay(1500);
}

void affichage_standard_init(){
  M5.Lcd.clear(); 
  M5.Lcd.setCursor(0, 0);M5.Lcd.print("Pos");M5.Lcd.setCursor(50, 0);M5.Lcd.print("X :");M5.Lcd.setCursor(150, 0);M5.Lcd.print("|Xp:");
  M5.Lcd.setCursor(0, 20);M5.Lcd.print("n-1");
  M5.Lcd.setCursor(50, 20);M5.Lcd.print("Y :");M5.Lcd.setCursor(150, 20);M5.Lcd.print("|Yp:");
  M5.Lcd.setCursor(0, 40);M5.Lcd.print("Pos");
  M5.Lcd.setCursor(50, 40);M5.Lcd.print("X :");
  M5.Lcd.setCursor(0, 60);M5.Lcd.print("Act");
  M5.Lcd.setCursor(50, 60);M5.Lcd.print("Y :");
  M5.Lcd.setCursor(10, 80);M5.Lcd.print("Trans I2C");
  M5.Lcd.setCursor(10, 100);M5.Lcd.print("Triangulation ");
  M5.Lcd.setCursor(10, 120);M5.Lcd.print("AngleRad");
  M5.Lcd.setCursor(10, 140);M5.Lcd.print("Tol x:");
  M5.Lcd.setCursor(10, 160);M5.Lcd.print("Tol y:");
  M5.Lcd.setCursor(10, 180);M5.Lcd.print("Case");
}

void affiche_erreur(char * chaine1, char * chaine2){
  M5.Lcd.clear();
  M5.Lcd.setCursor(10,10);
  M5.Lcd.setTextColor(RED);
  M5.Lcd.print("Erreur fatale :-(");
  M5.Lcd.setTextColor(WHITE);
  M5.Lcd.setCursor(10,30);
  M5.Lcd.print(chaine1); 
  M5.Lcd.setCursor(10,50);
  M5.Lcd.print(chaine2); 
}

void affiche_msg(char * chaine1, char * chaine2){
  M5.Lcd.clear();
  M5.Lcd.setCursor(10,10);
  M5.Lcd.print("Message :");
  M5.Lcd.setCursor(10,30);
  M5.Lcd.print(chaine1); 
  M5.Lcd.setCursor(10,50);
  M5.Lcd.print(chaine2); 
}

void gestion_match(){
  struct chassis_reception_t chassis_reception;
  struct chassis_emission_t chassis_emission;
  struct triangulation_reception_t triangulation_reception;
  struct detect_gradin_t detect_gradin;
  enum etat_action_t etat_action;
  static int translation_x_mm, translation_y_mm;
  static float rotation_rad;
  static int couleur;

  enum etat_strategie{
    ATTENTE_ORDRE=0,
    LECTURE_TRIANGULATION=1,
    DEPLACEMENT_RELATIF=2,
    MATCH_EN_COURS=3,
    TEST_DEPLACEMENT_ABSOLU=4,
    TEST_APPROCHE_GRADIN=5
  };

  switch(index_Maitre){
    case ATTENTE_ORDRE :    //---------------------------------------------------------------------------------------------------------
      /// Lecture des commandes venant du serveur web et envoi des commandes au chassis
      Web_gestion();
      M5.update();
      /// Si on a une réponse du client
      /// On lit la réponse
      if(Web_nouveau_message()){
        if(Web_type_requete() == WEB_DEPLACEMENT_RELATIF){
          chassis_emission = Web_get_donnees();
          translation_x_mm = chassis_emission.translation_x_mm;
          translation_y_mm = chassis_emission.translation_y_mm;
          rotation_rad = chassis_emission.rotation_z_rad;
        }
        Serial.printf("vit:%d, corrige: %d\n", vit, corrige);
        MemCmd_X = xA;             //Memorisation de la comande pour comparaison avec arrivé
        MemCmd_Y = yA;
        MemCmd_A = rot;
        //index_Maitre = 1; // Bloc le serveur WEB si on n'a pas la triangulation
        index_Maitre = DEPLACEMENT_RELATIF;
      }
      if(M5.BtnA.read() == 1){
        Serial.println("BtnA");
        // Déplacement en X
        translation_x_mm = 500;
        translation_y_mm = 0;
        rotation_rad = 0;
        
        index_Maitre = TEST_APPROCHE_GRADIN;      
      }
      if(M5.BtnB.read() == 1){
        Serial.println("BtnB");
        // Déplacement en Y       
        //Triangulation_send_immobile(0);
        index_Maitre = TEST_DEPLACEMENT_ABSOLU;
        
      }
      if(M5.BtnC.read() == 1){
        // Rotation
        Serial.println("BtnC");
        translation_x_mm = 0;
        translation_y_mm = 0;
        rotation_rad = 100;
        Triangulation_send_immobile(1);
        while(M5.BtnC.read()){
          M5.update();
        }
        delay(200);
        IHM_attente_match(&couleur);

        index_Maitre = MATCH_EN_COURS;
        
      }
    break;

    case 1 :    //---------------------------------------------------------------------------------------------------------
      Scan_Triangulation(&triangulation_reception);   //Prise de la position actuel
      MemPosition_X = Position_actuelle_X;
      MemPosition_Y = Position_actuelle_Y;
      if(Balises_OK == true && error == 0){   //triangulation calcul valide ************** a prendre sur I2c
        index_Maitre = 2;
      }else{
        //print probleme de lecture triangulatation
      }
      //Si position Pas OK ******************
      break;

    case DEPLACEMENT_RELATIF :    // Deplacement relatif en cours

      if(deplacement_relatif(translation_x_mm, translation_y_mm, rotation_rad, 1) == ACTION_TERMINEE){
        index_Maitre = ATTENTE_ORDRE;
      }
      break;

    case MATCH_EN_COURS:
      if(Strategie(couleur) == ACTION_TERMINEE){
        index_Maitre = ATTENTE_ORDRE;
      }
      break;

    case TEST_DEPLACEMENT_ABSOLU:
      if(deplacement_absolu(800, 800, 0, 0) == ACTION_TERMINEE){
        index_Maitre = ATTENTE_ORDRE;
      }
      break;

    case TEST_APPROCHE_GRADIN:
      if(gradin_approche() != ACTION_EN_COURS){
        index_Maitre = ATTENTE_ORDRE;
        affichage_standard_init();
      }

    /*
      do{
        char chaine[200];
        Detect_gradin(&detect_gradin);
        sprintf(chaine, "I2C OK\nStatus:%d\nCentre X:%4d\nCentre Y:%4d\nAngle:%2.2f\n", detect_gradin.status, 
          detect_gradin.centre_x_mm, detect_gradin.centre_y_mm, detect_gradin.angle_rad / M_PI * 180);
        affiche_msg("Detect gradin", chaine);
        // if(detect_gradin.status == 2){
        //   while(deplacement_relatif(0, 0, - detect_gradin.angle_rad, 0) != ACTION_TERMINEE);
        // }
      }while(fabs(detect_gradin.angle_rad / M_PI * 180) > 0.5);
      //index_Maitre = ATTENTE_ORDRE;
      //affichage_standard_init();
      */
      break;
    
  }
}

void IHM_attente_match(int * couleur){
  int pret = 0;
  int m_couleur = COULEUR_BLEU;
  affiche_msg("Choix couleur", "En attente");
  while(!pret){
    M5.update();
    if(M5.BtnA.read() == 1){
      affiche_msg("Couleur", "  BLEU     ");
      Triangulation_send_config(configuration_match_bleu);
      m_couleur = COULEUR_BLEU;
    }
    if(M5.BtnB.read() == 1){
      affiche_msg("Couleur", "  JAUNE    ");
      Triangulation_send_config(configuration_match_jaune);
      m_couleur = COULEUR_JAUNE;
    }
    if(M5.BtnC.read() == 1){
      pret = 1;
      *couleur = m_couleur;
    }
  }
}

enum etat_action_t Strategie(int couleur){
  static enum {
    STRAT_RECULE_BANDEROLE, // Deplacement relatif
    STRAT_ALLER_GRADINS_1_A,  // Déplacement absolu
    STRAT_ALLER_GRADINS_1_B,  // Déplacement relatif
    STRAT_ALLER_GRADINS_1_C,  // Déplacement relatif
    STRAT_ALLER_PREPA_BACKSTAGE, // Déplacement absolu
    STRAT_ALLER_BACKSTAGE // Déplacement relatif

  }etat_strategie = STRAT_RECULE_BANDEROLE;
  enum etat_action_t etat_action;
  int translation_x_mm, translation_y_mm;
  float rotation_rad;

  switch(etat_strategie){
    case STRAT_RECULE_BANDEROLE:
      // Déplacement en X
      translation_x_mm = -450;
      translation_y_mm = 0;
      rotation_rad = 0;
      etat_action = deplacement_relatif(translation_x_mm, translation_y_mm, rotation_rad, 1);
      if(etat_action == ACTION_TERMINEE){
        etat_strategie = STRAT_ALLER_GRADINS_1_A;
      }
      break;

    case STRAT_ALLER_GRADINS_1_A:
      if(couleur == COULEUR_JAUNE){
        etat_action = deplacement_absolu(800, 800, -M_PI/2., 1);
      }else{
        etat_action = deplacement_absolu(3000 - 800, 800, -M_PI/2., 1);
      }
      if(etat_action == ACTION_TERMINEE){
        etat_strategie = STRAT_ALLER_GRADINS_1_B;
      }
      break;

    case STRAT_ALLER_GRADINS_1_B:
      etat_action = deplacement_relatif(300, 0, 0, 0);
      if(etat_action == ACTION_TERMINEE){
        etat_strategie = STRAT_ALLER_GRADINS_1_C;
      }
      break;

    case STRAT_ALLER_GRADINS_1_C:
      etat_action = deplacement_relatif(-250, 0, 0, 0);
      if(etat_action == ACTION_TERMINEE){
        etat_strategie = STRAT_ALLER_PREPA_BACKSTAGE;
      }
      break;

    case STRAT_ALLER_PREPA_BACKSTAGE:
      if(couleur == COULEUR_JAUNE){
        etat_action = deplacement_absolu(550, 1150, M_PI/2., 0);
      }else{
        etat_action = deplacement_absolu(3000 - 550, 1150, M_PI/2., 0);
      }
      if(etat_action == ACTION_TERMINEE){
        etat_strategie = STRAT_ALLER_BACKSTAGE;
      }
      break;

    case STRAT_ALLER_BACKSTAGE:
      etat_action = deplacement_relatif(500, 0, 0, 0);
      if(etat_action == ACTION_TERMINEE){
        etat_strategie = STRAT_RECULE_BANDEROLE;
        return ACTION_TERMINEE;
      }
      break;
  }

  return ACTION_EN_COURS;

}

enum etat_action_t gradin_approche(void){
  static enum{
    GA_INIT,
    GA_CHERCHE_GAUCHE,
    GA_CHERCHE_DROIT,
    GA_GOTO_LARGE,
    GA_GOTO_EN_FACE,
    GA_TOURNE_LARGE,
    GA_GOTO_PROCHE,
    GA_GOTO_PREND,
    GA_RECULE
  } statu_approche_gradin = GA_INIT;
  static float angle_parcouru, angle_mem;
  static int nb_erreur;
  int translation_x, translation_y;
  float distance;
  struct detect_gradin_t detect_gradin;

  delay(150);
  Detect_gradin(&detect_gradin);
  char chaine[200];
  sprintf(chaine, "I2C OK\nStatus:%d\nCentre X:%4d\nCentre Y:%4d\nAngle:%2.2f\n", detect_gradin.status, 
    detect_gradin.centre_x_mm, detect_gradin.centre_y_mm, detect_gradin.angle_rad / M_PI * 180);
  affiche_msg("Detect gradin", chaine);

  switch(statu_approche_gradin){
    case GA_INIT:
      angle_parcouru = 0;
      statu_approche_gradin = GA_CHERCHE_GAUCHE;
      break;

    case GA_CHERCHE_GAUCHE:
      if(detect_gradin.status == 2){
        // On a trouvé !
        statu_approche_gradin = GA_GOTO_LARGE;
        angle_mem = detect_gradin.angle_rad;
        nb_erreur = 0;
      }else if(detect_gradin.centre_y_mm < 180){
        statu_approche_gradin = GA_RECULE;
      }else if(detect_gradin.status == 3){
        // On a un grain sur la droite
        statu_approche_gradin = GA_CHERCHE_DROIT;
      }else if(detect_gradin.status == 0){
        // On a perdu la détection
        while(deplacement_relatif(0, 0, 3. * M_PI / 180., 0) == ACTION_EN_COURS);
        statu_approche_gradin = GA_CHERCHE_DROIT;
      }else{
        // On tourne à gauche de quelques degrés
        while(deplacement_relatif(0, 0, -3. * M_PI / 180., 0) == ACTION_EN_COURS);
      }
      break;

    case GA_CHERCHE_DROIT:
      if(detect_gradin.status == 2){
        // On a trouvé !
        statu_approche_gradin = GA_GOTO_LARGE;
        angle_mem = detect_gradin.angle_rad;
        nb_erreur = 0;
      }else if(detect_gradin.centre_y_mm < 180){
        statu_approche_gradin = GA_RECULE;
      }else if(detect_gradin.status == 4){
        // On a un grain sur la gauche
        statu_approche_gradin = GA_CHERCHE_GAUCHE;
      }else if(detect_gradin.status == 0){
        // On a perdu la détection
        statu_approche_gradin = GA_INIT;
        return ACTION_ECHEC;
      }else{
        // On tourne à gauche de quelques degrés
        while(deplacement_relatif(0, 0, 3. * M_PI / 180., 0) == ACTION_EN_COURS);
      }
      break;

    case GA_GOTO_EN_FACE:
    case GA_GOTO_LARGE:
      Detect_gradin(&detect_gradin);
      if(detect_gradin.status != 2){
        nb_erreur++;
        if(nb_erreur > 100){
          affiche_erreur("Gradin Approche", "GA_GOTO_LARGE\n Status != 2");
          while(1);
        }
      }else if(detect_gradin.centre_y_mm < 180){
        // On est trop près, on ne détectera pas le gradin si on est devant
        statu_approche_gradin = GA_RECULE;
      }else{
        // On se centre par rapport au gradin
        //translation_x = (detect_gradin.centre_y_mm + 150) * tan(fabs(detect_gradin.angle_rad));
        //translation_y = -(detect_gradin.centre_y_mm + 150) * sin(detect_gradin.angle_rad) - detect_gradin.centre_x_mm;
        translation_x = (detect_gradin.centre_y_mm + 150) * tan(fabs(detect_gradin.angle_rad));
        translation_y = -(detect_gradin.centre_y_mm + 150) * sin(detect_gradin.angle_rad) - detect_gradin.centre_x_mm;

        while(deplacement_relatif(translation_x, translation_y, 0, 0) == ACTION_TERMINEE);
        statu_approche_gradin = GA_TOURNE_LARGE;
        angle_mem = detect_gradin.angle_rad;
        
      }
      break;

    case GA_TOURNE_LARGE:
      while(deplacement_relatif(0, 0, -angle_mem, 0) != ACTION_TERMINEE);
      delay(150);
      Detect_gradin(&detect_gradin);
      if(detect_gradin.status != 2){
        statu_approche_gradin = GA_CHERCHE_GAUCHE;
      }else if(detect_gradin.centre_y_mm < 180){
        statu_approche_gradin = GA_RECULE;
      }else if(fabs(detect_gradin.angle_rad) > GRADIN_PRECISION_ANGLE_RAD){
        angle_mem = detect_gradin.angle_rad;
      }else{
        if(detect_gradin.centre_y_mm > 240){
          statu_approche_gradin = GA_GOTO_PROCHE;
        }else{
          if(fabs(detect_gradin.centre_x_mm) > 10){
            statu_approche_gradin = GA_GOTO_LARGE;
          }else{
            statu_approche_gradin = GA_GOTO_PREND;
          }
        }
      }
      break;

    case GA_GOTO_PROCHE:
      Detect_gradin(&detect_gradin);
      if(deplacement_relatif((detect_gradin.centre_y_mm - 200), 0, 0, 0) == ACTION_TERMINEE){
          statu_approche_gradin = GA_GOTO_EN_FACE;
      }
      break;

    case GA_GOTO_PREND:
      while(deplacement_relatif((detect_gradin.centre_y_mm - 25), 0, 0, 0) != ACTION_TERMINEE);
      statu_approche_gradin = GA_INIT;
      delay(5000);
      return ACTION_TERMINEE;
  
      break;

    case GA_RECULE:
      while(deplacement_relatif((detect_gradin.centre_y_mm - 220), 0, 0, 0) != ACTION_TERMINEE);
      statu_approche_gradin = GA_CHERCHE_GAUCHE;
      break;


      

  }
  return ACTION_EN_COURS;

}


/// @brief : compare la position actuelle et la position lue par la balise
/// Note : Pour l'instant, on ne déclenche un mouvment qu'en cas d'ecart sur la distance, pas sur l'orientation.
void compar_cinematique(int consigne_x_mm, int consigne_y_mm, float consigne_orientation_rad, 
    struct triangulation_reception_t triangulation_reception, struct chassis_emission_t * chassis_emission){ 

  float compar_rotation;
  
  compar_X =  consigne_x_mm - triangulation_reception.pos_x_mm;     //compar de la position théoriquement atteinte avec la position actuel
  compar_Y =  consigne_y_mm - triangulation_reception.pos_y_mm;    //YR : position actuel    Y_futur : Position de départ + mouvement demander (donc point d'arrivé théorique)
  compar_rotation = consigne_orientation_rad - triangulation_reception.angle_rad;
  while(compar_rotation < -M_PI){
    compar_rotation += 2* M_PI;
  }
  while(compar_rotation > M_PI){
    compar_rotation -= 2* M_PI;
  }
  printf("compar_X:%d\tcompar_y:%d\tcompar_rot:%f\n", compar_X, compar_Y, compar_rotation);

  if(abs(compar_X) < tolerance_position && abs(compar_Y) < tolerance_position){
      if(abs(compar_rotation) > tolerance_orientation) {
        chassis_emission->translation_x_mm = 0;
        chassis_emission->translation_y_mm = 0;
        chassis_emission->rotation_z_rad = -compar_rotation;
        chassis_emission->status = MOUVEMENT_EN_COURS;
      }else{
        chassis_emission->status = MOUVEMENT_FINI;
      }
  }else{
    // print de la difference ; determiné cmd il nous faudrait faire à nouveau pour atteindre la position voulue

    // Distance à parcourir
    float distance_calculee = sqrt(sq(compar_X) + sq(compar_Y));
    float angle_robot_vers_destination = M_PI + atan2f(compar_Y, compar_X);

    chassis_emission->translation_x_mm = cos(angle_robot_vers_destination - triangulation_reception.angle_rad) * distance_calculee;
    chassis_emission->translation_y_mm = sin(angle_robot_vers_destination - triangulation_reception.angle_rad) * distance_calculee;
    chassis_emission->rotation_z_rad = 0;
    chassis_emission->status = MOUVEMENT_EN_COURS;
  }
}

enum etat_action_t deplacement_absolu(int consigne_x_mm, int consigne_y_mm, float consigne_orientation_rad, int evitement){
  static enum{
    DA_INIT,
    DA_COMPARE_POSITIONS,
    DA_MVT_EN_COURS,
    DA_ATTENTE,
  } etat_deplacement = DA_INIT;
  static int mem_consigne_x_mm, mem_consigne_y_mm;
  static float mem_consigne_orientation_rad;
  static struct chassis_emission_t chassis_emission;
  struct triangulation_reception_t triangulation_reception;
  enum etat_action_t etat_deplacement_relatif;

  switch(etat_deplacement){
    case DA_INIT:
      mem_consigne_x_mm = consigne_x_mm;
      mem_consigne_y_mm = consigne_y_mm;
      mem_consigne_orientation_rad = consigne_orientation_rad;
      etat_deplacement = DA_COMPARE_POSITIONS;
      Serial.printf("DA_INIT\n");
      break;

    case DA_COMPARE_POSITIONS:
      Scan_Triangulation(&triangulation_reception);   //Prise de la position actuel
      if(triangulation_reception.validite == true){
        Serial.printf("Compare cinematique\n");
        compar_cinematique(mem_consigne_x_mm, mem_consigne_y_mm, mem_consigne_orientation_rad,
          triangulation_reception, &chassis_emission);
        if(chassis_emission.status == MOUVEMENT_EN_COURS){
          // C'est que la fonction compar_cinematique indique qu'on doit se déplacer
          // Les valeurs du déplacement sont renseignées dans "chassis_emission".
          Serial.printf("DA_MVT_EN_COUR\n");
          Serial.printf("pos_x:%d\tpos_y:%d\tOrientation:%f\n",triangulation_reception.pos_x_mm, triangulation_reception.pos_y_mm,
          triangulation_reception.angle_rad); 
          Serial.printf("trans_x:%d\ttrans_y:%d\trot:%f\n",chassis_emission.translation_x_mm, 
          chassis_emission.translation_y_mm, chassis_emission.rotation_z_rad);
          etat_deplacement = DA_MVT_EN_COURS;
        }else{
          // Alors nous sommes arrivés
          // On réinitialise la mahcine à état
          etat_deplacement = DA_INIT;
          return ACTION_TERMINEE;
        }
      }
      break;
    
    case DA_MVT_EN_COURS:
      Scan_Triangulation(&triangulation_reception);   //Prise de la position actuel
      etat_deplacement_relatif = deplacement_relatif(- chassis_emission.translation_x_mm, 
          -chassis_emission.translation_y_mm, chassis_emission.rotation_z_rad, evitement);
      if(etat_deplacement_relatif == ACTION_TERMINEE){
        Serial.printf("DA_COMPARE_POSITIONS\n");
        etat_deplacement = DA_ATTENTE;
      }
      break;

    case DA_ATTENTE:
      delay(3000);
      etat_deplacement = DA_COMPARE_POSITIONS;
      break;
  }

  return ACTION_EN_COURS;
}

/// @brief Récupère les données de la carte de détection de l'advesraire et renoi 1 s'il y aun adversaire
/// Test les capteurs dans la direction d'avancement
/// @param angle_deplacement direction dans laquelle avance le robot, dans le référentiel du robot
int detection_adversaire(float angle_deplacement){
  int capteur_central, capteur_precedant, capteur_suivant;
  struct detect_adv_reception_t detect_adv_reception;
  // On ramène l'angle entre 0 et 2 PI.
  while(angle_deplacement < 0){
    angle_deplacement += 2 * M_PI;
  }
  while(angle_deplacement > 2 * M_PI){
    angle_deplacement -= 2 * M_PI;
  }
  // On obtient le capteur central.
  capteur_central = angle_deplacement / (M_PI * 2) * 12;
  capteur_precedant = capteur_central - 1;
  if(capteur_precedant < 0){
    capteur_precedant = 11;
  }
  capteur_suivant = capteur_central + 1;
  if(capteur_suivant > 11){
    capteur_suivant = 0;
  }
  Detect_adv_lire(&detect_adv_reception);

  if(detect_adv_reception.distance_cm[capteur_central] < 50 || 
      detect_adv_reception.distance_cm[capteur_precedant] < 50 ||
      detect_adv_reception.distance_cm[capteur_suivant] < 50 ){
    return 1;
  }
  return 0;
}

/// @brief Deplacement dans le repère du robot, pouvant prendre en compte la detection de l'adversaire
/// evitement : 1 pour s'arreter si adversaire detecté, 0 pour ignorer l'adversaire
enum etat_action_t deplacement_relatif(int distance_x_mm, int distance_y_mm, float rotation_rad, int evitement){
  static enum{
    DR_INIT,
    DR_MVT_EN_COUR,
  } etat_deplacement = DR_INIT;

  struct chassis_emission_t chassis_emission;
  struct chassis_reception_t chassis_reception;
  struct detect_adv_reception_t detect_adv_reception;

  switch(etat_deplacement){
    case DR_INIT:
      chassis_emission.status = MOUVEMENT_EN_COURS;
      chassis_emission.translation_x_mm = distance_x_mm;
      chassis_emission.translation_y_mm = distance_y_mm;
      chassis_emission.rotation_z_rad = rotation_rad;
      chassis_emission.vitesse = VITESSE_STANDARD;
      chassis_emission.acceleration = ACCELERATION_STANDARD;

      send_Chassis(&chassis_emission);
      Triangulation_send_immobile(0);
      etat_deplacement = DR_MVT_EN_COUR;
      break;

    case DR_MVT_EN_COUR:
      if(evitement){
        // On lit les capteurs
        Detect_adv_lire(&detect_adv_reception);
        // On analyse les valeurs - TODO : créer une fonction plus évoluée
        if(distance_x_mm > 0){
          if(detect_adv_reception.distance_cm[0] < 50){
            // Arrêt du mouvement
            chassis_emission.status = MOUVEMENT_INTERRUPTION;
            send_Chassis(&chassis_emission);
          }
        }else if(distance_x_mm < 0){
          if(detect_adv_reception.distance_cm[6] < 50){
            // Arrêt du mouvement
            chassis_emission.status = MOUVEMENT_INTERRUPTION;
            send_Chassis(&chassis_emission);
          }
        }
      }

      Scan_chassis(&chassis_reception);

      if(chassis_reception.status == MOUVEMENT_FINI){
        Triangulation_send_immobile(1);
        etat_deplacement = DR_INIT;
        return ACTION_TERMINEE;
      }
      break;
  }

  return ACTION_EN_COURS;
}


void scan_I2C_bus(){
  char error, address;
  char tampon[10];
  int nDevices=0;


  M5.Lcd.print("  ");
  for(address = 0; address < 16;  address++ ){
    sprintf(tampon, "%x", address);
    M5.Lcd.print(tampon);
  } 
  M5.Lcd.print("\n");

  for(address = 0; address < 128; address++ )
  {
    
    // The i2c_scanner uses the return value of
    // the Write.endTransmisstion to see if
    // a device did acknowledge to the address.
    if(address !=0){
      WIRE.beginTransmission(address);
      error = WIRE.endTransmission(true);
    }else{
      error = -1;
    }

    if((nDevices % 16) == 0){
      M5.Lcd.print("\n");
      sprintf(tampon, "%x ", address/16);
      M5.Lcd.print(tampon);
      
    }

    if (error == 0)
    {
      M5.Lcd.setTextColor(GREEN);
      M5.Lcd.print("X");
      Serial.printf("SUCCESS - endTransmission: %u\n", error);
      M5.Lcd.setTextColor(WHITE);

      WIRE.beginTransmission(address);
      WIRE.write("Hello !\n");
      WIRE.endTransmission(true);
    }
    else
    {
      Serial.printf("endTransmission: %u\n", error);
      M5.Lcd.print(".");
    }
    nDevices++;

  }
  delay(10000);
  
}