#include "GazeEstimation.h"

// TODO: This whole class need to be refactored and simplified

using namespace std;
using namespace cv;

GazeEstimation::GazeEstimation(QObject* parent)
    : QObject(parent)
    , calibrated(false)
    , isCalibrating(false)
    , gazeEstimationMethod(nullptr)
    , lastOverlayIdx(0)
    , settings(nullptr)
{
    availableGazeEstimationMethods.push_back(make_shared<BinocularPolyFit>(PolyFit::POLY_X_Y_XY_XX_YY_XYY_YXX_XXYY));
    availableGazeEstimationMethods.push_back(make_shared<BinocularPolyFit>(PolyFit::POLY_X_Y_XY));
    //availableGazeEstimationMethods.push_back(make_shared<BinocularPolyFit>(PolyFit::POLY_X_Y_XY_XX_YY));
    //availableGazeEstimationMethods.push_back(make_shared<BinocularPolyFit>(PolyFit::POLY_X_Y_XY_XX_YY_XXYY));
    //availableGazeEstimationMethods.push_back(make_shared<BinocularPolyFit>(PolyFit::POLY_X_Y_XY_XX_YY_XYY_YXX));
    //availableGazeEstimationMethods.push_back(make_shared<BinocularPolyFit>(PolyFit::POLY_X_Y_XY_XX_YY_XYY_YXX_XXX_YYY));

    availableGazeEstimationMethods.push_back(make_shared<PolyFit>(PolyFit::POLY_X_Y_XY_XX_YY_XYY_YXX_XXYY));
    availableGazeEstimationMethods.push_back(make_shared<PolyFit>(PolyFit::POLY_X_Y_XY));
    //availableGazeEstimationMethods.push_back(make_shared<PolyFit>(PolyFit::POLY_X_Y_XY_XX_YY));
    //availableGazeEstimationMethods.push_back(make_shared<PolyFit>(PolyFit::POLY_X_Y_XY_XX_YY_XXYY));
    //availableGazeEstimationMethods.push_back(make_shared<PolyFit>(PolyFit::POLY_X_Y_XY_XX_YY_XYY_YXX));
    //availableGazeEstimationMethods.push_back(make_shared<PolyFit>(PolyFit::POLY_X_Y_XY_XX_YY_XYY_YXX_XXX_YYY));

    availableGazeEstimationMethods.push_back(make_shared<Homography>());

    // Deactivated until the eye context gets integrated
    //availableGazeEstimationMethods.push_back(make_shared<GazeVectorBinocularPolyFit>(PolyFit::POLY_X_Y_XY_XX_YY_XYY_YXX_XXYY, GazeVectorBinocularPolyFit::Mode::INSTANTANEOUS));
    //availableGazeEstimationMethods.push_back(make_shared<GazeVectorBinocularPolyFit>(PolyFit::POLY_X_Y_XY, GazeVectorBinocularPolyFit::Mode::INSTANTANEOUS));

    //availableGazeEstimationMethods.push_back(make_shared<GazeVectorBinocularPolyFit>(PolyFit::POLY_X_Y_XY_XX_YY_XYY_YXX_XXYY, GazeVectorBinocularPolyFit::Mode::TEMPORAL));
    //availableGazeEstimationMethods.push_back(make_shared<GazeVectorBinocularPolyFit>(PolyFit::POLY_X_Y_XY, GazeVectorBinocularPolyFit::Mode::TEMPORAL));
}

GazeEstimation::~GazeEstimation()
{
    availableGazeEstimationMethods.clear();
}

void GazeEstimation::addTuple(CollectionTuple tuple)
{
    collectedTuples.push_back(tuple);
}
void GazeEstimation::addTuples(std::vector<CollectionTuple> tuples)
{
    collectedTuples.insert(collectedTuples.end(), tuples.begin(), tuples.end());
}
void GazeEstimation::reset(CollectionTuple::TupleType type)
{
    for (size_t i = collectedTuples.size(); i-- > 0;)
        if (collectedTuples[i].tupleType == type)
            collectedTuples.erase(collectedTuples.begin() + i);
}

