#include <set>
#include "fmt/format.h"

namespace sky {
constexpr std::size_t address_size = 3;
constexpr std::size_t payload_size = 15;
using address_t = uint8_t[address_size];
using payload_t = uint8_t[payload_size];

auto mcp_u32_to_address(address_t& dest, std::uint32_t const& addr) -> void {
    dest[0] = static_cast<std::uint8_t>((addr & 0x000000FF) >>  0);
    dest[1] = static_cast<std::uint8_t>((addr & 0x0000FF00) >>  8);
    dest[2] = static_cast<std::uint8_t>((addr & 0x00FF0000) >> 16);
}
}

template <typename T, std::size_t N>
class FixedSizeAllocator {
public:
    using value_type   = T;
    using pointer_type = T*;
    using size_type    = std::size_t;

    FixedSizeAllocator() : m_used_size(0) {}

    auto operator()(const T& lhs, const T& rhs) const -> bool {
        return lhs < rhs;
    }

    auto allocate(size_type n) -> pointer_type {
        if (m_used_size + n > N) throw std::bad_alloc();

        T* result = std::reinterpret_pointer_cast<value_type>(&m_pool[m_used_size]);
        m_used_size += n;
        return result;
    }

    auto deallocate(pointer_type ptr, size_type n) -> void {
        if (ptr != &m_pool[m_used_size - n])
            throw std::logic_error("Deallocating out of order");
        m_used_size -= n;
    }

private:
    std::uint8_t m_pool[N]{};
    std::size_t  m_used_size;
};

auto main() -> int {
    std::set<std::uint32_t, FixedSizeAllocator<std::uint32_t , 1024>> hello{};

    std::uint32_t addr1_u32 = 32;
    std::uint32_t addr2_u32 = 64;
    std::uint32_t addr3_u32 = 128;
    std::uint32_t addr4_u32 = 256;

    hello.insert(addr1_u32);
    hello.insert(addr2_u32);
    hello.insert(addr3_u32);
    hello.insert(addr4_u32);
    hello.insert(addr1_u32);
    hello.insert(addr1_u32);

    for (auto const& addr : hello) {
        sky::address_t buffer;
        sky::mcp_u32_to_address(buffer, addr);
        fmt::print("{:#04x}:{:#04x}:{:#04x}\n", buffer[2], buffer[1], buffer[0]);
    }

    return 0;
}