/*
Test de triangulation
*/
#include <M5Core2.h>
#include <math.h>
#include <WiFi.h>

const char* ssid = "riombotique";
const char* password = "password";
IPAddress local_IP(192, 168, 99, 102);
IPAddress gateway(192, 168, 99, 1);
IPAddress subnet(255, 255, 255, 0);
WiFiServer server(80);
boolean reading = false;
String Lecture;

#define CONV 0.000628318530717959
#define CONV2 1.570796326794896619
#define pi  3.141592653589793
#define pi2 6.283185307179586

uint8_t * data_i2C;
int Balise[4][2];               // 4 balises potentielles i de 0 a 3           
                                // Frequence modulation individuelle balises
                                // Balise[i][0] est la frequence de la balise
                                // Balise[i][1] est l'angle de la balise

bool Balise_Valide;
int A_Depart_Balise;
int A_Fin_Balise;
long Temps_Tour = millis();
long T_Depart_Balise = 0;
long T_Fin_Balise = 0;
long Frequence_Balise = 0;
long Nb_Pulses = 0;
long Chrono = 0;
int Angle = 0;
int Angle_Balise = 0;
int Nb_Balises = 0;
int Capteur = 2;
bool Trigger_Balises = true;  
bool Calcul_Valide = false;
bool Balise_0 = false;
bool Balise_1 = false;
bool Balise_2 = false;
int Nb_tours = 0;
int Old_Nb_tours = 0;
int Pulse_Moteur = 190;
int Frequence_0 = 5000; // frequence balise 0
int Frequence_1 = 6000; // frequence balise 1
int Frequence_2 = 4000; // frequence balise 2
int Bande_P = 200;      // bande passante balises
int X1 = -90;             // abcisse balise 0
int X2 = -90;           // abcisse balise 1
int X3 = 3090;           // abcisse balise 2
int Y1 = 1950;             // ordonnee balise 0
int Y2 = 50;             // ordonnee balise 1
int Y3 = 1000;            // ordonnee balise 2
int Xp1 = X1 - X2;
int Yp1 = Y1 - Y2;
int Xp3 = X3 - X2;
int Yp3 = Y3 - Y2;
int Xr, Yr, Angle_Robot_Deg_int;
int alignement = 2200;
float Angle_Robot_RAD, Angle_Robot_Deg, Angle_B1, angle_B1_calc, Angle_Ref_Theorique, calc1, calc2, offset;

void IRAM_ATTR fonction_AB(){     // fonction de comptage des pas du codeur angulaire
  Angle ++;
  if (Trigger_Balises){
    Capteur = 2;
    if (Angle > 1316) Capteur = 1;
    if (Angle > 2633) Capteur = 2;
    if (Angle > 4300) Capteur = 1;
    if (Angle > 5966) Capteur = 2;
    if (Angle > 7933) Capteur = 1;
    if (Angle > 9299) Capteur = 2;
  }
  if (((Angle - A_Fin_Balise) > 100) && (!Trigger_Balises)){
    if (Nb_Pulses > 20){ // analyse de la balise si plus de 10 pulses uniquement
      // calcul de la frequence moyenne et validation de la balise
      int Delta_T = (T_Fin_Balise - T_Depart_Balise) / 10;
      Frequence_Balise = 100000 * (Nb_Pulses);
      if (Delta_T > 0) Frequence_Balise = Frequence_Balise / Delta_T;
      if (A_Fin_Balise < A_Depart_Balise) A_Fin_Balise = A_Fin_Balise + 10000;
      Angle_Balise = (A_Fin_Balise + A_Depart_Balise) / 2;
      if (Angle_Balise > 10000) Angle_Balise = Angle_Balise - 10000;

      if ((Frequence_Balise > (Frequence_0 - Bande_P)) && (Frequence_Balise < (Frequence_0 + Bande_P)) && !Balise_0) {
         Balise[0][0] = Frequence_Balise;
         Balise[0][1] = Angle_Balise;
         Nb_Balises ++;
         Balise_0 = true; // la balise a ete vue, plus possible de la voir a nouveau
      }
      if ((Frequence_Balise > (Frequence_1 - Bande_P)) && (Frequence_Balise < (Frequence_1 + Bande_P)) && !Balise_1) {
         Balise[1][0] = Frequence_Balise;
         Balise[1][1] = Angle_Balise;
         Nb_Balises ++;
         Balise_1 = true; // la balise a ete vue, plus possible de la voir a nouveau
      }
      if ((Frequence_Balise > (Frequence_2 - Bande_P)) && (Frequence_Balise < (Frequence_2 + Bande_P)) && !Balise_2) {
         Balise[2][0] = Frequence_Balise;
         Balise[2][1] = Angle_Balise;
         Nb_Balises ++;
         Balise_2 = true; // la balise a ete vue, plus possible de la voir a nouveau          
      }

    }
    Trigger_Balises = true;
  }
}

