// SingleEyeFitter.cpp : Defines the entry point for the console application.
//

#include <boost/math/special_functions/sign.hpp>

#include <Eigen/StdVector>

#include <ceres/ceres.h>
#include <ceres/problem.h>
#include <ceres/autodiff_cost_function.h>
#include <ceres/solver.h>
#include <ceres/jet.h>

#include <singleeyefitter/singleeyefitter.h>

#include <singleeyefitter/utils.h>
#include <singleeyefitter/cvx.h>
#include <singleeyefitter/Conic.h>
#include <singleeyefitter/Ellipse.h>
#include <singleeyefitter/Circle.h>
#include <singleeyefitter/Conicoid.h>
#include <singleeyefitter/Sphere.h>
#include <singleeyefitter/solve.h>
#include <singleeyefitter/intersect.h>
#include <singleeyefitter/projection.h>
#include <singleeyefitter/fun.h>
#include <singleeyefitter/math.h>

#include "distance.h"

#include <spii/spii.h>
#include <spii/term.h>
#include <spii/function.h>
#include <spii/solver.h>

#define _USE_MATH_DEFINES
#include <math.h>

namespace ceres {
    using singleeyefitter::math::sq;

    template<typename T, int N>
    inline Jet<T,N> sq(Jet<T,N> val) {
        val.v *= 2*val.a;
        val.a *= val.a;
        return val;
    }
}

namespace singleeyefitter {

struct scalar_tag{};
struct ceres_jet_tag{};

template<typename T, typename Enabled=void>
struct ad_traits;

template<typename T>
struct ad_traits<T, typename std::enable_if< std::is_arithmetic<T>::value >::type >
{
    typedef scalar_tag ad_tag;
    typedef T scalar;
    static inline scalar value(const T& x) { return x; }
};

template<typename T, int N>
struct ad_traits<::ceres::Jet<T,N>>
{
    typedef ceres_jet_tag ad_tag;
    typedef T scalar;
    static inline scalar get(const ::ceres::Jet<T,N>& x) { return x.a; }
};

template<typename T>
struct ad_traits<T, typename std::enable_if< !std::is_same<T, typename std::decay<T>::type>::value >::type >
    : public ad_traits<typename std::decay<T>::type>
{
};

template<typename T>
inline T smootherstep(T edge0, T edge1, T x, scalar_tag)
{
    if (x >= edge1)
        return T(1);
    else if (x <= edge0)
        return T(0);
    else {
        x = (x - edge0)/(edge1 - edge0);
        return x*x*x*(x*(x*T(6) - T(15)) + T(10));
    }
}
template<typename T, int N>
inline ::ceres::Jet<T,N> smootherstep(T edge0, T edge1, const ::ceres::Jet<T,N>& f, ceres_jet_tag)
{
    if (f.a >= edge1)
        return ::ceres::Jet<T,N>(1);
    else if (f.a <= edge0)
        return ::ceres::Jet<T,N>(0);
    else {
        T x = (f.a - edge0)/(edge1 - edge0);

        // f is referenced by this function, so create new value for return.
        ::ceres::Jet<T,N> g;
        g.a = x*x*x*(x*(x*T(6) - T(15)) + T(10));
        g.v = f.v * (x*x*(x*(x*T(30) - T(60)) + T(30))/(edge1 - edge0));
        return g;
    }
}
template<typename T, int N>
inline ::ceres::Jet<T,N> smootherstep(T edge0, T edge1, ::ceres::Jet<T,N>&& f, ceres_jet_tag)
{
    if (f.a >= edge1)
        return ::ceres::Jet<T,N>(1);
    else if (f.a <= edge0)
        return ::ceres::Jet<T,N>(0);
    else {
        T x = (f.a - edge0)/(edge1 - edge0);

        // f is moved into this function, so reuse it.
        f.a = x*x*x*(x*(x*T(6) - T(15)) + T(10));
        f.v *= (x*x*(x*(x*T(30) - T(60)) + T(30))/(edge1 - edge0));
        return f;
    }
}
template<typename T>
inline auto smootherstep(typename ad_traits<T>::scalar edge0, typename ad_traits<T>::scalar edge1, T&& val)
    -> decltype(smootherstep(edge0, edge1, std::forward<T>(val), typename ad_traits<T>::ad_tag()))
{
    return smootherstep(edge0, edge1, std::forward<T>(val), typename ad_traits<T>::ad_tag());
}

template<typename T>
inline T norm(T x, T y, scalar_tag) {
    using std::sqrt;
    using math::sq;

    return sqrt(sq(x) + sq(y));
}
template<typename T, int N>
inline ::ceres::Jet<T,N> norm(const ::ceres::Jet<T,N>& x, const ::ceres::Jet<T,N>& y, ceres_jet_tag) {
    T anorm = norm<T>(x.a, y.a, scalar_tag());

    ::ceres::Jet<T,N> g;
    g.a = anorm;
    g.v = (x.a/anorm)*x.v + (y.a/anorm)*y.v;

    return g;
}
template<typename T>
inline typename std::decay<T>::type norm(T&& x, T&& y) {
    return norm(std::forward<T>(x), std::forward<T>(y), typename ad_traits<T>::ad_tag());
}

template<typename T>
inline auto Heaviside(T&& val, typename ad_traits<T>::scalar epsilon) -> decltype(smootherstep(-epsilon, epsilon, std::forward<T>(val))) {
    return smootherstep(-epsilon, epsilon, std::forward<T>(val));
}

template<typename Scalar>
cv::Rect bounding_box(const Ellipse2D<Scalar>& ellipse) {
    using std::sin;
    using std::cos;
    using std::sqrt;
    using std::floor;
    using std::ceil;

    Scalar ux = ellipse.major_radius * cos(ellipse.angle);
    Scalar uy = ellipse.major_radius * sin(ellipse.angle);
    Scalar vx = ellipse.minor_radius * cos(ellipse.angle + PI/2);
    Scalar vy = ellipse.minor_radius * sin(ellipse.angle + PI/2);

    Scalar bbox_halfwidth = sqrt(ux*ux + vx*vx);
    Scalar bbox_halfheight = sqrt(uy*uy + vy*vy);

    return cv::Rect((int)floor(ellipse.centre[0] - bbox_halfwidth), (int)floor(ellipse.centre[1] - bbox_halfheight),
		(int)(2.0 * ceil(bbox_halfwidth) + 1.0),
		(int)(2.0 * ceil(bbox_halfheight) + 1.0));
}

// Calculates:
//     r * (1 - ||A(p - t)||)
//
//          ||A(p - t)||   maps the ellipse to a unit circle
//      1 - ||A(p - t)||   measures signed distance from unit circle edge
// r * (1 - ||A(p - t)||)  scales this to major radius of ellipse, for (roughly) pixel distance
//
// Actually use (r - ||rAp - rAt||) and precalculate r, rA and rAt.
template<typename T>
class EllipseDistCalculator {
public:
    typedef typename ad_traits<T>::scalar Const;

    EllipseDistCalculator(const Ellipse2D<T>& ellipse) : r(ellipse.major_radius)
    {
        using std::sin;
        using std::cos;
        rA << r*cos(ellipse.angle)/ellipse.major_radius, r*sin(ellipse.angle)/ellipse.major_radius,
             -r*sin(ellipse.angle)/ellipse.minor_radius, r*cos(ellipse.angle)/ellipse.minor_radius;
        rAt = rA*ellipse.centre;
    }
    template<typename U>
    T operator()(U&& x, U&& y) {
        return calculate(std::forward<U>(x), std::forward<U>(y), typename ad_traits<T>::ad_tag(), typename ad_traits<U>::ad_tag());
    }

    template<typename U>
    T calculate(U&& x, U&& y, scalar_tag, scalar_tag) {
        T rAxt((rA(0,0) * x + rA(0,1) * y) - rAt[0]);
        T rAyt((rA(1,0) * x + rA(1,1) * y) - rAt[1]);

        T xy_dist = norm(rAxt, rAyt);

        return (r - xy_dist);
    }

