#include "cmsis_os2.h"
#include <stdbool.h>

void Error_Handler() {

}

void MX_USB_DEVICE_Init() {
    
}

#define NUM_BUFFERS 10
static uint32_t mockQueue[NUM_BUFFERS];
static int mockQueueFront = 0;
static int mockQueueBack = 0;
static bool mockQueueEmpty = true;

osThreadId_t osThreadNew(osThreadFunc_t func, void *argument, const osThreadAttr_t *attr)
{
    return (osThreadId_t)1;
}

osStatus_t osMessageQueueGet (osMessageQueueId_t mq_id, void *msg_ptr, uint8_t *msg_prio, uint32_t timeout) {
    if (!mockQueueEmpty)
    {
        // Remove the message from the queue
        *(uint32_t *)msg_ptr = mockQueue[mockQueueFront];
        mockQueueFront = (mockQueueFront + 1) % NUM_BUFFERS;
        if (mockQueueFront == mockQueueBack)
        {
            mockQueueEmpty = true;
        }
        return osOK;
    }
    else
    {
        // The queue is empty
        return osErrorResource;
    }
}

osStatus_t osMessageQueuePut (osMessageQueueId_t mq_id, const void *msg_ptr, uint8_t msg_prio, uint32_t timeout) {
    if (mockQueueEmpty || mockQueueBack != mockQueueFront)
    {
        // Add the message to the queue
        mockQueue[mockQueueBack] = *(uint32_t *)msg_ptr;
        mockQueueBack = (mockQueueBack + 1) % NUM_BUFFERS;
        mockQueueEmpty = false;
        return osOK;
    }
    else
    {
        // The queue is full
        return osErrorResource;
    }
}


osMessageQueueId_t osMessageQueueNew (uint32_t msg_count, uint32_t msg_size, const osMessageQueueAttr_t *attr) {
    return &mockQueue;
}