// Librairies
#include <AsyncTCP.h>
#include <ESPAsyncWebServer.h>
#include <WiFi.h>

// Librairies locales 
#include "config_gommettes.h"   // Configurations des paramètres 
#include "convert.h"            // Fonctions de conversions en steps
#include "controls_gommette.h"  // Fonctions de déplacement
#include "commands.h"         // Permet de traduire des commandes
#include "setup.h"

command_m command = {.running = false, .code = "0000", .stopped = false};                 // Objet regroupant les informations sur la commande à exécuter.
command_r reader = {.init = false, .newLine = false};                                    // Objet regroupant les informations concernant le lecteur de gommettes.
led_b led = {.light_on=false};                                                            // Objet permettant de faire clignoter la led.
distances distValues = {.straight=true, .length=moveLength, .initialLength=moveLength};   // Objet Permettant de gérer les distances d'avance en mode gommettes lorsque l'on tourne à 45°.
bool wifiMode = false;                                                                    // Mode de controle (WiFi ou gommettes). 


AsyncWebServer server(80);  // Création de l'objet server qui ne sera utilisé qu'en mode WiFi. 


void setup(){
  Serial.begin(115200);

  // Détection du mode de controle avant toute configuration :
  pinMode(switchMode, INPUT_PULLUP);
  wifiMode = digitalRead(switchMode);

  if (!wifiMode){
    Serial.println("Gommettes mode!");
    setupGommettes();
  }
  else{
    Serial.println("Wifi mode!");
    setupWifi();                    // Configuration des pins 
    connectWiFi(WiFi);              // Connexion au wifi
    serverConfig(server, command);  // Configuration du serveur
    server.begin();  
  } 
  
  led.start = millis();   // Permet d'initialiser le compteur pour le clignotement de la led.
}


void loop(){
  if (!wifiMode){
    loop_gommettes();
  }
  else{
    loop_wifi();
  }

}


// Mode Gommettes :
void loop_gommettes(){
  while(true){

  // Vérification de l'initialisation du lecteur :  
  if (!reader.init){
      Serial.println("Initialisation of the reader...");
      Serial.println("Waiting for the button being pressed...");
      while (!reader.init){
          InitReader(reader);
          Serial.print("reader.init = ");
          Serial.println(reader.init);
      }
  }

  // Faire clignoter la led :
  LedBlink(led);

  // Exécuter les commandes :
  if (!command.running){  // Si aucune commande n'est exécutée
    stopMotors();         // Alors on arrete tous les moteurs
    Serial.println("Reading paper!");
    if (Reader(reader,command)){    // Et on lance la lecture de la gommette suivante.
        translate(command.code,command,reader,led,distValues);  // On traduit le code en commande pour affecter lse bonnes valeurs à l'objet 'command'
    }
  }
  else {
      executeCommand(command);  // Sinon on exécute la commande déjà engagée.
  }
  }
}

// Mode sans fil :
void loop_wifi(){
  while(true){
   // 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.counter <= 0)
  {
    Serial.println("stop");
    command.running = false;
    command.stopped = false;
    stopMotors();
    return;
  }

  // Sinon executer la commande en cours
  executeCommand(command); // Met à jour la commande global
  if (command.counter > 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.counter);
  }
  }
}

/**
 * @brief Exécute la commande stockée dans l'objet globalCommand et désincrémente le compteur.
 *
 */
void executeCommand(command_m &globalCommand){
  if(globalCommand.counter>0){
    if (globalCommand.name == "forward"){
      forward();
    }
    else if (globalCommand.name == "backward"){
      backward();
    }
    else if (globalCommand.name == "right"){
      right();
    }
    else if (globalCommand.name == "left"){
      left();
    }
    else if (globalCommand.name == "pump"){
      pump();
    }
    globalCommand.counter--;
  }
  else{
    stopMotors();
    globalCommand.running = false;
  }
}
