#ifndef FREERTOS_CPP_QUEUE_HPP
#define FREERTOS_CPP_QUEUE_HPP

#include <stddef.h>
#include <stdint.h>
#include <type_traits>

extern "C"
{
#include "FreeRTOS.h"
#include "queue.h"
}

namespace freertos
{

template< typename T >
class Queue final
{
    static_assert( std::is_trivially_copyable< T >::value,
                   "Queue<T> requires T to be trivially copyable" );

public:
    explicit Queue( UBaseType_t capacity ) noexcept
        : handle_ { xQueueCreate( capacity, sizeof( T ) ) },
          capacity_ { capacity }
    {
    }

    ~Queue() noexcept
    {
        if( handle_ != nullptr )
        {
            vQueueDelete( handle_ );
        }
    }

    Queue( const Queue & ) = delete;
    Queue & operator=( const Queue & ) = delete;
    Queue( Queue && ) = delete;
    Queue & operator=( Queue && ) = delete;

    [[nodiscard]] bool valid() const noexcept { return handle_ != nullptr; }

    [[nodiscard]] bool send( const T & item,
                             TickType_t timeout = 0U ) noexcept
    {
        return handle_ != nullptr &&
               xQueueSend( handle_, &item, timeout ) == pdPASS;
    }

    [[nodiscard]] bool receive( T & item,
                                TickType_t timeout = 0U ) noexcept
    {
        return handle_ != nullptr &&
               xQueueReceive( handle_, &item, timeout ) == pdPASS;
    }

    [[nodiscard]] UBaseType_t size() const noexcept
    {
        return handle_ == nullptr ? 0U : uxQueueMessagesWaiting( handle_ );
    }

    [[nodiscard]] UBaseType_t capacity() const noexcept { return capacity_; }
    [[nodiscard]] QueueHandle_t native_handle() const noexcept { return handle_; }

private:
    QueueHandle_t handle_ {};
    const UBaseType_t capacity_;
};

template< typename T, size_t Capacity >
struct StaticQueueStorage final
{
    static_assert( std::is_trivially_copyable< T >::value,
                   "Queue<T> requires T to be trivially copyable" );
    static_assert( Capacity > 0U, "StaticQueue capacity must be positive" );

    StaticQueue_t control {};
    alignas( T ) uint8_t bytes[ Capacity * sizeof( T ) ] {};
};

template< typename T, size_t Capacity >
class StaticQueue final
{
    static_assert( std::is_trivially_copyable< T >::value,
                   "Queue<T> requires T to be trivially copyable" );
    static_assert( Capacity > 0U, "StaticQueue capacity must be positive" );

public:
    explicit StaticQueue( StaticQueueStorage< T, Capacity > & storage ) noexcept
        : handle_ { xQueueCreateStatic(
              static_cast< UBaseType_t >( Capacity ), sizeof( T ),
              storage.bytes, &storage.control ) },
          storage_ { storage }
    {
    }

    ~StaticQueue() noexcept
    {
        if( handle_ != nullptr )
        {
            vQueueDelete( handle_ );
        }
    }

    StaticQueue( const StaticQueue & ) = delete;
    StaticQueue & operator=( const StaticQueue & ) = delete;
    StaticQueue( StaticQueue && ) = delete;
    StaticQueue & operator=( StaticQueue && ) = delete;

    [[nodiscard]] bool valid() const noexcept { return handle_ != nullptr; }

    [[nodiscard]] bool send( const T & item,
                             TickType_t timeout = 0U ) noexcept
    {
        return handle_ != nullptr &&
               xQueueSend( handle_, &item, timeout ) == pdPASS;
    }

    [[nodiscard]] bool receive( T & item,
                                TickType_t timeout = 0U ) noexcept
    {
        return handle_ != nullptr &&
               xQueueReceive( handle_, &item, timeout ) == pdPASS;
    }

    [[nodiscard]] UBaseType_t size() const noexcept
    {
        return handle_ == nullptr ? 0U : uxQueueMessagesWaiting( handle_ );
    }

    static constexpr size_t capacity() noexcept { return Capacity; }
    static constexpr size_t item_size() noexcept { return sizeof( T ); }
    [[nodiscard]] QueueHandle_t native_handle() const noexcept { return handle_; }
    [[nodiscard]] const StaticQueueStorage< T, Capacity > & storage() const noexcept
    {
        return storage_;
    }

private:
    QueueHandle_t handle_ {};
    StaticQueueStorage< T, Capacity > & storage_;
};

} // namespace freertos

#endif