#include <Arduino_FreeRTOS.h>

int led_rojo = 2;      // Red LED
int led_verde = 4;     // Green LED
int emergencia = 8;    // Emergency LED
#define sensor_i A0
#define sensor_s A1
#define boton A2

void setup() {
  Serial.begin(9600);
  
  // Initialize LEDs
  pinMode(led_rojo, OUTPUT);
  pinMode(led_verde, OUTPUT);
  pinMode(emergencia, OUTPUT);
  
  // Initialize sensors and button
  pinMode(sensor_i, INPUT_PULLUP);  // Remove INPUT_PULLUP if unnecessary
  pinMode(sensor_s, INPUT_PULLUP);
  pinMode(boton, INPUT);

  // Create tasks
  xTaskCreate(Task_1, "Sensor_i", 100, NULL, 2, NULL);
  xTaskCreate(Task_2, "Sensor_s", 100, NULL, 2, NULL);
  xTaskCreate(Task_3, "EMERGENCIA", 100, NULL, 3, NULL);
}

static void Task_1(void* pvParameters) {
  while (1) {
    if (digitalRead(sensor_i) == HIGH) {
      digitalWrite(led_verde, HIGH);
      digitalWrite(led_rojo, LOW);
      digitalWrite(emergencia, LOW);
      Serial.println(F("Sensor_i ON!"));
    } else {
      digitalWrite(led_verde, LOW);  // Ensure LED turns off if condition is not met
    }
    vTaskDelay(1000 / portTICK_PERIOD_MS);
  }
}

static void Task_2(void* pvParameters) {
  while (1) {
    if (digitalRead(sensor_s) == HIGH) {
      digitalWrite(led_verde, LOW);
      digitalWrite(led_rojo, HIGH);
      digitalWrite(emergencia, LOW);
      Serial.println(F("Sensor_s ON!"));
    } else {
      digitalWrite(led_rojo, LOW);  // Ensure LED turns off if condition is not met
    }
    vTaskDelay(1100 / portTICK_PERIOD_MS);
  }
}

static void Task_3(void* pvParameters) {
  while (1) {
    if (digitalRead(boton) == HIGH) {
      digitalWrite(led_verde, LOW);
      digitalWrite(led_rojo, LOW);
      digitalWrite(emergencia, HIGH);
      Serial.println(F("EMERGENCIA ON!"));
    } else {
      digitalWrite(emergencia, LOW);  // Ensure emergency LED turns off if condition is not met
    }
    vTaskDelay(1200 / portTICK_PERIOD_MS);
  }
}

void loop() {
  // Loop remains empty as tasks handle everything
}