    // Expanded versions for Jet calculations so that Eigen can do some of its expression magic
    template<typename U>
    T calculate(U&& x, U&& y, scalar_tag, ceres_jet_tag) {
        T rAxt(rA(0,0) * x.a + rA(0,1) * y.a - rAt[0],
            rA(0,0) * x.v + rA(0,1) * y.v);
        T rAyt(rA(1,0) * x.a + rA(1,1) * y.a - rAt[1],
            rA(1,0) * x.v + rA(1,1) * y.v);

        T xy_dist = norm(rAxt, rAyt);

        return (r - xy_dist);
    }
    template<typename U>
    T calculate(U&& x, U&& y, ceres_jet_tag, scalar_tag) {
        T rAxt(rA(0,0).a * x + rA(0,1).a * y - rAt[0].a,
            rA(0,0).v * x + rA(0,1).v * y - rAt[0].v);
        T rAyt(rA(1,0).a * x + rA(1,1).a * y - rAt[1].a,
            rA(1,0).v * x + rA(1,1).v * y - rAt[1].v);

        T xy_dist = norm(rAxt, rAyt);

        return (r - xy_dist);
    }
    template<typename U>
    T calculate(U&& x, U&& y, ceres_jet_tag, ceres_jet_tag) {
        T rAxt(rA(0,0).a * x.a + rA(0,1).a * y.a - rAt[0].a,
            rA(0,0).v * x.a + rA(0,0).a * x.v + rA(0,1).v * y.a + rA(0,1).a * y.v - rAt[0].v);
        T rAyt(rA(1,0).a * x.a + rA(1,1).a * y.a - rAt[1].a,
            rA(1,0).v * x.a + rA(1,0).a * x.v + rA(1,1).v * y.a + rA(1,1).a * y.v - rAt[1].v);

        T xy_dist = norm(rAxt, rAyt);

        return (r - xy_dist);
    }
private:
    Eigen::Matrix<T, 2, 2> rA;
    Eigen::Matrix<T, 2, 1> rAt;
    T r;
};

// Calculates the x crossings of a conic at a given y value. Returns the number of crossings (0, 1 or 2)
template<typename Scalar>
int getXCrossing(const Conic<Scalar>& conic, Scalar y, Scalar& x1, Scalar& x2) {
    using std::sqrt;

    Scalar a = conic.A;
    Scalar b = conic.B*y + conic.D;
    Scalar c = conic.C*y*y + conic.E*y + conic.F;

    Scalar det = b*b - 4*a*c;
    if (det == 0) {
        x1 = -b/(2*a);
        return 1;
    } else if (det < 0) {
        return 0;
    } else {
        Scalar sqrtdet = sqrt(det);
        x1 = (-b - sqrtdet)/(2*a);
        x2 = (-b + sqrtdet)/(2*a);
        return 2;
    }
}

template<template<class, int> class Jet, class T, int N>
typename std::enable_if<std::is_same<typename ad_traits<Jet<T,N>>::ad_tag, ceres_jet_tag>::value, Ellipse2D<T>>::type
toConst(const Ellipse2D<Jet<T,N>>& ellipse) {
    return Ellipse2D<T>(
        ellipse.centre[0].a,
        ellipse.centre[1].a,
        ellipse.major_radius.a,
        ellipse.minor_radius.a,
        ellipse.angle.a);
}

template<class T>
Ellipse2D<T> scaledMajorRadius(const Ellipse2D<T>& ellipse, const T& target_radius) {
    return Ellipse2D<T>(
        ellipse.centre[0],
        ellipse.centre[1],
        target_radius,
        target_radius * ellipse.minor_radius/ellipse.major_radius,
        ellipse.angle);
};

namespace internal {
    template<class T> T ellipseGoodness(const Ellipse2D<T>& ellipse, const cv::Mat_<uint8_t>& eye, T band_width, T step_epsilon, scalar_tag);
    template<class T> T ellipseGoodness(const Ellipse2D<T>& ellipse, const cv::Mat_<uint8_t>& eye, typename ad_traits<T>::scalar band_width, typename ad_traits<T>::scalar step_epsilon, ceres_jet_tag);
}

// Calculates the "goodness" of an ellipse.
//
// This is defined as the difference in region means:
//
//    μ⁻ - μ⁺
//
// where
//         Σ_p (H(d(p)+w) - H(d(p))) I(p)
//    μ⁻ = ------------------------------
//           Σ_p (H(d(p)+w) - H(d(p)))
//
//         Σ_p (H(d(p)+w) - H(d(p))) I(p)
//    μ⁺ = ------------------------------
//           Σ_p (H(d(p)+w) - H(d(p)))
//
// (see eqs 16, 20, 21 in the PETMEI paper)
//
// The ellipse distance d(p) is defined as
//
//    d(p) = r * (1 - ||A(p - t)||)
//
// with r as the major radius and A as the matrix that transforms the ellipse to a unit circle.
//
//          ||A(p - t)||   maps the ellipse to a unit circle
//      1 - ||A(p - t)||   measures signed distance from unit circle edge
// r * (1 - ||A(p - t)||)  scales this to major radius of ellipse, for (roughly) pixel distance
//
template<class T>
inline T ellipseGoodness(const Ellipse2D<T>& ellipse, const cv::Mat_<uint8_t>& eye, typename ad_traits<T>::scalar band_width, typename ad_traits<T>::scalar step_epsilon) {
    // band_width     The width of each band (inner and outer)
    // step_epsilon   The epsilon of the soft step function

    return internal::ellipseGoodness<T>(ellipse, eye, band_width, step_epsilon, typename ad_traits<T>::ad_tag());
}

//#define DEBUG_ELLIPSE_GOODNESS
//#define USE_INLINED_ELLIPSE_DIST

#ifdef USE_INLINED_ELLIPSE_DIST
#define IF_INLINED_ELLIPSE_DIST(...) __VA_ARGS__
#else
#define IF_INLINED_ELLIPSE_DIST(...)
#endif

namespace internal {
// Non autodiff version of ellipse goodness calculation
template<class T>
T ellipseGoodness(const Ellipse2D<T>& ellipse, const cv::Mat_<uint8_t>& eye, T band_width, T step_epsilon, scalar_tag) {
    using std::max;
    using std::min;
    using std::ceil;
    using std::floor;
    using std::sin;
    using std::cos;

    // Ellipses (and corresponding conics) delimiting the region in which the band masks will be non-zero
    Ellipse2D<T> outerEllipse = scaledMajorRadius(ellipse, ellipse.major_radius + ((band_width + step_epsilon) + 0.5));
    Ellipse2D<T> innerEllipse = scaledMajorRadius(ellipse, ellipse.major_radius - ((band_width + step_epsilon) + 0.5));
    Conic<T> outerConic(outerEllipse);
    Conic<T> innerConic(innerEllipse);

    // Variables for calculating the mean
    T sum_inner = T(0), count_inner = T(0), sum_outer = T(0), count_outer = T(0);

    // Only iterate over pixels within the outer ellipse's bounding box
    cv::Rect bb = bounding_box(outerEllipse);
    bb &= cv::Rect(-eye.cols/2,-eye.rows/2,eye.cols,eye.rows);

#ifndef USE_INLINED_ELLIPSE_DIST
    // Ellipse distance calculator
    EllipseDistCalculator<T> ellipDist(ellipse);
#else
    // Instead of calculating
    //     r * (1 - ||A(p - t)||)
    // we use
    //     (r - ||rAp - rAt||)
    // and precalculate r, rA and rAt.
    Eigen::Matrix<T, 2, 2> rA;
    T r = ellipse.major_radius;
    rA << r*cos(ellipse.angle)/ellipse.major_radius, r*sin(ellipse.angle)/ellipse.major_radius,
        -r*sin(ellipse.angle)/ellipse.minor_radius, r*cos(ellipse.angle)/ellipse.minor_radius;
    Eigen::Matrix<T, 2, 1> rAt = rA*ellipse.centre;

    // Actually,
    ///    rAp - rAt = rA(0,y) + rA(x,0) - rAt
    // So, can perform a strength reduction to calculate rAp iteratively.

    // rA(0,y) - rAt, with y_0 = bb.y
    Eigen::Matrix<T, 2, 1> rA0yrAt(rA(0,1) * bb.y - rAt[0], rA(1,1) * bb.y - rAt[1]);
    // rA(1,0), for incrementing x
    Eigen::Matrix<T, 2, 1> rA10 = rA.col(0);
    // rA(0,1), for incrementing y
    Eigen::Matrix<T, 2, 1> rA01 = rA.col(1);
#endif

    for (int i = bb.y; i < bb.y + bb.height; ++i IF_INLINED_ELLIPSE_DIST(, rA0yrAt += rA01)) {
        // Image row pointer -- (0,0) is centre of image, so shift accordingly
        const uint8_t* eye_i = eye[i + eye.rows/2];

        // Only iterate over pixels between the inner and outer ellipse
        T ox1, ox2;
        int outerCrossings = getXCrossing<T>(outerConic, i, ox1, ox2);
        if (outerCrossings < 2) {
            // If we don't cross the outer ellipse at all, exit early
            continue;
        }
        T ix1, ix2;
        int innerCrossings = innerEllipse.minor_radius > 0 ? getXCrossing<T>(innerConic, i, ix1, ix2) : 0;

        // Define pairs of x values to iterate between
        std::vector<std::pair<int,int>> xpairs;
        if (innerCrossings < 2) {
            // If we don't cross the inner ellipse, iterate between the two crossings of the outer ellipse
            xpairs.emplace_back(max<int>((int)floor(ox1),bb.x), min<int>((int)ceil(ox2), bb.x+bb.width-1));
        } else {
            // Otherwise, iterate between outer-->inner, then inner-->outer.
            xpairs.emplace_back(max<int>((int)floor(ox1),bb.x), min<int>((int)ceil(ix1), bb.x+bb.width-1));
            xpairs.emplace_back(max<int>((int)floor(ix2),bb.x), min<int>((int)ceil(ox2), bb.x+bb.width-1));
        }

        // Go over x pairs (that is, outer-->outer or outer-->inner,inner-->outer)
        for (const auto& xpair : xpairs) {
            // Pixel pointer, shifted accordingly
            const uint8_t* eye_ij = eye_i + xpair.first + eye.cols/2;

#ifdef USE_INLINED_ELLIPSE_DIST
            // rA(0,y) + rA(x,0) - rAt, with x_0 = xpair.first
            Eigen::Matrix<T, 2, 1> rApt(rA0yrAt(0) + rA(0,0)*xpair.first, rA0yrAt(1) + rA(1,0)*xpair.first);
#endif

            for (int j = xpair.first; j <= xpair.second; ++j, ++eye_ij IF_INLINED_ELLIPSE_DIST(, rApt += rA10)) {
                auto eye_ij_val = *eye_ij;
                if (eye_ij_val > 200) {
                    // Ignore bright areas (i.e. glints)
                    continue;
                }

#ifdef USE_INLINED_ELLIPSE_DIST
                T dist = (r - norm(rApt(0), rApt(1)));
#else
                T dist = ellipDist(T(j), T(i));
#endif

                // Calculate mask values for each band
                T Hellip = Heaviside(dist, step_epsilon);
                T Houter = Heaviside(dist+band_width, step_epsilon);
                T Hinner = Heaviside(dist-band_width, step_epsilon);

                T outer_weight = (Houter - Hellip);
                T inner_weight = (Hellip - Hinner);

                sum_outer += outer_weight * eye_ij_val;
                count_outer += outer_weight;

                sum_inner += inner_weight * eye_ij_val;
                count_inner += inner_weight;
            }
        }
    }

    // Get mean values, defaulting to 255 and 0 if count_inner/count_outer are 0 (respectively)
    // Using 255 and 0 because these are the "worst" values, so some pixels will be preferred over none.
    T mu_inner = (count_inner==0 ? 255 : sum_inner/count_inner);
    T mu_outer = (count_outer==0 ? 0 : sum_outer/count_outer);

    // If count < 100 pixels, interpolate between mean value and "worst" value. This will push the
    // gradient away from small pixel counts in a vaguely smooth way.
    if (count_outer < 100) {
        mu_outer = math::lerp<T>(0, mu_outer, count_outer/100.0);
    }
    if (count_inner < 100) {
        mu_inner = math::lerp<T>(255, mu_inner, count_inner/100.0);
    }

    // Return difference of mean values
    return mu_outer - mu_inner;
}

// Autodiff version of ellipse goodness calculation
template<class Jet>
Jet ellipseGoodness(const Ellipse2D<Jet>& ellipse, const cv::Mat_<uint8_t>& eye, typename ad_traits<Jet>::scalar band_width, typename ad_traits<Jet>::scalar step_epsilon, ceres_jet_tag) {
    using std::max;
    using std::min;
    using std::ceil;
    using std::floor;

#ifdef DEBUG_ELLIPSE_GOODNESS
    cv::Mat_<cv::Vec3b> eye_proc = cv::Mat_<cv::Vec3b>::zeros(eye.rows, eye.cols);
    cv::Mat_<cv::Vec3b> eye_H = cv::Mat_<cv::Vec3b>::zeros(eye.rows, eye.cols);
#endif

    typedef typename ad_traits<Jet>::scalar T;
    typedef Jet Jet_t;

    // A constant version of the ellipse
    Ellipse2D<T> constEllipse = toConst(ellipse);

    // Ellipses (and corresponding conics) delimiting the region in which the band masks will be non-zero
    Ellipse2D<T> constOuterEllipse = scaledMajorRadius(constEllipse, constEllipse.major_radius + ((band_width + step_epsilon) + 0.5));
    Ellipse2D<T> constInnerEllipse = scaledMajorRadius(constEllipse, constEllipse.major_radius - ((band_width + step_epsilon) + 0.5));
    Conic<T> constOuterConic(constOuterEllipse);
    Conic<T> constInnerConic(constInnerEllipse);

    // Variables for calculating the mean
    Jet_t sum_inner = Jet_t(0), count_inner = Jet_t(0), sum_outer = Jet_t(0), count_outer = Jet_t(0);

    // Only iterate over pixels within the outer ellipse's bounding box
    cv::Rect bb = bounding_box(constOuterEllipse);
    bb &= cv::Rect(-eye.cols/2,-eye.rows/2,eye.cols,eye.rows);

#ifndef USE_INLINED_ELLIPSE_DIST
    // Ellipse distance calculator
    EllipseDistCalculator<Jet_t> ellipDist(ellipse);
    EllipseDistCalculator<T> constEllipDist(constEllipse);
#else
    // Instead of calculating
    //     r * (1 - ||A(p - t)||)
    // we use
    //     (r - ||rAp - rAt||)
    // and precalculate r, rA and rAt.
    Eigen::Matrix<T, 2, 2> rA;
    T r = constEllipse.major_radius;
    rA << r*cos(constEllipse.angle)/constEllipse.major_radius, r*sin(constEllipse.angle)/constEllipse.major_radius,
         -r*sin(constEllipse.angle)/constEllipse.minor_radius, r*cos(constEllipse.angle)/constEllipse.minor_radius;
    Eigen::Matrix<T, 2, 1> rAt = rA*constEllipse.centre;

    // And non-constant versions of the above
    Eigen::Matrix<Jet_t, 2, 2> rA_jet;
    Jet_t r_jet = ellipse.major_radius;
    rA_jet << r_jet*cos(ellipse.angle)/ellipse.major_radius, r_jet*sin(ellipse.angle)/ellipse.major_radius,
             -r_jet*sin(ellipse.angle)/ellipse.minor_radius, r_jet*cos(ellipse.angle)/ellipse.minor_radius;
    Eigen::Matrix<Jet_t, 2, 1> rAt_jet = rA_jet*ellipse.centre;

    // Actually,
    ///    rAp - rAt = rA(0,y) + rA(x,0) - rAt
    // So, can perform a strength reduction to calculate rAp iteratively.

    // rA(0,y) - rAt, with y_0 = bb.y
    Eigen::Matrix<T, 2, 1> rA0yrAt(rA(0,1) * bb.y - rAt[0], rA(1,1) * bb.y - rAt[1]);
    // rA(1,0), for incrementing x
    Eigen::Matrix<T, 2, 1> rA10 = rA.col(0);
    // rA(0,1), for incrementing y
    Eigen::Matrix<T, 2, 1> rA01 = rA.col(1);
#endif

    for (int i = bb.y; i < bb.y + bb.height; ++i IF_INLINED_ELLIPSE_DIST(, rA0yrAt += rA01)) {
        // Image row pointer -- (0,0) is centre of image, so shift accordingly
        const uint8_t* eye_i = eye[i + eye.rows/2];

        // Only iterate over pixels between the inner and outer ellipse
        T ox1, ox2;
        int outerCrossings = getXCrossing<T>(constOuterConic, i, ox1, ox2);
        if (outerCrossings < 2) {
            // If we don't cross the outer ellipse at all, exit early
            continue;
        }
        T ix1, ix2;
        int innerCrossings = constInnerEllipse.major_radius > 0 ? getXCrossing<T>(constInnerConic, i, ix1, ix2) : 0;

        // Define pairs of x values to iterate between
        std::vector<std::pair<int,int>> xpairs;
        if (innerCrossings < 2) {
            // If we don't cross the inner ellipse, iterate between the two crossings of the outer ellipse
            xpairs.emplace_back(max<int>((int)floor(ox1),bb.x), min<int>((int)ceil(ox2), bb.x+bb.width-1));
        } else {
            // Otherwise, iterate between outer-->inner, then inner-->outer.
            xpairs.emplace_back(max<int>((int)floor(ox1),bb.x), min<int>((int)ceil(ix1), bb.x+bb.width-1));
            xpairs.emplace_back(max<int>((int)floor(ix2),bb.x), min<int>((int)ceil(ox2), bb.x+bb.width-1));
        }

#ifdef USE_INLINED_ELLIPSE_DIST
        // Precalculate the gradient of
        //     rA(y,0) - rAt
        auto rAy0rAt_x_v = (rA_jet(0,1).v * i - rAt_jet(0).v).eval();
        auto rAy0rAt_y_v = (rA_jet(1,1).v * i - rAt_jet(1).v).eval();
#endif

        // Go over x pairs (that is, outer-->outer or outer-->inner,inner-->outer)
        for (const auto& xpair : xpairs) {

            // Pixel pointer, shifted accordingly
            const uint8_t* eye_ij = eye_i + xpair.first + eye.cols/2;

#ifdef USE_INLINED_ELLIPSE_DIST
            // rA(0,y) + rA(x,0) - rAt, with x_0 = xpair.first
            Eigen::Matrix<T, 2, 1> rApt(rA0yrAt(0) + rA(0,0)*xpair.first, rA0yrAt(1) + rA(1,0)*xpair.first);
#endif

            for (int j = xpair.first; j <= xpair.second; ++j, ++eye_ij IF_INLINED_ELLIPSE_DIST(, rApt += rA10)) {

                T eye_ij_val = *eye_ij;
                if (eye_ij_val > 200) {
                    // Ignore bright areas (i.e. glints)
                    continue;
                }

                // Calculate signed ellipse distance without gradient first, in case the gradient is 0
#ifdef USE_INLINED_ELLIPSE_DIST
                T dist_const = (r - norm(rApt(0), rApt(1)));
#else
                T dist_const = constEllipDist(T(j), T(i));
#endif

                // Check if we are within step_epsilon of the edges of the bands. If yes, calculate
                // the gradient. Otherwise, the gradient is known to be 0.
                if (abs(dist_const) < step_epsilon
                    || abs(dist_const-band_width) < step_epsilon
                    || abs(dist_const+band_width) < step_epsilon) {

#ifdef USE_INLINED_ELLIPSE_DIST
                    // Calculate the gradients of rApt, and use those to get the dist
                    Jet_t rAxt_jet(rApt(0),
                        rA_jet(0,0).v * j + rAy0rAt_x_v);
                    Jet_t rAyt_jet(rApt(1),
                        rA_jet(1,0).v * j + rAy0rAt_y_v);

                    //Eigen::Matrix<Jet,2,1> rApt_jet2 = rA_jet*Eigen::Matrix<Jet,2,1>(Jet(j),Jet(i)) - rAt_jet;

                    Jet_t dist = (r_jet - norm(rAxt_jet, rAyt_jet));
                    //Jet_t dist2 = ellipDist(T(j), T(i));
#else
                    Jet_t dist = ellipDist(T(j), T(i));
#endif

                    // Calculate mask values and derivatives for each band
                    Jet_t Hellip = Heaviside(dist, step_epsilon);
                    Jet_t Houter = Heaviside(dist+band_width, step_epsilon);
                    Jet_t Hinner = Heaviside(dist-band_width, step_epsilon);

                    Jet_t outer_weight = (Houter - Hellip);
                    Jet_t inner_weight = (Hellip - Hinner);

                    // Inline the Jet operator+= to allow eigen expression and noalias magic.
                    sum_outer.a += outer_weight.a * eye_ij_val;
                    sum_outer.v.noalias() += outer_weight.v * eye_ij_val;
                    count_outer.a += outer_weight.a;
                    count_outer.v.noalias() += outer_weight.v;

                    sum_inner.a += inner_weight.a * eye_ij_val;
                    sum_inner.v.noalias() += inner_weight.v * eye_ij_val;
                    count_inner.a += inner_weight.a;
                    count_inner.v.noalias() += inner_weight.v;

                    #ifdef DEBUG_ELLIPSE_GOODNESS
                        eye_H(i + eye.rows/2,j + eye.cols/2)[2] = outer_weight.a*255;
                        eye_H(i + eye.rows/2,j + eye.cols/2)[1] = inner_weight.a*255;
                        eye_H(i + eye.rows/2,j + eye.cols/2)[0] = 255;

                        eye_proc(i + eye.rows/2,j + eye.cols/2)[2] = outer_weight.a * eye_ij_val;
                        eye_proc(i + eye.rows/2,j + eye.cols/2)[1] = inner_weight.a * eye_ij_val;
                        eye_proc(i + eye.rows/2,j + eye.cols/2)[0] = 255;
                    #endif

                } else {
                    // Calculate mask values for each band
                    T Hellip = Heaviside(dist_const, step_epsilon);
                    T Houter = Heaviside(dist_const+band_width, step_epsilon);
                    T Hinner = Heaviside(dist_const-band_width, step_epsilon);

                    T outer_weight = (Houter - Hellip);
                    T inner_weight = (Hellip - Hinner);

                    sum_outer.a += outer_weight * eye_ij_val;
                    count_outer.a += outer_weight;

                    sum_inner.a += inner_weight * eye_ij_val;
                    count_inner.a += inner_weight;

                    #ifdef DEBUG_ELLIPSE_GOODNESS
                    eye_H(i + eye.rows/2,j + eye.cols/2)[2] = outer_weight*255;
                    eye_H(i + eye.rows/2,j + eye.cols/2)[1] = inner_weight*255;
                    eye_H(i + eye.rows/2,j + eye.cols/2)[0] = 0;

                    eye_proc(i + eye.rows/2,j + eye.cols/2)[2] = outer_weight * eye_ij_val;
                    eye_proc(i + eye.rows/2,j + eye.cols/2)[1] = inner_weight * eye_ij_val;
                    eye_proc(i + eye.rows/2,j + eye.cols/2)[0] = 255;
                    #endif
                }
            }
        }
    }

    // Get mean values, defaulting to 255 and 0 if count_inner/count_outer are 0 (respectively)
    // Using 255 and 0 because these are the "worst" values, so some pixels will be preferred over none.
    Jet mu_inner = (count_inner.a==0 ? Jet(255) : sum_inner/count_inner);
    Jet mu_outer = (count_outer.a==0 ? Jet(0) : sum_outer/count_outer);

    // If count < 100 pixels, interpolate between mean value and "worst" value. This will push the
    // gradient away from small pixel counts in a vaguely smooth way.
    if (count_outer.a < 100) {
        mu_outer = math::lerp<Jet>(Jet(0), mu_outer, count_outer/100.0);
    }
    if (count_inner.a < 100) {
        mu_inner = math::lerp<Jet>(Jet(255), mu_inner, count_inner/100.0);
    }

    // Return difference of mean values
    return mu_outer - mu_inner;
}
}

template<typename T>
Eigen::Matrix<T,3,1> sph2cart(T r, T theta, T psi) {
    using std::sin;
    using std::cos;

    return r * Eigen::Matrix<T,3,1>(sin(theta)*cos(psi), cos(theta), sin(theta)*sin(psi));
}

template<typename T>
T angleDiffGoodness(T theta1, T psi1, T theta2, T psi2, typename ad_traits<T>::scalar sigma) {
    using std::sin;
    using std::cos;
    using std::acos;
    using std::asin;
    using std::atan2;
    using std::sqrt;

    if (theta1 == theta2 && psi1 == psi2) {
        return T(1);
    }

    // Haversine distance
    auto dist = T(2)*asin(sqrt(sq(sin((theta1-theta2)/T(2))) + cos(theta1)*cos(theta2)*sq(sin((psi1-psi2)/T(2)))));
    return exp(-sq(dist)/sq(sigma));
}

template<typename T>
Circle3D<T> circleOnSphere(const Sphere<T>& sphere, T theta, T psi, T circle_radius) {
    typedef Eigen::Matrix<T,3,1> Vector3;

    Vector3 radial = sph2cart<T>(T(1), theta, psi);
    return Circle3D<T>(sphere.centre + sphere.radius * radial,
        radial,
        circle_radius);
}

template<typename T>
struct EllipseGoodnessFunction {
    T operator()(const Sphere<T>& eye, T theta, T psi, T pupil_radius, T focal_length, typename ad_traits<T>::scalar band_width, typename ad_traits<T>::scalar step_epsilon, const cv::Mat& mEye) {
        typedef Eigen::Matrix<T,3,1> Vector3;
        typedef typename ad_traits<T>::scalar Const;

        static const Vector3 camera_centre(T(0),T(0),T(0));

        // Check for bounds. The worst possible value of ellipseGoodness is -255, so use that as a starting point for out-of-bounds pupils

        // Pupil radius must be positive
        if (pupil_radius <= Const(0))
        {
            // Return -255 for radius == 0, and even lower values for
            // radius < 0
            // This should push the gradient towards positive radius,
            // rather than just returning flat -255
            return Const(-255.0) + pupil_radius;
        }

        Circle3D<T> pupil_circle = circleOnSphere(eye, theta, psi, pupil_radius);

        // Ellipse normal must point towards camera
        T normalDotPos = pupil_circle.normal.dot(camera_centre - pupil_circle.centre);
        if (normalDotPos <= Const(0))
        {
            // Return -255 for normalDotPos == 0, and even lower values for
            // normalDotPos < 0
            // This should push the gradient towards positive normalDotPos,
            // rather than just returning flat -255
            return Const(-255.0) + normalDotPos;
        }

        // Angles should be in the range
        //    theta: 0 -> pi
        //      psi: -pi -> 0
        // If we're outside of this range AND radialDotEye > 0, then we must
        // have gone all the way around, so just return worst case (i.e as bad
        // as radialDotEye == -1) with additional penalty for how far out we
        // are, again to push the gradient back inwards.
        if (theta < Const(0) || theta > Const(PI) || psi < Const(-PI) || psi > Const(0))
        {
            T ret = Const(-255.0) - (camera_centre - pupil_circle.centre).norm();
            if (theta < Const(0))
                ret -= (Const(0) - theta);
            else if (theta > Const(PI))
                ret -= (theta - Const(PI));
            if (psi < Const(-PI))
                ret -= (Const(-PI) - psi);
            else if (psi > Const(0))
                ret -= (psi - Const(0));
        }

        // Ok, everything looks good so far, calculate the actual goodness.

        Ellipse2D<T> pupil_ellipse(project(pupil_circle, focal_length));

        return ellipseGoodness<T>(pupil_ellipse, mEye, band_width, step_epsilon);
    }
};

template<typename Scalar>
class EllipseDistanceResidualFunction {
public:
    EllipseDistanceResidualFunction(const cv::Mat& eye_image, const std::vector<cv::Point2f>& pupil_inliers, const Scalar& eye_radius, const Scalar& focal_length) :
        eye_image(eye_image), pupil_inliers(pupil_inliers), eye_radius(eye_radius), focal_length(focal_length) {}

