#include <Arduino.h>

const int green1Pin = 17;   // Green light for direction 1
const int yellow1Pin = 16;  // Yellow light for direction 1
const int red1Pin = 4;      // Red light for direction 1

const int green2Pin = 18;   // Green light for direction 2
const int yellow2Pin = 19;  // Yellow light for direction 2
const int red2Pin = 21;     // Red light for direction 2

const int green3Pin = 22;   // Green light for direction 3
const int yellow3Pin = 23;  // Yellow light for direction 3
const int red3Pin = 5;      // Red light for direction 3

const int green4Pin = 25;   // Green light for direction 4
const int yellow4Pin = 26;  // Yellow light for direction 4
const int red4Pin = 27;     // Red light for direction 4

const TickType_t greenDelay = 3000 / portTICK_PERIOD_MS;
const TickType_t yellowDelay = 1000 / portTICK_PERIOD_MS;
const TickType_t updateLedDelay = 200 / portTICK_PERIOD_MS;

unsigned long startLedTime, endLedTime, startControlTime, endControlTime;  // Variáveis para medir tempos de execução

const int numDirections = 4; // Número de direções

// Array de handles globais para facilitar a criação e manipulação das tasks
TaskHandle_t taskHandles[numDirections];

// Definindo uma flag global para controlar o início da execução
volatile bool systemReady = false;

// 0-2: Direction 1, 3-5: Direction 2, 6-8: Direction 3, 9-11: Direction 4
int status[12];  

SemaphoreHandle_t xMutex;

void setupLEDs() {
  // Atualiza os LEDs da direção 1
  digitalWrite(green1Pin, status[0]);
  digitalWrite(yellow1Pin, status[1]);
  digitalWrite(red1Pin, status[2]);

  // Atualiza os LEDs da direção 2
  digitalWrite(green2Pin, status[3]);
  digitalWrite(yellow2Pin, status[4]);
  digitalWrite(red2Pin, status[5]);

  // Atualiza os LEDs da direção 3
  digitalWrite(green3Pin, status[6]);
  digitalWrite(yellow3Pin, status[7]);
  digitalWrite(red3Pin, status[8]);

  // Atualiza os LEDs da direção 4
  digitalWrite(green4Pin, status[9]);
  digitalWrite(yellow4Pin, status[10]);
  digitalWrite(red4Pin, status[11]);

  Serial.println("Todos os LEDs foram atualizados no setup.");
}

void createUpdateLEDTasks() {
  
  for (int i = 0; i < numDirections; i++) {
    // Cria uma tarefa para cada direção, passando 'i' como argumento
    if (xTaskCreate(updateLEDs, 
                    "Update LEDs", 
                    configMINIMAL_STACK_SIZE * 2, 
                    (void *)i,   // Passa 'i' como argumento
                    1,           // Prioridade
                    &taskHandles[i]  // Atribui o handle correspondente
                    ) != pdPASS) {
      Serial.print("Failed to create Update LEDs Direction ");
      Serial.println(i + 1);
    } else {
      Serial.print("Update LEDs Direction ");
      Serial.print(i + 1);
      Serial.println(" task created.");
    }
  }
}

void updateLEDs(void *pvParameters) {
  int direction = (int)pvParameters;  // Recebe o valor de 'i' passado na criação da tarefa

  // Define os pinos para cada direção
  int greenPin, yellowPin, redPin;
  switch (direction) {
      case 0:
          greenPin = green1Pin;
          yellowPin = yellow1Pin;
          redPin = red1Pin;
          break;
      case 1:
          greenPin = green2Pin;
          yellowPin = yellow2Pin;
          redPin = red2Pin;
          break;
      case 2:
          greenPin = green3Pin;
          yellowPin = yellow3Pin;
          redPin = red3Pin;
          break;
      case 3:
          greenPin = green4Pin;
          yellowPin = yellow4Pin;
          redPin = red4Pin;
          break;
      default:
          vTaskDelete(NULL);
          return;
  }

  // Aguarda até que todas as tasks estejam prontas
  while (!systemReady) {
      vTaskDelay(pdMS_TO_TICKS(100));  // Espera ativa até a flag ser liberada
  }

  while (true) {
    
    // Medição do tempo de execução da task
    startLedTime = millis();

    // Toma o mutex antes de acessar o vetor de status
    if (xSemaphoreTake(xMutex, portMAX_DELAY)) {
        // Atualiza os LEDs com base nos valores no vetor status
        digitalWrite(greenPin, status[direction * 3 + 0]);  // Verde
        digitalWrite(yellowPin, status[direction * 3 + 1]); // Amarelo
        digitalWrite(redPin, status[direction * 3 + 2]);    // Vermelho

        // Libera o mutex após a atualização dos LEDs
        xSemaphoreGive(xMutex);
    }

    // Finaliza a medição do tempo de execução da task
    endLedTime = millis();
    Serial.print("Atualização do Semáforo ");
    Serial.print(direction + 1);
    Serial.print(": ");
    Serial.print(endLedTime - startLedTime);
    Serial.println(" ms");

    // Atraso para o próximo ciclo de atualização
    vTaskDelay(updateLedDelay);
  }
}

