#include <stdint.h>

#include "cos_lpc51u68_bsp.h"
#include "cos_lpc51u68_rtos.h"

#define QUEUE_CAPACITY 4u

typedef struct {
    uint32_t id;
    uint32_t produced_ms;
} queue_message_t;

static queue_message_t g_queue[QUEUE_CAPACITY];
static uint32_t g_queue_head;
static uint32_t g_queue_tail;
static uint32_t g_queue_count;

static uint32_t queue_try_send(queue_message_t message, uint32_t *depth)
{
    uint32_t sent = 0u;

    rtos_critical_enter();
    if (g_queue_count < QUEUE_CAPACITY) {
        g_queue[g_queue_tail] = message;
        g_queue_tail = (g_queue_tail + 1u) & (QUEUE_CAPACITY - 1u);
        g_queue_count++;
        sent = 1u;
    }
    *depth = g_queue_count;
    rtos_critical_exit();

    return sent;
}

static uint32_t queue_try_recv(queue_message_t *message, uint32_t *depth)
{
    uint32_t received = 0u;

    rtos_critical_enter();
    if (g_queue_count != 0u) {
        *message = g_queue[g_queue_head];
        g_queue_head = (g_queue_head + 1u) & (QUEUE_CAPACITY - 1u);
        g_queue_count--;
        received = 1u;
    }
    *depth = g_queue_count;
    rtos_critical_exit();

    return received;
}

static void task_producer(void)
{
    uint32_t message_id = 0u;

    for (;;) {
        queue_message_t message = {
            .id = message_id++,
            .produced_ms = rtos_tick_ms(),
        };
        uint32_t depth = 0u;
        while (queue_try_send(message, &depth) == 0u) {
            rtos_delay_ticks(100u);
        }
        rtos_delay_ticks(500u);
    }
}

static void task_consumer(void)
{
    for (;;) {
        queue_message_t message = {0u, 0u};
        uint32_t depth = 0u;

        if (queue_try_recv(&message, &depth) != 0u) {
            cos_lpc51u68_led_step_t step = (cos_lpc51u68_led_step_t)(message.id & 0x3u);
            rtos_set_led_step(step);
            cos_lpc51u68_uart0_puts("queue cons tick ");
            cos_lpc51u68_uart0_put_hex32(rtos_tick_ms());
            cos_lpc51u68_uart0_puts(" msg ");
            cos_lpc51u68_uart0_put_hex32(message.id);
            cos_lpc51u68_uart0_puts(" depth ");
            cos_lpc51u68_uart0_put_hex32(depth);
            cos_lpc51u68_uart0_puts("\r\n");
        }

        rtos_delay_ticks(400u);
    }
}