    template <typename T>
    bool operator()(const T* const eye_param, const T* const pupil_param, T* e) const {
        typedef typename ad_traits<T>::scalar Const;

        Eigen::Matrix<T,3,1> eye_pos(eye_param[0], eye_param[1], eye_param[2]);
        Sphere<T> eye(eye_pos, T(eye_radius));

        Ellipse2D<T> pupil_ellipse(project(circleOnSphere(eye, pupil_param[0], pupil_param[1], pupil_param[2]), T(focal_length)));

        EllipseDistCalculator<T> ellipDist(pupil_ellipse);

        for (int i = 0; i < pupil_inliers.size(); ++i) {
            const cv::Point2f& inlier = pupil_inliers[i];
            e[i] = ellipDist(Const(inlier.x), Const(inlier.y));
        }

        return true;
    }
private:
    const cv::Mat& eye_image;
    const std::vector<cv::Point2f>& pupil_inliers;
    const Scalar& eye_radius;
    const Scalar& focal_length;
};

template<typename Scalar>
struct EllipsePointDistanceFunction {
    EllipsePointDistanceFunction(const Ellipse2D<Scalar>& el, Scalar x, Scalar y) : el(el), x(x), y(y) {}

    template <typename T>
    bool operator()(const T* const t, T* e) const
    {
        using std::sin;
        using std::cos;

        auto&& pt = pointAlongEllipse(el, t[0]);
        e[0] = norm(x - pt.x(), y - pt.y());

        return true;
    }

    const Ellipse2D<Scalar>& el;
    Scalar x, y;
};

template<bool has_eye_var=true>
struct PupilContrastTerm : public spii::Term {
    const Sphere<double>& init_eye;
    double focal_length;
    const cv::Mat eye_image;
    double band_width;
    double step_epsilon;

    int eye_var_idx() const { return has_eye_var ? 0 : -1; }
    int pupil_var_idx() const { return has_eye_var ? 1 : 0; }

    PupilContrastTerm(const Sphere<double>& eye, double focal_length, cv::Mat eye_image, double band_width, double step_epsilon) :
        init_eye(eye),
        focal_length(focal_length),
        eye_image(eye_image),
        band_width(band_width),
        step_epsilon(step_epsilon)
    {}

    virtual int number_of_variables() const override {
        int nvars = 1; // This pupil params
        if (has_eye_var)
            nvars++; // Eye params

        return nvars;
    }
    virtual int variable_dimension(int var) const override {
        if (var == eye_var_idx()) // Eye params (x,y,z)
            return 3;
        if (var == pupil_var_idx()) // This pupil params (theta, psi, r)
            return 3;
        return -1;
    };
    virtual double evaluate(double * const * const vars) const override
    {
        auto& pupil_vars = vars[pupil_var_idx()];

        auto eye = init_eye;
        if (has_eye_var) {
            auto& eye_vars = vars[eye_var_idx()];
            eye.centre = Sphere<double>::Vector(eye_vars[0], eye_vars[1], eye_vars[2]);
        }

        EllipseGoodnessFunction<double> goodnessFunction;
        auto theta = pupil_vars[0];
        auto psi = pupil_vars[1];
        auto r = pupil_vars[2];
        auto goodness = goodnessFunction(eye,
            theta, psi, r,
            focal_length,
            band_width, step_epsilon,
            eye_image);

        return -goodness;
    }
    virtual double evaluate(double * const * const vars, std::vector<Eigen::VectorXd>* gradient) const override
    {
        auto& pupil_vars = vars[pupil_var_idx()];

        double contrast_goodness_a;
        Eigen::Matrix<double,3,1> eye_contrast_goodness_v;
        Eigen::Matrix<double,3,1> pupil_contrast_goodness_v;

        // Get region contrast goodness using EllipseGoodnessFunction.
        if (has_eye_var) {
            // If varying the eye parameters, calculate the gradient wrt. to 6 params (3 eye + 3 pupil)
            typedef ceres::Jet<double, 6> EyePupilJet;

            auto& eye_vars = vars[eye_var_idx()];
            Eigen::Matrix<EyePupilJet,3,1> eye_pos(EyePupilJet(eye_vars[0], 0), EyePupilJet(eye_vars[1], 1), EyePupilJet(eye_vars[2], 2));
            Sphere<EyePupilJet> eye(eye_pos, EyePupilJet(init_eye.radius));

            EyePupilJet contrast_goodness;
            {
                EllipseGoodnessFunction<EyePupilJet> goodnessFunction;
                auto theta = EyePupilJet(pupil_vars[0], 3);
                auto psi = EyePupilJet(pupil_vars[1], 4);
                auto r = EyePupilJet(pupil_vars[2], 5);
                contrast_goodness = goodnessFunction(eye,
                    theta, psi, r,
                    EyePupilJet(focal_length),
                    band_width, step_epsilon,
                    eye_image);
            }

            contrast_goodness_a = contrast_goodness.a;
            eye_contrast_goodness_v = contrast_goodness.v.segment<3>(0);
            pupil_contrast_goodness_v = contrast_goodness.v.segment<3>(3);
        } else {
            // Otherwise, calculate the gradient wrt. to the 3 pupil params
            typedef ::ceres::Jet<double,3> PupilJet;

            Eigen::Matrix<PupilJet,3,1> eye_pos(PupilJet(init_eye.centre[0]), PupilJet(init_eye.centre[1]), PupilJet(init_eye.centre[2]));
            ::Sphere<PupilJet> eye(eye_pos, PupilJet(init_eye.radius));

            PupilJet contrast_goodness;
            {
                EllipseGoodnessFunction<PupilJet> goodnessFunction;
                auto theta = PupilJet(pupil_vars[0], 0);
                auto psi = PupilJet(pupil_vars[1], 1);
                auto r = PupilJet(pupil_vars[2], 2);
                contrast_goodness = goodnessFunction(eye,
                    theta, psi, r,
                    PupilJet(focal_length),
                    band_width, step_epsilon,
                    eye_image);
            }

            contrast_goodness_a = contrast_goodness.a;
            pupil_contrast_goodness_v = contrast_goodness.v;
        }

        double goodness;
        auto& eye_gradient = (*gradient)[eye_var_idx()];
        auto& pupil_gradient = (*gradient)[pupil_var_idx()];

        // No smoothness term, goodness and gradient are based only on frame goodness
        goodness = contrast_goodness_a;
        if (has_eye_var)
            eye_gradient = eye_contrast_goodness_v;
        pupil_gradient = pupil_contrast_goodness_v;

        // Flip sign to change goodness into cost (i.e. maximising into minimising)
        auto cost = -goodness;
        for (int i = 0; i < number_of_variables(); ++i) {
            (*gradient)[i] = -(*gradient)[i];
        }
        return cost;
    }
    virtual double evaluate(double * const * const variables,
        std::vector<Eigen::VectorXd>* gradient,
        std::vector< std::vector<Eigen::MatrixXd> >* hessian) const override {
            throw std::runtime_error("Not implemented");
    }

};

// Anthropomorphic term
struct PupilAnthroTerm : public spii::Term {
    double mean;
    double sigma;
    double scale;