void controlDirection(void *pvParameters) {
    while (!systemReady) {
        vTaskDelay(pdMS_TO_TICKS(100));
    }

    while (true) {
        for (int i = 0; i < 4; i++) {
            int baseIndex = i * 3;

            // Medição do tempo de execução
            startControlTime = millis();

            vTaskPrioritySet(taskHandles[i], 3);

            // Estado verde
            if (xSemaphoreTake(xMutex, portMAX_DELAY)) {
                status[baseIndex] = HIGH;
                status[baseIndex + 1] = LOW;
                status[baseIndex + 2] = LOW;
                xSemaphoreGive(xMutex);
            }
            vTaskDelay(greenDelay);

            // Estado amarelo
            if (xSemaphoreTake(xMutex, portMAX_DELAY)) {
                status[baseIndex] = LOW;
                status[baseIndex + 1] = HIGH;
                status[baseIndex + 2] = LOW;
                xSemaphoreGive(xMutex);
            }
            vTaskDelay(yellowDelay);

            // Estado vermelho
            if (xSemaphoreTake(xMutex, portMAX_DELAY)) {
                status[baseIndex] = LOW;
                status[baseIndex + 1] = LOW;
                status[baseIndex + 2] = HIGH;
                xSemaphoreGive(xMutex);
            }
            vTaskDelay(500 / portTICK_PERIOD_MS);

            vTaskPrioritySet(taskHandles[i], 1);

            // Finaliza a medição do tempo de execução
            endControlTime = millis();
            Serial.print("Tempo de execução do ciclo do semáforo ");
            Serial.print(i+1);
            Serial.print(": ");
            Serial.print(endControlTime - startControlTime);
            Serial.println(" ms");
        }
    }
}

void setup() {
  Serial.begin(115200);  // Initialize serial communication
  
  // Initialize the pin modes for LEDs
  pinMode(green1Pin, OUTPUT);
  pinMode(yellow1Pin, OUTPUT);
  pinMode(red1Pin, OUTPUT);

  pinMode(green2Pin, OUTPUT);
  pinMode(yellow2Pin, OUTPUT);
  pinMode(red2Pin, OUTPUT);

  pinMode(green3Pin, OUTPUT);
  pinMode(yellow3Pin, OUTPUT);
  pinMode(red3Pin, OUTPUT);

  pinMode(green4Pin, OUTPUT);
  pinMode(yellow4Pin, OUTPUT);
  pinMode(red4Pin, OUTPUT);

  // Initialize all LEDs as OFF, except red LEDs which are set to HIGH
  for (int i = 0; i < 12; i++) {
    if ((i + 1) % 3 == 0) {  // Indexes 2, 5, 8, and 11 correspond to red LEDs
      status[i] = HIGH;  // Red LEDs on
    } else {
      status[i] = LOW;   // Green and yellow LEDs off
    }
  }

  // Atualiza todos os LEDs no estado inicial
  setupLEDs();

  // Create the mutex
  xMutex = xSemaphoreCreateMutex();
  if (xMutex == NULL) {
    Serial.println("Failed to create mutex.");
    return;
  }
  Serial.println("Mutex created.");

  // Criação da task de controlar os LEDs
  if (xTaskCreate(controlDirection, "Control Direction", configMINIMAL_STACK_SIZE * 2, NULL, 2, NULL) != pdPASS) {
    Serial.println("Failed to create Control Direction task");
  } else {
    Serial.println("Control Direction task created.");
  }

  // Criação das tasks de atualizar LEDs
  createUpdateLEDTasks();

  systemReady = true;  // Liberando a flag após a criação de todas as tasks

}

void loop() {
  // Empty, as we are using FreeRTOS
}