void IRAM_ATTR fonction_Z(){     // appelée à chaque tour
  Nb_tours ++;
  Angle = 0;
}

void IRAM_ATTR fonction_Balise_capteur_1(){      // appelée à chaque detection de baslise capteur 1
  if (Capteur == 1) {
    A_Fin_Balise = Angle;
    // effectue a partir du 2eme pulse
    Chrono = micros();
    Nb_Pulses++;
    T_Fin_Balise = Chrono;
    // effectue au 1er pulse
    if (Trigger_Balises){
      A_Depart_Balise = Angle;
      T_Depart_Balise = Chrono;
      Nb_Pulses = 0;
      Trigger_Balises = false;
      //est-ce bien une balise modulee ? -> mesure du temps avant apparition de la prochaine impulsion impusion 
    }
  }
}

void IRAM_ATTR fonction_Balise_capteur_2(){      // appelée à chaque detection de baslise capteur 2
  if (Capteur == 2) {
    A_Fin_Balise = Angle;
    // effectue a partir du 2eme pulse
    Chrono = micros();
    Nb_Pulses++;
    T_Fin_Balise = Chrono;
    // effectue au 1er pulse
    if (Trigger_Balises){
      A_Depart_Balise = Angle;
      T_Depart_Balise = Chrono;
      Nb_Pulses = 0;
      Trigger_Balises = false;
      //est-ce bien une balise modulee ? -> mesure du temps avant apparition de la prochaine impulsion impusion 
    }
  }
}

void IRAM_ATTR traitement_donnees() {
  // calcul des coordonnees ......................... 
  Calcul_Valide = false;
  double A1 = (Balise[1][1] - Balise[0][1]) * CONV;
  double A2 = (Balise[2][1] - Balise[1][1]) * CONV;  
  if(sin(A1)==0 || sin(A2)==0) return;
  double T12 = cos(A1) / sin(A1);
  double T23 = cos(A2) / sin(A2);
  if ((T12 + T23)==0) return;
  double T31 = (1 - (T12 * T23)) / (T12 + T23);
  double Xp12 = Xp1 + (T12 * Yp1);
  double Yp12 = Yp1 - (T12 * Xp1);
  double Xp23 = Xp3 - (T23 * Yp3);
  double Yp23 = Yp3 + (T23 * Xp3);
  double Xp31 = (Xp3 + Xp1) + (T31 * (Yp3 - Yp1));
  double Yp31 = (Yp3 + Yp1) - (T31 * (Xp3 - Xp1));
  double Kp31 = (Xp1 * Xp3) + (Yp1 * Yp3) + (T31 * ((Xp1 * Yp3) - (Xp3 * Yp1)));
  double D = ((Xp12 - Xp23) * (Yp23 - Yp31)) - ((Yp12 - Yp23) * (Xp23 - Xp31));
  if (D==0) return;
  Xr = X2 + ((Kp31 * (Yp12 - Yp23)) / D);
  Yr = Y2 + ((Kp31 * (Xp23 - Xp12)) / D);
  Calcul_Valide = true;
}