bool GazeEstimation::isPupilOutlineValid(const EyeData& cur)
{
    if (cur.pupil.size.width <= 0 || cur.pupil.size.height <= 0)
        return false;
    return true;
}
bool GazeEstimation::isPupilRatioValid(const EyeData& prev, const EyeData& cur)
{
    // TODO: parametrize me
    int temporalThresholdMs = 500;
    double ratioThreshold = 0.75;

    // too far away in time, disconsider
    if (cur.timestamp - prev.timestamp > temporalThresholdMs)
        return true;

    std::pair<double, double> values = std::minmax(
        cur.pupil.size.area(),
        prev.pupil.size.area());

    if (values.second == 0)
        return true;

    return (values.first / values.second < ratioThreshold) ? false : true;
}
void GazeEstimation::findPupilPositionOutliers(const Mat& pupil, Mat& mask)
{
    unsigned int outliers = 0;
    double sigmaThreshold = 2.7;
    while (true) {
        Scalar mu, s;
        meanStdDev(pupil, mu, s);
        for (int i = 0; i < pupil.rows; i++) {
            if (mask.at<uchar>(i) == 0)
                continue;
            if (pupil.at<Vec2f>(i)[0] < mu[0] - sigmaThreshold * s[0]
                || pupil.at<Vec2f>(i)[0] > mu[0] + sigmaThreshold * s[0]
                || pupil.at<Vec2f>(i)[1] < mu[1] - sigmaThreshold * s[1]
                || pupil.at<Vec2f>(i)[1] > mu[1] + sigmaThreshold * s[1]) {
                outliers++;
                mask.at<uchar>(i) = 0;
            }
        }
        if (outliers <= 0)
            break;
        outliers = 0;
    }
}
void GazeEstimation::detectOutliers()
{
    // reset outlier information
    for (unsigned int i = 0; i < calibrationTuples.size(); i++)
        calibrationTuples[i]->outlierDesc = CollectionTuple::OD_INLIER;

    // Check for input type and pupil validity
    CollectionTuple* t;
    for (unsigned int i = 0; i < calibrationTuples.size(); i++) {
        t = calibrationTuples[i];
        switch (cfg.inputType) {
        case GazeEstimationMethod::BINOCULAR:
            if (!t->lEye.validPupil || !t->rEye.validPupil)
                t->outlierDesc = CollectionTuple::OD_MISSING_INPUT;
            break;
        case GazeEstimationMethod::MONO_LEFT:
            if (!t->lEye.validPupil)
                t->outlierDesc = CollectionTuple::OD_MISSING_INPUT;
            break;
        case GazeEstimationMethod::MONO_RIGHT:
            if (!t->rEye.validPupil)
                t->outlierDesc = CollectionTuple::OD_MISSING_INPUT;
            break;
        }
    }

    if (!cfg.removeOutliers)
        return;

    if (cfg.pupilRatioOutliers) {
        for (unsigned int i = 1; i < calibrationTuples.size(); i++) {
            CollectionTuple* prev = calibrationTuples[i - 1];
            CollectionTuple* cur = calibrationTuples[i];
            if (prev->isOutlier() || cur->isOutlier())
                continue;

            bool validLeft = isPupilRatioValid(prev->lEye, cur->lEye);
            bool validRight = isPupilRatioValid(prev->rEye, cur->rEye);

            switch (cfg.inputType) {
            case GazeEstimationMethod::BINOCULAR:
                if (!validLeft || !validRight)
                    cur->outlierDesc = CollectionTuple::OD_PUPIL_RATIO;
                break;
            case GazeEstimationMethod::MONO_LEFT:
                if (!validLeft)
                    cur->outlierDesc = CollectionTuple::OD_PUPIL_RATIO;
                break;
            case GazeEstimationMethod::MONO_RIGHT:
                if (!validRight)
                    cur->outlierDesc = CollectionTuple::OD_PUPIL_RATIO;
                break;
            }
        }
    }

    if (cfg.pupilPositionOutliers) {
        Mat lp(0, 0, CV_32FC2);
        Mat rp(0, 0, CV_32FC2);
        Mat mask(1, (int)calibrationTuples.size(), CV_8U);
        for (unsigned int i = 0; i < calibrationTuples.size(); i++) {
            CollectionTuple* t = calibrationTuples[i];
            lp.push_back(t->lEye.pupil.center);
            rp.push_back(t->rEye.pupil.center);
            mask.at<uchar>(i) = t->isOutlier() ? 0 : 1;
        }
        if (cfg.inputType != GazeEstimationMethod::MONO_RIGHT)
            findPupilPositionOutliers(lp, mask);
        if (cfg.inputType != GazeEstimationMethod::MONO_LEFT)
            findPupilPositionOutliers(rp, mask);
        for (unsigned int i = 0; i < calibrationTuples.size(); i++)
            if (mask.at<uchar>(i) == (uchar)0)
                calibrationTuples[i]->outlierDesc = CollectionTuple::OD_PUPIL_POSITION;
    }

    if (cfg.pupilOutlineOutliers) {
        for (unsigned int i = 0; i < calibrationTuples.size(); i++) {
            CollectionTuple* cur = calibrationTuples[i];
            if (cur->isOutlier())
                continue;

            bool validLeft = isPupilOutlineValid(cur->lEye);
            bool validRight = isPupilOutlineValid(cur->rEye);

            switch (cfg.inputType) {
            case GazeEstimationMethod::BINOCULAR:
                if (!validLeft || !validRight)
                    cur->outlierDesc = CollectionTuple::OD_PUPIL_POSITION;
                break;
            case GazeEstimationMethod::MONO_LEFT:
                if (!validLeft)
                    cur->outlierDesc = CollectionTuple::OD_PUPIL_POSITION;
                break;
            case GazeEstimationMethod::MONO_RIGHT:
                if (!validRight)
                    cur->outlierDesc = CollectionTuple::OD_PUPIL_POSITION;
                break;
            }
        }
    }

    if (cfg.pupilDiameterOutliers) {
        vector<float> ld, rd;
        for (auto& cur : calibrationTuples) {
            if (cur->isOutlier())
                continue;
            ld.push_back(cur->lEye.pupil.majorAxis());
            rd.push_back(cur->rEye.pupil.majorAxis());
        }
        if (ld.size() > 0 && rd.size() > 0) {
            double lmed = median(ld);
            double rmed = median(rd);
            for (auto& cur : calibrationTuples) {
                if (cur->isOutlier())
                    continue;

                auto valid = [&](const double med, const EyeData ed, const float tolerance = 0.1) {
                    float ma = ed.pupil.majorAxis();
                    return ma < (1 + tolerance) * med && ma > (1 - tolerance);
                };

                bool validLeft = valid(lmed, cur->lEye);
                bool validRight = valid(rmed, cur->rEye);

                switch (cfg.inputType) {
                case GazeEstimationMethod::BINOCULAR:
                    if (validLeft || validRight)
                        continue;
                    break;
                case GazeEstimationMethod::MONO_LEFT:
                    if (validLeft)
                        continue;
                    break;
                case GazeEstimationMethod::MONO_RIGHT:
                    if (validRight)
                        continue;
                    break;
                }

                cur->outlierDesc = CollectionTuple::OD_PUPIL_DIAMETER;
            }
        }
    }
}

