// task_config.cpp - FreeRTOS Resource Implementation

#include <Arduino.h>
#include "config.h"
#include "types.h"
#include "task_config.h"

TaskHandle_t canRxTaskHandle = NULL;
TaskHandle_t sdWriteTaskHandle = NULL;
TaskHandle_t canTxTaskHandle = NULL;
TaskHandle_t wsTxTaskHandle = NULL;
TaskHandle_t webServerTaskHandle = NULL;
TaskHandle_t timeSyncTaskHandle = NULL;

QueueHandle_t canRxQueue = NULL;
QueueHandle_t canTxQueue = NULL;
QueueHandle_t graphQueue = NULL;

SemaphoreHandle_t configMutex = NULL;
SemaphoreHandle_t sdMutex = NULL;
SemaphoreHandle_t rtcMutex = NULL;
SemaphoreHandle_t canMutex = NULL;

bool initFreeRTOSResources() {
  Serial.println("Initializing FreeRTOS resources...");

  canRxQueue = xQueueCreate(QUEUE_SIZE_CAN_RX, sizeof(CanFrame));
  if (canRxQueue == NULL) {
    Serial.println("Failed to create canRxQueue!");
    return false;
  }

  canTxQueue = xQueueCreate(QUEUE_SIZE_CAN_TX, sizeof(CanTxRequest));
  if (canTxQueue == NULL) {
    Serial.println("Failed to create canTxQueue!");
    return false;
  }

  graphQueue = xQueueCreate(QUEUE_SIZE_GRAPH, sizeof(CanFrame));
  if (graphQueue == NULL) {
    Serial.println("Failed to create graphQueue!");
    return false;
  }

  configMutex = xSemaphoreCreateMutex();
  if (configMutex == NULL) {
    Serial.println("Failed to create configMutex!");
    return false;
  }

  sdMutex = xSemaphoreCreateMutex();
  if (sdMutex == NULL) {
    Serial.println("Failed to create sdMutex!");
    return false;
  }

  rtcMutex = xSemaphoreCreateMutex();
  if (rtcMutex == NULL) {
    Serial.println("Failed to create rtcMutex!");
    return false;
  }

  canMutex = xSemaphoreCreateMutex();
  if (canMutex == NULL) {
    Serial.println("Failed to create canMutex!");
    return false;
  }

  Serial.println("FreeRTOS resources initialized successfully!");
  return true;
}

bool createAllTasks() {
  Serial.println("Creating FreeRTOS tasks...");

  BaseType_t result;

  result = xTaskCreatePinnedToCore(
    canRxTask,
    "CAN_RX",
    TASK_STACK_CAN_RX,
    NULL,
    TASK_PRIORITY_CAN_RX,
    &canRxTaskHandle,
    CORE_0
  );
  if (result != pdPASS) {
    Serial.println("Failed to create canRxTask!");
    return false;
  }

  result = xTaskCreatePinnedToCore(
    sdWriteTask,
    "SD_WRITE",
    TASK_STACK_SD_WRITE,
    NULL,
    TASK_PRIORITY_SD_WRITE,
    &sdWriteTaskHandle,
    CORE_1
  );
  if (result != pdPASS) {
    Serial.println("Failed to create sdWriteTask!");
    return false;
  }

  result = xTaskCreatePinnedToCore(
    canTxTask,
    "CAN_TX",
    TASK_STACK_CAN_TX,
    NULL,
    TASK_PRIORITY_CAN_TX,
    &canTxTaskHandle,
    CORE_0
  );
  if (result != pdPASS) {
    Serial.println("Failed to create canTxTask!");
    return false;
  }

  result = xTaskCreatePinnedToCore(
    webServerTask,
    "WEB_SRV",
    TASK_STACK_WEB_SERVER,
    NULL,
    TASK_PRIORITY_WEB_SERVER,
    &webServerTaskHandle,
    CORE_1
  );
  if (result != pdPASS) {
    Serial.println("Failed to create webServerTask!");
    return false;
  }

  result = xTaskCreatePinnedToCore(
    wsTxTask,
    "WS_TX",
    TASK_STACK_WS_TX,
    NULL,
    TASK_PRIORITY_WS_TX,
    &wsTxTaskHandle,
    CORE_1
  );
  if (result != pdPASS) {
    Serial.println("Failed to create wsTxTask!");
    return false;
  }

  result = xTaskCreatePinnedToCore(
    timeSyncTask,
    "TIME_SYNC",
    TASK_STACK_TIME_SYNC,
    NULL,
    TASK_PRIORITY_TIME_SYNC,
    &timeSyncTaskHandle,
    CORE_1
  );
  if (result != pdPASS) {
    Serial.println("Failed to create timeSyncTask!");
    return false;
  }

  Serial.println("All tasks created successfully!");
  return true;
}

void deleteAllTasks() {
  if (canRxTaskHandle != NULL) {
    vTaskDelete(canRxTaskHandle);
    canRxTaskHandle = NULL;
  }
  if (sdWriteTaskHandle != NULL) {
    vTaskDelete(sdWriteTaskHandle);
    sdWriteTaskHandle = NULL;
  }
  if (canTxTaskHandle != NULL) {
    vTaskDelete(canTxTaskHandle);
    canTxTaskHandle = NULL;
  }
  if (wsTxTaskHandle != NULL) {
    vTaskDelete(wsTxTaskHandle);
    wsTxTaskHandle = NULL;
  }
  if (webServerTaskHandle != NULL) {
    vTaskDelete(webServerTaskHandle);
    webServerTaskHandle = NULL;
  }
  if (timeSyncTaskHandle != NULL) {
    vTaskDelete(timeSyncTaskHandle);
    timeSyncTaskHandle = NULL;
  }

  if (canRxQueue != NULL) {
    vQueueDelete(canRxQueue);
    canRxQueue = NULL;
  }
  if (canTxQueue != NULL) {
    vQueueDelete(canTxQueue);
    canTxQueue = NULL;
  }
  if (graphQueue != NULL) {
    vQueueDelete(graphQueue);
    graphQueue = NULL;
  }

  if (configMutex != NULL) {
    vSemaphoreDelete(configMutex);
    configMutex = NULL;
  }
  if (sdMutex != NULL) {
    vSemaphoreDelete(sdMutex);
    sdMutex = NULL;
  }
  if (rtcMutex != NULL) {
    vSemaphoreDelete(rtcMutex);
    rtcMutex = NULL;
  }
  if (canMutex != NULL) {
    vSemaphoreDelete(canMutex);
    canMutex = NULL;
  }
}