#include "cyabs_rtos.h"

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

/**
 * @note FreeRTOS does not provide Join-like API for one task to wait for another task to exit,
 * so we have to implement the 'join' function with a binary semaphore.
 * The task being signalled to stop will release the semaphore right before the task is deleted,
 * then we can 'join' the task by simply wait for the semaphore to be given.
 * This semaphore is created along with the new task, and is deleted within 'join' function,
 * and the handle of the semaphore is stored in the Thread Local Storage (TLS) of the target task.
 *
 */

#define CY_RTOS_JOIN_SEMPHR_TLS_ID 0

cy_rslt_t cy_rtos_create_thread(cy_thread_t *thread, cy_thread_entry_fn_t entry_function, const char *name, void *stack,
                                uint32_t stack_size, cy_thread_priority_t priority, cy_thread_arg_t arg) {
    (void)stack;

    /* Create a semaphore to let the thread join */
    SemaphoreHandle_t thr_join_semphr = xSemaphoreCreateBinary();
    if (thr_join_semphr == NULL) {
        return CY_RTOS_NO_MEMORY;
    }

    if (xTaskCreate((TaskFunction_t)entry_function, name, stack_size / sizeof(portSTACK_TYPE), (void *)arg, priority,
                    (TaskHandle_t *)thread) != pdPASS) {
        vSemaphoreDelete(thr_join_semphr);
        return CY_RTOS_NO_MEMORY;
    }

    /* Store the semaphore handle into the thread local storage */
    vTaskSetThreadLocalStoragePointer(*thread, CY_RTOS_JOIN_SEMPHR_TLS_ID, thr_join_semphr);

    return CY_RSLT_SUCCESS;
}

cy_rslt_t cy_rtos_exit_thread(void) {
    TaskHandle_t      cur_task_handle   = NULL;
    SemaphoreHandle_t thr_join_semphore = NULL;

    /* Get current task handle and retrieve the semaphore handle from TLS */
    cur_task_handle                     = xTaskGetCurrentTaskHandle();
    thr_join_semphore = pvTaskGetThreadLocalStoragePointer(cur_task_handle, CY_RTOS_JOIN_SEMPHR_TLS_ID);

    /* Notify that the task is now exit. */
    xSemaphoreGive(thr_join_semphore);

    vTaskDelete(NULL);

    return CY_RSLT_SUCCESS;
}

cy_rslt_t cy_rtos_terminate_thread(cy_thread_t *thread) {
    (void)thread;

    /* Empty function, thread resource will be released after task function joined. */
    /* WARNING: Current WHD drivers does not call this function. LEAVE IT BLANK FOR NOW. */
    return CY_RSLT_SUCCESS;
}

cy_rslt_t cy_rtos_join_thread(cy_thread_t *thread) {
    SemaphoreHandle_t thr_join_semaphore = NULL;

    /* Retrieve the semaphore handle from TLS */
    thr_join_semaphore = pvTaskGetThreadLocalStoragePointer((TaskHandle_t)*thread, CY_RTOS_JOIN_SEMPHR_TLS_ID);

    /* Wait until the target task exit */
    if (xSemaphoreTake(thr_join_semaphore, portMAX_DELAY) != pdPASS) {
        /* Really unexpected */
        return CY_RTOS_GENERAL_ERROR;
    }

    /* Delete the semaphore here. See the comments of `cy_rtos_terminate_thread` above! */
    vSemaphoreDelete(thr_join_semaphore);

    return CY_RSLT_SUCCESS;
}

cy_rslt_t cy_rtos_init_semaphore(cy_semaphore_t *semaphore, uint32_t maxcount, uint32_t initcount) {
    *semaphore = (cy_semaphore_t)xSemaphoreCreateCounting(maxcount, initcount);
    if (*semaphore == NULL) {
        return CY_RTOS_NO_MEMORY;
    }

    return CY_RSLT_SUCCESS;
}

cy_rslt_t cy_rtos_get_semaphore(cy_semaphore_t *semaphore, cy_time_t timeout_ms, bool in_isr) {
    BaseType_t higher_priority_woken = pdFALSE;

    if (!in_isr) {
        if (xSemaphoreTake(*semaphore, pdMS_TO_TICKS(timeout_ms)) != pdPASS) {
            return CY_RTOS_TIMEOUT;
        }
    } else { /* Currently is not used */
        if (xSemaphoreTakeFromISR(*semaphore, &higher_priority_woken) != pdPASS) {
            return CY_RTOS_TIMEOUT;
        }
        if (higher_priority_woken != pdFALSE) {
            taskYIELD()
        }
    }

    return CY_RSLT_SUCCESS;
}

cy_rslt_t cy_rtos_set_semaphore(cy_semaphore_t *semaphore, bool in_isr) {
    BaseType_t higher_priority_woken = pdFALSE;

    if (!in_isr) {
        /*
         * Since the semaphores are implemented using queues, this call will also fail if the queue is full.
         * So do not check return values here.
         */
        xSemaphoreGive(*semaphore);
    } else {
        /* For the same reason as above, do not check return values here. */
        xSemaphoreGiveFromISR(*semaphore, &higher_priority_woken);
        if (higher_priority_woken != pdFALSE) {
            taskYIELD()
        }
    }

    return CY_RSLT_SUCCESS;
}

cy_rslt_t cy_rtos_deinit_semaphore(cy_semaphore_t *semaphore) {
    vSemaphoreDelete(*semaphore);

    return CY_RSLT_SUCCESS;
}

cy_rslt_t cy_rtos_get_time(cy_time_t *tval) {
    *tval = xTaskGetTickCount();

    return CY_RSLT_SUCCESS;
}

cy_rslt_t cy_rtos_delay_milliseconds(cy_time_t num_ms) {
    vTaskDelay(pdMS_TO_TICKS(num_ms));

    return CY_RSLT_SUCCESS;
}