#include "core/utils/Clock.hpp"

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

#include "ErrorSimulator.hpp"

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

Core::Error Core::Clock::update(Clock::Nanos& n) {
    Nanos current = 0;
    CORE_RETURN_ERROR(getNanos(current));
    if(last == 0) {
        last = current;
        n = 0;
        return ErrorCode::NONE;
    }
    index = (index + 1) & (time.getLength() - 1);
    sum -= time[index];
    time[index] = current - last;
    sum += time[index];
    last = current;
    n = time[index];
    return ErrorCode::NONE;
}

float Core::Clock::getUpdatesPerSecond() const {
    return (time.getLength() * 1000000000.0f) / static_cast<float>(sum);
}

Core::Error Core::Clock::getNanos(Clock::Nanos& n) {
    timespec ts;
    if(timespec_get(&ts, TIME_UTC) == 0 || CORE_TIME_GET_FAIL) {
        return ErrorCode::TIME_NOT_AVAILABLE;
    }
    n = static_cast<Clock::Nanos>(ts.tv_sec) * 1'000'000'000L +
        static_cast<Clock::Nanos>(ts.tv_nsec);
    return ErrorCode::NONE;
}

Core::Error Core::Clock::wait(Nanos nanos) {
    timespec t;
    t.tv_nsec = nanos % 1'000'000'000;
    t.tv_sec = nanos / 1'000'000'000;
    return thrd_sleep(&t, nullptr) != 0 ? ErrorCode::SLEEP_INTERRUPTED
                                        : ErrorCode::NONE;
}