Move C++ tree into firmware/

firmware/include/timer_queue.h

@@ -0,0 +1,50 @@
+#pragma once
+#include <functional>
+#include "pico/time.h"
+#include "sorted_list.h"
+
+struct timer_entry {
+    absolute_time_t when;
+    std::function<void()> fn;
+};
+
+inline bool operator<(const timer_entry& a, const timer_entry& b) {
+    return absolute_time_diff_us(b.when, a.when) < 0;
+}
+
+struct timer_queue {
+    sorted_list<timer_entry, 16> queue;
+    alarm_id_t alarm = -1;
+
+    void schedule(absolute_time_t when, std::function<void()> fn) {
+        queue.insert({when, std::move(fn)});
+        arm();
+    }
+
+    void schedule_ms(uint32_t ms, std::function<void()> fn) {
+        schedule(make_timeout_time_ms(ms), std::move(fn));
+    }
+
+    void run() {
+        while (!queue.empty()) {
+            auto& front = queue.front();
+            if (absolute_time_diff_us(get_absolute_time(), front.when) > 0) break;
+            auto fn = std::move(front.fn);
+            queue.pop_front();
+            fn();
+        }
+        arm();
+    }
+
+    bool empty() const { return queue.empty(); }
+
+private:
+    static int64_t alarm_cb(alarm_id_t, void*) { return 0; }
+
+    void arm() {
+        if (alarm >= 0) cancel_alarm(alarm);
+        alarm = -1;
+        if (!queue.empty())
+            alarm = add_alarm_at(queue.front().when, alarm_cb, nullptr, false);
+    }
+};