void setup() {
  Serial.begin(115200);
  M5.begin();
  M5.Axp.SetSpkEnable(0); 
  // moteur
  ledcSetup(0, 1000, 8);
  ledcAttachPin(2, 0);
  ledcWrite(0, Pulse_Moteur); 
  // I2C
  Wire.setPins(32, 33);
  I2C_Slave_init(0x30);
  M5.lcd.setTextSize(2); 
    //Initialisation wifi
   //------------WIFI------------
   //Initialisation wifi
  WiFi.config(local_IP, gateway, subnet);
  WiFi.begin(ssid, password);

  // Pour accéder aux données de l'I2C
  data_i2C = get_i2c_data();
  data_i2C[13] = 1; // On dit que le robot est immobile.

  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) {
    server.begin();
    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); 
  }
  else {
    // le routeur riombotique n'a pas été trouvé - création d'un point d'accès
    WiFi.mode(WIFI_OFF);
    if (!WiFi.softAP("triangulation", "ilestsecret")) {
      log_e("Soft AP creation failed.");
      while(1);
    }
    IPAddress myIP = WiFi.softAPIP();
    server.begin();
    M5.Lcd.clear();
    M5.Lcd.setCursor(10,10);
    M5.Lcd.print("Creation Hotspot    ;-)");
    M5.Lcd.setCursor(10,30);
    M5.Lcd.print("triangulation 192.168.4.1"); 
    M5.Lcd.setCursor(10,50);
    M5.Lcd.print("mdp : ilestsecret"); 
  }
  delay(1000);
  M5.Lcd.clear();
  // --------------------
  pinMode(19, INPUT_PULLDOWN);  // entree pulse tour codeur
  pinMode(27, INPUT_PULLDOWN);  // entree pulse angle codeur
  pinMode(25, INPUT_PULLDOWN);  // entree phototransistor capteur 1
  pinMode(26, INPUT_PULLDOWN);  // entree phototransistor capteur 2
  attachInterrupt(27, fonction_AB, RISING);       // appel fonction AB a chaque pulse codeur
  attachInterrupt(19, fonction_Z, RISING);        // appel fonction Z a chaque tour codeur
  attachInterrupt(25, fonction_Balise_capteur_1, FALLING);  // appel fonction Balise capteur 1 a chaque detection phototransistor
  attachInterrupt(26, fonction_Balise_capteur_2, FALLING);  // appel fonction Balise capteur 2 a chaque detection phototransistor
  M5.Lcd.setCursor(10,10);
  M5.lcd.print(" Salut Riombotique");
  delay(500);
  M5.Lcd.clear();
  //M5.Lcd.setCursor(10,10);
  //M5.Lcd.print("Nb tours : ");
  //M5.Lcd.setCursor(10,30);
  //M5.Lcd.print("Balise   : "); 
  M5.Lcd.setCursor(10,0);
  M5.Lcd.print("Angle 1  : ");
  M5.Lcd.setCursor(10,20);
  M5.Lcd.print("Angle 2  : ");
  M5.Lcd.setCursor(10,40);
  M5.Lcd.print("Angle 3  : ");
  //M5.Lcd.setCursor(10,110);
  //M5.Lcd.print("Frequence 1 : ");
  //M5.Lcd.setCursor(10,130);
  //M5.Lcd.print("Frequence 2 : ");
  //M5.Lcd.setCursor(10,150);
  //M5.Lcd.print("Frequence 3 : ");
  M5.Lcd.setCursor(0,60);
  M5.Lcd.print("A float = ");
  M5.Lcd.setCursor(0,80);
  M5.Lcd.print("a RAD = ");
  M5.Lcd.setCursor(0,100);
  M5.Lcd.print("angle = ");
  M5.Lcd.setCursor(10,120);
  M5.Lcd.print("Status  : ");
  M5.Lcd.setCursor(160,180);
  M5.Lcd.print("X = ");
  M5.Lcd.setCursor(160,210);
  M5.Lcd.print("Y = ");
}