    PupilAnthroTerm(double mean, double sigma, double scale) : mean(mean), sigma(sigma), scale(scale)
    {}

    virtual int number_of_variables() const override {
        int nvars = 1; // This pupil params
        return nvars;
    }
    virtual int variable_dimension(int var) const override {
        if (var == 0) // This pupil params (r)
            return 3;
        return -1;
    }
    virtual double evaluate(double * const * const vars) const override
    {
        using math::sq;

        auto r = vars[0][2];
        auto radius_anthro_goodness = exp(-sq(r - mean)/sq(sigma));

        double goodness = radius_anthro_goodness;

        // Flip sign to change goodness into cost (i.e. maximising into minimising)
        auto cost = -goodness*scale;
        return cost;
    }
    virtual double evaluate(double * const * const vars, std::vector<Eigen::VectorXd>* gradient) const override
    {
        using math::sq;

        auto r = ceres::Jet<double,1>(vars[0][2], 0);
        auto radius_anthro_goodness = exp(-sq(r - mean)/sq(sigma));

        double goodness = radius_anthro_goodness.a;
        (*gradient)[0].segment<1>(2) = radius_anthro_goodness.v;

        // Flip sign to change goodness into cost (i.e. maximising into minimising)
        auto cost = -goodness*scale;
        for (int i = 0; i < number_of_variables(); ++i) {
            (*gradient)[i] = -(*gradient)[i]*scale;
        }
        return cost;
    }
    virtual double evaluate(double * const * const variables,
        std::vector<Eigen::VectorXd>* gradient,
        std::vector< std::vector<Eigen::MatrixXd> >* hessian) const override {
            throw std::runtime_error("Not implemented");
    }

};

const EyeModelFitter::Vector3 EyeModelFitter::camera_centre = EyeModelFitter::Vector3::Zero();

EyeModelFitter::Pupil::Pupil(Observation observation) : observation(observation), params(0, 0, 0)
{

}

EyeModelFitter::Pupil::Pupil()
{

}

EyeModelFitter::PupilParams::PupilParams(double theta, double psi, double radius) : theta(theta), psi(psi), radius(radius)
{

}

EyeModelFitter::PupilParams::PupilParams() : theta(0), psi(0), radius(0)
{

}

EyeModelFitter::Observation::Observation(cv::Mat image, Ellipse ellipse, std::vector<cv::Point2f> inliers) : image(std::move(image)), ellipse(std::move(ellipse)), inliers(std::move(inliers))
{

}

EyeModelFitter::Observation::Observation()
{

}

}

singleeyefitter::EyeModelFitter::EyeModelFitter() : region_band_width(5), region_step_epsilon(0.5), region_scale(1)
{

}
singleeyefitter::EyeModelFitter::EyeModelFitter(double focal_length, double region_band_width, double region_step_epsilon) : focal_length(focal_length), region_band_width(region_band_width), region_step_epsilon(region_step_epsilon), region_scale(1)
{

}

singleeyefitter::EyeModelFitter::Index singleeyefitter::EyeModelFitter::add_observation(cv::Mat image, Ellipse pupil, int n_pseudo_inliers /*= 0*/)
{
    std::vector<cv::Point2f> pupil_inliers;
    for (int i = 0; i < n_pseudo_inliers; ++i) {
        auto p = pointAlongEllipse(pupil, i * 2 * M_PI / n_pseudo_inliers);
        pupil_inliers.emplace_back(static_cast<float>(p[0]), static_cast<float>(p[1]));
    }
    return add_observation(std::move(image), std::move(pupil), std::move(pupil_inliers));
}

singleeyefitter::EyeModelFitter::Index singleeyefitter::EyeModelFitter::add_observation(cv::Mat image, Ellipse pupil, std::vector<cv::Point2f> pupil_inliers)
{
    assert(image.channels() == 1 && image.depth() == CV_8U);

    std::lock_guard<std::mutex> lock_model(model_mutex);

    pupils.emplace_back(
        Observation(std::move(image), std::move(pupil), std::move(pupil_inliers))
        );
    return pupils.size() - 1;
}

void EyeModelFitter::reset()
{
    std::lock_guard<std::mutex> lock_model(model_mutex);
    pupils.clear();
    eye = Sphere::Null;
    model_version++;
}

singleeyefitter::EyeModelFitter::Circle singleeyefitter::EyeModelFitter::circleFromParams(const Sphere& eye, const PupilParams& params)
{
    if (params.radius == 0)
        return Circle::Null;

    Vector3 radial = sph2cart<double>(double(1), params.theta, params.psi);
    return Circle(eye.centre + eye.radius * radial,
        radial,
        params.radius);
}

singleeyefitter::EyeModelFitter::Circle singleeyefitter::EyeModelFitter::circleFromParams(const PupilParams& params) const
{
    return circleFromParams(eye, params);
}

void singleeyefitter::EyeModelFitter::print_single_contrast_metric(const Pupil& pupil) const
{
    if (!pupil.circle) {
        std::cout << "No pupil" << std::endl;
        return;
    }

    double params[3];
    params[0] = pupil.params.theta;
    params[1] = pupil.params.psi;
    params[2] = pupil.params.radius;
    double* vars[1];
    vars[0] = params;

    std::vector<Eigen::VectorXd> gradient;
    gradient.push_back(Eigen::VectorXd::Zero(3));

    PupilContrastTerm<false> contrast_term(
        eye,
        focal_length * region_scale,
        cvx::resize(pupil.observation.image, region_scale),
        region_band_width,
        region_step_epsilon);

    double contrast_val = contrast_term.evaluate(vars, &gradient);

    std::cout << "Contrast term: " << contrast_val << std::endl;
    std::cout << "     gradient: [ " << gradient[0].transpose() << " ]" << std::endl;
}

void singleeyefitter::EyeModelFitter::print_single_contrast_metric(Index id) const
{
    print_single_contrast_metric(pupils[id]);
}

double singleeyefitter::EyeModelFitter::single_contrast_metric(const Pupil& pupil) const
{
    if (!pupil.circle) {
        std::cout << "No pupil" << std::endl;
        return 0;
    }

    double params[3];
    params[0] = pupil.params.theta;
    params[1] = pupil.params.psi;
    params[2] = pupil.params.radius;
    double* vars[1];
    vars[0] = params;

    PupilContrastTerm<false> contrast_term(
        eye,
        focal_length * region_scale,
        cvx::resize(pupil.observation.image, region_scale),
        region_band_width,
        region_step_epsilon);

    double contrast_val = contrast_term.evaluate(vars);

    return contrast_val;
}

double singleeyefitter::EyeModelFitter::single_contrast_metric(Index id) const
{
    return single_contrast_metric(pupils[id]);
}

const singleeyefitter::EyeModelFitter::Circle& singleeyefitter::EyeModelFitter::refine_single_with_contrast(Pupil& pupil)
{
    if (!pupil.circle)
        return pupil.circle;

    double params[3];
    params[0] = pupil.params.theta;
    params[1] = pupil.params.psi;
    params[2] = pupil.params.radius;

    spii::Function f;
    f.add_variable(&params[0], 3);
    f.add_term(std::make_shared<PupilContrastTerm<false>>(
        eye,
        focal_length * region_scale,
        cvx::resize(pupil.observation.image, region_scale),
        region_band_width,
        region_step_epsilon), &params[0]);

    spii::LBFGSSolver solver;

	// Commented out due to Visual Studio 2015 error
	//solver.log_function = [](const std::string&) {};

	//solver.function_improvement_tolerance = 1e-5;
    spii::SolverResults results;
    solver.solve(f, &results);
    //std::cout << results << std::endl;

    pupil.params = PupilParams(params[0], params[1], params[2]);
    pupil.circle = circleFromParams(pupil.params);

    return pupil.circle;
}

const singleeyefitter::EyeModelFitter::Circle& singleeyefitter::EyeModelFitter::refine_single_with_contrast(Index id)
{
    return refine_single_with_contrast(pupils[id]);
}

const singleeyefitter::EyeModelFitter::Circle& singleeyefitter::EyeModelFitter::initialise_single_observation(Pupil& pupil)
{
    // Ignore the pupil circle normal, and intersect the pupil circle
    // centre projection line with the eyeball sphere
    try {
        auto pupil_centre_sphere_intersect = intersect(Line3(camera_centre, pupil.circle.centre.normalized()),
            eye);
        auto new_pupil_centre = pupil_centre_sphere_intersect.first;

        // Now that we have 3D positions for the pupil (rather than just a
        // projection line), recalculate the pupil radius at that position.
        auto pupil_radius_at_1 = pupil.circle.radius / pupil.circle.centre.z();
        auto new_pupil_radius = pupil_radius_at_1 * new_pupil_centre.z();

        // Parametrise this new pupil position using spherical coordinates
        Vector3 centre_to_pupil = new_pupil_centre - eye.centre;
        double r = centre_to_pupil.norm();
        pupil.params.theta = acos(centre_to_pupil[1] / r);
        pupil.params.psi = atan2(centre_to_pupil[2], centre_to_pupil[0]);
        pupil.params.radius = new_pupil_radius;

        // Update pupil circle to match parameters
        pupil.circle = circleFromParams(pupil.params);
    }
    catch (no_intersection_exception&) {
        pupil.circle = Circle::Null;
        pupil.params.theta = 0;
        pupil.params.psi = 0;
        pupil.params.radius = 0;
    }

    return pupil.circle;
}

const singleeyefitter::EyeModelFitter::Circle& singleeyefitter::EyeModelFitter::initialise_single_observation(Index id)
{
    initialise_single_observation(pupils[id]);

    /*if (id > 0 && pupils[id-1].circle) {
    // Try previous circle in case of bad fits
    EllipseGoodnessFunction<double> goodnessFunction;
    auto& pupil = pupils[id];
    auto& prevPupil = pupils[id-1];

    double currentGoodness, prevGoodness;
    if (pupil.circle) {
    currentGoodness = goodnessFunction(eye, pupil.params.theta, pupil.params.psi, pupil.params.radius, focal_length, pupil.observation.image);
    prevGoodness = goodnessFunction(eye, prevPupil.params.theta, prevPupil.params.psi, prevPupil.params.radius, focal_length, pupil.observation.image);
    }

    if (!pupil.circle || prevGoodness > currentGoodness) {
    pupil.circle = prevPupil.circle;
    pupil.params = prevPupil.params;
    }
    }*/

    return pupils[id].circle;
}

const singleeyefitter::EyeModelFitter::Circle& singleeyefitter::EyeModelFitter::unproject_single_observation(Pupil& pupil, double pupil_radius /*= 1*/) const
{
    if (eye == Sphere::Null) {
        throw std::runtime_error("Need to get eye centre estimate first (by unprojecting multiple observations)");
    }

    // Single pupil version of "unproject_observations"

    auto unprojection_pair = unproject(pupil.observation.ellipse, pupil_radius, focal_length);

    const Vector3& c = unprojection_pair.first.centre;
    const Vector3& v = unprojection_pair.first.normal;

    Vector2 c_proj = project(c, focal_length);
    Vector2 v_proj = project(v + c, focal_length) - c_proj;

    v_proj.normalize();

    Vector2 eye_centre_proj = project(eye.centre, focal_length);

    if ((c_proj - eye_centre_proj).dot(v_proj) >= 0) {
        pupil.circle = std::move(unprojection_pair.first);
    }
    else {
        pupil.circle = std::move(unprojection_pair.second);
    }

    return pupil.circle;
}

const singleeyefitter::EyeModelFitter::Circle& singleeyefitter::EyeModelFitter::unproject_single_observation(Index id, double pupil_radius /*= 1*/)
{
    return unproject_single_observation(pupils[id], pupil_radius);
}

