/***************************
 Configuration des commandes
 ***************************/


struct command_m
{
    bool running;
    String name;
    String code;
    int counter;
    bool stopped;
};

struct led_b
{
    bool light_on;
    float start;
};

struct command_r
{
    bool init;
    bool newLine;
    int actualClock;
    int previousClock;
    int counter;
};

struct distances
{
  bool straight;
  float length;
  float initialLength;
};

/**
 * @brief Change les valeurs de l'objet command selon les nouvelles valeurs
 *
 */
void setGlobals(String commandName, float commandValue, command_m &globalCommand)
{
    // Assert command before updating globals
    if (commandName != "forward" && commandName != "backward" && commandName != "left" && commandName != "right" && commandName != "pump")
    {
        return;
    }

    // Update globals
    globalCommand.name = commandName;
    globalCommand.running = true;
    if (commandName == "forward" || commandName == "backward")
    {
        globalCommand.counter = convertLengthToSteps(commandValue);
    }
    else if (commandName == "left" || commandName == "right")
    {
        globalCommand.counter = convertRotToSteps(commandValue);
    }
    else if (commandName == "pump")
    {
        globalCommand.counter = StepsPump;
    }
}

/**
 * @brief Traduit le code sur 4 bits en nom de commande et affecte la commande avec SetGlobals.
 * 
 */
void translate(String code, command_m &globalCommand, command_r &reader, led_b &led, distances &distValues){
    if (code == "0110"){
        Serial.println("Forward!");
        setGlobals("forward",distValues.length,globalCommand);
    }
    else if (code == "1100"){
        setGlobals("right",45,globalCommand);
        distValues.straight = !distValues.straight;
        if (distValues.straight){
          distValues.length = distValues.initialLength;
        }
        else {
          distValues.length = 1.414*distValues.initialLength;
        }
    }
    else if (code == "1101"){
        Serial.println("pump!");
        setGlobals("pump",0,globalCommand);
    }
    else if (code == "1001"){
        setGlobals("backward",distValues.length,globalCommand);
    }
    else if (code == "0011"){
        setGlobals("left",45,globalCommand);
        distValues.straight = !distValues.straight;
        if (distValues.straight){
          distValues.length = distValues.initialLength;
        }
        else {
          distValues.length = 1.414*distValues.initialLength;
        }
    }
    else if (code == "0101"){
        setGlobals("left",90,globalCommand);
    }
    else if (code == "1010"){
        setGlobals("right",90,globalCommand);
    }
    else if (code == "0000"){
        stopMotors();
        for (int i=0; i<20; i++){
            digitalWrite(IN3p, HIGH);
            delay(20);
            digitalWrite(IN3p,LOW);
            delay(20);
        }
        led.light_on = false;
        for (int i=0; i<1100; i++){
            forwardReader();
        }
        globalCommand.running = false;
        reader.init = false;
        stopMotors();
        globalCommand.code = "1111";
        distValues.straight = true;
        distValues.length = distValues.initialLength;
    }
    else {
        globalCommand.running = false;
    }
}

void LedBlink(led_b &led){
    if (millis()-led.start > period){
        if (led.light_on){
            digitalWrite(IN3p,LOW);
            led.light_on = false;
        }
        else {
            digitalWrite(IN3p,HIGH);
            led.light_on = true;
        }
        led.start = millis();
    }
}

void InitReader(command_r &reader){
    while (digitalRead(button)==HIGH){}
    Serial.println("Button pressed!");
    reader.init = true;
    while(digitalRead(clock)==0){
        forwardReader();
    }
    Serial.println("Reader initialised!");
  
}

bool Reader(command_r &reader, command_m &globalCommand){
    reader.actualClock = int(digitalRead(clock));
    if (reader.newLine){
        if (reader.actualClock-reader.previousClock>=0){ //Check if the new value is lower than the previous (falling edge) for the clock area
            forwardReader();
            reader.previousClock=reader.actualClock;
            reader.actualClock=int(digitalRead(clock));
            return false;
        }
        else if (reader.actualClock == 0){
            Serial.println("Clock detected!"); // If a clock is detected, continue to forward during "stepsReader" steps (to be more or less in the middle of the sticker)
            if (reader.counter > 0){
                forwardReader();
                reader.counter--;
                return false;
            }
            else {
                reader.counter = StepsReader;
                globalCommand.code = String(digitalRead(bit1)) + String(digitalRead(bit2)) + String(digitalRead(bit3)) + String(digitalRead(bit4)); //Update the value of the command flag
                Serial.print("Code is: ");
                Serial.println(globalCommand.code);
                stopMotors();
                reader.newLine = false;
                return true;
            }

        }
    }
    else {
        if (reader.counter > 0){
            forwardReader();
            reader.counter--;
            return false;
        }
        else{
            reader.newLine = true;
            reader.counter = StepsReader;
            return false;
        }

    }
    return false;
}


/**
 * @brief Lorsqu'une commande arrive au serveur, cette fonction permet.
 * @internal
 *
 * @param commandName Nom de la commande.
 * @param commandValue Valeur de la command (si 0 assigné à 474).
 */
void requestCheck(String commandName, float commandValue, command_m &globalCommand){
    if (commandName == "stop")
    {
        globalCommand.name = "stop";
        globalCommand.running = true;
        globalCommand.stopped = true;
    }
    else
    {
        setGlobals(commandName, commandValue, globalCommand);
    }

    if (commandValue == 0)
    {
        globalCommand.counter = 474;
    }
  }


void parseRequestParams(AsyncWebServerRequest *request, String &commandName, float &commandValue, command_m &globalCommand)
{
    // 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, globalCommand);
}

/**
 * @brief Configuration du serveur asynchrone.
 *
 * @param server Serveur à configurer.
 * @param globalCommand Commande globale (courante) à mettre à jour lors des requêtes.
 */
void serverConfig(AsyncWebServer &server, command_m &globalCommand)
{
    server.on("/get", HTTP_GET, [&globalCommand](AsyncWebServerRequest *request)
              {
    String commandName;
    float commandValue;

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

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


