#include <stdbool.h>
#include <stdint.h>

#ifdef __cplusplus
extern "C"
{
#endif

#include "app_config.h"
#include "apply_old_config.h"
#include "sdk_config.h"

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

#include "nrf_sdh.h"
#include "nrf_sdh_ble.h"
#include "nrf_sdh_freertos.h"

#include "ble_srv_common.h"

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

#ifdef __cplusplus
}
#endif
static QueueHandle_t regular_data_q       = NULL;
static SemaphoreHandle_t regular_data_sem = NULL;

typedef struct __PACKED
{
    uint32_t timestamp;
    uint8_t id;
    uint8_t type : 4;
    uint8_t      : 4;
    union
    {
        double double_val;
        float flaot_val;
        uint64_t u64_val;
        uint32_t u32_val;
        uint16_t u16_val;
        uint8_t u8_val;
        int64_t i64_val;
        int32_t i32_val;
        int16_t i16_val;
        int8_t i8_val;
    } content;
    uint8_t padding[2];
} channel_data_t;

#define REGULAR_I8_TYPE     0
#define REGULAR_I16_TYPE    1
#define REGULAR_I32_TYPE    2
#define REGULAR_I64_TYPE    3
#define REGULAR_U8_TYPE     4
#define REGULAR_U16_TYPE    5
#define REGULAR_U32_TYPE    6
#define REGULAR_U64_TYPE    7
#define REGULAR_FLOAT_TYPE  8
#define REGULAR_DOUBLE_TYPE 9
#define REGULAR_MAX_TYPE    10

const uint32_t channel_size_table[REGULAR_MAX_TYPE] = {
    [REGULAR_I8_TYPE]     = offsetof(channel_data_t, content) + sizeof(int8_t),
    [REGULAR_I16_TYPE]    = offsetof(channel_data_t, content) + sizeof(int16_t),
    [REGULAR_I32_TYPE]    = offsetof(channel_data_t, content) + sizeof(int32_t),
    [REGULAR_I64_TYPE]    = offsetof(channel_data_t, content) + sizeof(int64_t),
    [REGULAR_U8_TYPE]     = offsetof(channel_data_t, content) + sizeof(uint8_t),
    [REGULAR_U16_TYPE]    = offsetof(channel_data_t, content) + sizeof(uint16_t),
    [REGULAR_U32_TYPE]    = offsetof(channel_data_t, content) + sizeof(uint32_t),
    [REGULAR_U64_TYPE]    = offsetof(channel_data_t, content) + sizeof(uint64_t),
    [REGULAR_FLOAT_TYPE]  = offsetof(channel_data_t, content) + sizeof(float),
    [REGULAR_DOUBLE_TYPE] = offsetof(channel_data_t, content) + sizeof(double),
};

void pseudo_sensorAA_read(channel_data_t *p_channel_data)
{
    static uint32_t val             = 0;
    p_channel_data->timestamp       = xTaskGetTickCount();
    p_channel_data->id              = 0xAA;
    p_channel_data->type            = REGULAR_U32_TYPE;
    p_channel_data->content.u32_val = val++;
}

void pseudo_sensorBB_read(channel_data_t *p_channel_data)
{
    static uint32_t val             = 0;
    p_channel_data->timestamp       = xTaskGetTickCount();
    p_channel_data->id              = 0xBB;
    p_channel_data->type            = REGULAR_U32_TYPE;
    p_channel_data->content.u32_val = val--;
}

static void pseudo_data_task(void *pArg)
{
    void (*pfCallback)(channel_data_t *p_channel_data) = pArg;
    for (;;)
    {
        channel_data_t ch_data;
        memset(&ch_data, 0x00, sizeof(ch_data));
        pfCallback(&ch_data);
        if (xQueueSend(regular_data_q, &ch_data, pdMS_TO_TICKS(50)) == pdFAIL)
        {
            __BKPT(255);
        }
        else
        {
            portYIELD();
        }
    }
}

static union
{
    uint8_t raw[256];
    struct __PACKED
    {
        uint8_t len;
        uint8_t seq;
        uint16_t : 16;
        uint32_t time;
        uint8_t channel_len; /* channel len => group length */
    } header;
} payload;

