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

//estructura de objeto que almacena info de un mensaje, en este caso sera el pin
typedef struct AMessage {
  int interrupcion_pin;
} Objeto;  

//pines
const int infraccion_auto = 2;
const int infraccion_peaton = 3;
const int btn_peaton = 18;
const int interrupcion_auto_b = 19;
const int interrupcion_auto_a = 20;
const int interrupcion_ambulancia = 21;

//tareas
void vehicleNormalTask(void *parameters);
void tareaInterrupcionAutoRojo(void *parameters);
void tareaInterrupcionPeatonRojo(void *parameters);
void tareaInterrupcionPeaton(void *parameters);
void tareaInterrupcionAmbulancia(void *parameters);
void tareaInterrupcionAutoA(void *parameters);
void tareaInterrupcionAutoB(void *parameters);

//semaforo y cola
SemaphoreHandle_t mutex;
QueueHandle_t cola;

void setup() {
  Serial.begin(9600);
  //modo de los pines
  pinMode(infraccion_auto, INPUT_PULLUP);
  pinMode(infraccion_peaton, INPUT_PULLUP);
  pinMode(btn_peaton, INPUT_PULLUP);
  pinMode(interrupcion_auto_b, INPUT_PULLUP);
  pinMode(interrupcion_auto_a, INPUT_PULLUP);
  pinMode(interrupcion_ambulancia, INPUT_PULLUP);

  //inicializacion de la cola y el semaforo
  cola = xQueueCreate(6, sizeof(Objeto));
  mutex = xSemaphoreCreateMutex();

  //creacion de la tarea principal que corre el ciclo del semaforo
  xTaskCreate(vehicleNormalTask, "NormalA", 128, NULL, 1, NULL);

  //declaracion de interrupciones
  if (mutex != NULL) {
    attachInterrupt(digitalPinToInterrupt(infraccion_auto), interrupcionAutoRojo, FALLING);
    attachInterrupt(digitalPinToInterrupt(infraccion_peaton), interrupcionPeatonRojo, FALLING);
    attachInterrupt(digitalPinToInterrupt(btn_peaton), interrupcionPeaton, FALLING);
    attachInterrupt(digitalPinToInterrupt(interrupcion_auto_b), interrupcionAutoB, FALLING);
    attachInterrupt(digitalPinToInterrupt(interrupcion_auto_a), interrupcionAutoA, FALLING);
    attachInterrupt(digitalPinToInterrupt(interrupcion_ambulancia), interrupcionAmbulancia, FALLING);
  }

  //inicializacion del planificador
  vTaskStartScheduler();

}
// -----------------------------------------------------------------------------------------------
// -----------------------------------------------------------------------------------------------
// interrupcion y tarea de si un auto pasa el semaforo en rojo

void interrupcionAutoRojo(){
  //establezco una tarea auxiliar
  TaskHandle_t xHandle = NULL;
  //creo la tarea de la interrupcion y le paso por parametro el auxiliar para eliminarlo al finalizar el ciclo
  xTaskCreate(tareaInterrupcionAutoRojo, "Tarea Interrupcion Auto Rojo", 128, NULL, 1, &xHandle);
  //establezco el pin del objeto
  Objeto obj = { 2 };
  //libero el semaforo desde la ISR
  xSemaphoreGiveFromISR(mutex, NULL);
  //reviso si el semaforo esta ocupado
  if(xSemaphoreTakeFromISR(mutex, NULL) == pdTRUE) {
    //agrego la tarea a la cola
    if(xQueueSendFromISR(cola, &obj, NULL) == pdPASS) Serial.println("Se envio la interrupcion");
    xSemaphoreGiveFromISR(mutex, NULL);
  }
  //elimino la tarea despues de la ejecucion de la tarea
  if( xHandle != NULL ){vTaskDelete( xHandle );}
}

