#include "nh_shared_if.h"

#include "string.h"

#define NH_XFER_REQ_TIMEOUT      5000
#define NH_XFER_BLOCKING_TIMEOUT 5000
#define NH_XFER_MAX_SIZE         1600
#define NH_XFER_QUEUE_SIZE       1

#ifdef __GNUC__
#if __BYTE_ORDER__ == __ORDER_LITTLE_ENDIAN__
#define H_TO_LE16(x) (x)
#define LE16_TO_H(x) (x)
#else
#define H_TO_LE16(x) (((x & 0xFFU) << 8U) | (x >> 8U))
#endif
#else
#warning "This code only supports GCC for now."
#endif

typedef enum {
    NH_SHARED_IF_TYPE_STA    = 0U,
    NH_SHARED_IF_TYPE_AP     = 1U,
    NH_SHARED_IF_TYPE_SERIAL = 2U,
    NH_SHARED_IF_TYPE_PRIV   = 4U,
} nh_xfer_type_t;

typedef enum {
    NH_SHARED_IF_PRIV_TYPE_EVENT,
} nh_xfer_priv_type_t;

typedef struct {
    uint32_t type;
    uint8_t *buf;
    uint32_t buf_len;
} nh_xfer_queue_item_t;

typedef struct __attribute__((packed)) {
    uint8_t  if_type : 4; /* Offset: 0x00 */
    uint8_t  if_num : 4;  /* Offset: 0x00 */
    uint8_t  flags;       /* Offset: 0x01 */
    uint16_t len;         /* Offset: 0x02 */
    uint16_t offset;      /* Offset: 0x04 */
    uint16_t checksum;    /* Offset: 0x06 */
    uint16_t seq_num;     /* Offset: 0x08 */
    uint8_t  reserved2;   /* Offset: 0x0A */
    union {
        uint8_t reserved3;
        uint8_t hci_pkt_type;
        uint8_t priv_pkt_type;
    };               /* Offset: 0x0B */
} nh_xfer_pkt_hdr_t; /* Size: 0x0C (12 bytes) */

static void nh_shared_if_dispatch_sta(nh_shared_if_t *shared_if, uint8_t *payload, uint16_t len);
static void nh_shared_if_dispatch_ap(nh_shared_if_t *shared_if, uint8_t *payload, uint16_t len);
static void nh_shared_if_dispatch_if_serial(nh_shared_if_t *shared_if, uint8_t *payload, uint16_t len);
static void nh_shared_if_dispatch_if_priv(nh_shared_if_t *shared_if, nh_xfer_priv_type_t type, uint8_t *payload);

static uint16_t nh_shared_if_checksum_calculate(nh_xfer_pkt_hdr_t *hdr);
static bool     nh_shared_if_checksum_verify(nh_xfer_pkt_hdr_t *hdr);

nh_ret_t nh_shared_if_init(nh_shared_if_t *shared_if) {
    nh_ret_t ret;

    ret = shared_if->osa->sem_create(shared_if->osa->user_data, &shared_if->p_semaphore_xfer_req);
    if (ret != NH_RET_SUCCESS) {
        return ret;
    }

    ret = shared_if->osa->queue_create(shared_if->osa->user_data, &shared_if->p_queue_tx, sizeof(nh_xfer_queue_item_t),
                                       NH_XFER_QUEUE_SIZE);
    if (ret != NH_RET_SUCCESS) {
        goto err_free_sem_xfer;
    }

    ret = shared_if->osa->queue_create(shared_if->osa->user_data, &shared_if->p_queue_rx_data,
                                       sizeof(nh_xfer_queue_item_t), NH_XFER_QUEUE_SIZE);
    if (ret != NH_RET_SUCCESS) {
        goto err_free_queue_tx;
    }

    ret = shared_if->osa->queue_create(shared_if->osa->user_data, &shared_if->p_queue_rx_serial,
                                       sizeof(nh_xfer_queue_item_t), NH_XFER_QUEUE_SIZE);
    if (ret != NH_RET_SUCCESS) {
        goto err_free_queue_rx_data;
    }

    ret = shared_if->osa->buf_allocate(shared_if->osa->user_data, &shared_if->p_buf_frame_tx, NH_XFER_MAX_SIZE);
    if (ret != NH_RET_SUCCESS) {
        goto err_free_queue_rx_ctrl;
    }

    ret = shared_if->osa->buf_allocate(shared_if->osa->user_data, &shared_if->p_buf_frame_rx, NH_XFER_MAX_SIZE);
    if (ret != NH_RET_SUCCESS) {
        goto err_free_tx_buf;
    }

    return ret;

err_free_tx_buf:
    shared_if->osa->buf_free(shared_if->osa->user_data, shared_if->p_buf_frame_tx);

err_free_queue_rx_ctrl:
    shared_if->osa->queue_destroy(shared_if->osa->user_data, shared_if->p_queue_rx_serial);

err_free_queue_rx_data:
    shared_if->osa->queue_destroy(shared_if->osa->user_data, shared_if->p_queue_rx_data);

err_free_queue_tx:
    shared_if->osa->queue_destroy(shared_if->osa->user_data, shared_if->p_queue_tx);

err_free_sem_xfer:
    shared_if->osa->sem_destroy(shared_if->osa->user_data, shared_if->p_semaphore_xfer_req);

    return ret;
}

