picomap/firmware/include/sorted_list.h

#pragma once
#include <cstdint>
#include <new>
#include <utility>

template <typename T, int N>
struct sorted_list {
    struct node {
        alignas(T) uint8_t storage[sizeof(T)];
        node* next = nullptr;

        T& value() { return *reinterpret_cast<T*>(storage); }
        const T& value() const { return *reinterpret_cast<const T*>(storage); }
    };

    node nodes[N];
    node* head = nullptr;
    node* free_head = &nodes[0];

    sorted_list() {
        for (int i = 0; i < N - 1; i++) nodes[i].next = &nodes[i + 1];
        nodes[N - 1].next = nullptr;
    }

    bool empty() const { return head == nullptr; }
    bool full() const { return free_head == nullptr; }

    T& front() { return head->value(); }
    const T& front() const { return head->value(); }

    node* insert(T value) {
        if (full()) return nullptr;
        node* n = free_head;
        free_head = n->next;
        new (n->storage) T(std::move(value));

        if (!head || n->value() < head->value()) {
            n->next = head;
            head = n;
            return n;
        }

        node* cur = head;
        while (cur->next && !(n->value() < cur->next->value()))
            cur = cur->next;
        n->next = cur->next;
        cur->next = n;
        return n;
    }

    void pop_front() {
        if (empty()) return;
        node* n = head;
        head = n->next;
        n->value().~T();
        n->next = free_head;
        free_head = n;
    }

    bool remove(node* target) {
        if (!target || empty()) return false;
        if (head == target) {
            pop_front();
            return true;
        }
        node* cur = head;
        while (cur->next && cur->next != target)
            cur = cur->next;
        if (cur->next != target) return false;
        cur->next = target->next;
        target->value().~T();
        target->next = free_head;
        free_head = target;
        return true;
    }
};