#include <Arduino.h>
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#include "freertos/semphr.h"


// Definição dos pinos

// Semáforo Norte
#define N_RED     12
#define N_YELLOW  14
#define N_GREEN   27

// Semáforo Sul
#define S_RED     25
#define S_YELLOW  26
#define S_GREEN   33

// Semáforo Leste
#define L_RED     2
#define L_YELLOW  4
#define L_GREEN   5

// Semáforo Oeste
#define O_RED     18
#define O_YELLOW  19
#define O_GREEN   21

// LED azul (Watchdog)
#define LED_AZUL  13


// Estado dos semáforos

enum Cor { VERMELHO, AMARELO, VERDE };
Cor status[4]; // 0=N, 1=S, 2=L, 3=O

SemaphoreHandle_t mutex;


// Atualização dos LEDs

void atualizaLEDs() {
  digitalWrite(N_RED,    status[0] == VERMELHO);
  digitalWrite(N_YELLOW, status[0] == AMARELO);
  digitalWrite(N_GREEN,  status[0] == VERDE);

  digitalWrite(S_RED,    status[1] == VERMELHO);
  digitalWrite(S_YELLOW, status[1] == AMARELO);
  digitalWrite(S_GREEN,  status[1] == VERDE);

  digitalWrite(L_RED,    status[2] == VERMELHO);
  digitalWrite(L_YELLOW, status[2] == AMARELO);
  digitalWrite(L_GREEN,  status[2] == VERDE);

  digitalWrite(O_RED,    status[3] == VERMELHO);
  digitalWrite(O_YELLOW, status[3] == AMARELO);
  digitalWrite(O_GREEN,  status[3] == VERDE);
}


// Tarefa: Controle

void tarefaControle(void *params) {
  while (true) {
    // ===== Fase 1: Norte/Sul livres =====
    xSemaphoreTake(mutex, portMAX_DELAY);
    status[0] = VERDE;
    status[1] = VERDE;
    status[2] = VERMELHO;
    status[3] = VERMELHO;
    xSemaphoreGive(mutex);

    Serial.println(">> Fase 1: NORTE e SUL LIVRES | LESTE e OESTE FECHADAS");
    vTaskDelay(pdMS_TO_TICKS(3000));

    // Amarelo
    xSemaphoreTake(mutex, portMAX_DELAY);
    status[0] = AMARELO;
    status[1] = AMARELO;
    xSemaphoreGive(mutex);
    vTaskDelay(pdMS_TO_TICKS(1000));

    // Vermelho
    xSemaphoreTake(mutex, portMAX_DELAY);
    status[0] = VERMELHO;
    status[1] = VERMELHO;
    xSemaphoreGive(mutex);
    vTaskDelay(pdMS_TO_TICKS(1000));

    // ===== Fase 2: Leste/Oeste livres =====
    xSemaphoreTake(mutex, portMAX_DELAY);
    status[2] = VERDE;
    status[3] = VERDE;
    xSemaphoreGive(mutex);

    Serial.println(">> Fase 2: LESTE e OESTE LIVRES | NORTE e SUL FECHADAS");
    vTaskDelay(pdMS_TO_TICKS(3000));

    // Amarelo
    xSemaphoreTake(mutex, portMAX_DELAY);
    status[2] = AMARELO;
    status[3] = AMARELO;
    xSemaphoreGive(mutex);
    vTaskDelay(pdMS_TO_TICKS(1000));

    // Vermelho
    xSemaphoreTake(mutex, portMAX_DELAY);
    status[2] = VERMELHO;
    status[3] = VERMELHO;
    xSemaphoreGive(mutex);
    vTaskDelay(pdMS_TO_TICKS(1000));
  }
}


// Tarefa: Sinalização

void tarefaSinalizacao(void *params) {
  while (true) {
    xSemaphoreTake(mutex, portMAX_DELAY);
    atualizaLEDs();
    xSemaphoreGive(mutex);
    vTaskDelay(pdMS_TO_TICKS(100));
  }
}


// Tarefa: Watchdog

void tarefaWatchdog(void *params) {
  pinMode(LED_AZUL, OUTPUT);
  while (true) {
    digitalWrite(LED_AZUL, HIGH);
    Serial.println("[Watchdog] Sistema ativo...");
    vTaskDelay(pdMS_TO_TICKS(500));
    digitalWrite(LED_AZUL, LOW);
    vTaskDelay(pdMS_TO_TICKS(500));
  }
}


// Setup do sistema

void setup() {
  Serial.begin(115200);
  Serial.println("Inicializando sistema...");

  int gpios[] = {
    N_RED, N_YELLOW, N_GREEN,
    S_RED, S_YELLOW, S_GREEN,
    L_RED, L_YELLOW, L_GREEN,
    O_RED, O_YELLOW, O_GREEN,
    LED_AZUL
  };

  for (int i = 0; i < sizeof(gpios) / sizeof(int); i++) {
    pinMode(gpios[i], OUTPUT);
    digitalWrite(gpios[i], LOW);
  }

  for (int i = 0; i < 4; i++) {
    status[i] = VERMELHO;
  }

  mutex = xSemaphoreCreateMutex();

  xTaskCreatePinnedToCore(tarefaControle,    "Controle",    2048, NULL, 2, NULL, 1);
  xTaskCreatePinnedToCore(tarefaSinalizacao, "Sinalizacao", 2048, NULL, 1, NULL, 1);
  xTaskCreatePinnedToCore(tarefaWatchdog,    "Watchdog",    1024, NULL, 0, NULL, 0);
}

void loop() {
  // FreeRTOS gerencia todas as tarefas
}