#include <cassert>
#include <cstdint>
#include <new>
#include "freertos/isr_drivers.hpp"

struct FakeQueue
{
    UBaseType_t capacity;
    UBaseType_t item_size;
    UBaseType_t count;
    std::uint8_t * storage;
};
static std::uint32_t fake_yield_calls;
static BaseType_t fake_last_yield;

extern "C" QueueHandle_t xQueueCreateStatic( UBaseType_t capacity, UBaseType_t item_size,
                                              std::uint8_t * storage, StaticQueue_t * control )
{
    static_assert( sizeof( FakeQueue ) <= sizeof( StaticQueue_t ) );
    return new ( control->bytes ) FakeQueue{ capacity, item_size, 0U, storage };
}
extern "C" BaseType_t xQueueSendFromISR( QueueHandle_t queue, const void * item,
                                          BaseType_t * woken )
{
    if( queue->count == queue->capacity ) { return pdFAIL; }
    auto const * source = static_cast< const std::uint8_t * >( item );
    for( UBaseType_t index = 0U; index < queue->item_size; ++index )
    {
        queue->storage[ queue->count * queue->item_size + index ] = source[ index ];
    }
    ++queue->count;
    *woken = pdTRUE;
    return pdPASS;
}
extern "C" BaseType_t xQueueReceive( QueueHandle_t queue, void * item, TickType_t )
{
    if( queue->count == 0U ) { return pdFAIL; }
    auto * destination = static_cast< std::uint8_t * >( item );
    for( UBaseType_t index = 0U; index < queue->item_size; ++index )
    {
        destination[ index ] = queue->storage[ index ];
    }
    --queue->count;
    return pdPASS;
}
extern "C" void vTaskNotifyGiveFromISR( TaskHandle_t, BaseType_t * woken )
{
    *woken = pdTRUE;
}
extern "C" void fake_yield_from_isr( BaseType_t request )
{
    ++fake_yield_calls;
    fake_last_yield = request;
}

int main()
{
    StaticQueue_t control{};
    std::uint8_t bytes[ 1 ]{};
    freertos::StaticIsrQueue< std::uint8_t, 1U > queue{ control, bytes };
    freertos::UartRegisters uart_registers{};
    freertos::GpioRegisters gpio_registers{};
    freertos::Uart uart{ uart_registers };
    freertos::GpioPin gpio{ gpio_registers };
    freertos::IsrContext context;
    uart.task_view().enable_rx_irq();
    uart.task_view().inject_rx_pair_for_test( 0x41U, 0x42U );
    gpio.task_view().configure_output( 1U );
    gpio.task_view().inject_edge_for_test( 1U );
    auto uart_isr = uart.isr_view();
    auto gpio_isr = gpio.isr_view();
    std::uint8_t const first = uart_isr.read_rx_from_isr();
    std::uint8_t const second = uart_isr.read_rx_from_isr();
    assert( queue.send_from_isr( first, context ) == freertos::IsrSendResult::queued );
    assert( queue.send_from_isr( second, context ) == freertos::IsrSendResult::dropped_newest );
    assert( !uart_isr.rx_ready_from_isr() );
    assert( gpio_isr.edge_pending_from_isr( 1U ) );
    gpio_isr.clear_edge_from_isr( 1U );
    freertos::TaskNotificationRef notification{ reinterpret_cast< void * >( 1U ) };
    assert( notification.give_from_isr( context ) );
    assert( context.wake_requested() && context.yield_calls() == 0U );
    assert( context.yield_if_needed() && context.yield_calls() == 1U );
    assert( fake_yield_calls == 1U && fake_last_yield == pdTRUE );
    std::uint8_t received = 0U;
    assert( queue.receive( received, 0U ) && received == 0x41U );
    gpio.task_view().write_output( 1U, true );
    assert( gpio_registers.direction == 1U && gpio_registers.data == 1U );
    assert( gpio_registers.edge_status == 0U );
}