#include "bsp.h"

#define NRF_LOG_MODULE_NAME "TASK"

#include "nrf_log.h"
#include "nrf_log_ctrl.h"


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

#include "user_config.h"
#include "user_functions.h"
#include "user_tasks.h"

SemaphoreHandle_t xSemaphoreBLEEventReadyHandle = NULL;

void vTaskBlinkLED(void *pvParameters);
void vTaskBLE(void *pvParameters);
void vTaskLogger(void *pvParameters);

void init_tasks(void) {
    
    xSemaphoreBLEEventReadyHandle = xSemaphoreCreateBinary();
    if(NULL == xSemaphoreBLEEventReadyHandle) {
        APP_ERROR_HANDLER(NRF_ERROR_NO_MEM);
    }

    if(pdPASS != xTaskCreate(vTaskLogger, "TASKLOG0", 256, NULL, 1, &xTaskLoggerHandle)) {
        APP_ERROR_HANDLER(NRF_ERROR_NO_MEM);
    }

    if(pdPASS != xTaskCreate(vTaskBlinkLED, "TASKBLINK0", 128, NULL, 2, &xTaskBlinkLEDHandle)) {
        APP_ERROR_HANDLER(NRF_ERROR_NO_MEM);
    }

    if(pdPASS != xTaskCreate(vTaskBLE, "TASKBLE0", 256, NULL, 2, &xTaskBLEHandle)) {
        APP_ERROR_HANDLER(NRF_ERROR_NO_MEM);
    }
}

TaskHandle_t xTaskBlinkLEDHandle = NULL;
void vTaskBlinkLED(void *pvParameters) {
    UNUSED_PARAMETER(pvParameters);

    for(;;) {
        bsp_board_led_invert(BSP_BOARD_LED_0);
        NRF_LOG_INFO("Toggle LED 0\r\n");
        vTaskDelay(pdMS_TO_TICKS(1000));
    }
}

TaskHandle_t xTaskBLEHandle = NULL;
void vTaskBLE(void *pvParameters) {

    NRF_LOG_INFO("Created BLE task, initialize softdevice handler\r\n");

    nrf_clock_lf_cfg_t clock_lf_cfg = NRF_CLOCK_LFCLKSRC;
    SOFTDEVICE_HANDLER_INIT(&clock_lf_cfg, ble_new_event_handler);

    ble_enable_params_t ble_enable_params;
    uint32_t err_code = softdevice_enable_get_default_config(CENTRAL_LINK_COUNT, PERIPHERAL_LINK_COUNT, &ble_enable_params);
    APP_ERROR_CHECK(err_code);
    CHECK_RAM_START_ADDR(CENTRAL_LINK_COUNT, PERIPHERAL_LINK_COUNT);
    err_code = softdevice_enable(&ble_enable_params);
    APP_ERROR_CHECK(err_code);

    NRF_LOG_INFO("Softdevice enabled, waiting for events\r\n");

    for(;;) {
        while(pdFALSE == xSemaphoreTake(xSemaphoreBLEEventReadyHandle, portMAX_DELAY)) {
            //
        }
        NRF_LOG_INFO("BLE event occured!\r\n");
        intern_softdevice_events_execute();
    }
}

/* System tasks below */
TaskHandle_t xTaskLoggerHandle = NULL;
void vTaskLogger(void *pvParameters) {
    UNUSED_VARIABLE(pvParameters);

    for(;;) {
        NRF_LOG_FLUSH();
        vTaskSuspend(NULL);
    }
}