// 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 :
struct command_t
{
  bool running; // Une commande doit être exécutée
  bool stopped; // La commande doit être stoppée
  String name;  // Nom de la commande
  int value;    // Valeur (nombre de pas à effectuer)
} command = {false, false};

// 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 (!command.running)
    return;

  // Arrêter le robot si une commande est interrompue
  // ou si le nombre de pas a été effectué
  if (command.stopped || command.value <= 0)
  {
    Serial.println("stop");
    command.running = false;
    command.stopped = false;
    stopMotors();
    return;
  }

  // Sinon executer la commande en cours
  chooseCommand(); // Appeler la fonction "chooseCommand()"
  if (command.value > 0)
  { // S'il reste des pas à effectuer alors on l'affiche et on enlève un pas
    Serial.print(command.name);
    Serial.print(": ");
    Serial.println(command.value);
    command.value--;
  }
}

// Configuration du serveur asynchrone
void serverConfig()
{
  server.on("/get", HTTP_GET, [](AsyncWebServerRequest *request)
            {
    String commandName;
    float commandValue;

    // Parse request params and update command and value
    parseRequestParams(request, commandName, commandValue);

    Serial.print(commandName);
    Serial.print(": ");
    Serial.println(commandValue);
    request->send(200, "text/plain", commandName); });
}

void parseRequestParams(AsyncWebServerRequest *request, String &commandName, float &commandValue)
{
  // Stop here with error message if wrong command
  if (!request->hasParam("command"))
  {
    commandName = "No command provided";
    return;
  }

  // Update command from params
  commandName = request->getParam("command")->value();

  // Update value from params and use "0" as default
  commandValue = request->hasParam("value") ? request->getParam("value")->value().toFloat() : 0;

  requestCheck(commandName, commandValue);
}

// 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 commandName, float commandValue)
{
  if (commandName == "stop")
  {
    command.name = "stop";
    command.running = true;
    command.stopped = true;
  }
  else
  {
    setGlobals(commandName, commandValue);
  }

  if (commandValue == 0)
  {
    command.value = 474;
  }
}

void setGlobals(String commandName, float commandValue)
{
  // Assert command before updating globals
  if (commandName != "forward" && commandName != "backward" && commandName != "left" && commandName != "right")
  {
    return;
  }

  // Update globals
  command.name = commandName;
  command.running = true;
  command.stopped = false;
  command.value = convertLengthToSteps(commandValue);
}

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 (command.name == "forward")
  {
    forward();
  }
  else if (command.name == "left")
  {
    left();
  }
  else if (command.name == "right")
  {
    right();
  }
  else if (command.name == "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);
}