void loop() {
  //Effectue a chaque tour ........................
  if(data_i2C[13] != 1){
    Nb_Balises = 0; 
    Balise_0 = false;
    Balise_1 = false;
    Balise_2 = false;
    Calcul_Valide = false;
    uint8_t etat_balises = (Balise_0 | Balise_1 <<1 | Balise_2 <<2 | Calcul_Valide <<3);
    I2C_envoi_8bits(etat_balises,0);
  }
  if ((Old_Nb_tours != Nb_tours) && (Trigger_Balises)){
    Old_Nb_tours = Nb_tours;
    if (!Balise_Valide){
      Balise_Valide = true;
      rapport();
    }

    //_____________________________________________ gestion I2C et Data sent
    if(Xr>0){
  
      Angle_B1 = ((Balise[0][1] + 10000 - alignement) % 10000) * CONV;  // lorsque le robot est orienté vers la balise, angle codeur = 0

      //calc1 = (Y1-Yr)*1000/Xr;
      //Angle_Ref_Theorique = atan(calc1/1000);     // angle théorique entre axe X et alignement vers balise depuis la position X, Y
      calc1 = Xr*1000/(Y1-Yr);
      Angle_Ref_Theorique = atan(calc1/1000);     // angle théorique entre axe Y et alignement vers balise depuis la position X, Y

      //calc2 = fmod(((Angle_B1/pi2*360)+360 - ((CONV2-Angle_Ref_Theorique)/pi2*360)),360); // angle codeur - (90°-angle theorique mais sur Y) modulo 360° et oui le modulo ne prend que de l'entier...
      calc2 = fmod(((Angle_B1/pi2*360)+360 - (Angle_Ref_Theorique/pi2*360)),360); // angle codeur - angle theorique sur Y) modulo 360° et oui le modulo ne prend que de l'entier...
      
      Angle_Robot_RAD = calc2/360*pi2;  // en radian
      Angle_Robot_Deg_int = calc2;      // arrondi des degres dans un entier
      Angle_Robot_Deg = calc2;          // angle d'orientation robot en degres (0 sur l'axe Y)
    }
    uint8_t etat_balises = (Balise_0 | Balise_1 <<1 | Balise_2 <<2 | Calcul_Valide <<3);
    I2C_envoi_8bits(etat_balises,0);
    
    // TODO: Mettre Xr et Yr dans la mémoire I2C ################################
    I2C_envoi_32bits(Xr, 1);
    I2C_envoi_32bits(Yr, 5);
    //I2C_envoi_32bits(Angle_Robot_RAD, 9);
    I2C_envoi_32bits(Angle_Robot_Deg_int, 9);
    //_____________________________________________

    if (Nb_Balises == 3) traitement_donnees(); //calcul des coordonnees si 3 balises
    affichage_resultats();
    checkForClient();     
    Nb_Balises = 0; 
    Balise_0 = false;
    Balise_1 = false;
    Balise_2 = false; 
    for (int i = 0; i <= (2); i++) {
      //Remise à zero des balises
      Balise[i][0] = 0;
      Balise[i][1] = 0;
    } 
    vitesse_moteur();
  }
  // Si le robot n'est pas immobile, on invalide les balises.
  


  //________________________________________
}
void rapport(){
  Serial.print("Frequence ");
  Serial.print(Frequence_Balise);
  Serial.print(" / Pulses ");
  Serial.print(Nb_Pulses);
  Serial.print("/ Angle 1 ");
  Serial.print(A_Depart_Balise);
  Serial.print("/ Angle 2 ");
  Serial.println(A_Fin_Balise);
}

void vitesse_moteur() {
  if ((millis() - Temps_Tour) < 500){ //250 millisecondes par tour correspond à la vitesse de rotation desiree
    Pulse_Moteur ++;
  }
  else {
   Pulse_Moteur --;
  }
  ledcWrite(0, Pulse_Moteur); // Ajustement de la vitesse
  Temps_Tour = millis();
}

