//
// Created by epagris on 2022.12.21..
//

#include <stdlib.h>
#include "timer.h"
#include "dynmem.h"
#include "utils.h"

int64_t time_to_us(const TimePoint * t) {
    return (int64_t)t->s * 1000000 + (int64_t)t->us;
}

void time_from_us(TimePoint * t, int64_t us) {
    t->s = us / 1000000;
    t->us = us - ((int64_t)t->s * 1000000);
}

void time_add_us(TimePoint * t, int64_t us) {
    time_from_us(t, time_to_us(t) + us);
}

// ------------------------------

Timer *timer_new(uint32_t maxSched) {
    Timer * tmr = (Timer *) dynmem_alloc(sizeof(Timer) + maxSched * sizeof(AlarmAssignment));
    ASSERT_NULL(tmr);

    tmr->maxSched = maxSched;
    tmr->nSched = 0;
    tmr->nextAlarm = NULL;
    tmr->time.s = 0;
    tmr->time.us = 0;
    memset(tmr->alarms, 0, maxSched * sizeof(AlarmAssignment));

    return tmr;
}

static AlarmAssignment * timer_get_alarm_by_id(Timer * tmr, uint32_t id) {
    AlarmAssignment * slot = NULL;
    for (uint32_t i = 0; (i < tmr->maxSched) && (slot == NULL); i++) {
        if (tmr->alarms[i].id == id) {
            slot = (tmr->alarms) + i;
        }
    }
    return slot;
}

static uint32_t timer_generate_id(Timer * tmr) {
    uint32_t newId = 0;
    do {
        newId = ((uint32_t) rand()) | 0x01; // ID cannot be 0

    } while (timer_get_alarm_by_id(tmr, newId) != NULL);
    return newId;
}

static void timer_update_nearest_alarm(Timer * tmr) {
    if (tmr->nSched == 0) {
        tmr->nextAlarm = NULL;
        return;
    }

    int64_t t_c_us = time_to_us(&tmr->time);
    int64_t min_delta_t_us = 0;

    AlarmAssignment * nearest = NULL;
    for (uint32_t i = 0; i < tmr->maxSched; i++) {
        int64_t t_i_us = time_to_us(&(tmr->alarms[i].time));
        if ((nearest == NULL) && (tmr->alarms[i].id != 0)) {
            nearest = tmr->alarms + i;
            min_delta_t_us = t_i_us - t_c_us;
        } else {
            int64_t delta_t_us = t_i_us - t_c_us;
            if (delta_t_us < min_delta_t_us) {
                min_delta_t_us = delta_t_us;
                nearest = tmr->alarms + i;
            }
        }
    }

    tmr->nextAlarm = nearest;
}

uint32_t timer_sched(Timer * tmr, const TimePoint * t, TimerAlarmCb cb, AlarmUserData params) {
    if (tmr->nSched == tmr->maxSched) { // if cannot schedule more alarm
        return TIMER_SCHED_FAILED;
    }

    // if we can schedule get the first unused block
    AlarmAssignment * slot = timer_get_alarm_by_id(tmr, 0);

    // allocate on the first free slot
    slot->id = timer_generate_id(tmr);
    slot->cb = cb;
    slot->time = *t;
    slot->params = params;

    // increase allocation count
    tmr->nSched++;

    // replace nearest if needed
    if (tmr->nSched > 1) {
        timer_update_nearest_alarm(tmr);
    } else {
        tmr->nextAlarm = slot;
    }

    return slot->id;
}

uint32_t timer_sched_rel(Timer * tmr, int64_t us, TimerAlarmCb cb, AlarmUserData params) {
    TimePoint t = tmr->time;
    time_add_us(&t, us);
    return timer_sched(tmr, &t, cb, params);
}

void timer_unsched(Timer * tmr, uint32_t id) {
    AlarmAssignment * alarm = timer_get_alarm_by_id(tmr, id);
    if (alarm != NULL) {
        memset(alarm, 0, sizeof(AlarmAssignment));
        tmr->nSched--;
    }
}

void timer_set_time(Timer *tmr, const TimePoint *t) {
    tmr->time = *t;
}

int64_t timer_get_time_us(const Timer * tmr) {
    return time_to_us(&tmr->time);
}

TimePoint timer_get_time(const Timer * tmr) {
    return tmr->time;
}

void timer_tick(Timer * tmr, int64_t us) {
    // advance time
    time_add_us(&tmr->time, us);
    /*t_us += us;
    time_from_us(&tmr->time, t_us);*/

    int64_t t_us = timer_get_time_us(tmr);

    if ((tmr->nSched > 0) && (tmr->nextAlarm != NULL)) {
        int64_t t_alarm = time_to_us(&(tmr->nextAlarm->time));
        while (((t_alarm - t_us) <= 0) && (tmr->nSched > 0)) {
            // invoke callback
            if (tmr->nextAlarm->cb != NULL) {
                tmr->nextAlarm->cb(tmr, tmr->nextAlarm->params);
            }

            // clear alarm descriptor
            memset(tmr->nextAlarm, 0, sizeof(AlarmAssignment));

            // decrease scheduled alarm count
            tmr->nSched--;

            // update nearest alarm
            timer_update_nearest_alarm(tmr);
            if (tmr->nextAlarm != NULL) {
                t_alarm = time_to_us(&(tmr->nextAlarm->time));
            }
        }
    }
}

void timer_report(const Timer * tmr) {
    MSG("Time: %u.%06u\n\n", tmr->time.s, tmr->time.us);

    int64_t t_c_us = timer_get_time_us(tmr);
    if (tmr->nSched > 0) {
        for (uint32_t i = 0; i < tmr->maxSched; i++) {

            if (tmr->alarms[i].id == 0) {
                continue;
            }

            int64_t t_delta_us = time_to_us(&tmr->alarms[i].time) - t_c_us;
            if (t_delta_us < 1000000) {
                MSG("─ alarm (#%X) in %li us", tmr->alarms[i].id, t_delta_us);
            } else {
                TimePoint dt;
                time_from_us(&dt, t_delta_us);
                MSG("─ alarm (#%X) in %i.%06i s", tmr->alarms[i].id, dt.s, dt.us);
            }

            if ((tmr->alarms + i) == tmr->nextAlarm) {
                MSG(" <-- NEXT ALARM\n");
            } else {
                MSG("\n");
            }
        }
    }
    MSG("\nSchedules: %u/%u\n", tmr->nSched, tmr->maxSched);
}