void GazeEstimation::calibrate()
{
    updateTemporalGazes(collectedTuples);

    calibrated = false;
    autoVisualizationTimer.invalidate();
    errorVectors.clear();
    interpolationHull.clear();
    evaluationRegions.clear();

    QString error = "";
    if (!gazeEstimationMethod) {
        error = "No gaze estimation method available.";
        error = "No calibration tuples available.";
        qInfo() << error;
        emit calibrationFinished(false, error);
        return;
    }

    calibrationTuples.clear();

    for (unsigned int i = 0; i < collectedTuples.size(); i++)
        if (collectedTuples[i].isCalibration())
            calibrationTuples.push_back(&collectedTuples[i]);
    if (calibrationTuples.size() <= 0) {
        error = "No calibration tuples available.";
        qInfo() << error;
        emit calibrationFinished(false, error);
        return;
    }

    detectOutliers();

    if (cfg.autoEvaluation)
        selectEvaluationTuples(cfg.granularity, cfg.horizontalStride, cfg.verticalStride, cfg.rangeFactor);
    else {
        for (unsigned int i = 0; i < calibrationTuples.size(); i++) {
            calibrationTuples[i]->autoEval = CollectionTuple::AE_NO;
        }
    }

    vector<CollectionTuple> calibrationInliers;
    for (const auto& tuple : calibrationTuples)
        if (tuple->useForCalibration())
            calibrationInliers.push_back(*tuple);

    updateInterpolationHull(calibrationInliers);

    qInfo() << "Using" << calibrationInliers.size() << "/" << calibrationTuples.size() << "calibration tuples.";
    if (calibrationInliers.size() <= 0) {
        error = "No calibration tuples remaining.";
        qInfo() << error;
        emit calibrationFinished(false, error);
        return;
    }

    calibrated = gazeEstimationMethod->calibrate(calibrationInliers, error);

    if (!calibrated) {
        qInfo() << error;
        emit calibrationFinished(false, error);
        return;
    }
    evaluate();

    if (centralHullCoverage < cfg.minCentralAreaCoverage)
        error.append("Calibration didn't cover enough of the central area. ");
    if (peripheralHullCoverage < cfg.minPeriphericAreaCoverage)
        error.append("Calibration didn't cover enough of the peripheric area. ");
    if (cfg.maxReprojectionError > 0 && 100 * meanEvaluationError > cfg.maxReprojectionError)
        error.append("Reprojection error is too high. ");
    if (!error.isEmpty()) {
        qInfo() << error;
        emit calibrationFinished(false, error);
        calibrated = false;
        return;
    }

    autoVisualizationTimer.restart();
    emit calibrationFinished(true, "");
}

