Move C++ tree into firmware/

firmware/include/sorted_list.h

@@ -0,0 +1,58 @@
+#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(); }
+
+    void insert(T value) {
+        if (full()) return;
+        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;
+        }
+
+        node* cur = head;
+        while (cur->next && !(n->value() < cur->next->value()))
+            cur = cur->next;
+        n->next = cur->next;
+        cur->next = n;
+    }
+
+    void pop_front() {
+        if (empty()) return;
+        node* n = head;
+        head = n->next;
+        n->value().~T();
+        n->next = free_head;
+        free_head = n;
+    }
+};