static void regular_data_msg_task(void *pArg)
{

    for (;;)
    {
        channel_data_t ch_data;

        uint32_t offset = sizeof(payload.header);
        /*
            Receive a message on the regular_data_q queue.
            Block for forever if a message is not immediately available.
        */
        if (xQueueReceive(regular_data_q, &ch_data, portMAX_DELAY) == pdTRUE)
        {
            uint32_t channel_size = channel_size_table[ch_data.type % REGULAR_MAX_TYPE];
            memcpy(&payload.raw[offset], &ch_data, channel_size);
            offset += channel_size;
            /* set channel len = 1 */
            payload.header.channel_len = 1;
        }
        else
        {
            continue;
        }

        extern uint16_t le_gatt_mtu(void);
        uint16_t att_mtu = le_gatt_mtu() - 3;
        for (;;)
        {
            /*
                Receive a message on the regular_data_q queue.
                Block for 50ms if a message is not immediately available.
                if there is no new message after 50ms, breaks out of the loop.
            */
            if (xQueueReceive(regular_data_q, &ch_data, pdMS_TO_TICKS(50)) == pdTRUE)
            {
                /* set channel data total length. */
                uint32_t channel_size = channel_size_table[ch_data.type % REGULAR_MAX_TYPE];
                /* copy channel data to payload buffer. */
                memcpy(&payload.raw[offset], &ch_data, channel_size);
                offset += channel_size;
                /* increase channel len */
                payload.header.channel_len++;
                /* if payload length is almost full, breaks out of the loop. */
                if (offset > (att_mtu - (sizeof(ch_data) - 2) - 1))
                {
                    break;
                }
            }
            else
            {
                break;
            }
        }

        extern bool le_regular_data_notify_is_enable(void);
        if (le_regular_data_notify_is_enable())
        {
            extern ret_code_t le_regular_data_notify(uint8_t * p_value, uint16_t len);
            /* update payload length & sequence number */
            payload.header.len = offset + 1;
            payload.header.seq++;
            for (int i = 0; i < 3; i++)
            {
                /* update payload notify time */
                payload.header.time = xTaskGetTickCount();
                /* calculate checksum */
                uint8_t chksum = 0;
                for (uint32_t i = 0; i < offset; i++)
                {
                    chksum += payload.raw[i];
                }
                /* append checksum */
                payload.raw[offset] = chksum;
                /* try to send notify */
                ret_code_t err_code = le_regular_data_notify(payload.raw, offset + 1);
                /* if notify success, breaks out of the loop. */
                if (err_code == NRF_SUCCESS)
                {
                    break;
                }
                /* if timeout, breaks out of the loop. */
                if (xSemaphoreTake(regular_data_sem, pdMS_TO_TICKS(50)) == false)
                {
                    NRF_LOG_INFO("regulat data underrun.");
                    break;
                }
            }
        }
    }
}

static void edc_regular_data_handler(ble_evt_t const *p_ble_evt, void *p_context)
{
    switch (p_ble_evt->header.evt_id)
    {
        case BLE_GAP_EVT_CONNECTED:
            payload.header.seq = 0xFF;
            xQueueReset(regular_data_q);
            break;
        case BLE_GAP_EVT_DISCONNECTED:
            break;
        case BLE_GATTS_EVT_HVN_TX_COMPLETE:
            xSemaphoreGive(regular_data_sem);
            break;
        default:
            break;
    }
}

void edc_regular_data_init(void)
{
    regular_data_q = xQueueCreate(64, sizeof(channel_data_t));
    if (regular_data_q == NULL)
    {
        // Will not get here unless there is insufficient RAM.
        __BKPT(255);
    }

    regular_data_sem = xSemaphoreCreateBinary();
    if (regular_data_sem == NULL)
    {
        // Will not get here unless there is insufficient RAM.
        __BKPT(255);
    }

    xTaskCreate(regular_data_msg_task, "regular_msg", 192, NULL, 5, NULL);
    xTaskCreate(pseudo_data_task, "pseudo_AA", 192, pseudo_sensorAA_read, 2, NULL);
    xTaskCreate(pseudo_data_task, "pseudo_BB", 192, pseudo_sensorBB_read, 2, NULL);
    NRF_SDH_BLE_OBSERVER(m_edc_regular_data_observer, 3, edc_regular_data_handler, NULL);
}