#include <STM32FreeRTOS.h>

const int ledPin1 = D2;
const int ledPin2 = D3;

void Task_blinkLED1(void *parameter);
void Task_blinkLED2(void *parameter);
void Task_monitor(void *parameter);

TaskHandle_t handleLED1;
TaskHandle_t handleLED2;

void setup() {
  Serial.begin(115200);
  Serial.println("\nIniciando FreeRTOS en STM32 Nucleo-F446RE...");

  xTaskCreate(Task_blinkLED1, "Blink LED 1", 128, NULL, 1, &handleLED1);
  xTaskCreate(Task_blinkLED2, "Blink LED 2", 128, NULL, 1, &handleLED2);
  xTaskCreate(Task_monitor, "Monitor", 512, NULL, 1, NULL);

  vTaskStartScheduler();
}

void loop() {

}

void Task_blinkLED1(void *parameter) {
  pinMode(ledPin1, OUTPUT);
  while (true) {
    digitalWrite(ledPin1, HIGH);
    vTaskDelay(500 / portTICK_PERIOD_MS);
    digitalWrite(ledPin1, LOW);
    vTaskDelay(500 / portTICK_PERIOD_MS);
  }
}

void Task_blinkLED2(void *parameter) {
  pinMode(ledPin2, OUTPUT);
  while (true) {
    digitalWrite(ledPin2, HIGH);
    vTaskDelay(2000 / portTICK_PERIOD_MS);
    digitalWrite(ledPin2, LOW);
    vTaskDelay(2000 / portTICK_PERIOD_MS);
  }
}

void Task_monitor(void *parameter) {
  static size_t heapMinFree = 0;

  while (true) {
    size_t heapFree = xPortGetFreeHeapSize();
    if (heapMinFree == 0 || heapFree < heapMinFree)
      heapMinFree = heapFree;

    UBaseType_t stackLED1 = uxTaskGetStackHighWaterMark(handleLED1);
    UBaseType_t stackLED2 = uxTaskGetStackHighWaterMark(handleLED2);

    Serial.println("===== MONITOR =====");
    Serial.print("Heap libre actual: ");
    Serial.println(heapFree);
    Serial.print("Heap mínimo libre: ");
    Serial.println(heapMinFree);
    Serial.print("Stack LED1 (palabras libres): ");
    Serial.println(stackLED1);
    Serial.print("Stack LED2 (palabras libres): ");
    Serial.println(stackLED2);
    Serial.println("===================\n");

    vTaskDelay(2000 / portTICK_PERIOD_MS);
  }
}

extern "C" void vApplicationStackOverflowHook(TaskHandle_t xTask, char *pcTaskName) {
  Serial.print("¡Desbordamiento de pila en tarea: ");
  Serial.println(pcTaskName);
  while (1);
}