void tareaInterrupcionAutoRojo(void *parameters){
  while(1){
    //tomo el semaforo si esta libre 
    if(xSemaphoreTake(mutex, portMAX_DELAY) == pdTRUE)
    {
      // le digo que suene la bocina
      Serial.println("Cruzo un auto en rojo");
      tone(4, 1000, 4000);
      }
    } 
    //espero 3 seg
    vTaskDelay(3000/portTICK_PERIOD_MS);
    //libero el semaforo
    xSemaphoreGive(mutex);
  }

// -----------------------------------------------------------------------------------------------
// -----------------------------------------------------------------------------------------------

// ******* LAS DEMAS TAREAS E INTERRUPCIONES TIENEN LA MISMA ESTRUCTURA Y LOGICA QUE LAS PRIMERAS ******* 
// ******* LAS DEMAS TAREAS E INTERRUPCIONES TIENEN LA MISMA ESTRUCTURA Y LOGICA QUE LAS PRIMERAS ******* 
// ******* LAS DEMAS TAREAS E INTERRUPCIONES TIENEN LA MISMA ESTRUCTURA Y LOGICA QUE LAS PRIMERAS ******* 

// -----------------------------------------------------------------------------------------------
// -----------------------------------------------------------------------------------------------

void interrupcionPeatonRojo(){
  TaskHandle_t xHandle = NULL;
  xTaskCreate(tareaInterrupcionPeatonRojo, "Tarea Interrupcion Peaton Rojo", 128, NULL, 1,  &xHandle);
  Objeto obj = { 3 };
  xSemaphoreGiveFromISR(mutex, NULL);
  if(xSemaphoreTakeFromISR(mutex, NULL) == pdTRUE) {
    if(xQueueSendFromISR(cola, &obj, NULL) == pdPASS) Serial.println("Se envio la interrupcion");
    xSemaphoreGiveFromISR(mutex, NULL);
  }
  //elimino la tarea despues de la ejecucion de la tarea
  if( xHandle != NULL ){vTaskDelete( xHandle );}
}

void tareaInterrupcionPeatonRojo(void *parameters){
  while(1){
    if(xSemaphoreTake(mutex, portMAX_DELAY) == pdTRUE){
      Serial.println("Cruzo un peaton en rojo");
      tone(4, 1000, 4000);
      }
    } 
    vTaskDelay(5000/portTICK_PERIOD_MS);
    xSemaphoreGive(mutex);
  
}

// -----------------------------------------------------------------------------------------------
// -----------------------------------------------------------------------------------------------

void interrupcionPeaton(){
  TaskHandle_t xHandle = NULL;
  xTaskCreate(tareaInterrupcionPeaton, "Tarea Interrupcion Peaton", 128, NULL, 1, &xHandle );
  Objeto obj = { 18 };
    xSemaphoreGiveFromISR(mutex, NULL);
    if(xSemaphoreTakeFromISR(mutex, NULL) == pdTRUE) {
      if(xQueueSendFromISR(cola, &obj, NULL) == pdPASS) Serial.println("Se envio la interrupcion");
      xSemaphoreGiveFromISR(mutex, NULL);
    }
    
   //elimino la tarea despues de la ejecucion de la tarea
  if( xHandle != NULL ){vTaskDelete( xHandle );}

}

void tareaInterrupcionPeaton(void *parameters){
  while(1){
    if(xSemaphoreTake(mutex, portMAX_DELAY) == pdTRUE)
    {
      Serial.println("PEATON CRUZANDO");
      semaforoA();
    } 
  }
  
  xSemaphoreGive(mutex);
}

// -----------------------------------------------------------------------------------------------
// -----------------------------------------------------------------------------------------------

void interrupcionAutoB(){
  TaskHandle_t xHandle = NULL;
  xTaskCreate(tareaInterrupcionAutoB, "Tarea Interrupcion Auto B", 128, NULL, 1, &xHandle );
  Objeto obj = { 19 };
  xSemaphoreGiveFromISR(mutex, NULL);
  if(xSemaphoreTakeFromISR(mutex, NULL) == pdTRUE) {
    if(xQueueSendFromISR(cola, &obj, NULL) == pdPASS) Serial.println("Se envio la interrupcion");
    xSemaphoreGiveFromISR(mutex, NULL);
  }
  //elimino la tarea despues de la ejecucion de la tarea
  if( xHandle != NULL ){vTaskDelete( xHandle );}
}

