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

// Définition des ports pour contrôler 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 = "ESP32";
const char *password = "test12345";

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

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

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

// Code exécuté au démarrage (paramétrage) :
void setup()
{
  // On indique que les ports de contrôle 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 exécuté en boucle jusqu'à l'extinction du robot
void loop()
{
  // Ne rien faire si aucune commande en cours
  if (!commandRunning)
    return;

  // Arrêter le robot si une commande est interrompue
  if (stopCommand)
  {
    Serial.println("stop");
    commandRunning = false;
    stopCommand = false;
    stopMotors();
    return;
  }

  // Sinon executer la commande en cours
  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();
  }
}

// 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 writeMotor(uint8_t left, uint8_t right)
{
  digitalWrite(IN4a, left & 0b1000);
  digitalWrite(IN3a, left & 0b0100);
  digitalWrite(IN2a, left & 0b0010);
  digitalWrite(IN1a, left & 0b0001);
  digitalWrite(IN4b, right & 0b1000);
  digitalWrite(IN3b, right & 0b0100);
  digitalWrite(IN2b, right & 0b0010);
  digitalWrite(IN1b, right & 0b0001);
}

void forward()
{
  writeMotor(0b1000, 0b0001);
  delay(delayTime);
  writeMotor(0b0100, 0b0010);
  delay(delayTime);
  writeMotor(0b0010, 0b0100);
  delay(delayTime);
  writeMotor(0b0001, 0b1000);
  delay(delayTime);
}

void backward()
{
  writeMotor(0b0001, 0b1000);
  delay(delayTime);
  writeMotor(0b0010, 0b0100);
  delay(delayTime);
  writeMotor(0b0100, 0b0010);
  delay(delayTime);
  writeMotor(0b1000, 0b0001);
  delay(delayTime);
}

void right()
{
  writeMotor(0b1000, 0b1000);
  delay(delayTime);
  writeMotor(0b0100, 0b0100);
  delay(delayTime);
  writeMotor(0b0010, 0b0010);
  delay(delayTime);
  writeMotor(0b0001, 0b0001);
  delay(delayTime);
}

void left()
{
  writeMotor(0b0001, 0b0001);
  delay(delayTime);
  writeMotor(0b0010, 0b0010);
  delay(delayTime);
  writeMotor(0b0100, 0b0100);
  delay(delayTime);
  writeMotor(0b1000, 0b1000);
  delay(delayTime);
}

void stopMotors(void)
{
  writeMotor(0b0000, 0b0000);
}