#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);
    }
};