void singleeyefitter::EyeModelFitter::refine_with_inliers(const CallbackFunction& callback /*= CallbackFunction()*/)
{
    int current_model_version;
    Eigen::Matrix<double, Eigen::Dynamic, 1> x;
    {
        std::lock_guard<std::mutex> lock_model(model_mutex);

        current_model_version = model_version;

        x = Eigen::Matrix<double, Eigen::Dynamic, 1>(3 + 3 * pupils.size());
        x.segment<3>(0) = eye.centre;
        for (int i = 0; i < pupils.size(); ++i) {
            const PupilParams& pupil_params = pupils[i].params;
            x.segment<3>(3 + 3 * i)[0] = pupil_params.theta;
            x.segment<3>(3 + 3 * i)[1] = pupil_params.psi;
            x.segment<3>(3 + 3 * i)[2] = pupil_params.radius;
        }
    }

    ceres::Problem problem;
    for (int i = 0; i < pupils.size(); ++i) {
        const cv::Mat& eye_image = pupils[i].observation.image;
        const auto& pupil_inliers = pupils[i].observation.inliers;

        problem.AddResidualBlock(
            new ceres::AutoDiffCostFunction<EllipseDistanceResidualFunction<double>, ceres::DYNAMIC, 3, 3>(
            new EllipseDistanceResidualFunction<double>(eye_image, pupil_inliers, eye.radius, focal_length),
            (int)pupil_inliers.size()
            ),
            NULL, &x[0], &x[3 + 3 * i]);
    }

    ceres::Solver::Options options;
    options.linear_solver_type = ceres::DENSE_SCHUR;
    options.max_num_iterations = 1000;
    options.function_tolerance = 1e-10;
    options.minimizer_progress_to_stdout = true;
    options.update_state_every_iteration = true;
    if (callback) {
        struct CallCallbackWrapper : public ceres::IterationCallback
        {
            double eye_radius;
            const CallbackFunction& callback;
            const Eigen::Matrix<double, Eigen::Dynamic, 1>& x;

            CallCallbackWrapper(const EyeModelFitter& fitter, const CallbackFunction& callback, const Eigen::Matrix<double, Eigen::Dynamic, 1>& x)
                : eye_radius(fitter.eye.radius), callback(callback), x(x) {}

            virtual ceres::CallbackReturnType operator() (const ceres::IterationSummary& summary) {
                Eigen::Matrix<double, 3, 1> eye_pos(x[0], x[1], x[2]);
                Sphere eye(eye_pos, eye_radius);

                std::vector<Circle> pupils;
                for (int i = 0; i < (x.size() - 3)/3; ++i) {
                    auto&& pupil_param_v = x.segment<3>(3 + 3 * i);
                    pupils.push_back(EyeModelFitter::circleFromParams(eye, PupilParams(pupil_param_v[0], pupil_param_v[1], pupil_param_v[2])));
                }

                callback(eye, pupils);

                return ceres::SOLVER_CONTINUE;
            }
        };
        options.callbacks.push_back(new CallCallbackWrapper(*this, callback, x));
    }
    ceres::Solver::Summary summary;
    ceres::Solve(options, &problem, &summary);
    std::cout << summary.BriefReport() << "\n";

    {
        std::lock_guard<std::mutex> lock_model(model_mutex);

        if (current_model_version != model_version)    {
            std::cout << "Old model, not applying refined parameters" << std::endl;
            return;
        }

        eye.centre = x.segment<3>(0);

        for (int i = 0; i < pupils.size(); ++i) {
            auto&& pupil_param = x.segment<3>(3 + 3 * i);
            pupils[i].params = PupilParams(pupil_param[0], pupil_param[1], pupil_param[2]);
            pupils[i].circle = circleFromParams(eye, pupils[i].params);
        }
    }
}

void singleeyefitter::EyeModelFitter::refine_with_region_contrast(const CallbackFunction& callback /*= CallbackFunction()*/)
{
    int current_model_version;
    Eigen::Matrix<double, Eigen::Dynamic, 1> x0;
    spii::Function f;
    {
        std::lock_guard<std::mutex> lock_model(model_mutex);

        current_model_version = model_version;

        x0 = Eigen::Matrix<double, Eigen::Dynamic, 1>(3 + 3 * pupils.size());
        x0.segment<3>(0) = eye.centre;
        for (int i = 0; i < pupils.size(); ++i) {
            const PupilParams& pupil_params = pupils[i].params;
            x0.segment<3>(3 + 3 * i)[0] = pupil_params.theta;
            x0.segment<3>(3 + 3 * i)[1] = pupil_params.psi;
            x0.segment<3>(3 + 3 * i)[2] = pupil_params.radius;
        }

        f.add_variable(&x0[0], 3);
        for (int i = 0; i < pupils.size(); ++i) {
            if (pupils[i].circle) {
                f.add_variable(&x0[3 + 3 * i], 3);

                f.add_term(
                    std::make_shared<PupilContrastTerm<true>>(
                    eye,
                    focal_length * region_scale,
                    cvx::resize(pupils[i].observation.image, region_scale),
                    region_band_width,
                    region_step_epsilon),
                    &x0[0], &x0[3 + 3 * i]);
                //f.add_term(std::make_shared<PupilAnthroTerm>(2.5, 1, 0.001), &x0[3+3*i]);

                /*if (i == 0 || !pupils[i-1].circle) {
                } else {
                vars.push_back(&x0[3+3*(i-1)]);
                f.add_term(std::make_shared<SinglePupilTerm<true,true,true>>(*this, pupils[i].observation.image), vars);
                }*/
            }
        }
    }

    spii::LBFGSSolver solver;
    solver.maximum_iterations = 1000;
    solver.function_improvement_tolerance = 1e-5;
    spii::SolverResults results;

#if 0 // Turned off due to Visual Studio 2015 error
    if (callback) {
        double eye_radius = eye.radius;
        solver.callback_function = [eye_radius, &callback](const spii::CallbackInformation& info) {
            auto& x = *info.x;

            Eigen::Matrix<double, 3, 1> eye_pos(x(0), x(1), x(2));
            Sphere eye(eye_pos, eye_radius);

            std::vector<Circle> pupils;
            for (int i = 0; i < (x.size() - 3) / 3; ++i) {
                pupils.push_back(EyeModelFitter::circleFromParams(eye, PupilParams(x(3 + 3 * i + 0), x(3 + 3 * i + 1), x(3 + 3 * i + 2))));
            }

            callback(eye, pupils);

            return true;
        };
    }
#endif
    solver.solve(f, &results);
    std::cout << results << std::endl;

    {
        std::lock_guard<std::mutex> lock_model(model_mutex);

        if (current_model_version != model_version)    {
            std::cout << "Old model, not applying refined parameters" << std::endl;
            return;
        }

        eye.centre = x0.segment<3>(0);

        for (int i = 0; i < (x0.size() - 3) / 3; ++i) {
            auto pupil_param = x0.segment<3>(3 + 3 * i);
            pupils[i].params = PupilParams(pupil_param[0], pupil_param[1], pupil_param[2]);
            pupils[i].circle = circleFromParams(pupils[i].params);
        }
    }
}

void singleeyefitter::EyeModelFitter::initialise_model()
{
    std::lock_guard<std::mutex> lock_model(model_mutex);

    if (eye == Sphere::Null) {
        return;
    }

    // Find pupil positions on eyeball to get radius
    //
    // For each image, calculate the 'most likely' position of the pupil
    // circle given the eyeball sphere estimate and gaze vector. Re-estimate
    // the gaze vector to be consistent with this position.

    // First estimate of pupil centre, used only to get an estimate of eye radius

    double eye_radius_acc = 0;
    int eye_radius_count = 0;

    for (const auto& pupil : pupils) {
        if (!pupil.circle) {
            continue;
        }
        if (!pupil.init_valid) {
            continue;
        }

        // Intersect the gaze from the eye centre with the pupil circle
        // centre projection line (with perfect estimates of gaze, eye
        // centre and pupil circle centre, these should intersect,
        // otherwise find the nearest point to both lines)

        Vector3 pupil_centre = nearest_intersect(Line3(eye.centre, pupil.circle.normal),
            Line3(camera_centre, pupil.circle.centre.normalized()));

        auto distance = (pupil_centre - eye.centre).norm();

        eye_radius_acc += distance;
        ++eye_radius_count;
    }

    // Set the eye radius as the mean distance from pupil centres to eye centre
    eye.radius = eye_radius_acc / eye_radius_count;

    // Second estimate of pupil radius, used to get position of pupil on eye

    for (auto& pupil : pupils) {
        initialise_single_observation(pupil);
    }

    // Scale eye to anthropomorphic average radius of 12mm
    auto scale = 12.0 / eye.radius;
    eye.radius = 12.0;
    eye.centre *= scale;
    for (auto& pupil : pupils) {
        pupil.params.radius *= scale;
        pupil.circle = circleFromParams(pupil.params);
    }

    model_version++;

    // Try previous circle in case of bad fits
    /*EllipseGoodnessFunction<double> goodnessFunction;
    for (int i = 1; i < pupils.size(); ++i) {
    auto& pupil = pupils[i];
    auto& prevPupil = pupils[i-1];

    if (prevPupil.circle) {
    double currentGoodness, prevGoodness;
    if (pupil.circle) {
    currentGoodness = goodnessFunction(eye, pupil.params.theta, pupil.params.psi, pupil.params.radius, focal_length, pupil.observation.image);
    prevGoodness = goodnessFunction(eye, prevPupil.params.theta, prevPupil.params.psi, prevPupil.params.radius, focal_length, pupil.observation.image);
    }

    if (!pupil.circle || prevGoodness > currentGoodness) {
    pupil.circle = prevPupil.circle;
    pupil.params = prevPupil.params;
    }
    }
    }*/
}

void singleeyefitter::EyeModelFitter::unproject_observations(double pupil_radius /*= 1*/, double eye_z /*= 20*/, bool use_ransac /*= true*/)
{
    using math::sq;

    std::lock_guard<std::mutex> lock_model(model_mutex);

    if (pupils.size() < 2) {
        throw std::runtime_error("Need at least two observations");
    }

    std::vector<std::pair<Circle, Circle>> pupil_unprojection_pairs;
    std::vector<Line> pupil_gazelines_proj;

    for (const auto& pupil : pupils) {
        // Get pupil circles (up to depth)
        //
        // Do a per-image unprojection of the pupil ellipse into the two fixed
        // size circles that would project onto it. The size of the circles
        // doesn't matter here, only their centre and normal does.
        auto unprojection_pair = unproject(pupil.observation.ellipse,
            pupil_radius, focal_length);

        // Get projected circles and gaze vectors
        //
        // Project the circle centres and gaze vectors down back onto the image
        // plane. We're only using them as line parametrisations, so it doesn't
        // matter which of the two centres/gaze vectors we use, as the
        // two gazes are parallel and the centres are co-linear.

        const auto& c = unprojection_pair.first.centre;
        const auto& v = unprojection_pair.first.normal;

        Vector2 c_proj = project(c, focal_length);
        Vector2 v_proj = project(v + c, focal_length) - c_proj;

        v_proj.normalize();

        pupil_unprojection_pairs.push_back(std::move(unprojection_pair));
        pupil_gazelines_proj.emplace_back(c_proj, v_proj);
    }

    // Get eyeball centre
    //
    // Find a least-squares 'intersection' (point nearest to all lines) of
    // the projected 2D gaze vectors. Then, unproject that circle onto a
    // point a fixed distance away.
    //
    // For robustness, use RANSAC to eliminate stray gaze lines
    //
    // (This has to be done here because it's used by the pupil circle
    // disambiguation)

    Vector2 eye_centre_proj;
    bool valid_eye;

    if (use_ransac) {
        auto indices = fun::range_<std::vector<size_t>>(pupil_gazelines_proj.size());

        const int n = 2;
        double w = 0.3;
        double p = 0.9999;
        int k = (int)ceil(log(1 - p) / log(1 - pow(w, n)));

        double epsilon = 10;
        auto huber_error = [&](const Vector2& point, const Line& line) {
            double dist = euclidean_distance(point, line);
            if (sq(dist) < sq(epsilon))
                return sq(dist) / 2;
            else
                return epsilon*(abs(dist) - epsilon / 2);
        };
        auto m_error = [&](const Vector2& point, const Line& line) {
            double dist = euclidean_distance(point, line);
            if (sq(dist) < sq(epsilon))
                return sq(dist);
            else
                return sq(epsilon);
        };
        auto error = m_error;

        auto best_inlier_indices = decltype(indices)();
        Vector2 best_eye_centre_proj;// = nearest_intersect(pupil_gazelines_proj);
        double best_line_distance_error = std::numeric_limits<double>::infinity();// = fun::sum(LAMBDA(const Line& line)(error(best_eye_centre_proj,line)), pupil_gazelines_proj);

        for (int i = 0; i < k; ++i) {
            auto index_sample = singleeyefitter::randomSubset(indices, n);
            auto sample = fun::map([&](size_t i){ return pupil_gazelines_proj[i]; }, index_sample);

            auto sample_centre_proj = nearest_intersect(sample);

            auto index_inliers = fun::filter(
                [&](size_t i){ return euclidean_distance(sample_centre_proj, pupil_gazelines_proj[i]) < epsilon; },
                indices);
            auto inliers = fun::map([&](size_t i){ return pupil_gazelines_proj[i]; }, index_inliers);

            if (inliers.size() <= w*pupil_gazelines_proj.size()) {
                continue;
            }

            auto inlier_centre_proj = nearest_intersect(inliers);

            double line_distance_error = fun::sum(
                [&](size_t i){ return error(inlier_centre_proj, pupil_gazelines_proj[i]); },
                indices);

            if (line_distance_error < best_line_distance_error) {
                best_eye_centre_proj = inlier_centre_proj;
                best_line_distance_error = line_distance_error;
                best_inlier_indices = std::move(index_inliers);
            }
        }

        std::cout << "Inliers: " << best_inlier_indices.size()
            << " (" << (100.0*best_inlier_indices.size() / pupil_gazelines_proj.size()) << "%)"
            << " = " << best_line_distance_error
            << std::endl;

        for (auto& pupil : pupils) {
            pupil.init_valid = false;
        }
        for (auto& i : best_inlier_indices) {
            pupils[i].init_valid = true;
        }

        if (best_inlier_indices.size() > 0) {
            eye_centre_proj = best_eye_centre_proj;
            valid_eye = true;
        }
        else {
            valid_eye = false;
        }
    }
    else {
        for (auto& pupil : pupils) {
            pupil.init_valid = true;
        }
        eye_centre_proj = nearest_intersect(pupil_gazelines_proj);
        valid_eye = true;
    }

    if (valid_eye) {
        eye.centre << eye_centre_proj * eye_z / focal_length,
            eye_z;
        eye.radius = 1;

        // Disambiguate pupil circles using projected eyeball centre
        //
        // Assume that the gaze vector points away from the eye centre, and
        // so projected gaze points away from projected eye centre. Pick the
        // solution which satisfies this assumption
        for (size_t i = 0; i < pupils.size(); ++i) {
            const auto& pupil_pair = pupil_unprojection_pairs[i];
            const auto& line = pupil_gazelines_proj[i];

            const auto& c_proj = line.origin();
            const auto& v_proj = line.direction();

            // Check if v_proj going away from est eye centre. If it is, then
            // the first circle was correct. Otherwise, take the second one.
            // The two normals will point in opposite directions, so only need
            // to check one.
            if ((c_proj - eye_centre_proj).dot(v_proj) >= 0) {
                pupils[i].circle = std::move(pupil_pair.first);
            }
            else {
                pupils[i].circle = std::move(pupil_pair.second);
            }
        }
    }
    else {
        // No inliers, so no eye
        eye = Sphere::Null;

        // Arbitrarily pick first circle
        for (size_t i = 0; i < pupils.size(); ++i) {
            const auto& pupil_pair = pupil_unprojection_pairs[i];
            pupils[i].circle = std::move(pupil_pair.first);
        }
    }

    model_version++;
}


