//Librairies utilisées dans le code :
#include <AsyncTCP.h>
#include <ESPAsyncWebSrv.h>
#include <WiFi.h>

//Définition des ports pour controler les moteurs
//Moteur gauche :
#define IN1a 27
#define IN2a 14
#define IN3a 12
#define IN4a 13
//Moteur droit :
#define IN1b 5
#define IN2b 18
#define IN3b 19
#define IN4b 21

//Paramètres de connexion wifi :
const char* ssid = "ledemon";
const char* password = "hell66666";

//Temps d'attente entre deux impulsions :
int delayTime = 3;

//Variables de commande :
bool commandRunning = false;  //Une commande doit etre éxécutée
bool stopCommand = false;     //La commande doit etre stoppée
String globalCommand;         //Nom de la commande
int globalValue;              //Valeur (nombre de pas à effectuer)

//Création du serveur asynchrone :
AsyncWebServer server(80);

//Code éxécuté au démarrage (paramétrage) :
void setup() {
  //On indique que les ports de controle des moteurs sont des sorties (output)
  pinMode(IN1a, OUTPUT);
  pinMode(IN2a, OUTPUT);
  pinMode(IN3a, OUTPUT);
  pinMode(IN4a, OUTPUT);

  pinMode(IN1b, OUTPUT);
  pinMode(IN2b, OUTPUT);
  pinMode(IN3b, OUTPUT);
  pinMode(IN4b, OUTPUT);


  Serial.begin(115200);   //Démarrage d'une communication Série avec l'ordinateur s'il est connecté
  connectWiFi();          //Connexion au wifi
  serverConfig();         //Configuration du serveur  
  server.begin();         //Démarrage du serveur
}

//Code éxécuté en boucle jusqu'à l'extinction du robot
void loop() {
  if (commandRunning){
    if (!stopCommand){
      chooseCommand();                  //Appeler la fonction "chooseCommand()"
      if (globalValue>0){               //S'il reste des pas à effectuer alors on l'affiche et on enlève un pas 
        Serial.print(globalCommand);
        Serial.print(": ");
        Serial.println(globalValue);
        globalValue--;
      }
      else{                             //Sinon on stoppe les moteurs 
        Serial.println("stop");
        commandRunning=false;
        stopCommand=false;
        stopMotors();
      }
    }
    else{
      Serial.println("stop");
      commandRunning=false;
      stopCommand=false;
      stopMotors();
    }
  }

}

//Configuration du serveur asynchrone
void serverConfig(){
  server.on("/get",HTTP_GET,[](AsyncWebServerRequest *request){
    String command;
    float value;
    if (request->hasParam("command")) {
      command = request->getParam("command")->value();
      if (request->hasParam("value")){
        value = request->getParam("value")->value().toFloat();
      }
      else{
        value = 0;
      }
      requestCheck(command,value);
    }
    else {
      command = "Not the good command";
    }
    Serial.print(command);
    Serial.print(": ");
    Serial.println(value);
    request->send(200, "text/plain", command);
  });
}

//Fonction pour convertir une longueur en cm vers un nombre de pas moteur.
int convertLengthToSteps(float length){
  float result = length*512/(4*3.1415);
  return int(result);
}

//fonction pour convertir une rotation en degrés vers un nombre de pas moteur
int convertRotToSteps(int rotation){
  int result = convertLengthToSteps(rotation*3.1415/180*7.8);
  return result;
}

//Lorsqu'une commande arrive au serveur, cette fonction permet 
void requestCheck(String command, float value){
  if (command=="forward"){
    globalCommand="forward";
    commandRunning=true;
    stopCommand=false;
    globalValue=convertLengthToSteps(value);
  }
  else if (command=="backward"){
    globalCommand="backward";
    commandRunning=true;
    stopCommand=false;
    globalValue=convertLengthToSteps(value);
  }
  else if (command=="left"){
    globalCommand="left";
    commandRunning=true;
    stopCommand=false;
    globalValue=convertRotToSteps(value);
  }
  else if (command=="right"){
    globalCommand="right";
    commandRunning=true;
    stopCommand=false;
    globalValue=convertRotToSteps(value);
  }
  else if (command=="stop"){
    globalCommand="stop";
    commandRunning=true;
    stopCommand=true;
  }
  if (value==0){
    globalValue = 474;
  }
}


void connectWiFi()
{
  WiFi.mode(WIFI_AP);
  WiFi.softAP(ssid, password);
  Serial.print("[+] AP Created with IP Gateway ");
  Serial.println(WiFi.softAPIP());
  Serial.println("");
}





void chooseCommand(){
  if (globalCommand=="forward"){
    forward();
  }
  else if (globalCommand=="left"){
    left();
  }
  else if (globalCommand=="right"){
    right();
  }
  else if (globalCommand=="backward"){
    backward();
  }

}





void forward(){
  digitalWrite(IN4a, HIGH);
  digitalWrite(IN3a, LOW);
  digitalWrite(IN2a, LOW);
  digitalWrite(IN1a, LOW);
  digitalWrite(IN4b, LOW);
  digitalWrite(IN3b, LOW);
  digitalWrite(IN2b, LOW);
  digitalWrite(IN1b, HIGH);
  delay(delayTime);
  digitalWrite(IN4a, LOW);
  digitalWrite(IN3a, HIGH);
  digitalWrite(IN2a, LOW);
  digitalWrite(IN1a, LOW);
  digitalWrite(IN4b, LOW);
  digitalWrite(IN3b, LOW);
  digitalWrite(IN2b, HIGH);
  digitalWrite(IN1b, LOW);
  delay(delayTime);
  digitalWrite(IN4a, LOW);
  digitalWrite(IN3a, LOW);
  digitalWrite(IN2a, HIGH);
  digitalWrite(IN1a, LOW);
  digitalWrite(IN4b, LOW);
  digitalWrite(IN3b, HIGH);
  digitalWrite(IN2b, LOW);
  digitalWrite(IN1b, LOW);
  delay(delayTime);
  digitalWrite(IN4a, LOW);
  digitalWrite(IN3a, LOW);
  digitalWrite(IN2a, LOW);
  digitalWrite(IN1a, HIGH);
  digitalWrite(IN4b, HIGH);
  digitalWrite(IN3b, LOW);
  digitalWrite(IN2b, LOW);
  digitalWrite(IN1b, LOW);
  delay(delayTime);
}

