#include <Arduino.h>
#include <array>
#include <utility>
#include <stdexcept> // For std::overflow_error

template <typename T, size_t Capacity>
class MessageQueue {
private:
    std::array<std::pair<unsigned long,T>, Capacity> data;
    size_t last = 0;
    size_t current_size = 0;

public:
    void check()
    {
        if (current_size == 0) return;
        unsigned long now = millis();
        for (size_t ptr = 0; ptr < Capacity; ptr++) {
            if (data[ptr].first && now > data[ptr].first) {
                data[ptr].first = 0;
                current_size--;
            }
        }
        while (data[last].first == 0 && last > 0) last--;
    }

    void add(const T &value, unsigned long timeout)
    {
        unsigned long now = millis();
        check();
        size_t pos = last;
        while (pos < Capacity && data[pos].first)
            pos++;
        if (pos == Capacity) {
            pos = 0; 
            while (pos < last - 1 && data[pos].first) 
                pos++;
        }
        if (data[pos].first == 0)
            current_size++;        
        data[pos].first = millis() + timeout;
        data[pos].second = value;
        last = pos;
    }

    T* get()
    {
        check();
        if (current_size == 0) {
            return NULL;
        }
        if (data[last].first)
            return &data[last].second;
        size_t pos = last;
        while (pos > 0 && data[pos].first == 0)
            pos--;
        if (data[pos].first) {
            last = pos;
            return &data[last].second;
        }
        pos = last + 1;
        while (pos < Capacity && data[pos].first == 0)
            pos++;
        if (data[pos].first) {
            last = pos;
            return &data[last].second;
        }
        return NULL;
    }

    bool empty() const
    {
        return current_size == 0;
    }

    bool full() const
    {
        return current_size == Capacity;
    }

    size_t size() const
    {
        return current_size;
    }

    size_t capacity() const
    {
        return Capacity;
    }
};