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

// Pines para LEDs de semáforos (simulación)
#define LED_VERDE_C1 2
#define LED_AMARILLO_C1 3
#define LED_ROJO_C1 4
#define LED_VERDE_C2 5
#define LED_AMARILLO_C2 6
#define LED_ROJO_C2 7

// Pines para sensores
#define SENSOR_PEATON_C1 8
#define SENSOR_PEATON_C2 9
#define SENSOR_PRIORIDAD_C1 10
#define SENSOR_PRIORIDAD_C2 11
#define SENSOR_CRUCE_ROJO_C1 12
#define SENSOR_CRUCE_ROJO_C2 13

// Sirena de alerta
#define SIRENA 14

// Variables de estado para interrupciones
volatile bool vehiculo_prioridad_c1 = false;
volatile bool vehiculo_prioridad_c2 = false;
volatile bool peaton_c1 = false;
volatile bool peaton_c2 = false;
volatile bool cruce_rojo_c1 = false;
volatile bool cruce_rojo_c2 = false;

void setup()
{
  // Configuración de pines
  pinMode(LED_VERDE_C1, OUTPUT);
  pinMode(LED_AMARILLO_C1, OUTPUT);
  pinMode(LED_ROJO_C1, OUTPUT);
  pinMode(LED_VERDE_C2, OUTPUT);
  pinMode(LED_AMARILLO_C2, OUTPUT);
  pinMode(LED_ROJO_C2, OUTPUT);
  pinMode(SIRENA, OUTPUT);

  pinMode(SENSOR_PEATON_C1, INPUT);
  pinMode(SENSOR_PEATON_C2, INPUT);
  pinMode(SENSOR_PRIORIDAD_C1, INPUT);
  pinMode(SENSOR_PRIORIDAD_C2, INPUT);
  pinMode(SENSOR_CRUCE_ROJO_C1, INPUT);
  pinMode(SENSOR_CRUCE_ROJO_C2, INPUT);

  // Configuración de interrupciones
  attachInterrupt(digitalPinToInterrupt(SENSOR_PRIORIDAD_C1), ISR_VehiculoPrioridadC1, RISING);
  attachInterrupt(digitalPinToInterrupt(SENSOR_PRIORIDAD_C2), ISR_VehiculoPrioridadC2, RISING);
  attachInterrupt(digitalPinToInterrupt(SENSOR_PEATON_C1), ISR_PeatonC1, RISING);
  attachInterrupt(digitalPinToInterrupt(SENSOR_PEATON_C2), ISR_PeatonC2, RISING);
  attachInterrupt(digitalPinToInterrupt(SENSOR_CRUCE_ROJO_C1), ISR_CruceRojoC1, RISING);
  attachInterrupt(digitalPinToInterrupt(SENSOR_CRUCE_ROJO_C2), ISR_CruceRojoC2, RISING);

  // Crear la tarea cíclica para la rotación de semáforos
  xTaskCreate(TaskRotacionSemaforos, "RotacionSemaforos", 1000, NULL, 1, NULL);

  Serial.begin(9600);
  vTaskStartScheduler(); // Iniciar el scheduler de FreeRTOS
}

// Funciones de interrupción
void ISR_VehiculoPrioridadC1()
{
  vehiculo_prioridad_c1 = true;
}

void ISR_VehiculoPrioridadC2()
{
  vehiculo_prioridad_c2 = true;
}

void ISR_PeatonC1()
{
  peaton_c1 = true;
}

void ISR_PeatonC2()
{
  peaton_c2 = true;
}

void ISR_CruceRojoC1()
{
  cruce_rojo_c1 = true;
}

void ISR_CruceRojoC2()
{
  cruce_rojo_c2 = true;
}

// Tarea para manejar la rotación de semáforos
void TaskRotacionSemaforos(void *pvParameters)
{
  while (1)
  {
    // Semáforo Calle 1: Verde, Amarillo, Rojo
    digitalWrite(LED_VERDE_C1, HIGH);
    vTaskDelay(pdMS_TO_TICKS(5000));
    digitalWrite(LED_VERDE_C1, LOW);
    digitalWrite(LED_AMARILLO_C1, HIGH);
    vTaskDelay(pdMS_TO_TICKS(2000));
    digitalWrite(LED_AMARILLO_C1, LOW);
    digitalWrite(LED_ROJO_C1, HIGH);

    // Semáforo Calle 2: Rojo, Verde, Amarillo
    digitalWrite(LED_VERDE_C2, HIGH);
    digitalWrite(LED_ROJO_C1, LOW);
    vTaskDelay(pdMS_TO_TICKS(5000));
    digitalWrite(LED_VERDE_C2, LOW);
    digitalWrite(LED_AMARILLO_C2, HIGH);
    vTaskDelay(pdMS_TO_TICKS(2000));
    digitalWrite(LED_AMARILLO_C2, LOW);
    digitalWrite(LED_ROJO_C2, HIGH);

    // Verificar estado de las interrupciones para manejar condiciones especiales
    if (vehiculo_prioridad_c1)
    {
      activarPrioridadCalle1();
      vehiculo_prioridad_c1 = false;
    }
    if (vehiculo_prioridad_c2)
    {
      activarPrioridadCalle2();
      vehiculo_prioridad_c2 = false;
    }
    if (peaton_c1 || peaton_c2)
    {
      activarAlertaPeaton();
      peaton_c1 = false;
      peaton_c2 = false;
    }
    if (cruce_rojo_c1 || cruce_rojo_c2)
    {
      activarAlertaCruceRojo();
      cruce_rojo_c1 = false;
      cruce_rojo_c2 = false;
    }

    // Tiempo de espera antes de la siguiente rotación
    vTaskDelay(pdMS_TO_TICKS(1000));
  }
}

// Funciones para condiciones de interrupción
void activarPrioridadCalle1()
{
  Serial.println("Prioridad para vehículo en Calle 1");
  // Lógica para manejar prioridad en Calle 1
}

void activarPrioridadCalle2()
{
  Serial.println("Prioridad para vehículo en Calle 2");
  // Lógica para manejar prioridad en Calle 2
}

void activarAlertaPeaton()
{
  Serial.println("Alerta: Peatón en cruce con semáforo en verde");
  digitalWrite(SIRENA, HIGH);
  vTaskDelay(pdMS_TO_TICKS(2000));
  digitalWrite(SIRENA, LOW);
}

void activarAlertaCruceRojo()
{
  Serial.println("Alerta: Vehículo cruzando en rojo");
  digitalWrite(SIRENA, HIGH);
  vTaskDelay(pdMS_TO_TICKS(2000));
  digitalWrite(SIRENA, LOW);
}

void loop()
{
  // El loop se deja vacío ya que FreeRTOS maneja la ejecución en tareas
}