void backward(){
  digitalWrite(IN4a, LOW);
  digitalWrite(IN3a, LOW);
  digitalWrite(IN2a, LOW);
  digitalWrite(IN1a, HIGH);
  digitalWrite(IN4b, HIGH);
  digitalWrite(IN3b, LOW);
  digitalWrite(IN2b, LOW);
  digitalWrite(IN1b, LOW);
  delay(delayTime);
  digitalWrite(IN4a, LOW);
  digitalWrite(IN3a, LOW);
  digitalWrite(IN2a, HIGH);
  digitalWrite(IN1a, LOW);
  digitalWrite(IN4b, LOW);
  digitalWrite(IN3b, HIGH);
  digitalWrite(IN2b, LOW);
  digitalWrite(IN1b, LOW);
  delay(delayTime);
  digitalWrite(IN4a, LOW);
  digitalWrite(IN3a, HIGH);
  digitalWrite(IN2a, LOW);
  digitalWrite(IN1a, LOW);
  digitalWrite(IN4b, LOW);
  digitalWrite(IN3b, LOW);
  digitalWrite(IN2b, HIGH);
  digitalWrite(IN1b, LOW);
  delay(delayTime);
  digitalWrite(IN4a, HIGH);
  digitalWrite(IN3a, LOW);
  digitalWrite(IN2a, LOW);
  digitalWrite(IN1a, LOW);
  digitalWrite(IN4b, LOW);
  digitalWrite(IN3b, LOW);
  digitalWrite(IN2b, LOW);
  digitalWrite(IN1b, HIGH);
  delay(delayTime);
}

void right(){
  digitalWrite(IN4a, HIGH);
  digitalWrite(IN3a, LOW);
  digitalWrite(IN2a, LOW);
  digitalWrite(IN1a, LOW);
  digitalWrite(IN4b, HIGH);
  digitalWrite(IN3b, LOW);
  digitalWrite(IN2b, LOW);
  digitalWrite(IN1b, LOW);
  delay(delayTime);
  digitalWrite(IN4a, LOW);
  digitalWrite(IN3a, HIGH);
  digitalWrite(IN2a, LOW);
  digitalWrite(IN1a, LOW);
  digitalWrite(IN4b, LOW);
  digitalWrite(IN3b, HIGH);
  digitalWrite(IN2b, LOW);
  digitalWrite(IN1b, LOW);
  delay(delayTime);
  digitalWrite(IN4a, LOW);
  digitalWrite(IN3a, LOW);
  digitalWrite(IN2a, HIGH);
  digitalWrite(IN1a, LOW);
  digitalWrite(IN4b, LOW);
  digitalWrite(IN3b, LOW);
  digitalWrite(IN2b, HIGH);
  digitalWrite(IN1b, LOW);
  delay(delayTime);
  digitalWrite(IN4a, LOW);
  digitalWrite(IN3a, LOW);
  digitalWrite(IN2a, LOW);
  digitalWrite(IN1a, HIGH);
  digitalWrite(IN4b, LOW);
  digitalWrite(IN3b, LOW);
  digitalWrite(IN2b, LOW);
  digitalWrite(IN1b, HIGH);
  delay(delayTime);
}

void left(){
  digitalWrite(IN4a, LOW);
  digitalWrite(IN3a, LOW);
  digitalWrite(IN2a, LOW);
  digitalWrite(IN1a, HIGH);
  digitalWrite(IN4b, LOW);
  digitalWrite(IN3b, LOW);
  digitalWrite(IN2b, LOW);
  digitalWrite(IN1b, HIGH);
  delay(delayTime);
  digitalWrite(IN4a, LOW);
  digitalWrite(IN3a, LOW);
  digitalWrite(IN2a, HIGH);
  digitalWrite(IN1a, LOW);
  digitalWrite(IN4b, LOW);
  digitalWrite(IN3b, LOW);
  digitalWrite(IN2b, HIGH);
  digitalWrite(IN1b, LOW);
  delay(delayTime);
  digitalWrite(IN4a, LOW);
  digitalWrite(IN3a, HIGH);
  digitalWrite(IN2a, LOW);
  digitalWrite(IN1a, LOW);
  digitalWrite(IN4b, LOW);
  digitalWrite(IN3b, HIGH);
  digitalWrite(IN2b, LOW);
  digitalWrite(IN1b, LOW);
  delay(delayTime);
  digitalWrite(IN4a, HIGH);
  digitalWrite(IN3a, LOW);
  digitalWrite(IN2a, LOW);
  digitalWrite(IN1a, LOW);
  digitalWrite(IN4b, HIGH);
  digitalWrite(IN3b, LOW);
  digitalWrite(IN2b, LOW);
  digitalWrite(IN1b, LOW);
  delay(delayTime);
}


void stopMotors(void) {
  digitalWrite(IN4a, LOW);
  digitalWrite(IN3a, LOW);
  digitalWrite(IN2a, LOW);
  digitalWrite(IN1a, LOW);

  digitalWrite(IN4b, LOW);
  digitalWrite(IN3b, LOW);
  digitalWrite(IN2b, LOW);
  digitalWrite(IN1b, LOW);
}

