#include "FreeRTOS.h"
#include "semphr.h"
#include "task.h"

//TODO namespace the names

/**
 * Task descriptors
 */
#define NUMBER_OF_TASKS 100
SemaphoreHandle_t taskMutex = NULL;
StaticSemaphore_t taskMutexDescriptor;
size_t nextFreeTaskDescriptor = 0;
StaticTask_t taskDescriptors[NUMBER_OF_TASKS];

/**
 * Queue descriptors
 */
#define NUMBER_OF_QUEUES 300
SemaphoreHandle_t queueMutex = NULL;
StaticSemaphore_t queueMutexDescriptor;
size_t nextFreeQueueDescriptor = 0;
StaticQueue_t queueDescriptors[NUMBER_OF_QUEUES];

// TODO check and return
void initFreeRTOSHelper() {
  taskMutex = xSemaphoreCreateRecursiveMutexStatic(&taskMutexDescriptor);
  queueMutex = xSemaphoreCreateRecursiveMutexStatic(&queueMutexDescriptor);
}

const char *get_task_name() { return pcTaskGetName(NULL); }

void stop_it() { taskENTER_CRITICAL(); }

// TODO return some error code?
void *create_task(TaskFunction_t taskFunction, void *parameter, void *buffer,
                  size_t buffersize) {
  if (xSemaphoreTakeRecursive(taskMutex, portMAX_DELAY) != pdTRUE) {
    return NULL;
  }
  // we hold the task mutex and are now allowed to access the taskDescriptors
  if (nextFreeTaskDescriptor >=
      sizeof(taskDescriptors) / sizeof(*taskDescriptors)) {
    return NULL;
  }

  TaskHandle_t newTask = xTaskCreateStatic(
      taskFunction, "rust", buffersize / sizeof(StackType_t), parameter, 4,
      buffer, taskDescriptors + nextFreeTaskDescriptor);

  nextFreeTaskDescriptor++;

  xSemaphoreGiveRecursive(taskMutex);
  return newTask;
}

void task_delay(uint32_t milliseconds) {
  vTaskDelay(pdMS_TO_TICKS(milliseconds));
}

void *create_queue(size_t length, size_t element_size, uint8_t *buffer) {
  if (xSemaphoreTakeRecursive(queueMutex, portMAX_DELAY) != pdTRUE) {
    return NULL;
  }
  // we hold the queue mutex and are now allowed to access the taskDescriptors
  if (nextFreeQueueDescriptor >=
      sizeof(queueDescriptors) / sizeof(*queueDescriptors)) {
    return NULL;
  }
  QueueHandle_t newQueue = xQueueCreateStatic(
      length, element_size, buffer, queueDescriptors + nextFreeQueueDescriptor);
  nextFreeQueueDescriptor++;
  xSemaphoreGiveRecursive(queueMutex);
  return newQueue;
}

uint8_t queue_receive(void *queue, void *message) {
  if (xQueueReceive(queue, message, portMAX_DELAY) == pdPASS) {
    return 1;
  } else {
    return 0;
  }
}

uint8_t queue_send(void *queue, void *message) {
  if (xQueueSend(queue, message, 0) != pdPASS) {
    return 1;
  } else {
    return 0;
  }
}