#include <CMDParser.hpp>
#include <FileBuffer.hpp>
#include <calibvolume.hpp>
#include <rgbdsensor.hpp>
#include <DataTypes.hpp>
#include <NearestNeighbourSearch.hpp>

#include <squish.h>
#include <boost/thread/thread.hpp>
#include <boost/bind.hpp>

#include <iostream>
#include <fstream>
#include <sstream>

namespace{

  // bilateral filter
  ////////////////////////////////////////////////////////////////////
  int kernel_size = 6; // in pixel
  int kernel_end = kernel_size + 1;
  float dist_space_max_inv = 1.0/float(kernel_size);

  float computeGaussSpace(float dist_space){
    float gauss_coord = dist_space * dist_space_max_inv;
    return 1.0 - gauss_coord;
  }

  float dist_range_max = 0.05; // in meter
  float dist_range_max_inv = 1.0/dist_range_max;

  float computeGaussRange(float dist_range){
    float gauss_coord = std::min(dist_range, dist_range_max) * dist_range_max_inv;
    return 1.0 - gauss_coord;
  }

  bool is_outside(const float d, const unsigned s_num, const std::vector<CalibVolume*> cvs){
    return (d < cvs[s_num]->min_d) || (d > cvs[s_num]->max_d);
  }

  float look_up_depth(const int x, const int y, const unsigned s_num, const RGBDSensor& sensor){

    if( (x < 0) ||
	(x > (sensor.config.size_d.x - 1)) ||
	(y < 0) ||
	(x > (sensor.config.size_d.y - 1))
	){
      return 0.0;
    }
    const unsigned d_idx = y* sensor.config.size_d.x + x;
    return s_num == 0 ? sensor.frame_d[d_idx] : sensor.slave_frames_d[s_num - 1][d_idx];
  }

  float bilateral_filter(const int x, const int y, const RGBDSensor& sensor, const unsigned s_num, const std::vector<CalibVolume*> cvs){
    float filtered_depth = 0.0;

    float depth = look_up_depth(x, y, s_num, sensor);
    if(is_outside(depth, s_num, cvs)){
      return 0.0;
    }

    // the valid range scales with depth
    float max_depth = 4.5; // Kinect V2
    float d_dmax = depth/max_depth;
    dist_range_max = 0.35 * d_dmax; // threshold around
    dist_range_max_inv = 1.0/dist_range_max;

    float depth_bf = 0.0;

    float w = 0.0;
    float w_range = 0.0;
    float border_samples = 0.0;
    float num_samples = 0.0;

    for(int y_s = -kernel_size; y_s < kernel_end; ++y_s){
      for(int x_s = -kernel_size; x_s < kernel_end; ++x_s){
	num_samples += 1.0;

	const float depth_s = look_up_depth(x + x_s, y + y_s, s_num, sensor);

	const float depth_range = std::abs(depth_s - depth);
	if(is_outside(depth_s, s_num, cvs) || (depth_range > dist_range_max)){
	  border_samples += 1.0;
	  continue;
	}

	float gauss_space = computeGaussSpace(glm::length(glm::vec2(x_s,y_s)));
	float gauss_range = computeGaussRange(depth_range);
	float w_s = gauss_space * gauss_range;
	depth_bf += w_s * depth_s;
	w += w_s;
	w_range += gauss_range;
      }
    }

    const float lateral_quality  = 1.0 - border_samples/num_samples;

    if(w > 0.0)
      filtered_depth = depth_bf/w;
    else
      filtered_depth = 0.0;

    if(w_range < (num_samples * 0.65)){
      filtered_depth = 0.0;
    }

    return filtered_depth;

  }

  template <class T>
  inline std::string
  toStringP(T value, unsigned p)
  {
    std::ostringstream stream;
    stream << std::setw(p) << std::setfill('0') << value;
    return stream.str();
  }

  float calcAvgDist(const std::vector<nniSample>& neighbours, const nniSample& s){
    float avd = 0.0;
    for(const auto& n : neighbours){
      const float dist = glm::length(glm::vec3(s.s_pos.x - n.s_pos.x,s.s_pos.y - n.s_pos.y,s.s_pos.z - n.s_pos.z));
      avd += dist;
    }
    avd /= neighbours.size();
    return avd;
  }

  glm::vec3 bbx_min = glm::vec3(-1.2, -0.05, -1.2);
  glm::vec3 bbx_max = glm::vec3( 1.2, 2.4,  1.2);

  bool clip(const glm::vec3& p){
    if(p.x < bbx_min.x ||
       p.y < bbx_min.y ||
       p.z < bbx_min.z ||
       p.x > bbx_max.x ||
       p.y > bbx_max.y ||
       p.z > bbx_max.z){
      return true;
    }
    return false;
  }

  float filter_pass_1_max_avg_dist_in_meter = 0.025;
  float filter_pass_2_sd_fac = 1.0;
  unsigned filter_pass_1_k = 50;
  unsigned filter_pass_2_k = 50;

  void filterPerThread(NearestNeighbourSearch* nns, std::vector<nniSample>* nnisamples, std::vector<std::vector<nniSample> >* results, const unsigned tid, const unsigned num_threads){

    for(unsigned sid = tid; sid < nnisamples->size(); sid += num_threads){

      nniSample s = (*nnisamples)[sid];

      if(filter_pass_1_k > 2){
	std::vector<nniSample> neighbours = nns->search(s,filter_pass_1_k);
	if(neighbours.empty()){
	  continue;
	}

	const float avd = calcAvgDist(neighbours, s);
	if(avd > filter_pass_1_max_avg_dist_in_meter){
	  continue;
	}
      }

      if(filter_pass_2_k > 2){
	std::vector<nniSample> neighbours = nns->search(s,filter_pass_2_k);
	const float avd = calcAvgDist(neighbours, s);
	std::vector<float> dists;
	for(const auto& n : neighbours){
	  std::vector<nniSample> local_neighbours = nns->search(n,filter_pass_2_k);
	  if(!local_neighbours.empty()){
	    dists.push_back(calcAvgDist(local_neighbours, n));
	  }
	}
	double mean;
	double sd;
	calcMeanSD(dists, mean, sd);
	if((avd - mean) > filter_pass_2_sd_fac * sd){
	  continue;
	}
      }
      (*results)[tid].push_back(s);
    }

  }

}

int main(int argc, char* argv[]){

  unsigned num_threads = 16;
  bool rgb_is_compressed = false;
  std::string stream_filename;
  CMDParser p("basefilename_cv .... basefilename_for_output");
  p.addOpt("s",1,"stream_filename", "specify the stream filename which should be converted");
  p.addOpt("n",1,"num_threads", "specify how many threads should be used, default 16");
  p.addOpt("c",-1,"rgb_is_compressed", "enable compressed support for rgb stream, default: false (not compressed)");

  p.addOpt("p1d",1,"filter_pass_1_max_avg_dist_in_meter", "filter pass 1 skips points which have an average distance of more than this to it k neighbors, default 0.025");
  p.addOpt("p1k",1,"filter_pass_1_k", "filter pass 1 number of neighbors, default 50");
  p.addOpt("p2s",1,"filter_pass_2_sd_fac", "filter pass 2, specify how many times a point's distance should be above (values higher than 1.0) / below (values smaller than 1.0) is allowed to be compared to standard deviation of its k neighbors, default 1.0");
  p.addOpt("p2k",1,"filter_pass_2_k", "filter pass 2 number of neighbors (the higher the more to process), default 50");

  p.addOpt("bbx",6,"bounding_box", "specify the bounding box x_min y_min z_min x_max y_max z_max in meters, default -1.2 -0.05 -1.2 1.2 2.4 1.2");

  p.addOpt("b",1,"bil_filter_depth_kernel", "specify the kernel size of the bilateral depth filter, e.g. -b 6, default 0 (no bilateral filter ist applied)");

  p.addOpt("f",1,"frames", "specify how many frames should be processed at maximum, e.g. -f 1, default 0 (all frames in the stream will be processed)");

  p.init(argc,argv);

  if(p.isOptSet("bbx")){
    bbx_min = glm::vec3(p.getOptsFloat("bbx")[0], p.getOptsFloat("bbx")[1], p.getOptsFloat("bbx")[2]);
    bbx_max = glm::vec3(p.getOptsFloat("bbx")[3], p.getOptsFloat("bbx")[4], p.getOptsFloat("bbx")[5]);
    std::cout << "setting bounding box to min: " << bbx_min << " -> max: " << bbx_max << std::endl;
  }

  if(p.isOptSet("p1d")){
    filter_pass_1_max_avg_dist_in_meter = p.getOptsFloat("p1d")[0];
    std::cout << "setting filter_pass_1_max_avg_dist_in_meter to " << filter_pass_1_max_avg_dist_in_meter << std::endl;
  }
  if(p.isOptSet("p1k")){
    filter_pass_1_k = p.getOptsInt("p1k")[0];
    std::cout << "setting filter_pass_1_k to " << filter_pass_1_k << std::endl;
  }
  if(p.isOptSet("p2s")){
    filter_pass_2_sd_fac = p.getOptsFloat("p2s")[0];
    std::cout << "setting filter_pass_2_sd_fac to " << filter_pass_2_sd_fac << std::endl;
  }
  if(p.isOptSet("p2k")){
    filter_pass_2_k = p.getOptsInt("p2k")[0];
    std::cout << "setting filter_pass_2_k to " << filter_pass_2_k << std::endl;
  }

  if(p.isOptSet("s")){
    stream_filename = p.getOptsString("s")[0];
  }
  else{
    std::cerr << "ERROR, please specify stream filename with flag -s, see " << argv[0] << " -h for help" << std::endl;
    return 0;
  }

  if(p.isOptSet("n")){
    num_threads = p.getOptsInt("n")[0];
  }

  if(p.isOptSet("c")){
    rgb_is_compressed = true;
  }

  bool using_bf = false;
  if(p.isOptSet("b")){
    kernel_size = std::max(0, p.getOptsInt("b")[0]);
    kernel_end = kernel_size + 1;
    dist_space_max_inv = 1.0/float(kernel_size);
    using_bf = true;
    std::cout << "performing bilateral filtering with kernel size of: " << kernel_size << std::endl;
  }

  const unsigned num_streams(p.getArgs().size() - 1);
  const std::string basefilename_for_output = p.getArgs()[num_streams];

  std::vector<CalibVolume*> cvs;
  for(unsigned i = 0; i < num_streams; ++i){
    std::string basefilename = p.getArgs()[i];
    std::string filename_xyz(basefilename + "_xyz");
    std::string filename_uv(basefilename + "_uv");
    cvs.push_back(new CalibVolume(filename_xyz.c_str(), filename_uv.c_str()));
  }

  RGBDConfig cfg;
  cfg.size_rgb = glm::uvec2(1280, 1080);
  cfg.size_d   = glm::uvec2(512, 424);
  RGBDSensor sensor(cfg, num_streams - 1);

  unsigned char* tmp_rgb = 0;
  unsigned char* tmp_rgba = 0;
  const unsigned colorsize_tmp = cfg.size_rgb.x * cfg.size_rgb.y * 3;
  const unsigned colorsize_tmpa = cfg.size_rgb.x * cfg.size_rgb.y * 4;
  if(rgb_is_compressed){
    tmp_rgb = new unsigned char [colorsize_tmp];
    tmp_rgba = new unsigned char [colorsize_tmpa];
  }
  const unsigned colorsize = rgb_is_compressed ? 691200 : cfg.size_rgb.x * cfg.size_rgb.y * 3;
  const unsigned depthsize = cfg.size_d.x * cfg.size_d.y * sizeof(float);

  FileBuffer fb(stream_filename.c_str());
  if(!fb.open("r")){
    std::cerr << "ERROR, while opening " << stream_filename << " exiting..." << std::endl;
    return 1;
  }

  unsigned num_frames = fb.calcNumFrames(num_streams * (colorsize + depthsize));
  if(p.isOptSet("f")){
    num_frames = std::min(std::max(0u, (unsigned) p.getOptsInt("f")[0]), num_frames);
    std::cout << "processing " << num_frames << " frames of the stream" << std::endl;
  }
  double curr_frame_time = 0.0;

  unsigned frame_num = 0;
  while(frame_num < num_frames){
    ++frame_num;

    for(unsigned s_num = 0; s_num < num_streams; ++s_num){
      fb.read((unsigned char*) (s_num == 0 ? sensor.frame_rgb : sensor.slave_frames_rgb[s_num - 1]), colorsize);

      if(s_num == 0){
	memcpy((char*) &curr_frame_time, (const char*) sensor.frame_rgb, sizeof(double));
	std::cout << "curr_frame_time: " << curr_frame_time << std::endl;
      }

      if(rgb_is_compressed){
         // uncompress to rgb_tmp from (unsigned char*) (s_num == 0 ? sensor.frame_rgb : sensor.slave_frames_rgb[s_num - 1]) to tmp_rgba
         squish::DecompressImage (tmp_rgba, cfg.size_rgb.x, cfg.size_rgb.y,
                                  (unsigned char*) (s_num == 0 ? sensor.frame_rgb : sensor.slave_frames_rgb[s_num - 1]), squish::kDxt1);
         // copy back rgbsensor
         unsigned buffida = 0;
         unsigned buffid = 0;
         for(unsigned y = 0; y < cfg.size_rgb.y; ++y){
	   for(unsigned x = 0; x < cfg.size_rgb.x; ++x){
	     tmp_rgb[buffid++] = tmp_rgba[buffida++];
	     tmp_rgb[buffid++] = tmp_rgba[buffida++];
	     tmp_rgb[buffid++] = tmp_rgba[buffida++];
	     buffida++;
           }
         }
         memcpy((unsigned char*) (s_num == 0 ? sensor.frame_rgb : sensor.slave_frames_rgb[s_num - 1]), tmp_rgb, colorsize_tmp);
      }

      fb.read((unsigned char*) (s_num == 0 ? sensor.frame_d : sensor.slave_frames_d[s_num - 1]), depthsize);
    }

    std::vector<nniSample> nnisamples;
    for(unsigned s_num = 0; s_num < num_streams; ++s_num){
      // do 3D reconstruction for each depth pixel
      for(unsigned y = 0; y < sensor.config.size_d.y; ++y){
	for(unsigned x = 0; x < (sensor.config.size_d.x - 3); ++x){

	  float d = 0.0;
	  if(!using_bf){
	    const unsigned d_idx = y* sensor.config.size_d.x + x;
	    d = s_num == 0 ? sensor.frame_d[d_idx] : sensor.slave_frames_d[s_num - 1][d_idx];
	  }
	  else{
	    d = bilateral_filter(x, y, sensor, s_num, cvs);
	  }

	  if(d < cvs[s_num]->min_d || d > cvs[s_num]->max_d){
	    continue;
	  }

	  glm::vec3 pos3D;
	  glm::vec2 pos2D_rgb;

	  pos3D = cvs[s_num]->lookupPos3D( x * 1.0/sensor.config.size_d.x,
					   y * 1.0/sensor.config.size_d.y, d);

	  if(clip(pos3D)){
	    continue;
	  }

	  nniSample nnis;
	  nnis.s_pos.x = pos3D.x;
	  nnis.s_pos.y = pos3D.y;
	  nnis.s_pos.z = pos3D.z;

	  glm::vec2 pos2D_rgb_norm = cvs[s_num]->lookupPos2D_normalized( x * 1.0/sensor.config.size_d.x,
									 y * 1.0/sensor.config.size_d.y, d);
	  pos2D_rgb = glm::vec2(pos2D_rgb_norm.x * sensor.config.size_rgb.x,
				pos2D_rgb_norm.y * sensor.config.size_rgb.y);

	  glm::vec3 rgb = sensor.get_rgb_bilinear_normalized(pos2D_rgb, s_num);

	  nnis.s_pos_off.x = rgb.x;
	  nnis.s_pos_off.y = rgb.y;
	  nnis.s_pos_off.z = rgb.z;

          nnisamples.push_back(nnis);

	}
      }
    }

    std::cout << "start building acceleration structure for filtering " << nnisamples.size() << " points..." << std::endl;
    NearestNeighbourSearch nns(nnisamples);
    std::vector<std::vector<nniSample> > results;
    for(unsigned tid = 0; tid < num_threads; ++tid){
      results.push_back(std::vector<nniSample>() );
    }

    std::cout << "start filtering frame " << frame_num << " using " << num_threads << " threads." << std::endl;

    boost::thread_group threadGroup;
    for (unsigned tid = 0; tid < num_threads; ++tid){
      threadGroup.create_thread(boost::bind(&filterPerThread, &nns, &nnisamples, &results, tid, num_threads));
    }
    threadGroup.join_all();
#if 0
    unsigned tid = 0;
    for(unsigned sid = 0; sid < nnisamples.size(); ++sid){

      nniSample s = nnisamples[sid];

      std::vector<nniSample> neighbours = nns.search(s,filter_pass_1_k);
      if(neighbours.empty()){
	continue;
      }

      const float avd = calcAvgDist(neighbours, s);
      if(avd > filter_pass_1_max_avg_dist_in_meter){
	continue;
      }

      std::vector<float> dists;
      for(const auto& n : neighbours){
	std::vector<nniSample> local_neighbours = nns.search(n,filter_pass_2_k);
	if(!local_neighbours.empty()){
	  dists.push_back(calcAvgDist(local_neighbours, n));
	}
      }
      double mean;
      double sd;
      calcMeanSD(dists, mean, sd);
      if((avd - mean) > filter_pass_2_sd_fac * sd){
	continue;
      }
      results[tid].push_back(s);
    }
#endif

    const std::string pcfile_name(basefilename_for_output + "_" + toStringP(frame_num, 5 /*fill*/) + ".xyz");
    std::ofstream pcfile(pcfile_name.c_str());
    std::cout << "start writing to file " << pcfile_name << " ..." << std::endl;
    for(unsigned tid = 0; tid < num_threads; ++tid){
      for(const auto& s : results[tid]){

	int red   = (int) std::max(0.0f , std::min(255.0f, s.s_pos_off.x * 255.0f));
	int green = (int) std::max(0.0f , std::min(255.0f, s.s_pos_off.y * 255.0f));
	int blue  = (int) std::max(0.0f , std::min(255.0f, s.s_pos_off.z * 255.0f));
	pcfile << s.s_pos.x << " " << s.s_pos.y << " " << s.s_pos.z << " "
	       << red << " "
	       << green << " "
	       << blue << std::endl;

      }

    }

    pcfile.close();

    const std::string tsfile_name(basefilename_for_output + "_" + toStringP(frame_num, 5 /*fill*/) + ".timestamp");
    std::ofstream tsfile(tsfile_name.c_str());
    tsfile << curr_frame_time << std::endl;
    tsfile.close();

    std::cout << frame_num
	      << " from "
	      << num_frames
	      << " processed and saved to: "
	      << pcfile_name << std::endl << std::endl;

  }

  return 0;
}


