#include <Arduino_FreeRTOS.h>
#include <semphr.h>

#define YellowLEDPin 8
#define BlueLEDPin 7
#define Button1 2
#define Button2 3

SemaphoreHandle_t xSemaphoreLEDControl;
TaskHandle_t yellowLEDTaskHandle, blueLEDTaskHandle;

const char sequence[] = "YB1E";
int sequenceIndex = 0;
bool sequenceRunning = false;
char sequences[10];

void turnOffAllLEDs()
{
  digitalWrite(YellowLEDPin, LOW);
  digitalWrite(BlueLEDPin, LOW);
}

char *checkSequences(int sequenceIndex){
  while(1){
    //Serial.print("Current char: ");
    //Serial.println(sequence[sequenceIndex]);

    if (sequence[sequenceIndex] >= '0' && sequence[sequenceIndex] <= '9'){
      sequences[sequenceIndex] = sequence[sequenceIndex];
    } else {
      sequences[sequenceIndex] = sequence[sequenceIndex];
    }

      // Verificar se o array contém algum número
    int posicaoNumero = -1;  // Inicializa com -1, indicando que não foi encontrado
    for (int i = 0; sequences[i] >= strlen(sequences); i++) { ///0 é terminação nula 
      if (sequences[i] >= '0' && sequences[i] <= '9') {
        //posicaoNumero = i;
        break;
      }
    }
      sequenceIndex++;

      // Verifica se algum número foi encontrado e mostra sua posição
      if (posicaoNumero != -1) {
        //Serial.print("O array contem um numero na posicao ");
        //Serial.println(posicaoNumero);
        break;
      } else {
        //Serial.println("O array NAO contem numeros.");
      }
      delay(10); // Pequeno atraso para evitar loop muito rápido
    }
    return sequences;
}

void generalTaskController()
{
  TickType_t xLastWakeTime = xTaskGetTickCount();
  bool foundYellowLED = false;
  bool foundBlueLED = false;

  while (1)
  {
    //Serial.print("Current char: ");
    //Serial.println(sequence[sequenceIndex]);

    char *sequencesResult = checkSequences(sequenceIndex); // 2 , 0 e 1 estão com letra

    if(sequencesResult[sequenceIndex] == 'Y'){
      //Serial.println("Creating Yellow LED Task...");
      xTaskCreate(taskYellowLED, "Yellow LED Task", 128, NULL, 3, &yellowLEDTaskHandle);
      foundYellowLED = true;
      sequenceIndex++;
    } 
    
    if (sequencesResult[sequenceIndex] == 'B')
    {
      //Serial.println("Creating Blue LED Task...");
      xTaskCreate(taskBlueLED, "Blue LED Task", 128, NULL, 3, &blueLEDTaskHandle);
      foundBlueLED = true;
      sequenceIndex++;
    }

    if (sequencesResult[sequenceIndex] >= '0' && sequencesResult[sequenceIndex] <= '9')
    {
      if (xSemaphoreTake(xSemaphoreLEDControl, pdMS_TO_TICKS(10)) == pdTRUE)
      {
        int delayTime = (sequence[sequenceIndex] - '0') * 1000;
        vTaskDelay(pdMS_TO_TICKS(delayTime));
        xSemaphoreGive(xSemaphoreLEDControl);

        if (foundYellowLED)
        {
          //Serial.println("Turning off Yellow LED.");
          digitalWrite(YellowLEDPin, LOW);
          foundYellowLED = false;
        }
        if (foundBlueLED)
        {
          //Serial.println("Turning off Blue LED.");
          digitalWrite(BlueLEDPin, LOW);
          foundBlueLED = false;
        }
        sequenceIndex++;
      }
    }

    if (sequence[sequenceIndex] == 'E')
    {
      turnOffAllLEDs();
      sequenceIndex = 0;
      foundYellowLED = false;
      foundBlueLED = false;
    }
  }
}

void taskYellowLED(void *pvParameters)
{
  //Serial.println("Yellow LED should turn on.");
  digitalWrite(YellowLEDPin, HIGH);
  vTaskDelay(pdMS_TO_TICKS(100));
}

void taskBlueLED(void *pvParameters)
{
  //Serial.println("Blue LED should turn on.");
  digitalWrite(BlueLEDPin, HIGH);
  vTaskDelay(pdMS_TO_TICKS(100));
}

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

  pinMode(YellowLEDPin, OUTPUT);
  pinMode(BlueLEDPin, OUTPUT);

  turnOffAllLEDs();

  pinMode(Button1, INPUT_PULLUP);
  pinMode(Button2, INPUT_PULLUP);

  xSemaphoreLEDControl = xSemaphoreCreateBinary();
  if (xSemaphoreLEDControl != NULL)
  {
    xSemaphoreGive(xSemaphoreLEDControl);
  }
  Serial.print("Current checksequences: ");
  char *sequencesResult = checkSequences(sequenceIndex);
  Serial.println(sequencesResult);
  Serial.println(sequencesResult[0]);
  generalTaskController();
  xTaskCreate(taskButtonHandler, "Button Handler Task", 128, NULL, 1, NULL);
}

void loop() {}

void taskButtonHandler(void *pvParameters)
{
  while (1)
  {
    if (digitalRead(Button1) == LOW)
    {
      if (!sequenceRunning)
      {
        sequenceIndex = 0;
        sequenceRunning = true;
        vTaskResume(yellowLEDTaskHandle);
        vTaskResume(blueLEDTaskHandle);
      }
      else
      {
        vTaskSuspend(yellowLEDTaskHandle);
        vTaskSuspend(blueLEDTaskHandle);
        sequenceRunning = false;
      }

      delay(500); // Debounce
      while (digitalRead(Button1) == LOW)
      {
        vTaskDelay(pdMS_TO_TICKS(10));
      }
    }

    if (digitalRead(Button2) == LOW)
    {
      if (xSemaphoreTake(xSemaphoreLEDControl, pdMS_TO_TICKS(10)) == pdTRUE)
      {
        Serial.println("Receiving sequence over serial...");
        // Processar sequência aqui, se necessário
        xSemaphoreGive(xSemaphoreLEDControl);
      }
      delay(500); // Debounce
      while (digitalRead(Button2) == LOW)
      {
        vTaskDelay(pdMS_TO_TICKS(10));
      }
    }

    vTaskDelay(pdMS_TO_TICKS(10));
  }
}