GazeEstimate GazeEstimation::getGazeEstimate(const DataTuple& dataTuple)
{
    GazeEstimate gazeEstimate;
    if (calibrated && gazeEstimationMethod) {
        GazeEstimate left, right, binocular;
        gazeEstimationMethod->estimate(dataTuple, left, right, binocular);

        switch (cfg.inputType) {
        case GazeEstimationMethod::BINOCULAR:
            gazeEstimate = binocular;
            break;
        case GazeEstimationMethod::MONO_LEFT:
            gazeEstimate = left;
            break;
        case GazeEstimationMethod::MONO_RIGHT:
            gazeEstimate = right;
            break;
        }
    }
    return gazeEstimate;
}

void GazeEstimation::estimate(DataTuple dataTuple)
{
    dataTuple.field.gazeEstimate = getGazeEstimate(dataTuple);
    drawGazeEstimationInfo(dataTuple);
    emit gazeEstimationDone(dataTuple);
}

void GazeEstimation::printAccuracyInfo(const cv::Mat& errors, const QString& which, const double& diagonal, float& mu, float& sigma)
{
    Scalar m, std;
    meanStdDev(errors, m, std);
    qInfo() << which.toLatin1().data() << "Error ( N =" << errors.rows << "):";
    //qInfo() << QString("m = %1 ( std = %2 ) pixels."
    //    ).arg(m[0], 6, 'f', 2
    //    ).arg(std[0], 6, 'f', 2).toLatin1().data();
    mu = m[0] / diagonal;
    sigma = std[0] / diagonal;
    qInfo() << QString("m = %1 ( std = %2 ) % of the image diagonal.").arg(100 * mu, 6, 'f', 2).arg(100 * sigma, 6, 'f', 2).toLatin1().data();
}