//  Created by xufeiwang on 21/12/19.
#include <cstdlib>
#include <iostream>
#include <RcppArmadillo.h>
#define _USE_MATH_DEFINES
#include <cmath>
#include <ctime>
#include <vector>
#include <fstream>
#include <sstream>
#include <algorithm>
#include <utility>
#include <armadillo>
using namespace std;
using namespace Rcpp;
using namespace arma;

double log_res(float m, mat xx, colvec xy, double yy)
{
  if (det(xx) < 1e-10)
    return (log(yy));
  double res = yy - as_scalar(trans(xy) * solve(xx, xy));
  return (log(res) - log(m));
}

double compute_loss(mat xx, colvec xy, double yy, colvec beta)
{
  return (yy - 2 * as_scalar(trans(xy) * beta) + as_scalar(trans(beta) * (xx * beta)));
}

double index(int n, int i, int j)
{
  int ind = (2*n-i+1)*i/2+j-i-1;
  return (ind);
}

void M_cal(NumericVector* F, vector<int>* S, int p, double gamma)
{
  NumericVector M(p+1,0.0);
  for (int i=1; i<p+1; i++)
  {
    M[i] = gamma +(*F)[i-1];
  }

  for (int i=2;i<p+1; i++)
  {
    for (int k=1; k<i; k++)
    {
      double temp = gamma + M[k] +(*F)[index(p,k,i)];
      if (temp > M[i])
      {
        M[i] = temp;
        (*S)[i] = k;
      }
    }
  }

  return;
}

// [[Rcpp::export]]
NumericVector knots_selection_cpp(NumericMatrix X, NumericVector y, int m, double lam0, NumericVector Knots, NumericVector u)
{
  int n = X.nrow();
  int l = X.ncol();

  NumericMatrix SXX(n+1, l*l);
  NumericMatrix SXy(n+1, l);
  NumericVector Syy(n+1);

  for (int j=0; j<l*l; j++)
    SXX(0, j) = 0;
  for (int j=0; j<l; j++)
    SXy(0, j) = 0;
  Syy[0] = 0;
  for (int i=1; i<n+1; i++)
  {
    SXy(i, 0) = SXy(i-1, 0) + y[i-1];
    Syy[i] = Syy[i-1] + y[i-1] * y[i-1];
    for (int j=0; j<l; j++) {
      SXy(i, j) = SXy(i-1, j) + X(i-1, j) * y[i-1];
      for (int k=0; k<l; k++)
        SXX(i, j*l+k) = SXX(i-1, j*l+k) + X(i-1, j) * X(i-1, k);
    }
  }

  int p;
  if (m == 0)
    p = Knots.size() + 1;
  else
    p = n / m;
  NumericVector num(p+1, 0.0);
  if (m == 0) {
    int ind = 0;
    for (int i=0; i<n; i++) {
      if (u[i] > Knots[ind]) {
        num[ind] = i;
        ind = ind + 1;
      }
      num[p] = n;
    }
  } else {
    int r = n - p * m;
    int ind = 0 ;
    for (int i=1;i<p+1;i++) {
      if (i <= r)
        ind += m+1;
      else
        ind += m;
      num[i] = ind;
    }
  }

  int len = p*(p+1)/2;
  NumericVector *ls = new NumericVector(len, 0.0);
  vector<int> *S = new vector<int>(p+1, 0);

  NumericMatrix SXX_simple(p+1, l*l);
  NumericMatrix SXy_simple(p+1, l);
  NumericVector Syy_simple(p+1);

  for (int i=1;i<p+1;i++) {
    int ind = int(num[i]);
    for (int k=0; k<l*l; k++)
      SXX_simple(i,k) = SXX(ind,k);
    for (int k=0; k<l; k++)
      SXy_simple(i,k) = SXy(ind,k);
    Syy_simple[i] = Syy[ind];
  }

  for (int i=0; i<p; i++) {
    for (int j=i+1; j<p+1; j++){
      mat xx(l, l);
      colvec xy(l);
      for (int k=0; k<l; k++)
        xy[k] = SXy_simple(j, k) - SXy_simple(i, k);
      for (int k1=0; k1<l; k1++)
        for (int k2=0;k2<l; k2++)
          xx(k1, k2) = SXX_simple(j, k1*l+k2) - SXX_simple(i, k1*l+k2);
      (*ls)[index(p,i,j)] = -(num[j]-num[i])/2.0*log_res(num[j]-num[i], xx, xy, Syy_simple[j] - Syy_simple[i]);
    }
  }
  double lam = -lam0*log(n)/2;
  M_cal(ls, S, p, lam);
  int slice = 1;
  int temp = (*S)[p];
  NumericVector knots(1);
  knots[0] = n;
  while (temp != 0)
  {
    knots.push_back(num[temp]);
    temp = (*S)[temp];
    slice += 1;
  }
  delete ls;
  delete S;
  return (knots);
}


#ifndef STAN_MATH_PRIM_SCAL_PROB_EXPONENTIAL_LPDF_HPP
#define STAN_MATH_PRIM_SCAL_PROB_EXPONENTIAL_LPDF_HPP

#include <stan/math/prim/scal/meta/is_constant_struct.hpp>
#include <stan/math/prim/scal/meta/operands_and_partials.hpp>
#include <stan/math/prim/scal/err/check_consistent_sizes.hpp>
#include <stan/math/prim/scal/err/check_nonnegative.hpp>
#include <stan/math/prim/scal/err/check_not_nan.hpp>
#include <stan/math/prim/scal/err/check_positive_finite.hpp>
#include <stan/math/prim/scal/fun/value_of.hpp>
#include <stan/math/prim/scal/meta/length.hpp>
#include <stan/math/prim/scal/meta/scalar_seq_view.hpp>
#include <stan/math/prim/scal/meta/VectorBuilder.hpp>
#include <stan/math/prim/scal/meta/partials_return_type.hpp>
#include <stan/math/prim/scal/meta/return_type.hpp>
#include <stan/math/prim/scal/fun/constants.hpp>
#include <stan/math/prim/scal/meta/include_summand.hpp>
#include <boost/random/exponential_distribution.hpp>
#include <boost/random/variate_generator.hpp>
#include <cmath>

namespace stan {
  namespace math {

    /**
     * The log of an exponential density for y with the specified
     * inverse scale parameter.
     * Inverse scale parameter must be greater than 0.
     * y must be greater than or equal to 0.
     *
     \f{eqnarray*}{
     y
     &\sim&
     \mbox{\sf{Expon}}(\beta) \\
     \log (p (y \, |\, \beta) )
     &=&
     \log \left( \beta \exp^{-\beta y} \right) \\
     &=&
     \log (\beta) - \beta y \\
     & &
     \mathrm{where} \; y > 0
     \f}
     *
     * @param y A scalar variable.
     * @param beta Inverse scale parameter.
     * @throw std::domain_error if beta is not greater than 0.
     * @throw std::domain_error if y is not greater than or equal to 0.
     * @tparam T_y Type of scalar.
     * @tparam T_inv_scale Type of inverse scale.
     */
    template <bool propto, typename T_y, typename T_inv_scale>
    typename return_type<T_y, T_inv_scale>::type
    exponential_lpdf(const T_y& y, const T_inv_scale& beta) {
      static const char* function("exponential_lpdf");
      typedef typename stan::partials_return_type<T_y, T_inv_scale>::type
        T_partials_return;

      if (!(stan::length(y) && stan::length(beta)))
        return 0.0;

      using std::log;

      T_partials_return logp(0.0);
      check_nonnegative(function, "Random variable", y);
      check_positive_finite(function, "Inverse scale parameter", beta);
      check_consistent_sizes(function,
                             "Random variable", y,
                             "Inverse scale parameter", beta);

      scalar_seq_view<T_y> y_vec(y);
      scalar_seq_view<T_inv_scale> beta_vec(beta);
      size_t N = max_size(y, beta);

      VectorBuilder<include_summand<propto, T_inv_scale>::value,
                    T_partials_return, T_inv_scale> log_beta(length(beta));
      for (size_t i = 0; i < length(beta); i++)
        if (include_summand<propto, T_inv_scale>::value)
          log_beta[i] = log(value_of(beta_vec[i]));

      operands_and_partials<T_y, T_inv_scale>
        ops_partials(y, beta);

      for (size_t n = 0; n < N; n++) {
        const T_partials_return beta_dbl = value_of(beta_vec[n]);
        const T_partials_return y_dbl = value_of(y_vec[n]);
        if (include_summand<propto, T_inv_scale>::value)
          logp += log_beta[n];
        if (include_summand<propto, T_y, T_inv_scale>::value)
          logp -= beta_dbl * y_dbl;

        if (!is_constant_struct<T_y>::value)
          ops_partials.edge1_.partials_[n] -= beta_dbl;
        if (!is_constant_struct<T_inv_scale>::value)
          ops_partials.edge2_.partials_[n] += 1 / beta_dbl - y_dbl;
      }
      return ops_partials.build(logp);
    }

