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

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

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

    timer_handle schedule(absolute_time_t when, std::function<void()> fn) {
        auto* n = queue.insert({when, std::move(fn)});
        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) {
        bool removed = queue.remove(h);
        if (removed) arm();
        return removed;
    }

    void run() {
        if (!irq_pending) return;
        irq_pending = false;
        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* user_data) {
        static_cast<timer_queue*>(user_data)->irq_pending = true;
        return 0;
    }

    void arm() {
        if (alarm >= 0) cancel_alarm(alarm);
        alarm = -1;
        if (!queue.empty())
            alarm = add_alarm_at(queue.front().when, alarm_cb, this, false);
    }
};