void GazeEstimation::evaluate()
{
    if (!calibrated)
        return;

    evaluationTuples.clear();
    for (unsigned int i = 0; i < collectedTuples.size(); i++)
        if (collectedTuples[i].isEvaluation() || (cfg.autoEvaluation && collectedTuples[i].isAutoEval())) {
            if (getGazeEstimate(collectedTuples[i]).valid)
                evaluationTuples.push_back(&collectedTuples[i]);
        }

    centralHullCoverage = 0;
    peripheralHullCoverage = 0;
    if (!interpolationHull.empty()) {
        float delta = 0.25;
        Size size(calibrationTuples[0]->field.width, calibrationTuples[0]->field.height);
        Mat centralRegion = Mat::zeros(size, CV_8U);
        rectangle(centralRegion, Point(delta * size.width, delta * size.height), Point((1 - delta) * size.width, (1 - delta) * size.height), Scalar(255), -1);
        Mat peripheralRegion = 255 - centralRegion;

        Mat hullRegion = Mat::zeros(size, CV_8U);
        vector<vector<Point>> contours;
        contours.push_back(interpolationHull);
        drawContours(hullRegion, contours, 0, Scalar(255), -1);

        vector<Point> nonZero;
        findNonZero(centralRegion, nonZero);
        float centralPixels = nonZero.size();
        findNonZero(peripheralRegion, nonZero);
        float peripheralPixels = nonZero.size();

        Mat intersection;
        bitwise_and(hullRegion, centralRegion, intersection);
        findNonZero(intersection, nonZero);
        centralHullCoverage = nonZero.size() / centralPixels;

        bitwise_and(hullRegion, peripheralRegion, intersection);
        findNonZero(intersection, nonZero);
        peripheralHullCoverage = nonZero.size() / peripheralPixels;

        qInfo() << QString("Central Interpolation Coverage: %1 %").arg(100 * centralHullCoverage, 0, 'f', 2).toLatin1().data();
        qInfo() << QString("Peripheral Interpolation Coverage: %1 %").arg(100 * peripheralHullCoverage, 0, 'f', 2).toLatin1().data();
    }

    meanEvaluationError = 0;
    stdEvaluationError = 0;
    evaluationRegionCoverage = 1;

    if (evaluationTuples.size() <= 0 || calibrationTuples.size() <= 0)
        return;

    // evaluate for known points
    errorVectors.clear();
    Mat errors;
    for (unsigned int i = 0; i < evaluationTuples.size(); i++) {
        Point3f gt = evaluationTuples[i]->field.collectionMarker.center;
        Point3f gaze = to3D(getGazeEstimate(*evaluationTuples[i]).gp);
        errorVectors.push_back(ErrorVector(gt, gaze));
        errors.push_back(errorVectors.back().magnitude());
    }

    double imDiagonal = sqrt(pow(evaluationTuples[0]->field.width, 2) + pow(evaluationTuples[0]->field.height, 2));
    printAccuracyInfo(errors, "Gaze Evaluation", imDiagonal, meanEvaluationError, stdEvaluationError);

    if (cfg.autoEvaluation) {
        double evaluationRegionsCount = pow(1 + 2 * cfg.granularity, 2);
        double evaluationRegionsSelected = 0;
        for (unsigned int i = 0; i < collectedTuples.size(); i++)
            if (collectedTuples[i].isAutoEval())
                evaluationRegionsSelected++;
        evaluationRegionCoverage = evaluationRegionsSelected / evaluationRegionsCount;
        qInfo() << QString("Auto Evaluation Region Coverage: %1 %").arg(100 * evaluationRegionCoverage, 0, 'f', 2).toLatin1().data();
    }
}

