#include "Kernel.h"
#include "FreeRTOS.h"
#include "osif.h"
#include "timers.h"
#include "task.h"
#include "queue.h"

#define     MAX_QUEUE      200
#define     MAX_TASK       200
#define     MAX_TIMER      100

static QueueHandle_t QueueHandles[MAX_QUEUE];
static TaskHandle_t TaskHandles[MAX_TASK];
static TimerHandle_t TimerHandles[MAX_TIMER];
static TimerFunction TimerCallbacks[MAX_TIMER];

static void timer_callback( TimerHandle_t timer )
{
    uint8_t id = (uint8_t)(uint32_t)pvTimerGetTimerID(timer);

    if(TimerCallbacks[id])
    {
        TimerCallbacks[id]();
    }
}

void KERN_start()
{
    vTaskStartScheduler();
}

void KERN_createTask(TaskFunction task_proc, const char* name, uint8_t id, uint16_t stack_depth, uint32_t priority, void* param)
{
    if(id < MAX_TASK)
    {
        xTaskCreate(task_proc, name, stack_depth, param, priority, &TaskHandles[id]);
    }
}

void KERN_createQueue(uint8_t id, uint32_t length, size_t item_size)
{
    if(id < MAX_QUEUE)
    {
        QueueHandles[id] = xQueueCreate(length, item_size);
    }
}

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

void* KERN_malloc(size_t size)
{
    return pvPortMalloc(size);
}

void KERN_free(void * mem)
{
    vPortFree(mem);
}

void* KERN_createMutex()
{
    mutex_t* mutex = pvPortMalloc(sizeof(mutex_t));
    
    if(NULL != mutex)
    {
        OSIF_MutexCreate(mutex);
    }

    return mutex;
}

void KERN_destroyMutex(void* mutex)
{
    OSIF_MutexDestroy((mutex_t*)mutex);
    vPortFree(mutex);
}

void KERN_lock(void* mutex)
{
    OSIF_MutexLock((mutex_t*)mutex, OSIF_WAIT_FOREVER);
}

void KERN_unlock(void* mutex)
{
    OSIF_MutexUnlock((mutex_t*)mutex);
}

void* KERN_createSemaphore(const uint8_t init_value)
{
    semaphore_t* semaphore = pvPortMalloc(sizeof(semaphore_t));
    
    if(NULL != semaphore)
    {
        OSIF_SemaCreate(semaphore, init_value);
    }

    return semaphore;
}

void KERN_destroySemaphore(void* semaphore)
{
    OSIF_SemaDestroy((semaphore_t*)semaphore);
}

void KERN_wait(void* semaphore, uint32_t milliseconds)
{
    OSIF_SemaWait((semaphore_t*)semaphore, milliseconds);
}

void KERN_post(void* semaphore)
{
    OSIF_SemaPost((semaphore_t*)semaphore);
}

void KERN_registerTimer(uint8_t id, uint32_t milliseconds, uint8_t reload, TimerFunction callback)
{
    if(id < MAX_TIMER)
    {
        TimerCallbacks[id] = callback;
        TimerHandles[id] = xTimerCreate("Timer's name", pdMS_TO_TICKS(milliseconds), reload, (void*)(uint32_t)id, timer_callback);
    }
}

void KERN_releaseTimer(uint8_t id)
{
    if(id < MAX_TIMER)
    {
        xTimerDelete(TimerHandles[id], NULL);
        TimerHandles[id] = NULL;
        TimerCallbacks[id] = NULL;
    }
}

void KERN_startTimer(uint8_t id)
{
    if(id < MAX_TIMER)
    {
        xTimerStart(TimerHandles[id], NULL);
    }
}

void KERN_stopTimer(uint8_t id)
{
    if(id < MAX_TIMER)
    {
        xTimerStop(TimerHandles[id], NULL);
    }
}

void KERN_resetTimer(uint8_t id)
{
    if(id < MAX_TIMER)
    {
        if(xTimerIsTimerActive(TimerHandles[id]) == pdTRUE)
        {
            xTimerReset(TimerHandles[id], NULL);
        }
    }
}

void KERN_changeTimer(uint8_t id, uint32_t milliseconds, TimerFunction callback)
{
    if(id < MAX_TIMER)
    {
        xTimerChangePeriod(TimerHandles[id], pdMS_TO_TICKS(milliseconds), NULL);

        if(callback != NULL)
        {
            TimerCallbacks[id] = callback;
        }
    }
}

int32_t KERN_sndMsg(uint8_t queue_id, void * item, uint32_t milliseconds)
{
    TickType_t ticks_wait = portMAX_DELAY;

    if(queue_id >= MAX_QUEUE || NULL == QueueHandles[queue_id])
    {
        return KERN_STATUS_INVALID_MSG;
    }

    if(milliseconds != KERN_WAIT_INFINITE)
    {
        ticks_wait = pdMS_TO_TICKS(milliseconds);
    }

    if(pdPASS != xQueueSend(QueueHandles[queue_id], item, ticks_wait))
    {
        return KERN_STATUS_QUEUE_FULL;
    }

    return KERN_STATUS_SUCCESS;
}

int32_t KERN_sndMsgFromISR(uint8_t queue_id, void * item)
{
    if(queue_id >= MAX_QUEUE || NULL == QueueHandles[queue_id])
    {
        return KERN_STATUS_INVALID_MSG;
    }

    if(pdPASS != xQueueSendToBackFromISR(QueueHandles[queue_id], item, 0))
    {
        return KERN_STATUS_QUEUE_FULL;
    }

    return KERN_STATUS_SUCCESS;
}

int32_t KERN_sndMsgToFront(uint8_t queue_id, void * item, uint32_t milliseconds)
{
    TickType_t ticks_wait = portMAX_DELAY;

    if(queue_id >= MAX_QUEUE || NULL == QueueHandles[queue_id])
    {
        return KERN_STATUS_INVALID_MSG;
    }

    if(milliseconds != KERN_WAIT_INFINITE)
    {
        ticks_wait = pdMS_TO_TICKS(milliseconds);
    }

    if(pdPASS != xQueueSendToFront(QueueHandles[queue_id], item, ticks_wait))
    {
        return KERN_STATUS_QUEUE_FULL;
    }

    return KERN_STATUS_SUCCESS;
}

int32_t KERN_sndMsgToFrontFromISR(uint8_t queue_id, void * item)
{
    if(queue_id >= MAX_QUEUE || NULL == QueueHandles[queue_id])
    {
        return KERN_STATUS_INVALID_MSG;
    }

    if(pdPASS != xQueueSendToFrontFromISR(QueueHandles[queue_id], item, 0))
    {
        return KERN_STATUS_QUEUE_FULL;
    }

    return KERN_STATUS_SUCCESS;
}

int32_t KERN_rcvMsg(uint8_t queue_id, void * buffer, uint32_t milliseconds)
{
    TickType_t ticks_wait = portMAX_DELAY;

    if(queue_id >= MAX_QUEUE || NULL == QueueHandles[queue_id])
    {
        return KERN_STATUS_INVALID_MSG;
    }

    if(milliseconds != KERN_WAIT_INFINITE)
    {
        ticks_wait = pdMS_TO_TICKS(milliseconds);
    }
    
    if(pdPASS != xQueueReceive(QueueHandles[queue_id], buffer, ticks_wait))
    {
        return KERN_STATUS_TIMEOUT;
    }

    return KERN_STATUS_SUCCESS;
}

uint32_t KERN_getCurrentTime()
{
    return OSIF_GetMilliseconds();
}