picomap/firmware/include/timer_queue.h

#pragma once
#include <functional>
#include "pico/time.h"
#include "callback_list.h"

struct timer_entry {
    absolute_time_t when;
    std::function<void()> fn;
};

using timer_handle = callback_list<timer_entry, 16>::node*;

struct timer_queue {
    callback_list<timer_entry, 16> list;
    alarm_id_t alarm = -1;
    volatile bool irq_pending = false;

    timer_handle schedule(absolute_time_t when, std::function<void()> fn) {
        auto* n = list.insert_sorted({when, std::move(fn)},
            [](const timer_entry& a, const timer_entry& b) {
                return absolute_time_diff_us(b.when, a.when) < 0;
            });
        arm();
        return n;
    }

    timer_handle schedule_ms(uint32_t ms, std::function<void()> fn) {
        return schedule(make_timeout_time_ms(ms), std::move(fn));
    }

    bool cancel(timer_handle h) {
        if (!h) return false;
        list.remove(h);
        arm();
        return true;
    }

    void run() {
        if (!irq_pending) return;
        irq_pending = false;
        while (auto* n = list.front()) {
            if (absolute_time_diff_us(get_absolute_time(), n->value.when) > 0) break;
            auto fn = std::move(n->value.fn);
            list.remove(n);
            fn();
        }
        arm();
    }

    bool empty() const { return list.empty(); }

private:
    static int64_t alarm_cb(alarm_id_t, void* user_data) {
        static_cast<timer_queue*>(user_data)->irq_pending = true;
        return 0;
    }

    void arm() {
        if (alarm >= 0) cancel_alarm(alarm);
        alarm = -1;
        if (auto* n = list.front())
            alarm = add_alarm_at(n->value.when, alarm_cb, this, false);
    }
};