#include "utils/Clock.h"

#include <threads.h>
#include <time.h>

Clock::Clock() : index(0), last(0), sum(0), time(0) {
}

Error Clock::update(Clock::Nanos& n) {
    Nanos current = 0;
    Error e = getNanos(current);
    if(e.has()) {
        return e;
    } else if(last == 0) {
        last = current;
        n = 0;
        return Error();
    }
    index = (index + 1) & (LENGTH - 1);
    sum -= time[index];
    time[index] = current - last;
    sum += time[index];
    last = current;
    n = time[index];
    return Error();
}

float Clock::getUpdatesPerSecond() const {
    return (LENGTH * 1000000000.0f) / static_cast<float>(sum);
}

Error Clock::getNanos(Clock::Nanos& n) {
    struct timespec ts;
    if(timespec_get(&ts, TIME_UTC) == 0) {
        return {"Could not get time"};
    }
    n = static_cast<Clock::Nanos>(ts.tv_sec) * 1'000'000'000L +
        static_cast<Clock::Nanos>(ts.tv_nsec);
    return Error();
}

Error Clock::wait(Nanos nanos) const {
    struct timespec t;
    t.tv_nsec = nanos % 1'000'000'000;
    t.tv_sec = nanos / 1'000'000'000;
    if(thrd_sleep(&t, nullptr) == 0) {
        return Error();
    }
    return {"Sleep was interrupted"};
}