void affichage_resultats() {
  // affichage des resultats  .........................
  //M5.Lcd.setCursor(140,10);
  //M5.Lcd.print(Nb_tours); 
  //M5.Lcd.print("  ");  
  //M5.Lcd.setCursor(140,30);
  //M5.Lcd.print(Nb_Balises); 
  //M5.Lcd.print("     ");  
  M5.Lcd.setCursor(140,0);
  M5.Lcd.print(Balise[0][1]);
  //M5.Lcd.print(" / ");  
  //M5.Lcd.print(Angle_B[0][3]);
  M5.Lcd.print("     ");  
  M5.Lcd.setCursor(140,20);
  M5.Lcd.print(Balise[1][1]);
  //M5.Lcd.print(" / ");  
 // M5.Lcd.print(Angle_B[1][3]);
  M5.Lcd.print("     ");  
  M5.Lcd.setCursor(140,40);     
  M5.Lcd.print(Balise[2][1]);
  M5.Lcd.print("     ");  
  M5.Lcd.setCursor(140,120);
  M5.Lcd.print(Balise_0);
  M5.Lcd.setCursor(150,120);
  M5.Lcd.print(Balise_1);
  M5.Lcd.setCursor(160,120);
  M5.Lcd.print(Balise_2);
 // M5.Lcd.print(" / ");  
 // M5.Lcd.print(Angle_B[2][3]);
  M5.Lcd.print("     ");  
  //M5.Lcd.setCursor(200,110);     
  //M5.Lcd.print(Balise[0][0]);
  //M5.Lcd.print("    ");
  //M5.Lcd.setCursor(200,130);     
  //M5.Lcd.print(Balise[1][0]);
  //M5.Lcd.print("    ");
  //M5.Lcd.setCursor(200,150);     
  //M5.Lcd.print(Balise[2][0]);
  //M5.Lcd.print("    ");
  M5.Lcd.setCursor(150,60);
  M5.Lcd.print(Angle_B1);
  M5.Lcd.setCursor(150,80);
  M5.Lcd.print(Angle_Ref_Theorique);
  M5.Lcd.setCursor(150,100);
  M5.Lcd.print(Angle_Robot_Deg);
  M5.Lcd.print(" °      ");  
  M5.Lcd.setCursor(200,180);
  M5.Lcd.print(Xr);
  M5.Lcd.print(" mm      ");  
  M5.Lcd.setCursor(200,210);
  M5.Lcd.print(Yr);
  M5.Lcd.print(" mm      "); 
}

void checkForClient(){
  WiFiClient client = server.available();
  if (client) {
    while (client.connected()) {
      if (client.available()) {
        char c = client.read();
        if (c == '\n') {
          // fin du message
          client.println("HTTP/1.1 200 OK");
          client.println("Content-Type: text/html");
          client.println("Connection: close");
          client.println();
          client.println("<!DOCTYPE HTML>");
          client.println("<meta http-equiv='Refresh' content='2'>");
          client.print("<html>");
          //client.print("Nb tours : ");
          //client.print(Nb_tours);
          //client.print("<BR>");
          //client.print("Balise   : ");
          //client.print(Nb_Balises); 
          //client.print("<BR>");
          client.print("Angle 1  : ");
          client.print(Balise[0][1]);
          client.print("<BR>");
          client.print("Angle 2  : ");
          client.print(Balise[1][1]);
          client.print("<BR>");     
          client.print("Angle 3  : ");
          client.print(Balise[2][1]);
          client.print("<BR>");     
          //client.print("Frequence 1 : ");
          //client.print(Balise[0][0]);
          //client.print("<BR>");     
          //client.print("Frequence 2 : ");
          //client.print(Balise[1][0]);
          //client.print("<BR>");     
          //client.print("Frequence 3 : ");
          //client.print(Balise[2][0]);
          //client.print("<BR>"); 
          client.print("X = ");
          if (Xr < 3000) client.print(Xr);
          client.println(" mm");
          client.print("<BR>");
          client.print("Y = ");
          if (Yr < 3000) client.print(Yr);
          client.println(" mm");
          client.println("</html>");
          break;
        }
      }
      else {
        break;
      }
    }
    delay(10); // give the web browser time to receive the data
    client.stop(); // clos la connection
    delay(10);
  } 
}