#include <DHTesp.h>
#include <freertos/FreeRTOS.h>
#include <freertos/task.h>
#include <freertos/queue.h>
#include <freertos/semphr.h>
#include <Adafruit_SSD1306.h>

#define DHT_PIN 15 // Pin where the DHT22 sensor is connected
#define LDR_PIN 12 // Pin where the LDR sensor is connected
#define BUTTON_PIN 14  // Pin where the button is connected
#define SYSTEM_ON_LED_PIN 27  // Pin for system on LED
#define CALDERA_ON_LED_PIN 2  // Pin for boiler on LED
#define CALDERA_OFF_LED_PIN 4  // Pin for boiler off LED
#define POT_PIN 36 // Pin where the potentiometer is connected

DHTesp dhtSensor;  // Object for the DHT22 sensor

#define SCREEN_WIDTH 128 // OLED display width, in pixels
#define SCREEN_HEIGHT 64 // OLED display height, in pixels
#define OLED_RESET     -1 // Reset pin # (or -1 if sharing Arduino reset pin)
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);

volatile bool systemState = false;  // System state On/Off
volatile unsigned long lastDebounceTime = 0;  // Time of the last button press
const unsigned long debounceDelay = 200;  // Debounce delay time

// Declaration of queues and semaphores
QueueHandle_t qTemperature;  // Queue for sending temperatures
SemaphoreHandle_t buttonSemaphore;  // Semaphore for the button
SemaphoreHandle_t displayMutex;  // Mutex for display access

// Manejadores de tareas
TaskHandle_t readSensorsTaskHandle = NULL;
TaskHandle_t controlBoilerTaskHandle = NULL;
TaskHandle_t updateDisplayTaskHandle = NULL;

// Structure to hold temperatures
struct TemperatureData {
  float sensorTemperature;
  float potTemperature;
};

/**
 * @brief Interrupt Service Routine (ISR) for handling button press.
 *
 * This function is triggered by a falling edge on the button pin. It debounces the button press
 * and gives the button semaphore from ISR context.
 */
void IRAM_ATTR handleButtonPress() {
  unsigned long currentTime = millis();
  if ((currentTime - lastDebounceTime) > debounceDelay) { // Control of debouncing in button
    lastDebounceTime = currentTime;
    xSemaphoreGiveFromISR(buttonSemaphore, NULL);
  }
}

/**
 * @brief Main application entry point.
 *
 * Initializes the serial communication, DHT sensor, OLED display, and pin configurations.
 * Sets up the button interrupt for system on/off control and initializes semaphores and queues.
 * Creates FreeRTOS tasks for reading sensors, controlling the boiler, updating the display,
 * and checking the light sensor.
 */
void app_main(){
  Serial.begin(115200);
  dhtSensor.setup(DHT_PIN, DHTesp::DHT22);
  // Pin configuration
  pinMode(BUTTON_PIN, INPUT_PULLUP);
  pinMode(LDR_PIN, INPUT_PULLUP);
  pinMode(SYSTEM_ON_LED_PIN, OUTPUT);
  pinMode(CALDERA_ON_LED_PIN, OUTPUT);
  pinMode(CALDERA_OFF_LED_PIN, OUTPUT);
  pinMode(POT_PIN, INPUT); // Configure pin for potentiometer
  // Initialize LEDs off
  digitalWrite(SYSTEM_ON_LED_PIN, LOW);
  digitalWrite(CALDERA_ON_LED_PIN, LOW);
  digitalWrite(CALDERA_OFF_LED_PIN, LOW);
  if(!display.begin(SSD1306_SWITCHCAPVCC, 0x3C)) {
    Serial.println(F("SSD1306 allocation failed\n"));
    for(;;); // Don't proceed, loop forever
  }

  // Configure button interrupt to detect falling edge to turn the system on and off
  attachInterrupt(digitalPinToInterrupt(BUTTON_PIN), handleButtonPress, FALLING);

  // Initialize semaphores and queues
  buttonSemaphore = xSemaphoreCreateBinary();
  displayMutex = xSemaphoreCreateMutex();
  qTemperature = xQueueCreate(10, sizeof(TemperatureData));

  // Create tasks
  xTaskCreate(readSensorsTask, "Read Sensor", 2048, NULL, 1, &readSensorsTaskHandle);
  xTaskCreate(updateDisplayTask, "Update Display", 2048, NULL, 1, &updateDisplayTaskHandle);
  xTaskCreate(controlBoilerLedsTask, "Handle Boiler", 2048, NULL, 1, &controlBoilerTaskHandle);
  xTaskCreate(handleButtonTask, "Handle Button", 2048, NULL, 1, NULL); // Task to handle button presses
  xTaskCreatePinnedToCore(checkLightLDRTask, "Check Light", 2048, NULL, 1, NULL, 1); // Task to read the light sensor (core 1). No handler is used as it will not be suspended

  // If everything has started correctly, the welcome message is displayed
  welcomeMsg();
}

