#include "route_msg.h"

#include <string.h>

#include "task.h"

typedef struct {
    route_msg_t msg;
    uint8_t data[ROUTE_MSG_MAX_PAYLOAD];
    uint8_t in_use;
} route_slot_t;

static route_slot_t g_route_slots[ROUTE_MSG_POOL_SIZE];

void route_msg_init(void)
{
    memset(g_route_slots, 0, sizeof(g_route_slots));
}

static route_msg_t *route_msg_try_alloc_locked(void)
{
    uint32_t index;

    for (index = 0; index < ROUTE_MSG_POOL_SIZE; ++index) {
        if (g_route_slots[index].in_use == 0u) {
            g_route_slots[index].in_use = 1u;
            g_route_slots[index].msg.data = g_route_slots[index].data;
            g_route_slots[index].msg.len = 0u;
            g_route_slots[index].msg.src_id = 0u;
            g_route_slots[index].msg.dst_mask = 0u;
            g_route_slots[index].msg.conn_type = 0u;
            return &g_route_slots[index].msg;
        }
    }

    return NULL;
}

route_msg_t *route_msg_alloc(TickType_t wait_ticks)
{
    TickType_t start_tick = xTaskGetTickCount();
    route_msg_t *msg;

    do {
        taskENTER_CRITICAL();
        msg = route_msg_try_alloc_locked();
        taskEXIT_CRITICAL();
        if (msg != NULL) {
            return msg;
        }
        if (wait_ticks == 0u) {
            break;
        }
        vTaskDelay(pdMS_TO_TICKS(1));
    } while ((xTaskGetTickCount() - start_tick) < wait_ticks);

    return NULL;
}

route_msg_t *route_msg_alloc_from_isr(BaseType_t *xHigherPriorityTaskWoken)
{
    route_msg_t *msg;
    UBaseType_t saved_interrupt_status;

    (void)xHigherPriorityTaskWoken;
    saved_interrupt_status = taskENTER_CRITICAL_FROM_ISR();
    msg = route_msg_try_alloc_locked();
    taskEXIT_CRITICAL_FROM_ISR(saved_interrupt_status);
    return msg;
}

void route_msg_free(route_msg_t *msg)
{
    uint32_t index;

    if (msg == NULL) {
        return;
    }

    taskENTER_CRITICAL();
    for (index = 0; index < ROUTE_MSG_POOL_SIZE; ++index) {
        if (&g_route_slots[index].msg == msg) {
            g_route_slots[index].in_use = 0u;
            g_route_slots[index].msg.len = 0u;
            break;
        }
    }
    taskEXIT_CRITICAL();
}

void route_msg_free_from_isr(route_msg_t *msg)
{
    uint32_t index;
    UBaseType_t saved_interrupt_status;

    if (msg == NULL) {
        return;
    }

    saved_interrupt_status = taskENTER_CRITICAL_FROM_ISR();
    for (index = 0; index < ROUTE_MSG_POOL_SIZE; ++index) {
        if (&g_route_slots[index].msg == msg) {
            g_route_slots[index].in_use = 0u;
            g_route_slots[index].msg.len = 0u;
            break;
        }
    }
    taskEXIT_CRITICAL_FROM_ISR(saved_interrupt_status);
}

static bool route_prepare(route_msg_t *msg,
                          uint8_t src_id,
                          uint8_t dst_mask,
                          uint8_t conn_type,
                          const uint8_t *data,
                          uint16_t len)
{
    if (msg == NULL || data == NULL || len == 0u || len > ROUTE_MSG_MAX_PAYLOAD) {
        return false;
    }

    msg->src_id = src_id;
    msg->dst_mask = dst_mask;
    msg->conn_type = conn_type;
    msg->len = len;
    memcpy(msg->data, data, len);
    return true;
}

bool route_send(QueueHandle_t queue,
                uint8_t src_id,
                uint8_t dst_mask,
                uint8_t conn_type,
                const uint8_t *data,
                uint16_t len,
                TickType_t wait_ticks)
{
    route_msg_t *msg = route_msg_alloc(wait_ticks);

    if (!route_prepare(msg, src_id, dst_mask, conn_type, data, len)) {
        route_msg_free(msg);
        return false;
    }

    if (xQueueSend(queue, &msg, wait_ticks) != pdPASS) {
        route_msg_free(msg);
        return false;
    }

    return true;
}

bool route_send_from_isr(QueueHandle_t queue,
                         uint8_t src_id,
                         uint8_t dst_mask,
                         uint8_t conn_type,
                         const uint8_t *data,
                         uint16_t len,
                         BaseType_t *xHigherPriorityTaskWoken)
{
    route_msg_t *msg = route_msg_alloc_from_isr(xHigherPriorityTaskWoken);

    if (!route_prepare(msg, src_id, dst_mask, conn_type, data, len)) {
        route_msg_free_from_isr(msg);
        return false;
    }

    if (xQueueSendFromISR(queue, &msg, xHigherPriorityTaskWoken) != pdPASS) {
        route_msg_free_from_isr(msg);
        return false;
    }

    return true;
}