void nh_shared_if_inject_data_ready(nh_shared_if_t *shared_if) {
    shared_if->osa->sem_give(shared_if->osa->user_data, shared_if->p_semaphore_xfer_req);
}

nh_ret_t nh_shared_if_serial_if_send(nh_shared_if_t *shared_if, uint8_t *tx_payload, uint32_t len,
                                     uint32_t timeout_ms) {
    nh_ret_t ret = NH_RET_SUCCESS;

    uint16_t header_len;
    nh_serial_header_length(NH_SERIAL_EP_RESP, &header_len);

    nh_xfer_queue_item_t item = {
        .type    = NH_SHARED_IF_TYPE_SERIAL,
        .buf_len = len + header_len,
    };

    ret = shared_if->osa->buf_allocate(shared_if->osa->user_data, &item.buf, item.buf_len);
    if (ret != NH_RET_SUCCESS) {
        return ret;
    }

    uint8_t *data_start;
    nh_serial_write_header(NH_SERIAL_EP_RESP, item.buf, len, &data_start);

    memcpy(data_start, tx_payload, len);

    ret = shared_if->osa->queue_enqueue(shared_if->osa->user_data, shared_if->p_queue_tx, &item, timeout_ms);
    if (ret != NH_RET_SUCCESS) {
        shared_if->osa->buf_free(shared_if->osa->user_data, item.buf);

        return ret;
    }

    ret = shared_if->osa->sem_give(shared_if->osa->user_data, shared_if->p_semaphore_xfer_req);
    return ret;
}

nh_ret_t nh_shared_if_serial_if_recv(nh_shared_if_t *shared_if, nh_serial_ep_type_t *ep_type, uint8_t **rx_payload,
                                     uint32_t *len, uint32_t timeout_ms) {
    nh_ret_t ret = NH_RET_SUCCESS;

    nh_xfer_queue_item_t item;

    ret = shared_if->osa->queue_dequeue(shared_if->osa->user_data, shared_if->p_queue_rx_serial, &item, timeout_ms);
    if (ret != NH_RET_SUCCESS) {
        return ret;
    }

    *rx_payload = item.buf;
    *len        = item.buf_len;
    *ep_type    = item.type;

    return ret;
}

nh_ret_t nh_shared_if_serial_if_free(nh_shared_if_t *shared_if, uint8_t *rx_payload) {
    return shared_if->osa->buf_free(shared_if->osa->user_data, rx_payload);
}

void nh_shared_if_task(nh_shared_if_t *shared_if) {
    nh_ret_t ret;

    ret = shared_if->osa->sem_take(shared_if->osa->user_data, shared_if->p_semaphore_xfer_req, NH_XFER_REQ_TIMEOUT);
    if (ret != NH_RET_SUCCESS) {
        return;
    }

    /* Taken the xfer request semaphore. The possible reasons are:
     * 1: Some data (could from any interfaces) needs to be sent
     * 2: Data ready pin is high when HS at rising, some data needs to be received
     */

    ret = shared_if->ops.hs_poll(shared_if->user_data, NH_XFER_REQ_TIMEOUT);
    if (ret != NH_RET_SUCCESS) {
        /* HS is not ready, queue another transfer */
        shared_if->osa->sem_give(shared_if->osa->user_data, shared_if->p_semaphore_xfer_req);

        return;
    }

    bool rx_available = false;

    /* Check if new data is available */
    ret = shared_if->ops.drdy_read(shared_if->user_data, &rx_available);
    if (ret != NH_RET_SUCCESS) {
        return;
    }

    nh_xfer_queue_item_t item;

    /* Clear TX header */
    memset(shared_if->p_buf_frame_tx, 0U, sizeof(nh_xfer_pkt_hdr_t));

    /* Check if we have data to send */
    ret = shared_if->osa->queue_dequeue(shared_if->user_data, shared_if->p_queue_tx, &item, 0U);
    if (ret == NH_RET_SUCCESS) {
        /* Write header and payload since we have data to send */
        nh_xfer_pkt_hdr_t *hdr = (nh_xfer_pkt_hdr_t *)shared_if->p_buf_frame_tx;

        hdr->if_type = item.type;
        hdr->offset  = H_TO_LE16(sizeof(nh_xfer_pkt_hdr_t)); /* Payload follows header... */
        hdr->len     = H_TO_LE16(item.buf_len);

        /* Copy payload */
        memcpy(&shared_if->p_buf_frame_tx[sizeof(nh_xfer_pkt_hdr_t)], item.buf, item.buf_len);

        hdr->checksum = H_TO_LE16(nh_shared_if_checksum_calculate(hdr));

        /* Queue consumer owns the buffer. */
        shared_if->osa->buf_free(shared_if->osa->user_data, item.buf);
    }

    /* Exchange buffers */
    ret = shared_if->ops.xfer(shared_if->user_data, shared_if->p_buf_frame_tx, shared_if->p_buf_frame_rx,
                              NH_XFER_MAX_SIZE);
    if (ret != NH_RET_SUCCESS) {
        return;
    }

    /* If we have RX data... */
    if (rx_available) {
        nh_xfer_pkt_hdr_t *hdr = (nh_xfer_pkt_hdr_t *)shared_if->p_buf_frame_rx;
        if (!nh_shared_if_checksum_verify(hdr)) {
            return;
        }

        item.type    = LE16_TO_H(hdr->if_type);
        item.buf_len = LE16_TO_H(hdr->len);
        item.buf     = &shared_if->p_buf_frame_rx[LE16_TO_H(hdr->offset)]; /* DO NOT FREE THIS!! */

        if (!item.buf_len) {
            return;
        }

        if (item.type == NH_SHARED_IF_TYPE_SERIAL) {
            nh_shared_if_dispatch_if_serial(shared_if, item.buf, item.buf_len);
        } else if (item.type == NH_SHARED_IF_TYPE_PRIV) {
            nh_shared_if_dispatch_if_priv(shared_if, hdr->priv_pkt_type, item.buf);
        } else if (item.type == NH_SHARED_IF_TYPE_STA) {
            nh_shared_if_dispatch_sta(shared_if, item.buf, item.buf_len);
        } else if(item.type == NH_SHARED_IF_TYPE_AP) {
            nh_shared_if_dispatch_ap(shared_if, item.buf, item.buf_len);
        }
    }
}