void tareaInterrupcionAutoB(void *parameters){
  while(1){
    if(xSemaphoreTake(mutex, portMAX_DELAY) == pdTRUE)
    {
      Serial.println("semaforo b en verde");
      semaforoB();
    }
  }
  vTaskDelay(5000/portTICK_PERIOD_MS);
  xSemaphoreGive(mutex);
}

// -----------------------------------------------------------------------------------------------
// -----------------------------------------------------------------------------------------------

void interrupcionAutoA(){
  TaskHandle_t xHandle = NULL;
  xTaskCreate(tareaInterrupcionAutoA, "Tarea Interrupcion Auto A", 128, NULL, 1, &xHandle);
  Objeto obj = { 20 };
  xSemaphoreGiveFromISR(mutex, NULL);
  if(xSemaphoreTakeFromISR(mutex, NULL) == pdTRUE) {
    if(xQueueSendFromISR(cola, &obj, NULL) == pdPASS) Serial.println("Se envio la interrupcion");
    xSemaphoreGiveFromISR(mutex, NULL);
  }
  //elimino la tarea despues de la ejecucion de la tarea
  if( xHandle != NULL ){vTaskDelete( xHandle );}
}

void tareaInterrupcionAutoA(void *parameters){
  while(1){
    if(xSemaphoreTake(mutex, portMAX_DELAY) == pdTRUE)
    {
        Serial.println("semaforo a en verde");
        semaforoA();
    } 
  }
  vTaskDelay(5000/portTICK_PERIOD_MS);
  xSemaphoreGive(mutex);
}

// -----------------------------------------------------------------------------------------------
// -----------------------------------------------------------------------------------------------

void interrupcionAmbulancia(){
  TaskHandle_t xHandle = NULL;
  xTaskCreate(tareaInterrupcionAmbulancia, "Tarea Interrupcion Ambulancia", 128, NULL, 1, &xHandle);
  Objeto obj = { 21 };
  xSemaphoreGiveFromISR(mutex, NULL);
  if(xSemaphoreTakeFromISR(mutex, NULL) == pdTRUE) {
    if(xQueueSendFromISR(cola, &obj, NULL) == pdPASS) Serial.println("Se envio la interrupcion");
    xSemaphoreGiveFromISR(mutex, NULL);
  }
  //elimino la tarea despues de la ejecucion de la tarea
  if( xHandle != NULL ){vTaskDelete( xHandle );}
}

void tareaInterrupcionAmbulancia(void *parameters){
  while(1){
    if(xSemaphoreTake(mutex, portMAX_DELAY) == pdTRUE)
    {
      Serial.println("Ambulancia cruzando...");
      semaforoB();
    } 
  }
  vTaskDelay(2000/portTICK_PERIOD_MS);
  xSemaphoreGive(mutex);
}

// -----------------------------------------------------------------------------------------------
// -----------------------------------------------------------------------------------------------

//Tarea ciclica del semaforo
void vehicleNormalTask(void *pvParameters) {
  while (true) { 
    Serial.println("calle A");
    semaforoA();
    vTaskDelay(3000/portTICK_PERIOD_MS);
    Serial.println("calle B");
    semaforoB();
    vTaskDelay(3000/portTICK_PERIOD_MS);
  } 
}

//prende y apaga luces semaforo A
void semaforoA(){
  digitalWrite(7, LOW);
  digitalWrite(5, LOW);
  digitalWrite(6, HIGH);
  digitalWrite(8, HIGH);
}
//prende y apaga luces semaforo B
void semaforoB(){
  digitalWrite(8, LOW);
  digitalWrite(6, LOW);
  digitalWrite(7, HIGH);
  digitalWrite(5, HIGH);
}

// -----------------------------------------------------------------------------------------------
// -----------------------------------------------------------------------------------------------
void loop() { }