/* ESP32 WiFi Scanning example */

#include "WiFi.h"
#include <mutex>

TaskHandle_t Task1;
TaskHandle_t Task2;

std::mutex command_mtx;
String currentCommand = "";
bool runMotor=false;

void setup() {
  Serial.begin(115200);
  Serial.println("Initializing WiFi...");
  WiFi.mode(WIFI_STA);

  // tasking
  xTaskCreatePinnedToCore(
                    readSerial,   /* Task function. */
                    "Task1",     /* name of task. */
                    10000,       /* Stack size of task */
                    NULL,        /* parameter of the task */
                    1,           /* priority of the task */
                    &Task1,      /* Task handle to keep track of created task */
                    0);          /* pin task to core 0 */                  

  //create a task that will be executed in the Task2code() function, with priority 1 and executed on core 1
  xTaskCreatePinnedToCore(
                    stateMachine,   /* Task function. */
                    "Task2",     /* name of task. */
                    10000,       /* Stack size of task */
                    NULL,        /* parameter of the task */
                    1,           /* priority of the task */
                    &Task2,      /* Task handle to keep track of created task */
                    1);          /* pin task to core 1 */
  

  pinMode(33, OUTPUT);
  Serial.println("Setup done!");
}

void readSerial(void * pvParameters) {
  char incomingByte = 0; // for incoming serial data
  int nbChar = 0;
  char buff[256] = "";
  Serial.print("readSerial : ");
  Serial.println(xPortGetCoreID());

  for(;;) {
    if (Serial.available() > 0) {
        // read the incoming byte:
        incomingByte = Serial.read();

        if((int)incomingByte == 10) { // LF
          std::lock_guard<std::mutex> lck(command_mtx);
          buff[nbChar] = '\0';
          currentCommand = String(buff);
          nbChar = 0;
          Serial.println("Command end");
          Serial.println(currentCommand);
        }
        else {
          buff[nbChar++] = incomingByte;
          // say what you got:
          Serial.print("I received: ");
          Serial.println(char(incomingByte));
        }
    }
  }
}

void stateMachine(void * pvParameters) {
  Serial.print("stateMachine : ");
  Serial.println(xPortGetCoreID());
  for(;;) {
    if(currentCommand != "") {
      std::lock_guard<std::mutex> lck(command_mtx);
      Serial.print("New command received : ");
      Serial.println(currentCommand);
      if(currentCommand == "ON") {
        runMotor = true;
      }
      else if(currentCommand == "OFF") {
        runMotor = false;
      }
      currentCommand = "";
    }
  }
}

void loop() {
  //stepper 
  if(runMotor) {
    digitalWrite(33, HIGH);
    delay(5);
    digitalWrite(33, LOW);
    delay(10);
  }
}