#include "DHTesp.h"
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#include "freertos/queue.h"
#include "freertos/semphr.h"
#include "SPI.h"
#include "Wire.h"
#include "Adafruit_GFX.h"
#include "Adafruit_SSD1306.h"

#define LED_PIN 4 // Define el pin donde está conectado el LED de encendido del sistema
#define BUTTON_PIN 5 // Define el pin donde está conectado el botón
#define NOTIFICATION_PIN 26 // Define el pin donde está conectado el LED de encendido de la caldera
#define DHT_PIN 15 // Define el pin donde está conectado el sensor de temperatura
#define LDR_PIN 13 // Pin del sensor de luz

#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)

DHTesp dhtSensor;
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);

struct Temperaturas {
  float temperaturaActual;
  float temperaturaDeseada;
};

volatile bool encendido;

bool tareasCreadas;

QueueHandle_t xQueue;
QueueHandle_t xMutexDisplay;

SemaphoreHandle_t xCambioEstado;

TaskHandle_t leerLuzHandler;
TaskHandle_t leerTempHandler;
TaskHandle_t printTempHandler;

void IRAM_ATTR toggleSystemState() {
  if (encendido) {
    vTaskSuspend(leerLuzHandler);
    vTaskSuspend(leerTempHandler);
    vTaskSuspend(printTempHandler);

    digitalWrite(NOTIFICATION_PIN, LOW);
    digitalWrite(LED_PIN, LOW);

    xQueueReset(xQueue);

    // Lo dejo comentado porque el rendimiento de la simulación baja al 7% si lo intento hacer
    // Además de por lo que comento abajo
    // printDisplay("Sistema apagado manualmente", 1);
  } else {
    if (tareasCreadas) {
      reanudarTareas();
    } else {
      crearTareas();
    }

    digitalWrite(LED_PIN, HIGH);
  }

  encendido = !encendido;
}

void toggleSystemStateSensor() {
  if (encendido) {
    vTaskSuspend(leerTempHandler);
    vTaskSuspend(printTempHandler);

    digitalWrite(NOTIFICATION_PIN, LOW);
    digitalWrite(LED_PIN, LOW);

    xQueueReset(xQueue);
  } else {
    reanudarTareas();
  
    digitalWrite(LED_PIN, HIGH);
  }

  encendido = !encendido;
}

// Tarea para mostrar las temperaturas por LCD
void vTaskPrintTemp(void *pvParam) {
  portTickType xLastWakeTime;
  xLastWakeTime = xTaskGetTickCount();

  for(;;) {
    Temperaturas temperaturas; 

    if (xQueueReceive(xQueue, &temperaturas, (TickType_t)(250 / portTICK_PERIOD_MS))) {
      printDisplay("Temperatura: " + (String)temperaturas.temperaturaActual + "C\n" +
                    "Termostato : " + (String)temperaturas.temperaturaDeseada + "C", 1);
    }

    vTaskDelayUntil(&xLastWakeTime, (250 / portTICK_PERIOD_MS));
  }
}

// Tarea para leer las temperaturas
void vTaskLeerTemp(void *pvParam) {
  portTickType xLastWakeTime;
  xLastWakeTime = xTaskGetTickCount();

  for (;;) {
    Temperaturas temperaturas;
    temperaturas.temperaturaActual = dhtSensor.getTempAndHumidity().temperature;
    temperaturas.temperaturaDeseada = analogRead(36)/230.0 + 13;

    if (temperaturas.temperaturaActual < temperaturas.temperaturaDeseada) {
      digitalWrite(NOTIFICATION_PIN, HIGH);
    } else if (digitalRead(NOTIFICATION_PIN) == HIGH) {
      digitalWrite(NOTIFICATION_PIN, LOW);
    }

    xQueueSend(xQueue, (void *)&temperaturas, (TickType_t)(250 / portTICK_PERIOD_MS));

    vTaskDelayUntil(&xLastWakeTime, (250 / portTICK_PERIOD_MS));
  }
}

// Tarea para leer la cantidad de luz
void vTaskLeerLuz(void *pvParam) {
  portTickType xLastWakeTime;
  xLastWakeTime = xTaskGetTickCount();

  bool luzSuficiente;
  
  for (;;) {
    luzSuficiente = digitalRead(LDR_PIN) == LOW;

    if (!luzSuficiente && encendido) {
      xSemaphoreGive(xCambioEstado);

      // Mostraría este mensaje, pero, no sé por qué, hay veces que cuando el sistema
      // se apaga por falta de luz, no se vuelve a encender al volver la luz
      // Comentar esto hace que funcione sin problemas
      // Esa es la razón por la que no uso el display en otros lados
      // printDisplay("Sistema apagado\nNiveles bajos de luz", 1);
    } else if (luzSuficiente && !encendido) {
      xSemaphoreGive(xCambioEstado);
    }

    vTaskDelayUntil(&xLastWakeTime, (5000 / portTICK_PERIOD_MS));
  }
}

void crearTareas() {
  if (xQueue != NULL && xMutexDisplay != NULL && xCambioEstado != NULL) {
      xTaskCreate(vTaskLeerLuz, "Tarea Leer Luz", CONFIG_SYSTEM_EVENT_TASK_STACK_SIZE, NULL, 1, &leerLuzHandler);
      xTaskCreate(vTaskLeerTemp, "Tarea Leer Temperaturas", CONFIG_SYSTEM_EVENT_TASK_STACK_SIZE, NULL, 2, &leerTempHandler);
      xTaskCreate(vTaskPrintTemp, "Tarea Printear Temperaturas", CONFIG_SYSTEM_EVENT_TASK_STACK_SIZE, NULL, 3, &printTempHandler);
      
      tareasCreadas = true;
      Serial.println("Tareas creadas");
    }
}

void reanudarTareas() {
  vTaskResume(leerLuzHandler);
  vTaskResume(leerTempHandler);
  vTaskResume(printTempHandler);
}

void printDisplay(String cadena, int size) {
  if (xSemaphoreTake(xMutexDisplay, portMAX_DELAY) == pdTRUE) {
    display.clearDisplay();
    display.setTextColor(WHITE);
    display.setCursor(1,0);
    display.setTextSize(size);
    display.println(cadena);
    display.display();

    xSemaphoreGive(xMutexDisplay);
  }
}

void setup() {
  Serial.begin(115200);
  pinMode(LED_PIN, OUTPUT);
  pinMode(BUTTON_PIN, INPUT);
  pinMode(NOTIFICATION_PIN, OUTPUT);
  pinMode(LDR_PIN, INPUT);

  dhtSensor.setup(DHT_PIN, DHTesp::DHT22);

  attachInterrupt(digitalPinToInterrupt(BUTTON_PIN), toggleSystemState, FALLING);

  xQueue = xQueueCreate(10, sizeof(Temperaturas));
  xMutexDisplay = xSemaphoreCreateMutex();
  xCambioEstado = xSemaphoreCreateBinary();

  // SSD1306_SWITCHCAPVCC = generate display voltage from 3.3V internally
  if(!display.begin(SSD1306_SWITCHCAPVCC, 0x3C)) { 
    Serial.println(F("SSD1306 allocation failed\n"));
    for(;;); // Don't proceed, loop forever
  }

  printDisplay("Simulacion\niniciada", 2);

  encendido = false;
  tareasCreadas = false;
}

void loop() {
  if (xSemaphoreTake(xCambioEstado, portMAX_DELAY) == pdTRUE) {
    toggleSystemStateSensor();
  }
}