static void nh_shared_if_dispatch_sta(nh_shared_if_t *shared_if, uint8_t *payload, uint16_t len) {
}

static void nh_shared_if_dispatch_ap(nh_shared_if_t *shared_if, uint8_t *payload, uint16_t len) {
}

static void nh_shared_if_dispatch_if_priv(nh_shared_if_t *shared_if, nh_xfer_priv_type_t type, uint8_t *payload) {
    if (type == NH_SHARED_IF_PRIV_TYPE_EVENT) {
        /* Event is using PRIV interface. */
        nh_event_type_t evt_type;
        nh_event_get_type(&evt_type, payload);

        switch (evt_type) {
            case NH_EVENT_TYPE_INIT: {
                nh_event_init_t evt_init;
                nh_event_parse_init(&evt_init, payload);

                if (!shared_if->cb.init) {
                    return;
                }

                shared_if->cb.init(shared_if->user_data, &evt_init);
            }
        }
    }
}

static void nh_shared_if_dispatch_if_serial(nh_shared_if_t *shared_if, uint8_t *payload, uint16_t len) {
    nh_ret_t ret = NH_RET_SUCCESS;

    /* Payload in SERIAL interface has two types, coded in TLV, event and response.
     * Both payloads are wrapped in protobuf, so we don't need to distinguish them.
     * Pass them to upper layer (CTRL).
     */

    uint8_t            *ctrl_data;
    uint16_t            ctrl_data_len;
    nh_serial_ep_type_t ep_type;

    ret = nh_serial_get_type(&ep_type, payload);
    if (ret != NH_RET_SUCCESS) {
        return;
    }

    /* Try to find data and length from TLV */
    ret = nh_serial_get_payload(payload, &ctrl_data, &ctrl_data_len);
    if (ret != NH_RET_SUCCESS) {
        return;
    }

    nh_xfer_queue_item_t item;

    item.type    = ep_type;
    item.buf_len = ctrl_data_len;

    /* Allocate payload */
    ret = shared_if->osa->buf_allocate(shared_if->osa->user_data, &item.buf, item.buf_len);
    if (ret != NH_RET_SUCCESS) {
        return;
    }

    memcpy(item.buf, ctrl_data, item.buf_len);

    /* Enqueue response */
    ret = shared_if->osa->queue_enqueue(shared_if->osa->user_data, shared_if->p_queue_rx_serial, &item,
                                        NH_XFER_BLOCKING_TIMEOUT);
    if (ret != NH_RET_SUCCESS) {
        /* Queue is full, maybe the ctrl interface is not active, drop the packet */
        shared_if->osa->buf_free(shared_if->osa->user_data, item.buf);
    }
}

static uint16_t nh_shared_if_checksum_calculate(nh_xfer_pkt_hdr_t *hdr) {
    uint16_t checksum = 0U;

    uint16_t checksum_end = sizeof(nh_xfer_pkt_hdr_t) + LE16_TO_H(hdr->len);

    for (uint16_t i = 0; i < checksum_end; i++) {
        checksum += ((uint8_t *)hdr)[i];
    }

    return checksum;
}

static bool nh_shared_if_checksum_verify(nh_xfer_pkt_hdr_t *hdr) {
    bool ret = true;

    uint16_t checksum = LE16_TO_H(hdr->checksum);
    hdr->checksum     = 0U;

    if (checksum != nh_shared_if_checksum_calculate(hdr)) {
        return false;
    }

    hdr->checksum = H_TO_LE16(checksum);

    return ret;
}