#include "Synchronizer.h"

using namespace std;

Synchronizer::Synchronizer(QObject* parent)
    : QObject(parent)
    , maxAgeMs(100)
    , safeGuardMs(5)
    , updated(false)
    , lastSynchronization(0)
{

    // TODO: allow user to pick sampling rate
    bool userSelected = false;
    if (userSelected) {
        // This may be dangerous and is in practice much less efficient than
        // synchronizing one of the cameras as trigger; e.g., due to timer imprecisions
        int userSynchronizeMs = 8;
        timer = new QTimer(this);
        connect(timer, SIGNAL(timeout()),
            this, SLOT(synchronize()));
        timer->start(userSynchronizeMs);
    }
    emptyEyeData.timestamp = emptyEyeData.processingTimestamp = -1;
    emptyFieldData.timestamp = emptyFieldData.processingTimestamp = -1;
}

template <class T>
void Synchronizer::updateDeque(std::deque<T>& dataDeque, T& data)
{
    dataDeque.emplace_back(data);
    while (dataDeque.back().timestamp - dataDeque.front().timestamp > maxAgeMs)
        dataDeque.pop_front();
    updated = true;
}

void Synchronizer::newRightEyeData(EyeData eyeData)
{
    updateDeque(rEyeDeque, eyeData);
    synchronize(rEyeDeque.back().timestamp, Trigger::RIGHT_EYE);
}

void Synchronizer::newLeftEyeData(EyeData eyeData)
{
    updateDeque(lEyeDeque, eyeData);
    synchronize(lEyeDeque.back().timestamp, Trigger::LEFT_EYE);
}

void Synchronizer::newFieldData(FieldData fieldData)
{
    updateDeque(fieldDeque, fieldData);
    synchronize(fieldDeque.back().timestamp, Trigger::FIELD_EYE);
}

template <class T>
T* Synchronizer::getClosestInTime(Timestamp timestamp, deque<T>& dataDeque)
{
    T* closest = nullptr;
    if (dataDeque.empty())
        return closest;

    Timestamp minimum = std::numeric_limits<Timestamp>::max();
    for (auto& data : dataDeque) {
        unsigned int diff = abs(timestamp - data.timestamp);
        if (diff < minimum) {
            minimum = diff;
            closest = &data;
        }
    }

    if (minimum > maxAgeMs)
        return nullptr;

    return closest;
}

bool Synchronizer::shouldSynchronize(Timestamp timestamp, Trigger trigger)
{
    switch (trigger) {
    case Trigger::FIELD_EYE:
        if (!lEyeDeque.empty())
            if (timestamp - lEyeDeque.back().timestamp < maxAgeMs)
                return false;
    case Trigger::LEFT_EYE:
        if (!rEyeDeque.empty())
            if (timestamp - rEyeDeque.back().timestamp < maxAgeMs)
                return false;
    case Trigger::RIGHT_EYE:
    default:
        return true;
    }
}

void Synchronizer::synchronize(Timestamp timestamp, Trigger trigger)
{
    if (!updated)
        return;
    updated = false;

    if (!shouldSynchronize(timestamp, trigger))
        return;

    if (timestamp == maxTimestamp) // nothing recent was available
        return;

    // Make sure we don't overload the thread event loop
    if (timestamp - lastSynchronization < safeGuardMs && lastSynchronization != 0)
        return;

    lastSynchronization = timestamp;

    FieldData* field = getClosestInTime<FieldData>(timestamp, fieldDeque);
    EyeData* lEye = getClosestInTime<EyeData>(timestamp, lEyeDeque);
    EyeData* rEye = getClosestInTime<EyeData>(timestamp, rEyeDeque);

    DataTuple dataTuple(
        timestamp,
        (lEye ? *lEye : emptyEyeData),
        (rEye ? *rEye : emptyEyeData),
        (field ? *field : emptyFieldData));

    emit newData(dataTuple);
}