/* ------------------------------------------------- AUX FUNCTIONS --------------------------------------------------- */

/**
 * @brief Displays a welcome message with an animation effect.
 *
 * This function clears the display, sets the text size and color, and then
 * displays the welcome message "## Welcome ##" letter by letter with a delay
 * between each letter to create an animation effect.
 */
void welcomeMsg(){
  // Animation of welcome text
  xSemaphoreTake(displayMutex, portMAX_DELAY); // Mutex for exclusive access
  display.clearDisplay();
  display.setTextSize(2);
  display.setTextColor(WHITE);
  display.setCursor(10, 10);
  String welcomeText = "#Welcome#";
  for (int i = 0; i < welcomeText.length(); i++) {
    display.print(welcomeText[i]); // show text letter by letter
    display.display();
    delay(100);
  }
  xSemaphoreGive(displayMutex); // Release the mutex
}

/**
 * @brief Reads the temperature from the DHT22 sensor.
 *
 * @return float The current temperature in degrees Celsius.
 */
float getTemp(void){
  TempAndHumidity data = dhtSensor.getTempAndHumidity();
  return data.temperature;
}

/**
 * @brief Reads the value from the potentiometer.
 *
 * If the system is on, it reads the potentiometer value and maps it to a temperature range
 * between 18ºC and 30ºC.
 *
 * @return float The mapped temperature value.
 */
float readPotentiometer() {
  // If the system is on, read the potentiometer value
  if (systemState) {
    int value = analogRead(POT_PIN);
    // Maps the potentiometer value from 0-4095 to the range of 18ºC to 30ºC
    return map(value, 0, 4095, 18, 30);
  }
  return 0;
}

/**
 * @brief Toggles the system state between on and off.
 *
 * Updates the OLED display to show the current system state, controls the system and boiler LEDs,
 * and prints the system state to the serial monitor.
 */
void controlSystem() {
  systemState = !systemState;
  xSemaphoreTake(displayMutex, portMAX_DELAY); // Mutex for exclusive access
  display.clearDisplay();
  display.setTextSize(1);
  // Dibujar icono de estado
  if (systemState) {
    display.fillCircle(64, 16, 10, SSD1306_WHITE); 
    display.drawRect(62, 8, 4, 8, SSD1306_BLACK);
  } else {
    display.drawCircle(64, 16, 10, SSD1306_WHITE);
  }
  display.setCursor((SCREEN_WIDTH - 12 * 6) / 2, 32); 
  display.print(systemState ? "System On" : "System Off");

  // LEDs y mensaje en el Serial
  digitalWrite(SYSTEM_ON_LED_PIN, systemState ? HIGH : LOW);
  digitalWrite(CALDERA_ON_LED_PIN, LOW);
  digitalWrite(CALDERA_OFF_LED_PIN, LOW);
  Serial.println("--------------------");
  Serial.println(systemState ? "### Encendida ###" : "### Apagada ###");
  Serial.println("--------------------");
  display.display();
  xSemaphoreGive(displayMutex); // Release the mutex
}

void controlLedsStatus(float temperature, float potTemperature){
  if (temperature < potTemperature) {
      digitalWrite(CALDERA_ON_LED_PIN, HIGH);
      digitalWrite(CALDERA_OFF_LED_PIN, LOW);
      Serial.println("Boiler Activated.");
    } else {
      digitalWrite(CALDERA_ON_LED_PIN, LOW);
      digitalWrite(CALDERA_OFF_LED_PIN, HIGH);
      Serial.println("Boiler Deactivated.");
    }
}

/* ------------------------------------------------- TASK FUNCTIONS --------------------------------------------------- */
/**
 * @brief FreeRTOS task to update the OLED display with temperature information.
 *
 * Receives current and desired temperature values from the queues and updates the OLED display
 * with this information. This task runs indefinitely with a delay of 250 milliseconds between updates.
 *
 * @param parameter Task parameter (not used).
 */
void updateDisplayTask(void *parameter) {
  TemperatureData lastTempDataReceived = {0, 0}; // Variable para guardar el último dato recibido
  for(;;) {
    TemperatureData tempData;
    if (xQueueReceive(qTemperature, &tempData, portMAX_DELAY) == pdPASS) {
      // Solo actualizar si el dato ha cambiado
        lastTempDataReceived = tempData; // Actualiza el último dato recibido
        xSemaphoreTake(displayMutex, portMAX_DELAY); // Mutex para acceso exclusivo
        display.clearDisplay();
        display.setTextSize(1);
        display.setTextColor(WHITE);
        display.setCursor(0, 0);
        display.print("Current Temp: ");
        display.print(tempData.sensorTemperature);
        display.print(" C");
        display.setCursor(0, 10);
        display.print("Set Temp: ");
        display.print(tempData.potTemperature);
        display.print(" C");
        display.display();
        Serial.println("Actualizando pantalla...");
        xSemaphoreGive(displayMutex); // Liberar el mutex
    }
  }
  vTaskDelete(NULL);
}
/**
 * @brief FreeRTOS task to read the temperature sensor.
 *
 * If the system is on, it reads the temperature from the DHT22 sensor and sends it to the
 * temperature queue. This task runs indefinitely with a delay of 1 second between readings.
 *
 * @param parameter Task parameter (not used).
 */
