#ifndef H_97BF73781549459A88697C808E132C22
#define H_97BF73781549459A88697C808E132C22

#include "pico/util/queue.h"
#include <type_traits>
//#include <utility>

template <typename T>
class Queue
{
    private:
        queue_t queue;

    public:
        static constexpr bool wrapped = !std::is_pod<T>();
        Queue(uint size)
        {
            if constexpr (wrapped)
            {
                queue_init(&queue, sizeof(T*), size);
            }
            else
            {
                queue_init(&queue, sizeof(T), size);
            }
        }
        bool try_add(T *item)
        {
            if constexpr (wrapped)
            {
                T* temp = new T(*item);
                return queue_try_add(&queue, &temp);
            }
            else
            {
                return queue_try_add(&queue, item);
            }
        }
        void blocking_add(T* item)
        {
            if constexpr (wrapped)
            {
                T* temp = new T(*item);
                queue_add_blocking(&queue, &temp);
            }
            else
            {
                queue_add_blocking(&queue, item);
            }
        }
        bool try_remove(T* item)
        {
            if constexpr (wrapped)
            {
                T** temp;
                bool success = queue_try_remove(&queue, temp);
                if (success)
                    *item = **temp;
                return success;
            }
            else
            {
                return queue_try_remove(&queue, item);
            }
        }
        void blocking_remove(T* item)
        {
            if constexpr (wrapped)
            {
                T** temp;
                queue_remove_blocking(&queue, temp);
                *item = **temp;
            }
            else
            {
                queue_remove_blocking(&queue, item);
            }
        }
        ~Queue()
        {
            queue_free(&queue);
        }
};

#endif //H_97BF73781549459A88697C808E132C22