/*
 * File loading / saving
 */
template <typename T>
static void addData(map<QString, uint>& idxMap, QStringList& tokens, QString key, T& data)
{
    if (idxMap.count(key) == 0)
        return;

    data = QVariant(tokens[idxMap[key]]).value<T>();
}
template <>
inline void addData(map<QString, uint>& idxMap, QStringList& tokens, QString key, vector<Marker>& markers)
{
    if (idxMap.count(key) == 0)
        return;

    QString markersStr = tokens[idxMap[key]];
    for (auto& markerStr : markersStr.split(Token::MarkerEnd)) {
        Marker tmp;
        if (tmp.parse(markerStr))
            markers.push_back(std::move(tmp));
    }
}
void GazeEstimation::loadTuplesFromFile(CollectionTuple::TupleType tupleType, QString fileName)
{
    vector<CollectionTuple> tuples;
    TSVReader tsv;
    if (!tsv.open(fileName))
        return;
    for (int i = 0; i < tsv.size(); i++) {
        CollectionTuple tuple = CollectionTuple(DataTuple(tsv, i));
        tuple.tupleType = tupleType;
        tuples.emplace_back(tuple);
    }
    addTuples(tuples);
    calibrate();
}

void GazeEstimation::saveTuplesToFile(CollectionTuple::TupleType tupleType, QString fileName)
{
    vector<CollectionTuple*> pertinentTuples;
    for (unsigned int i = 0; i < collectedTuples.size(); i++) {
        //if ( tupleType == CollectionTuple::CALIBRATION)
        //	if (collectedTuples[i].isEvaluation())
        //		continue;

        if (tupleType == CollectionTuple::EVALUATION) {
            if (collectedTuples[i].isCalibration())
                continue;
        }

        pertinentTuples.push_back(&collectedTuples[i]);
    }

    saveTuplesToFile(pertinentTuples, fileName, QIODevice::WriteOnly);
}

void GazeEstimation::saveTuplesToFile(const std::vector<CollectionTuple*>& tuples, QString fileName, QFlags<QIODevice::OpenModeFlag> flags)
{
    if (fileName.isEmpty() || tuples.size() == 0)
        return;

    QString buffer;
    buffer.append(tuples[0]->header() + Token::Newline);
    for (unsigned int i = 0; i < tuples.size(); i++)
        buffer.append(tuples[i]->toQString() + Token::Newline);

    QFile f(fileName);
    f.open(flags);
    QTextStream s(&f);
    if (s.status() == QTextStream::Ok)
        s << buffer.toStdString().c_str();
    f.close();
}

void GazeEstimation::updateConfig()
{
    cfg.load(settings);
    for (int i = 0; i < availableGazeEstimationMethods.size(); i++) {
        QString name = availableGazeEstimationMethods[i]->description().c_str();
        if (cfg.gazeEstimationMethod == name) {
            gazeEstimationMethod = availableGazeEstimationMethods[i];
            break;
        }
    }
    if (!gazeEstimationMethod) {
        qWarning() << "Unknown gaze estimation method:" << cfg.gazeEstimationMethod;
        if (availableGazeEstimationMethods.size() > 0) {
            gazeEstimationMethod = availableGazeEstimationMethods.front();
            qWarning() << "Defaulting to" << gazeEstimationMethod->description().c_str();
        }
    }
    calibrate();
}

