/*
 *******************************************************************************
 *                                   INCLUDE
 *******************************************************************************
 */
#include "FrameSynchronizer.h"
#include "util/Math.h"
#include <thread>
#include <iostream>

namespace Video
{
namespace Core
{
/*
 *******************************************************************************
 *                                CLASS DEFINE
 *******************************************************************************
 */
FrameSynchronizer::DriftPredictor::DriftPredictor(size_t windowSize): _windowSize(windowSize), _lastPrediction(0)
{
}

void FrameSynchronizer::DriftPredictor::update(int64_t drift)
{
    _samples.push_back(drift);

    if (_samples.size() > _windowSize)
        _samples.pop_front();

    constexpr double alpha = 0.2;
    _lastPrediction        = alpha * drift + (1 - alpha) * _lastPrediction;
}

int64_t FrameSynchronizer::DriftPredictor::predict()
{
    if (_samples.empty())
        return _lastPrediction;

    if (_samples.size() < 5)
        return _lastPrediction;

    double sum = 0;
    for (auto v : _samples)
        sum += v;
    double mean = sum / _samples.size();

    constexpr int64_t MAX_DRIFT = 30000;

    int64_t result = static_cast<int64_t>(0.7 * mean + 0.3 * _lastPrediction);
    return Util::Math::clamp(result, -MAX_DRIFT, MAX_DRIFT);
}

void FrameSynchronizer::DriftPredictor::reset()
{
    _samples.clear();
    _lastPrediction = 0;
}

/*
 *******************************************************************************
 *                                CLASS DEFINE
 *******************************************************************************
 */
FrameSynchronizer::FrameSynchronizer(const std::string& name): _name(name), _baseClock(0), _lastSysTS(0)
{
    _predictor.reset();
}

FrameSynchronizer::~FrameSynchronizer()
{
}

void FrameSynchronizer::reset()
{
    _baseClock = 0;
    _lastSysTS = 0;
    _predictor.reset();
}

void FrameSynchronizer::syncFrame(uint64_t frameTS)
{
    const uint64_t now = System::timestamp();

    if (_baseClock == 0)
    {
        _baseClock = now - frameTS;
        _lastSysTS = now;
        return;
    }

    uint64_t ts = frameTS + _baseClock;
    if (ts + 500000 < _lastSysTS || ts > _lastSysTS + 10000000)
    {
        _predictor.reset();
        _baseClock = now - frameTS;
        _lastSysTS = now;
        return;
    }

    const uint64_t targetTime = frameTS + _baseClock + _predictor.predict();
    int64_t        waitTime   = static_cast<int64_t>(targetTime) - static_cast<int64_t>(now);
    if (waitTime > 0)
        std::this_thread::sleep_for(std::chrono::microseconds(waitTime));

    const uint64_t actualTime = System::timestamp();
    _predictor.update(static_cast<int64_t>(actualTime - targetTime));
    _lastSysTS = actualTime;
}
}
}