    template <typename T_y, typename T_inv_scale>
    inline
    typename return_type<T_y, T_inv_scale>::type
    exponential_lpdf(const T_y& y, const T_inv_scale& beta) {
      return exponential_lpdf<false>(y, beta);
    }

  }
}
#endif


// Copyright Michael Drexl 2005, 2006.
// Distributed under the Boost Software License, Version 1.0.
// (See accompanying file LICENSE_1_0.txt or copy at
// http://boost.org/LICENSE_1_0.txt)

// Example use of the resource-constrained shortest paths algorithm.
#include <boost/config.hpp>

#ifdef BOOST_MSVC
#pragma warning(disable : 4267)
#endif

#include <boost/graph/adjacency_list.hpp>

#include <boost/graph/r_c_shortest_paths.hpp>
#include <iostream>

using namespace boost;

struct SPPRC_Example_Graph_Vert_Prop
{
    SPPRC_Example_Graph_Vert_Prop(int n = 0, int e = 0, int l = 0)
    : num(n), eat(e), lat(l)
    {
    }
    int num;
    // earliest arrival time
    int eat;
    // latest arrival time
    int lat;
};

struct SPPRC_Example_Graph_Arc_Prop
{
    SPPRC_Example_Graph_Arc_Prop(int n = 0, int c = 0, int t = 0)
    : num(n), cost(c), time(t)
    {
    }
    int num;
    // traversal cost
    int cost;
    // traversal time
    int time;
};

typedef adjacency_list< vecS, vecS, directedS, SPPRC_Example_Graph_Vert_Prop,
    SPPRC_Example_Graph_Arc_Prop >
    SPPRC_Example_Graph;

// data structures for spp without resource constraints:
// ResourceContainer model
struct spp_no_rc_res_cont
{
    spp_no_rc_res_cont(int c = 0) : cost(c) {};
    spp_no_rc_res_cont& operator=(const spp_no_rc_res_cont& other)
    {
        if (this == &other)
            return *this;
        this->~spp_no_rc_res_cont();
        new (this) spp_no_rc_res_cont(other);
        return *this;
    }
    int cost;
};

bool operator==(
    const spp_no_rc_res_cont& res_cont_1, const spp_no_rc_res_cont& res_cont_2)
{
    return (res_cont_1.cost == res_cont_2.cost);
}

bool operator<(
    const spp_no_rc_res_cont& res_cont_1, const spp_no_rc_res_cont& res_cont_2)
{
    return (res_cont_1.cost < res_cont_2.cost);
}

// ResourceExtensionFunction model
class ref_no_res_cont
{
public:
    inline bool operator()(const SPPRC_Example_Graph& g,
        spp_no_rc_res_cont& new_cont, const spp_no_rc_res_cont& old_cont,
        graph_traits< SPPRC_Example_Graph >::edge_descriptor ed) const
    {
        new_cont.cost = old_cont.cost + g[ed].cost;
        return true;
    }
};

// DominanceFunction model
class dominance_no_res_cont
{
public:
    inline bool operator()(const spp_no_rc_res_cont& res_cont_1,
        const spp_no_rc_res_cont& res_cont_2) const
    {
        // must be "<=" here!!!
        // must NOT be "<"!!!
        return res_cont_1.cost <= res_cont_2.cost;
        // this is not a contradiction to the documentation
        // the documentation says:
        // "A label $l_1$ dominates a label $l_2$ if and only if both are
        // resident at the same vertex, and if, for each resource, the resource
        // consumption of $l_1$ is less than or equal to the resource
        // consumption of $l_2$, and if there is at least one resource where
        // $l_1$ has a lower resource consumption than $l_2$." one can think of
        // a new label with a resource consumption equal to that of an old label
        // as being dominated by that old label, because the new one will have a
        // higher number and is created at a later point in time, so one can
        // implicitly use the number or the creation time as a resource for
        // tie-breaking
    }
};
// end data structures for spp without resource constraints:

// data structures for shortest path problem with time windows (spptw)
// ResourceContainer model
struct spp_spptw_res_cont
{
    spp_spptw_res_cont(int c = 0, int t = 0) : cost(c), time(t) {}
    spp_spptw_res_cont& operator=(const spp_spptw_res_cont& other)
    {
        if (this == &other)
            return *this;
        this->~spp_spptw_res_cont();
        new (this) spp_spptw_res_cont(other);
        return *this;
    }
    int cost;
    int time;
};

bool operator==(
    const spp_spptw_res_cont& res_cont_1, const spp_spptw_res_cont& res_cont_2)
{
    return (res_cont_1.cost == res_cont_2.cost
        && res_cont_1.time == res_cont_2.time);
}

bool operator<(
    const spp_spptw_res_cont& res_cont_1, const spp_spptw_res_cont& res_cont_2)
{
    if (res_cont_1.cost > res_cont_2.cost)
        return false;
    if (res_cont_1.cost == res_cont_2.cost)
        return res_cont_1.time < res_cont_2.time;
    return true;
}

// ResourceExtensionFunction model
class ref_spptw
{
public:
    inline bool operator()(const SPPRC_Example_Graph& g,
        spp_spptw_res_cont& new_cont, const spp_spptw_res_cont& old_cont,
        graph_traits< SPPRC_Example_Graph >::edge_descriptor ed) const
    {
        const SPPRC_Example_Graph_Arc_Prop& arc_prop = get(edge_bundle, g)[ed];
        const SPPRC_Example_Graph_Vert_Prop& vert_prop
            = get(vertex_bundle, g)[target(ed, g)];
        new_cont.cost = old_cont.cost + arc_prop.cost;
        int& i_time = new_cont.time;
        i_time = old_cont.time + arc_prop.time;
        i_time < vert_prop.eat ? i_time = vert_prop.eat : 0;
        return i_time <= vert_prop.lat ? true : false;
    }
};

// DominanceFunction model
class dominance_spptw
{
public:
    inline bool operator()(const spp_spptw_res_cont& res_cont_1,
        const spp_spptw_res_cont& res_cont_2) const
    {
        // must be "<=" here!!!
        // must NOT be "<"!!!
        return res_cont_1.cost <= res_cont_2.cost
            && res_cont_1.time <= res_cont_2.time;
        // this is not a contradiction to the documentation
        // the documentation says:
        // "A label $l_1$ dominates a label $l_2$ if and only if both are
        // resident at the same vertex, and if, for each resource, the resource
        // consumption of $l_1$ is less than or equal to the resource
        // consumption of $l_2$, and if there is at least one resource where
        // $l_1$ has a lower resource consumption than $l_2$." one can think of
        // a new label with a resource consumption equal to that of an old label
        // as being dominated by that old label, because the new one will have a
        // higher number and is created at a later point in time, so one can
        // implicitly use the number or the creation time as a resource for
        // tie-breaking
    }
};
// end data structures for shortest path problem with time windows (spptw)

// example graph structure and cost from
// http://www.boost.org/libs/graph/example/dijkstra-example.cpp
enum nodes
{
    A,
    B,
    C,
    D,
    E
};
char name[] = "ABCDE";

int main()
{
    SPPRC_Example_Graph g;

    add_vertex(SPPRC_Example_Graph_Vert_Prop(A, 0, 0), g);
    add_vertex(SPPRC_Example_Graph_Vert_Prop(B, 5, 20), g);
    add_vertex(SPPRC_Example_Graph_Vert_Prop(C, 6, 10), g);
    add_vertex(SPPRC_Example_Graph_Vert_Prop(D, 3, 12), g);
    add_vertex(SPPRC_Example_Graph_Vert_Prop(E, 0, 100), g);

    add_edge(A, C, SPPRC_Example_Graph_Arc_Prop(0, 1, 5), g);
    add_edge(B, B, SPPRC_Example_Graph_Arc_Prop(1, 2, 5), g);
    add_edge(B, D, SPPRC_Example_Graph_Arc_Prop(2, 1, 2), g);
    add_edge(B, E, SPPRC_Example_Graph_Arc_Prop(3, 2, 7), g);
    add_edge(C, B, SPPRC_Example_Graph_Arc_Prop(4, 7, 3), g);
    add_edge(C, D, SPPRC_Example_Graph_Arc_Prop(5, 3, 8), g);
    add_edge(D, E, SPPRC_Example_Graph_Arc_Prop(6, 1, 3), g);
    add_edge(E, A, SPPRC_Example_Graph_Arc_Prop(7, 1, 5), g);
    add_edge(E, B, SPPRC_Example_Graph_Arc_Prop(8, 1, 4), g);

    // the unique shortest path from A to E in the dijkstra-example.cpp is
    // A -> C -> D -> E
    // its length is 5
    // the following code also yields this result

    // with the above time windows, this path is infeasible
    // now, there are two shortest paths that are also feasible with respect to
    // the vertex time windows:
    // A -> C -> B -> D -> E and
    // A -> C -> B -> E
    // however, the latter has a longer total travel time and is therefore not
    // pareto-optimal, i.e., it is dominated by the former path
    // therefore, the code below returns only the former path

    // spp without resource constraints
    graph_traits< SPPRC_Example_Graph >::vertex_descriptor s = A;
    graph_traits< SPPRC_Example_Graph >::vertex_descriptor t = E;

    std::vector<
        std::vector< graph_traits< SPPRC_Example_Graph >::edge_descriptor > >
        opt_solutions;
    std::vector< spp_no_rc_res_cont > pareto_opt_rcs_no_rc;

    r_c_shortest_paths(g, get(&SPPRC_Example_Graph_Vert_Prop::num, g),
        get(&SPPRC_Example_Graph_Arc_Prop::num, g), s, t, opt_solutions,
        pareto_opt_rcs_no_rc, spp_no_rc_res_cont(0), ref_no_res_cont(),
        dominance_no_res_cont(),
        std::allocator< r_c_shortest_paths_label< SPPRC_Example_Graph,
            spp_no_rc_res_cont > >(),
        default_r_c_shortest_paths_visitor());

    std::cout << "SPP without resource constraints:" << std::endl;
    std::cout << "Number of optimal solutions: ";
    std::cout << static_cast< int >(opt_solutions.size()) << std::endl;
    for (int i = 0; i < static_cast< int >(opt_solutions.size()); ++i)
    {
        std::cout << "The " << i << "th shortest path from A to E is: ";
        std::cout << std::endl;
        for (int j = static_cast< int >(opt_solutions[i].size()) - 1; j >= 0;
             --j)
            std::cout << name[source(opt_solutions[i][j], g)] << std::endl;
        std::cout << "E" << std::endl;
        std::cout << "Length: " << pareto_opt_rcs_no_rc[i].cost << std::endl;
    }
    std::cout << std::endl;

    // spptw
    std::vector<
        std::vector< graph_traits< SPPRC_Example_Graph >::edge_descriptor > >
        opt_solutions_spptw;
    std::vector< spp_spptw_res_cont > pareto_opt_rcs_spptw;

    r_c_shortest_paths(g, get(&SPPRC_Example_Graph_Vert_Prop::num, g),
        get(&SPPRC_Example_Graph_Arc_Prop::num, g), s, t, opt_solutions_spptw,
        pareto_opt_rcs_spptw, spp_spptw_res_cont(0, 0), ref_spptw(),
        dominance_spptw(),
        std::allocator< r_c_shortest_paths_label< SPPRC_Example_Graph,
            spp_spptw_res_cont > >(),
        default_r_c_shortest_paths_visitor());

    std::cout << "SPP with time windows:" << std::endl;
    std::cout << "Number of optimal solutions: ";
    std::cout << static_cast< int >(opt_solutions.size()) << std::endl;
    for (int i = 0; i < static_cast< int >(opt_solutions.size()); ++i)
    {
        std::cout << "The " << i << "th shortest path from A to E is: ";
        std::cout << std::endl;
        for (int j = static_cast< int >(opt_solutions_spptw[i].size()) - 1;
             j >= 0; --j)
            std::cout << name[source(opt_solutions_spptw[i][j], g)]
                      << std::endl;
        std::cout << "E" << std::endl;
        std::cout << "Length: " << pareto_opt_rcs_spptw[i].cost << std::endl;
        std::cout << "Time: " << pareto_opt_rcs_spptw[i].time << std::endl;
    }

    // utility function check_r_c_path example
    std::cout << std::endl;
    bool b_is_a_path_at_all = false;
    bool b_feasible = false;
    bool b_correctly_extended = false;
    spp_spptw_res_cont actual_final_resource_levels(0, 0);
    graph_traits< SPPRC_Example_Graph >::edge_descriptor ed_last_extended_arc;
    check_r_c_path(g, opt_solutions_spptw[0], spp_spptw_res_cont(0, 0), true,
        pareto_opt_rcs_spptw[0], actual_final_resource_levels, ref_spptw(),
        b_is_a_path_at_all, b_feasible, b_correctly_extended,
        ed_last_extended_arc);
    if (!b_is_a_path_at_all)
        std::cout << "Not a path." << std::endl;
    if (!b_feasible)
        std::cout << "Not a feasible path." << std::endl;
    if (!b_correctly_extended)
        std::cout << "Not correctly extended." << std::endl;
    if (b_is_a_path_at_all && b_feasible && b_correctly_extended)
    {
        std::cout << "Actual final resource levels:" << std::endl;
        std::cout << "Length: " << actual_final_resource_levels.cost
                  << std::endl;
        std::cout << "Time: " << actual_final_resource_levels.time << std::endl;
        std::cout << "OK." << std::endl;
    }

    return 0;
}