void GazeEstimation::selectEvaluationTuples(const int g, const double dx, const double dy, const double rf)
{
    vector<CollectionTuple*> evaluationCandidates;
    vector<Point> points;
    CollectionTuple* t;

    evaluationRegions.clear();

    /*
     * collect non-outlier candidates
     */
    for (unsigned int i = 0; i < calibrationTuples.size(); i++) {
        t = calibrationTuples[i];
        t->autoEval = CollectionTuple::AE_NO;
        if (t->isOutlier())
            continue;
        evaluationCandidates.push_back(t);
        points.push_back(to2D(t->field.collectionMarker.center));
    }

    if (evaluationCandidates.size() <= 0)
        return;
    t = evaluationCandidates[0];

    /*
     *  exclude points in the hull so we don't lose interpolation range
     */
    const float minDistFromHullPx = 3;
    convexHull(points, interpolationHull);

//#define DBG_AUTOEVAL
#ifdef DBG_AUTOEVAL
    const Point* ep[1] = { &interpolationHull[0] };
    int epn = (int)interpolationHull.size();
    Mat dbg = Mat::zeros(evaluationCandidates[0]->field.height, evaluationCandidates[0]->field.width, CV_8UC3);
    fillPoly(dbg, ep, &epn, 1, Scalar(255, 255, 255));
    for (size_t i = evaluationCandidates.size(); i-- > 0;) {
        Point gaze = to2D(evaluationCandidates[i]->field.collectionMarker.center);
        circle(dbg, gaze, 5, Scalar(0, 255, 255), -1);
    }
#endif

    for (size_t i = evaluationCandidates.size(); i-- > 0;) {
        Point gaze = to2D(evaluationCandidates[i]->field.collectionMarker.center);
        for (unsigned int j = 0; j < interpolationHull.size(); j++) {
            if (ED(gaze, interpolationHull[j]) <= minDistFromHullPx) {
#ifdef DBG_AUTOEVAL
                circle(dbg, gaze, 5, Scalar(255, 0, 0), -1);
#endif
                evaluationCandidates.erase(evaluationCandidates.begin() + i);
                break;
            }
        }
    }

    /*
     * Select points that minimize distance to evaluation region center for evaluation
     */
    Point center(t->field.width / 2, t->field.height / 2);
    int dxPx = dx * t->field.width;
    int dyPx = dy * t->field.height;
    int w = rf * dxPx;
    int h = rf * dyPx;
    vector<Point> evaluationPoints;
    for (int x = -g; x <= g; x++) {
        for (int y = -g; y <= g; y++) {
            EvaluationRegion er(center.x + x * dxPx, center.y + y * dyPx, w, h);
            for (int i = 0; i < evaluationCandidates.size(); i++)
                er.eval(evaluationCandidates[i]);
            if (er.selected) {
                er.selected->autoEval = CollectionTuple::AE_YES;
                evaluationPoints.push_back(to2D(er.selected->field.collectionMarker.center));
            }
            evaluationRegions.push_back(er);
#ifdef DBG_AUTOEVAL
            er.draw(dbg);
#endif
        }
    }

    /*
     * Mark calibration points that could bias the auto evaluation
     */
    const float minDistFromEvaluationPx = 5;
    for (unsigned int i = 0; i < evaluationPoints.size(); i++) {
        Point ep = evaluationPoints[i];
        for (unsigned int j = 0; j < evaluationCandidates.size(); j++) {
            t = evaluationCandidates[j];
            if (t->autoEval == CollectionTuple::AE_NO) {
                Point cp = to2D(t->field.collectionMarker.center);
                if (ED(ep, cp) < minDistFromEvaluationPx) {
                    t->autoEval = CollectionTuple::AE_BIASED;
#ifdef DBG_AUTOEVAL
                    circle(dbg, cp, 2, Scalar(255, 255, 0), -1);
#endif
                }
            }
        }
    }

#ifdef DBG_AUTOEVAL
    imshow("dbg", dbg);
#endif
}