void readSensorsTask(void *parameter) {
  for(;;) {
    if (systemState) {
      float temperature = getTemp();
      float potTemperature = readPotentiometer();
      Serial.println("--------------------");
      Serial.println("Temperature: " + String(temperature, 2) + "ºC");
      Serial.println("--------------------");
      // Sends the temperature to the queue to be processed by the boiler control task
      TemperatureData tempData = {temperature, potTemperature};
      if (xQueueSend(qTemperature, &tempData, portMAX_DELAY) != pdPASS) {
        Serial.println("Queue Full. Temperature not sent.");
      }
    }
    vTaskDelay(pdMS_TO_TICKS(1000)); // Wait 1 second before the next reading
  }
  vTaskDelete(NULL);
}
/**
 * @brief FreeRTOS task to control the boiler based on temperature readings.
 *
 * Receives temperature readings from the temperature queue and reads the potentiometer value.
 * Activates or deactivates the boiler based on the current temperature and the desired temperature
 * set by the potentiometer. This task runs indefinitely.
 *
 * @param parameter Task parameter (not used).
 */
void controlBoilerLedsTask(void *parameter) {
  TemperatureData lastTempDataReceived = {0, 0}; // Variable para guardar el último dato recibido
  for(;;) {
    TemperatureData tempData;
    if (xQueueReceive(qTemperature, &tempData, portMAX_DELAY) == pdPASS) {
      if (tempData.sensorTemperature != lastTempDataReceived.sensorTemperature ||
          tempData.potTemperature != lastTempDataReceived.potTemperature) {
            lastTempDataReceived = tempData; // Actualiza el último dato recibido
            xQueueSend(qTemperature, &tempData, portMAX_DELAY);
        }
        controlLedsStatus(tempData.sensorTemperature, tempData.potTemperature);
        Serial.println("Controlando LEDs de caldera...");
    }
  }
  vTaskDelete(NULL);
}
/**
 * @brief FreeRTOS task to check the light sensor (LDR) status.
 *
 * If the system is on, it reads the LDR sensor to check for light. If no light is detected, it suspends
 * the sensor reading, boiler control, and display update tasks. If light is detected, it resumes these tasks.
 * This task runs indefinitely with a delay of 5 seconds between checks.
 *
 * @param parameter Task parameter (not used).
 */
void checkLightLDRTask(void *parameter) {
  const TickType_t xDelay = 5000 / portTICK_PERIOD_MS;
  for(;;) {
    // If the system is on, read the LDR state, which is configured as digital
    if (digitalRead(LDR_PIN) == HIGH) { // If no light is detected, suspend the tasks
      Serial.println("No light detected. Suspending tasks...");
      vTaskSuspend(readSensorsTaskHandle);
      vTaskSuspend(controlBoilerTaskHandle);
      vTaskSuspend(updateDisplayTaskHandle);
      display.clearDisplay();
      display.setTextSize(1);
      display.setCursor(0, 0);
      display.print("## No light ##");
      display.display();
      digitalWrite(CALDERA_ON_LED_PIN, LOW);
      digitalWrite(CALDERA_OFF_LED_PIN, LOW);
    } else {
      Serial.println("Light detected. Resuming tasks..."); // If light is detected, resume the suspended tasks
        vTaskResume(readSensorsTaskHandle);
        vTaskResume(controlBoilerTaskHandle);
        vTaskResume(updateDisplayTaskHandle);
      }
      vTaskDelay(xDelay); // Wait 5 seconds before the next check
    }
    vTaskDelete(NULL);
  }

/**
 * @brief FreeRTOS task to handle button presses and toggle the system state.
 *
 * Waits for the button semaphore to be taken and toggles the system state.
 * This task runs indefinitely.
 *
 * @param parameter Task parameter (not used).
 */
void handleButtonTask(void *parameter) {
  for(;;) {
    if (xSemaphoreTake(buttonSemaphore, portMAX_DELAY) == pdTRUE) {
      controlSystem();
    }
    vTaskDelay(pdMS_TO_TICKS(1000)); // Wait 1 second before the next reading
  }
  vTaskDelete(NULL);
}

/* ------------------------------------------------- DEFAULT FUNCTIONS --------------------------------------------------- */
/**
 * @brief Arduino setup function.
 *
 * Calls the main application entry point.
 */
void setup() {
  app_main();
}

/**
 * @brief Arduino loop function.
 *
 */
void loop() {
  // Empty loop
}