#include <cassert>
#include <string.h>
#include <type_traits>

#include "freertos/queue.hpp"

namespace
{
unsigned dynamic_creates;
unsigned static_creates;
unsigned deletes;
TickType_t last_timeout;

struct Sample
{
    uint32_t sequence;
    uint32_t value;
};

bool equal( const Sample & a, const Sample & b )
{
    return a.sequence == b.sequence && a.value == b.value;
}
} // namespace

extern "C" QueueHandle_t k10_create_dynamic( UBaseType_t capacity,
                                               UBaseType_t item_size )
{
    auto * queue = new QueueDefinition {};
    queue->capacity = capacity;
    queue->item_size = item_size;
    queue->storage = new uint8_t[ capacity * item_size ];
    queue->dynamic_control = pdTRUE;
    ++dynamic_creates;
    return queue;
}

extern "C" QueueHandle_t k10_create_static( UBaseType_t capacity,
                                              UBaseType_t item_size,
                                              uint8_t * bytes,
                                              StaticQueue_t * control )
{
    *control = QueueDefinition {};
    control->capacity = capacity;
    control->item_size = item_size;
    control->storage = bytes;
    control->dynamic_control = pdFALSE;
    ++static_creates;
    return control;
}

extern "C" BaseType_t k10_send( QueueHandle_t queue, const void * item,
                                  TickType_t timeout )
{
    last_timeout = timeout;
    if( queue->count == queue->capacity ) return pdFAIL;
    memcpy( queue->storage + queue->tail * queue->item_size,
            item, queue->item_size );
    queue->tail = ( queue->tail + 1U ) % queue->capacity;
    ++queue->count;
    return pdPASS;
}

extern "C" BaseType_t k10_receive( QueueHandle_t queue, void * item,
                                     TickType_t timeout )
{
    last_timeout = timeout;
    if( queue->count == 0U ) return pdFAIL;
    memcpy( item, queue->storage + queue->head * queue->item_size,
            queue->item_size );
    queue->head = ( queue->head + 1U ) % queue->capacity;
    --queue->count;
    return pdPASS;
}

extern "C" UBaseType_t uxQueueMessagesWaiting( QueueHandle_t queue )
{
    return queue->count;
}

extern "C" void vQueueDelete( QueueHandle_t queue )
{
    ++deletes;
    if( queue->dynamic_control != pdFALSE )
    {
        delete[] queue->storage;
        delete queue;
    }
}

static_assert( std::is_trivially_copyable< Sample >::value );
static_assert( !std::is_copy_constructible< freertos::Queue< Sample > >::value );
static_assert( !std::is_move_constructible<
               freertos::StaticQueue< Sample, 2U > >::value );

int main()
{
    {
        freertos::Queue< Sample > dynamic { 2U };
        assert( dynamic.valid() && dynamic.capacity() == 2U );
        assert( dynamic_creates == 1U );
        Sample source { 1U, 0x1111U };
        const Sample expected = source;
        assert( dynamic.send( source ) );
        source.value = 0xDEADU;
        Sample copied {};
        assert( dynamic.receive( copied ) );
        assert( equal( copied, expected ) );
    }
    assert( deletes == 1U );

    freertos::StaticQueueStorage< Sample, 2U > storage {};
    {
        freertos::StaticQueue< Sample, 2U > statik { storage };
        assert( statik.valid() );
        assert( statik.native_handle() == &storage.control );
        assert( static_creates == 1U );
        const Sample first { 2U, 0x2222U };
        const Sample second { 3U, 0x3333U };
        const Sample third { 4U, 0x4444U };
        assert( statik.send( first ) && statik.send( second ) );
        assert( statik.size() == 2U );
        assert( !statik.send( third, 7U ) && last_timeout == 7U );
        Sample out {};
        assert( statik.receive( out ) && equal( out, first ) );
        assert( statik.receive( out ) && equal( out, second ) );
        assert( !statik.receive( out ) );
    }
    assert( deletes == 2U );
}