void GazeEstimation::drawGazeEstimationInfo(DataTuple& dataTuple)
{
    dataTuple.showGazeEstimationVisualization = false;

    // TODO: improve this? maybe draw on an overlay canvas that is only updated
    // when something changes. Then we overlay the canvas on a copy of the image
    if (dataTuple.field.input.empty() || !cfg.visualize)
        return;

    bool shouldDisplay = false;
    if (isCalibrating)
        shouldDisplay = true;
    else {
        if (autoVisualizationTimer.isValid()) {
            if (autoVisualizationTimer.elapsed() < cfg.visualizationTimeS * 1e3)
                shouldDisplay = true;
            else
                autoVisualizationTimer.invalidate();
        }
    }
    if (!shouldDisplay) {
        lastOverlayIdx = 0;
        return;
    }

    dataTuple.showGazeEstimationVisualization = true;
    dataTuple.gazeEstimationVisualization = vis;

    // avoid drawing every single frame
    //static Timestamp lastGazeEstimationVisualizationTimestamp = 0;
    //Timestamp current = gTimer.elapsed();
    //bool shouldDraw = current - lastGazeEstimationVisualizationTimestamp > 40;
    //if (!shouldDraw)
    //	return;
    //lastGazeEstimationVisualizationTimestamp = current;

    vis = dataTuple.field.input.clone();
    int r = max<int>(1, 0.003125 * max<int>(vis.rows, vis.cols));

    if (isCalibrating) {

        if (lastOverlayIdx > collectedTuples.size()) // sample removed, restart
            lastOverlayIdx = 0;

        if (lastOverlayIdx == 0)
            overlay = Mat::zeros(vis.rows, vis.cols, CV_8UC3);

        for (; lastOverlayIdx < collectedTuples.size(); lastOverlayIdx++) {
            circle(overlay, to2D(collectedTuples[lastOverlayIdx].field.collectionMarker.center), r, CV_ALMOST_BLACK, -1);
            if (collectedTuples[lastOverlayIdx].isCalibration())
                circle(overlay, to2D(collectedTuples[lastOverlayIdx].field.collectionMarker.center), r + 1, CV_GREEN, 0.5 * r);
            else
                circle(overlay, to2D(collectedTuples[lastOverlayIdx].field.collectionMarker.center), r + 1, CV_CYAN, 0.5 * r);
        }

    } else {
        // Calibration finished; overlay results and restart
        overlay = Mat::zeros(vis.rows, vis.cols, CV_8UC3);
        lastOverlayIdx = 0;

        if (!interpolationHull.empty()) {
            vector<vector<Point>> contours;
            contours.push_back(interpolationHull);
            drawContours(overlay, contours, 0, CV_GREEN, r);
        }

        for (size_t i = 0; i < errorVectors.size(); i++)
            errorVectors[i].draw(overlay, 2, CV_RED);

        if (cfg.autoEvaluation)
            for (size_t i = 0; i < evaluationRegions.size(); i++)
                evaluationRegions[i].draw(overlay, r);

        for (size_t i = 0; i < collectedTuples.size(); i++) {
            if (!collectedTuples[i].isCalibration()) {
                circle(overlay, to2D(collectedTuples[i].field.collectionMarker.center), r, CV_BLACK, -1);
                circle(overlay, to2D(collectedTuples[i].field.collectionMarker.center), r + 1, CV_CYAN, 0.5 * r);
            }
        }
    }

    // Overlay on visualization image; notice we use CV_ALMOST_BLACK instead of
    // CV_BLACK so we don't need to create an additional mask :-)
    overlay.copyTo(vis, overlay);
}

void GazeEstimation::setCalibrating(bool v)
{
    isCalibrating = v;
}

void GazeEstimation::updateInterpolationHull(const std::vector<CollectionTuple>& tuples)
{
    if (tuples.size() <= 0)
        return;

    vector<Point> points;
    for (unsigned int i = 0; i < tuples.size(); i++)
        points.push_back(to2D(tuples[i].field.collectionMarker.center));

    convexHull(points, interpolationHull);
}

void GazeEstimation::updateTemporalGazes(std::vector<CollectionTuple>& tuples)
{
    (void)tuples;
}