#ifndef INCLUDE_MIMKL_MODELS_UMKL_KNN_HPP_
#define INCLUDE_MIMKL_MODELS_UMKL_KNN_HPP_

#include <dlib/optimization.h>
#include <mimkl/definitions.hpp>
#include <mimkl/linear_algebra.hpp>
#include <mimkl/models/model.hpp>
#include <queue>
#include <spdlog/spdlog.h>
#include <thread>
#include <type_traits>
#include <utility>

using dlib::mat;
using mimkl::definitions::Index;
using mimkl::kernels_handler::KernelsHandler;
using mimkl::utilities::sort_indices_decending;

namespace mimkl
{
namespace models
{

static std::shared_ptr<spdlog::logger> logger_umkl_knn =
spdlog::stdout_color_mt("UMKLKNN");

template <typename Scalar, typename Kernel>
void find_and_update_neighbours_using_kernels(
KernelsHandler<Scalar, Kernel> &kernels_handler,
const Index number_of_kernels,
const Index number_of_support_vectors,
const Index k,
MATRIX(Scalar) & W)
{
// for each kernel, get k-NN and add it to upper triangular part of W
// TODO: consider allocation of W_local outside of the loop
// currently not in place given SegFault in development
#pragma omp parallel for shared(kernels_handler, W)
    for (Index m = 0; m < number_of_kernels; ++m)
    {
        // set W_local copy
        MATRIX(Scalar)
        W_local = MATRIX(Scalar)::Zero(number_of_support_vectors,
                                       number_of_support_vectors);
        // TODO: think about how to avoid this copy
        MATRIX(Scalar) kernel = kernels_handler[m];
        for (Index c = 0; c < number_of_support_vectors; ++c)
        {
            // no self loops admitted in W we have to treat the handle the case
            // i==c see original implementation
            // https://github.com/cran/mixKernel/blob/master/R/combine.kernels.R
            // starting by filling a neighbors heap
            std::priority_queue<std::pair<Scalar, Index>,
                                std::vector<std::pair<Scalar, Index>>,
                                std::greater<std::pair<Scalar, Index>>>
            nn_heap;
            Index i = 0;
            // initialize initial k elements
            while (nn_heap.size() < k)
            {
                if (i != c)
                {
                    nn_heap.push(std::pair<Scalar, Index>(kernel(i, c), i));
                }
                // keep track of the index
                ++i;
            }
            // compute k nearest neighbors for c
            for (; i < number_of_support_vectors; ++i)
            {
                if (i != c && nn_heap.top().first < kernel(i, c))
                {
                    nn_heap.pop();
                    nn_heap.push(std::pair<Scalar, Index>(kernel(i, c), i));
                }
            }
            // consuming the neighbors heap and populating the knn graph
            while (!nn_heap.empty())
            {
                Index neighbor = nn_heap.top().second;
                neighbor > c ? W_local(c, neighbor) = 1 :
                               W_local(neighbor, c) = 1;
                nn_heap.pop();
            }
        }
// update W with W_local
#pragma omp critical
        {
            W.template triangularView<Eigen::Upper>() += W_local;
        }
    }
}

//! Unsupervised Multiple Kernel Learning (2017), notion of original topology
//! from k-NN of kernels
/*! (sparse version from paper)
 *
 */
template <typename Scalar, typename Kernel>
class UMKLKNN : public Model<Scalar, Kernel>
{

    private:
    // inheritance of templatized base members
    using Model<Scalar, Kernel>::_kernels_handler;
    using Model<Scalar, Kernel>::_trained;
    using Model<Scalar, Kernel>::_precompute;
    using Model<Scalar, Kernel>::_trace_normalization;
    using Model<Scalar, Kernel>::_number_of_support_vectors;
    using Model<Scalar, Kernel>::_number_of_kernels;

    std::shared_ptr<spdlog::logger> _logger = spdlog::get("UMKLKNN");

    typedef MATRIX(Scalar) Matrix;
    typedef COLUMN(Scalar) Column;
    typedef Eigen::Map<const Column> MapColumn;
    typedef dlib::matrix<Scalar, 0, 1, dlib::default_memory_manager, dlib::column_major_layout>
    DlibColumn;

    Index _k = 5; // for k nearest neighbors (k-NN)
    double _epsilon = 0.0001;
    Index _maxiter_qp = 100000;

    Matrix _W;
    Matrix _S;

    DlibColumn _beta_dlib;
    Column _beta;

    void compute_w_matrix();
    void compute_s_matrix();
    void optimize_beta();
    void fit();

    public:
    UMKLKNN(const std::vector<Kernel> & = std::vector<Kernel>(),
            const bool precompute = true,
            const bool trace_normalization = true,
            const Index k = 5,
            const double epsilon = 0.0001,
            const Index maxiter_qp = 100000);
    UMKLKNN(const std::vector<Matrix> & = std::vector<Matrix>(),
            const bool precompute = true,
            const bool trace_normalization = true,
            const Index k = 5,
            const double epsilon = 0.0001,
            const Index maxiter_qp = 100000);

    void fit(const Matrix &);
    void fit(const std::vector<Matrix> &);
    Matrix predict(const Matrix &);
    Matrix predict(const std::vector<Matrix> &);

    Index get_k() const;
    double get_epsilon() const;
    Index get_maxiter_qp() const;
    Column get_beta() const;
    Matrix get_optimal_kernel();

    void set_k(const Index);
    void set_epsilon(const double);
    void set_maxiter_qp(const Index);
    void set_parameters(const Index,
                        const double epsilon = 0.0001,
                        const Index maxiter_qp = 100000);
    void set_beta(const Column &);
};

template <typename Scalar, typename Kernel>
UMKLKNN<Scalar, Kernel>::UMKLKNN(const std::vector<Kernel> &kernel_functions,
                                 const bool precompute,
                                 const bool trace_normalization,
                                 const Index k,
                                 const double epsilon,
                                 const Index maxiter_qp)
: Model<Scalar, Kernel>(kernel_functions, precompute, trace_normalization)
{
    _beta = Column::Constant(_number_of_kernels, (double)1. / _number_of_kernels);
    _beta_dlib = mat(_beta);
    set_parameters(k, epsilon, maxiter_qp);
}

template <typename Scalar, typename Kernel>
UMKLKNN<Scalar, Kernel>::UMKLKNN(const std::vector<Matrix> &kernel_matrices,
                                 const bool precompute,
                                 const bool trace_normalization,
                                 const Index k,
                                 const double epsilon,
                                 const Index maxiter_qp)
: Model<Scalar, Kernel>(kernel_matrices, precompute, trace_normalization)
{
    _beta = Column::Constant(_number_of_kernels, (double)1. / _number_of_kernels);
    _beta_dlib = mat(_beta);
    set_parameters(k, epsilon, maxiter_qp);
}

template <typename Scalar, typename Kernel>
void UMKLKNN<Scalar, Kernel>::fit()
{
    _number_of_support_vectors = _kernels_handler.get_lhs_size();
    _number_of_kernels = _kernels_handler.get_number_of_kernels();
    compute_w_matrix();
    compute_s_matrix();
    optimize_beta();
    _trained = true;
    _logger->debug("all done");
}

template <typename Scalar, typename Kernel>
void UMKLKNN<Scalar, Kernel>::fit(const std::vector<Matrix> &kernel_matrices)
{
    _kernels_handler.set_matrices(kernel_matrices, true);
    fit();
}

template <typename Scalar, typename Kernel>
void UMKLKNN<Scalar, Kernel>::fit(const Matrix &X)
{
    _kernels_handler.set_lhs(X);
    fit();
}

template <typename Scalar, typename Kernel>
typename UMKLKNN<Scalar, Kernel>::Matrix
UMKLKNN<Scalar, Kernel>::predict(const Matrix &X)
{
    if (!_trained)
        throw std::logic_error("The model should be trained first (after "
                               "instantiation or change in parameters)");
    _kernels_handler.set_rhs(X);
    return get_optimal_kernel();
}

template <typename Scalar, typename Kernel>
typename UMKLKNN<Scalar, Kernel>::Matrix
UMKLKNN<Scalar, Kernel>::predict(const std::vector<Matrix> &kernel_matrices)
{
    if (!_trained)
        throw std::logic_error("The model should be trained first (after "
                               "instantiation or change in parameters)");
    if (kernel_matrices[0].rows() != _number_of_support_vectors)
        throw std::length_error("Similarities must be provided for all support "
                                "vectors; matrices have wrong number of rows.");
    if (kernel_matrices.size() != _number_of_kernels)
        throw std::length_error(
        "Same number of kernels as on training is required");
    _kernels_handler.set_matrices(kernel_matrices);
    return get_optimal_kernel();
}

template <typename Scalar, typename Kernel>
void UMKLKNN<Scalar, Kernel>::compute_w_matrix()
{
    // ensure a clean status
    _W.resize(_number_of_support_vectors, _number_of_support_vectors);
    _W.setZero();

    // find k-nearest neighbors filling upper part of _W
    find_and_update_neighbours_using_kernels(_kernels_handler, _number_of_kernels,
                                             _number_of_support_vectors, _k, _W);

    // symmetrize _W
    _W.template triangularView<Eigen::Lower>() = _W.transpose();
    // check if we handled properly the self loops
    if (_W.diagonal().sum() > 0.0)
    {
        _logger->critical("W diagonal contains non zero elements");
    }
    _logger->trace("Matrix W:\n{}", _W);
    _logger->debug("W done");
}

template <typename Scalar, typename Kernel>
void UMKLKNN<Scalar, Kernel>::compute_s_matrix()
{
    _S.resize(_number_of_kernels, _number_of_kernels);
    _S.setZero();
    Matrix K_st(_number_of_support_vectors, _number_of_support_vectors);
    for (Index s = 0; s < _number_of_kernels; ++s)
    {
        for (Index t = s; t < _number_of_kernels; ++t)
        { // compute
            K_st = _kernels_handler[s] * _kernels_handler[t];
            _S(s, t) =
            ((_W * K_st.diagonal()).sum() - _W.cwiseProduct(K_st).sum());
            if (t > s)
            {
                _S(t, s) = _S(s, t);
            }
        }
    }
    _logger->trace("Matrix S:\n{}", _S);
    _logger->debug("S done");
}

template <typename Scalar, typename Kernel>
void UMKLKNN<Scalar, Kernel>::optimize_beta()
{
    _beta = Column::Constant(_number_of_kernels, (double)1. / _number_of_kernels);
    _beta_dlib = mat(_beta);
    Index iterations = dlib::solve_qp_using_smo(
    mat(_S), dlib::zeros_matrix<Scalar>(0, _number_of_kernels), _beta_dlib,
    _epsilon, _maxiter_qp); // 2*mat(_S) would be correct, but not needed
    if (iterations > _maxiter_qp)
    {
        _logger->critical("the qp-solver did not finish in {} iterations",
                          _maxiter_qp);
    }
    _logger->debug("{} qp iterations", iterations);
    _beta = mimkl::linear_algebra::dlib_to_eigen(_beta_dlib);
}

template <typename Scalar, typename Kernel>
Index UMKLKNN<Scalar, Kernel>::get_k() const
{
    return _k;
}

template <typename Scalar, typename Kernel>
double UMKLKNN<Scalar, Kernel>::get_epsilon() const
{
    return _epsilon;
}

template <typename Scalar, typename Kernel>
Index UMKLKNN<Scalar, Kernel>::get_maxiter_qp() const
{
    return _maxiter_qp;
}

template <typename Scalar, typename Kernel>
typename UMKLKNN<Scalar, Kernel>::Column UMKLKNN<Scalar, Kernel>::get_beta() const
{
    _logger->debug(
    "The model has not been fit. Maybe the parameters were changed?");
    return _beta;
}

template <typename Scalar, typename Kernel>
typename UMKLKNN<Scalar, Kernel>::Matrix
UMKLKNN<Scalar, Kernel>::get_optimal_kernel()
{
    return _kernels_handler.sum(_beta);
}

template <typename Scalar, typename Kernel>
void UMKLKNN<Scalar, Kernel>::set_k(const Index k)
{
    _k = k;
    _trained = false;
    _logger->debug("changing parameters requires refitting before prediction");
}

template <typename Scalar, typename Kernel>
void UMKLKNN<Scalar, Kernel>::set_epsilon(const double epsilon)
{
    _epsilon = epsilon;
    _trained = false;
    _logger->debug("changing parameters requires refitting before prediction");
}

template <typename Scalar, typename Kernel>
void UMKLKNN<Scalar, Kernel>::set_maxiter_qp(const Index maxiter_qp)
{
    _maxiter_qp = maxiter_qp;
}

template <typename Scalar, typename Kernel>
void UMKLKNN<Scalar, Kernel>::set_parameters(const Index k,
                                             const double epsilon,
                                             const Index maxiter_qp)
{
    _k = k;
    _epsilon = epsilon;
    _maxiter_qp = maxiter_qp;
    _trained = false;
    _logger->debug("changing parameters requires refitting before prediction");
    _logger->debug("k: {}", _k);
    _logger->debug("epsilon: {}", _epsilon);
    _logger->debug("maxiter_qp: {}", _maxiter_qp);
}

template <typename Scalar, typename Kernel>
void UMKLKNN<Scalar, Kernel>::set_beta(const Column &beta)
{
    if (beta.rows() != _number_of_kernels)
        throw std::length_error(
        "passed beta does not have one weight for each kernel");
    _beta = beta;
    _beta_dlib = mat(_beta);
}

} // namespace models
} // namespace mimkl

#endif /* INCLUDE_MIMKL_MODELS_UMKL_KNN_HPP_ */


#ifndef _ITERATIVE_CLOSEST_POINT_HPP_
#define _ITERATIVE_CLOSEST_POINT_HPP_

#include <boost/geometry.hpp>
#include <boost/geometry/algorithms/distance.hpp>
#include <boost/geometry/geometries/register/point.hpp>
#include <boost/geometry/geometries/point.hpp>
#include <boost/geometry/geometries/box.hpp>
#include <boost/geometry/geometries/polygon.hpp>
#include <boost/geometry/algorithms/correct.hpp>
#include <boost/math/special_functions/fpclassify.hpp>

#include <boost/geometry/index/rtree.hpp>

#include <cmath>
#include <vector>
#include <iostream>

namespace bg = boost::geometry;
namespace bgi = boost::geometry::index;

#include "hornRegistration.hpp"
#include "TerminationCriteria.hpp"

/// Finds the nearest point on a line segment to a point in space.  Called by the function OutsideOfTriangle()
/// to determine where the nearest point is to each side of the triangle.
/// @param a is the point in space
/// @param p is one end of the line segment represented by an Eigen::Vector3d
/// @param q is the other end of the line segment represented by an Eigen::Vector3d
/// @see page 8 of PointPairs.pdf from the notes
Eigen::Vector4d ProjectOnSegment(const Eigen::Vector3d& c, const Eigen::Vector3d& p, const Eigen::Vector3d& q)
{
    Eigen::Vector3d pMinusQ = q - p;
    double lambda = (c-p).dot(pMinusQ)/(pMinusQ.dot(pMinusQ));
    double zero = 0.0;
    double one = 1.0;
    double vertexlogic = one;
    if (lambda <= 0 || lambda >= 1){
        vertexlogic = zero;
    }
    lambda = std::max(zero,std::min(lambda,one));
    Eigen::Vector4d cNew;
    cNew.block<3,1>(0,0) = p+lambda*pMinusQ;
    cNew(3) = vertexlogic;
    return cNew;
}

/// Determines if two points are equal.  Used by the function OutsideOfTriangle to determine if the nearest
/// point on the triangle lies on a vertice
/// @param a is the point in space
/// @param p is the first vertice of the triangle represented by an Eigen::Vector3d
bool PointEqualityCheck(const Eigen::Vector3d& a, const Eigen::Vector3d& b){
    bool tf = (a(0) == b(0) && a(1) == b(1) && a(2) == b(2));
    return tf;
}

/// Finds the closest point on the triangle to a point in space if the closest point lies on an edge or vertice
/// @param a is the point in space
/// @param p is the first vertice of the triangle represented by an Eigen::Vector3d
/// @param q is the second vertice of the triangle represented by an Eigen::Vector3d
/// @param r is the third vertice of the triangle represented by an Eigen::Vector3d
/// @see page 8 of PointPairs.pdf from the notes
Eigen::Vector3d OutsideOfTriangle(const Eigen::Vector3d& a, const Eigen::Vector3d& p, const Eigen::Vector3d& q, const Eigen::Vector3d& r)
{
    Eigen::MatrixXd c(3,4);
    c.block<1,4>(0,0) = ProjectOnSegment(a,r,p).transpose();
    c.block<1,4>(1,0) = ProjectOnSegment(a,p,q).transpose();
    c.block<1,4>(2,0) = ProjectOnSegment(a,q,r).transpose();
    Eigen::Vector3d cnew;
    if (PointEqualityCheck(c.block<1,3>(0,0),c.block<1,3>(1,0))) cnew = c.block<1,3>(0,0);
    else if (PointEqualityCheck(c.block<1,3>(0,0),c.block<1,3>(2,0))) cnew = c.block<1,3>(0,0);
    else if (PointEqualityCheck(c.block<1,3>(1,0),c.block<1,3>(2,0))) cnew = c.block<1,3>(1,0);
    else{
        for (int i=0; i<3; i++){
            if (c(i,3) == 1){
                cnew = c.block<1,3>(i,0);
            }
        }
    }
    return cnew;
}

/// Finds the closest point on the triangle to a point in space.  If the closest point lies with in triangle,
/// then the function finds the nearest point internally.  Else if the closest point lies on an edge or vertice,
/// the function OutsideOfTriangle() is called to find the nearest point
/// @param a is the point in space represented by an Eigen::Vector3d
/// @param vertices are the three vertices of the triangle represented by standard vector of Eigen::Vector3d
/// @see page 7 of PointPairs.pdf from the notes
Eigen::Vector3d FindClosestPoint(const Eigen::Vector3d& p, const Eigen::Vector3d& p1, const Eigen::Vector3d& p2, const Eigen::Vector3d& p3)
{
    Eigen::Vector3d u = p2-p1;
    Eigen::Vector3d v = p3-p1;
    Eigen::Vector3d w = p-p1;
    Eigen::Vector3d n = u.cross(v);
    double gamma = u.cross(w).dot(n)/(n.dot(n));
    double beta = w.cross(v).dot(n)/(n.dot(n));
    double alpha = 1-gamma-beta;
    Eigen::Vector3d projectedPoint;
    if ((alpha < 0 || alpha > 1) || (beta < 0 || beta > 1) || (gamma < 0 || gamma > 1)){
        projectedPoint = OutsideOfTriangle(p,p1,p2,p3);
    }
    else projectedPoint = alpha*p1+beta*p2+gamma*p3;

    return projectedPoint;
}

Eigen::Vector3d FindClosestPoint(const Eigen::Vector3d& p, const std::vector<Eigen::Vector3d>& vertices, const Eigen::VectorXd& triangle) {

    Eigen::Vector3d p1 = vertices[triangle(0)];
    Eigen::Vector3d p2 = vertices[triangle(1)];
    Eigen::Vector3d p3 = vertices[triangle(2)];

    return FindClosestPoint(p,p1,p2,p3);
}

typedef Eigen::Vector3d EV3d;
typedef bg::cs::cartesian bgcscart;

#define ZEROINDEX operator()(0)
#define ONEINDEX operator()(1)
#define TWOINDEX operator()(2)

BOOST_GEOMETRY_REGISTER_POINT_3D(EV3d, double, bgcscart, ZEROINDEX, ONEINDEX, TWOINDEX)

namespace cis {

    //typedef bg::model::point<double, 3, bg::cs::cartesian> point;
    typedef Eigen::Vector3d point;
    typedef bg::model::box<point> box;
    typedef bg::model::polygon<point, false, false> polygon; // ccw, open polygon
    typedef std::pair<box, polygon> value;
}

/// perform ICPregistration on source data consisting of sensor data,
/// prior known body data, and a triangle mesh.
///
/// @pre NA and NB vectors, rather than data, must be of equal length. Corresponding to the same quantity of measured sample time points.
///
/// @param[in]  NA list of sensor data point sets for pointer A
/// @param[in]  NB list of sensor data point sets for fiducial B
/// @param[in]  Atip location of tip of pointerA in pointer A body coordinates
/// @param[in]  bodyAmarkerLEDs location of LED markers on pointer A in pointer A body coordinates
/// @param[in]  bodyAmarkerLEDs location of LED markers on fiducial B in fiducial B body coordinatesnd 3 correspond to neighbors. -1 indicates not a neighbor. List of triangles on mesh, corresponding to bone surface.
/// @return dkList location of Atip in fiducial B body coordinates, nx3 matrix of transposed vectors
Eigen::MatrixXd
dkKnownMeshPointsBaseFrame(const std::vector<Eigen::MatrixXd>& NA,
                                const std::vector<Eigen::MatrixXd>& NB,
                                const Eigen::Vector3d& Atip,
                                const Eigen::MatrixXd& bodyAmarkerLEDs,
                                const Eigen::MatrixXd& bodyBmarkerLEDs){

    Eigen::MatrixXd dkList(NA.size(),3);

    for (int i=0; i<NA.size(); i++){
        Eigen::Affine3d FaAffine(hornRegistration(bodyAmarkerLEDs,NA[i])); // a: PA3-A-Debug-SampleReadingsTest A: Problem3-BodyA
        Eigen::Affine3d FbInverseAffine(hornRegistration(NB[i], bodyBmarkerLEDs)); // b: PA3-A-Debug-SampleReadingsTest B: Problem3-BodyB

        // Atip: Problem3-BodyA (last line)
        Eigen::Vector3d dk_i(Eigen::Vector3d(FbInverseAffine*FaAffine*Atip));
        dkList.block<1,3>(i,0) = dk_i.transpose();
    }

    return dkList;
}

/// Perform one iteration of ICPregistration on source data consisting of sensor data,
/// prior known body data, and a triangle mesh.
///
/// @param[out] dkList location of Atip in fiducial B body coordinates, nx3 matrix of transposed vectors
/// @param[in]  vertices list of vertices on mesh, corresponding to bone surface
/// @param[in]  vertexTriangleNeighborIndex list of 1x6 vectors. First 3 Elements are indices into vertices list, Second 3 correspond to neighbors. -1 indicates not a neighbor. List of triangles on mesh, corresponding to bone surface.
/// @param[out] ck location of CT mesh closest to sample points, nx3 matrix of transposed vectors
/// @param[out] errork norm between ck and dk
void ICPwithSimpleSearchStep(
                              const Eigen::MatrixXd& dkList,
                              const std::vector<Eigen::Vector3d>& vertices,
                              const std::vector<Eigen::VectorXd>& vertexTriangleNeighborIndex,
                              Eigen::Affine3d& Freg,
                              Eigen::MatrixXd& skList,
                              Eigen::MatrixXd& ckList,
                              std::vector<double>& errork){

    errork.clear();
    ckList.resize(dkList.rows(),3);
    skList.resize(dkList.rows(),3);

    for (int i=0; i<dkList.rows(); i++){
        double errorMin=std::numeric_limits<double>::max();
	    Eigen::Vector3d ckMin;
        Eigen::Vector3d dk_i(dkList.block<1,3>(i,0).transpose());

        Eigen::Vector3d sk(Freg*dk_i);
        skList.block<1,3>(i,0) = sk.transpose();

        //if(i % NA.size() == 3) std::cout << "\n\nsk[3]\n\n" << sk << "\n\n";
        for (auto&& triangle : vertexTriangleNeighborIndex){
            Eigen::Vector3d ckTemp = FindClosestPoint(sk, vertices, triangle);
            double errorTemp = (sk-ckTemp).norm();
            if (errorTemp < errorMin){
                ckMin = ckTemp;
                errorMin = errorTemp;
            }
        }
		ckList.block<1,3>(i,0) = ckMin.transpose();
        errork.push_back(errorMin);
    }

	Freg = hornRegistration(dkList,ckList);
}

/// perform ICPregistration on source data consisting of sensor data,
/// prior known body data, and a triangle mesh. Uses brute force iteration.
/// Runs many iterations and stops
/// optimization based on the input terminationCriteria object, which also
/// returns statistics of the execution by default.
///
/// @param[in] dkList location of Atip in fiducial B body coordinates, n x 3 matrix of transposed vectors
/// @param[in]  vertices list of vertices on mesh, corresponding to bone surface
/// @param[in]  vertexTriangleNeighborIndex list of 1x6 vectors. First 3 Elements are indices into vertices list, Second 3 correspond to neighbors. -1 indicates not a neighbor. List of triangles on mesh, corresponding to bone surface.
/// @param[in,out] terminationCriteria  Collects statistics and determines when algorithm should stop. C++ concept matches TerminationCriteria class.
/// @param[out] ck location of CT mesh closest to sample points, nx3 matrix of transposed vectors
/// @param[out] errork norm between ck and dk
template<typename TerminationType = TerminationCriteria>
void ICPwithSimpleSearch(
                              const Eigen::MatrixXd& dkList,
                              const std::vector<Eigen::Vector3d>& vertices,
                              const std::vector<Eigen::VectorXd>& vertexTriangleNeighborIndex,
                              TerminationType& terminationCriteria,
                              Eigen::Affine3d& Freg,
                              Eigen::MatrixXd& skList,
                              Eigen::MatrixXd& ckList,
                              std::vector<double>& errork,
                              bool debug = false){

    Freg.setIdentity();

    for(int i = 0; !terminationCriteria.shouldTerminate(); i++){

        if(debug) std::cout << "\n\nFreg before iteration " << i << ":\n\n" << Freg.matrix() << "\n\n";

        if(i) terminationCriteria.nextIteration();
		ICPwithSimpleSearchStep(dkList,vertices,vertexTriangleNeighborIndex,Freg,skList,ckList,errork);

        for(auto && err : errork){
            terminationCriteria(err);
        }

	}
}

/// Perform one iteration of ICPregistration on source data consisting of sensor data,
/// prior known body data, and a triangle mesh. Uses a spatial index to accelerate
/// nearest neighbor lookup of triangles.
///
/// @tparam boost::geometry::rtree data structure type configured with the triangle set
///
/// @param[in] dkList location of Atip in fiducial B body coordinates, n x 3 matrix of transposed vectors
/// @param[in] vertices list of vertices on mesh, corresponding to bone surface
/// @param[in] rtree actual triangle data index
/// @param[in,out] terminationCriteria  Collects statistics and determines when algorithm should stop. C++ concept matches TerminationCriteria class.
/// @param[out] ck location of CT mesh closest to sample points, nx3 matrix of transposed vectors
/// @param[out] errork norm between ck and dk
/// @param[out] errork norm between ck and dk
/// @param[in]  initialQuerySize is the number of boxes that will initially be selected in spatialIndex. Affects algorithm performance.
template<typename RTREE>
void ICPwithSpatialIndexStep(
                      const Eigen::MatrixXd& dkList,
                      const RTREE& rtree,
                      Eigen::Affine3d& Freg,
                      Eigen::MatrixXd& skList,
                      Eigen::MatrixXd& ckList,
                      std::vector<double>& errork,
                      const std::size_t initialQuerySize = 4){

    errork.clear();
    ckList.resize(dkList.rows(),3);
    skList.resize(dkList.rows(),3);

    for (int i=0; i<dkList.rows(); ++i){
        double minErrorAKAdistanceToClosestTriangle=std::numeric_limits<double>::max();
        Eigen::Vector3d ckClosestPointOnMesh;
        Eigen::Vector3d dk_i(dkList.block<1,3>(i,0).transpose());

        Eigen::Vector3d sk(Freg*dk_i);
        skList.block<1,3>(i,0) = sk.transpose();

        // starting query size will affect performance
        /// @todo consider making querySize a parameter

        bool closestFound = false;
        std::size_t querySize = initialQuerySize;
        while(!closestFound){
            // If the distance to the next box was greater than to the nearest triangle
            // or the distance to the triangle was equal to 0
            // break the loop.

            // find 2 nearest values to a point
            std::vector<cis::value> result_n;
            rtree.query(bgi::nearest(sk, querySize), std::back_inserter(result_n));

            // results should be listed in order from closest box to furthest box,
            // but the boxes can overlap so untl the closest point on the nearest triangle
            // is closer than boxes later in the list, there may be closer triangle points
            // than the current one.
            for(auto&& result : result_n){
                /// @todo Eliminate redundantly checking first querySize boxes in result_n on subsequent iterations of while(!closestFound == false).
                Eigen::Vector3d ckClosestPointOnCurrentTriangle = FindClosestPoint(sk, result.second.outer()[0], result.second.outer()[1], result.second.outer()[2]);
                double distanceToCurrentBox = bg::distance(sk,result.first);
                double distanceToCurrentTriangle = bg::distance(sk,ckClosestPointOnCurrentTriangle);

                if (distanceToCurrentTriangle < minErrorAKAdistanceToClosestTriangle){
                    // put dk (qk)  into A put ck into B
                    ckClosestPointOnMesh = ckClosestPointOnCurrentTriangle;
                    minErrorAKAdistanceToClosestTriangle = distanceToCurrentTriangle;
                }

                // leave the loop if the distance to the box is larger than the distance to the polygon
                if( minErrorAKAdistanceToClosestTriangle < distanceToCurrentBox || distanceToCurrentTriangle == 0) {
                    closestFound = true;
                    break;
                }
            }

            // increase the query size and rerun it if we didn't find the closest point for certain
            querySize *=2;
        }

        ckList.block<1,3>(i,0) = ckClosestPointOnMesh.transpose();
        BOOST_VERIFY(!boost::math::isnan(minErrorAKAdistanceToClosestTriangle));
        errork.push_back(minErrorAKAdistanceToClosestTriangle);
    }

    Freg = hornRegistration(dkList,ckList);
}

/// perform ICPregistration on source data consisting of sensor data,
/// prior known body data, and a triangle mesh.  Uses a spatial index to accelerate
/// nearest neighbor lookup of triangles. Runs many iterations and stops
/// optimization based on the input terminationCriteria object, which also
/// returns statistics of the execution by default.
///
/// @param[in] dkList location of Atip in fiducial B body coordinates, n x 3 matrix of transposed vectors
/// @param[in]  vertices list of vertices on mesh, corresponding to bone surface
/// @param[in]  vertexTriangleNeighborIndex list of 1x6 vectors. First 3 Elements are indices into vertices list, Second 3 correspond to neighbors. -1 indicates not a neighbor. List of triangles on mesh, corresponding to bone surface.
/// @param[in,out] terminationCriteria  Collects statistics and determines when algorithm should stop. C++ concept matches TerminationCriteria class.
/// @param[out] ck location of CT mesh closest to sample points, nx3 matrix of transposed vectors
/// @param[out] errork norm between ck and dk
template<typename TerminationType = TerminationCriteria>
void ICPwithSpatialIndex(
                  const Eigen::MatrixXd& dkList,
                  const std::vector<Eigen::Vector3d>& vertices,
                  const std::vector<Eigen::VectorXd>& vertexTriangleNeighborIndex,
                  TerminationType& terminationCriteria,
                  Eigen::Affine3d& Freg,
                  Eigen::MatrixXd& skList,
                  Eigen::MatrixXd& ckList,
                  std::vector<double>& errork,
                  bool debug = false){

    Freg.setIdentity();

    // polygons
    std::vector<cis::polygon> polygons;

    // create some polygons
    for ( auto&& triangle : vertexTriangleNeighborIndex )
    {
        // create a polygon
        cis::polygon p;

        Eigen::Vector3d p1 = vertices[triangle(0)];
        Eigen::Vector3d p2 = vertices[triangle(1)];
        Eigen::Vector3d p3 = vertices[triangle(2)];
        p.outer().push_back(p1);
        p.outer().push_back(p2);
        p.outer().push_back(p3);

        // add polygon
        polygons.push_back(p);
    }

    // create the rtree using default constructor
    bgi::rtree< cis::value, bgi::rstar<16, 4> > rtree;

    // fill the spatial index
    for ( auto && polygon : polygons )
    {

        // calculate polygon bounding box
        cis::box b = bg::return_envelope<cis::box>(polygon);
        // insert new value
        rtree.insert(std::make_pair(b, polygon));
    }

    for(int i = 0; !terminationCriteria.shouldTerminate(); i++){

        if(debug) std::cout << "\n\nFreg before iteration " << i << ":\n\n" << Freg.matrix() << "\n\n";
        if(i) terminationCriteria.nextIteration();
        ICPwithSpatialIndexStep(dkList,rtree,Freg,skList,ckList,errork);

        for(auto && err : errork){
            terminationCriteria(err);
        }
    }
}

#endif // _ITERATIVE_CLOSEST_POINT_HPP_


#include <iostream>
#include <utility>
#include <algorithm>
#include <vector>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/graph_traits.hpp>
#include <boost/property_map/transform_value_property_map.hpp>
#include <boost/property_map/function_property_map.hpp>
#include <boost/graph/dijkstra_shortest_paths.hpp>
#include <future>
#include <torch/extension.h>
#define EPS 1e-20

class union_find{
public:
    union_find(int n){
        this->count = n;
        for(auto i=0; i<n; i++){
            parent[i] = i;
        }
    }

    void make_set(int x){
        this->parent[x] = x;
        count++;
    }
    int find(int x){
        int tmp = x;
        while(parent[tmp] != tmp){
            parent[tmp] = parent[parent[tmp]];
            tmp = parent[tmp];
        }
        return tmp;
    }
    // For now set parent[y] = x. Union by rank may come later. In that case we need to make sure the root is
    // always lower f value
    void link(int x, int y){
        parent[y] = x;
        count--;
    }
    int num_connected_component() const{
        return this->count;
    }
private:
    std::map<int, int> parent;
    int count;

};
using std::vector;
using torch::Tensor;
typedef std::pair<long, long> Edge;
typedef std::pair<int, Edge> Pers;
typedef boost::adjacency_list<boost::vecS, boost::vecS, boost::undirectedS, boost::no_property, boost::property<boost::edge_weight_t, int>> Graph;
typedef boost::graph_traits<Graph>::vertex_descriptor VertexDesc;
typedef boost::graph_traits<Graph>::edge_descriptor EdgeDesc;
typedef boost::graph_traits<Graph>::vertex_iterator VertexIter;
typedef boost::graph_traits<Graph>::edge_iterator EdgeIter;
using PathType = vector<vector<VertexDesc>>;
using namespace torch::indexing;

PathType find_path(int start, VertexDesc goal, const Graph& _graph)  {
    vector<VertexDesc> p(num_vertices(_graph));
    vector<int> d(num_vertices(_graph));
    VertexDesc s = vertex(start, _graph);

    //auto idmap = boost::get(boost::vertex_index, _graph);

    //vector<VertexDesc> predecessors(boost::num_vertices(_graph), Graph::null_vertex());
    //vector<int> distances(boost::num_vertices(_graph));
    //boost::property_map<Graph, boost::edge_weight_t>::type weightmap = boost::get(boost::edge_weight, _graph);
    dijkstra_shortest_paths(_graph, s, boost::predecessor_map(&p[0]).distance_map(&d[0]));

    // extract path
    VertexDesc current = goal;
    PathType path { };
    //PathType edge_path;

    do {
        auto const pred = p.at(current);

        //std::cout << "extract path: " << current << " " << _graph[current].coord << " <- " << pred << std::endl;

        if(current == pred)
            break;
        path.push_back({current, pred});
        current = pred;

    } while(current != start);

    //std::reverse(path.begin(), path.end());

    return path;
}

bool mycmp(Edge a, Edge b){
    float a_val = std::max(a.first, a.second);
    float b_val = std::max(b.first, b.second);
    if (a_val < b_val)
        return true;
    else if(std::abs(a_val - b_val) < EPS)
        return a.second - b.second;
    return false;
}

bool vcmp(const vector<VertexDesc>& a, const vector<VertexDesc>& b){
    float a_val = std::max(a[0], a[1]);
    float b_val = std::max(b[0], b[1]);
    if (a_val < b_val)
        return true;
    else if(std::abs(a_val - b_val) < EPS)
        return a[1] - b[1];
    return false;
}

void print_pairs(vector<Pers> &ed) {
    for (auto e: ed) {
        std::cout << e.first << " (" << e.second.first << "," << e.second.second << ")" << std::endl;
    }
}

void print_path(const PathType& p){
    for(auto v : p){
        std::cout << "(" << v[0] << "," << v[1] << ")" << std::endl;
    }
}

struct CustomEdgeCompare {
    Tensor vert_fil;
    CustomEdgeCompare(const Tensor &vertex_filtration){
        this->vert_fil = vertex_filtration;
    }

    bool operator()(const vector<VertexDesc>& a, const vector<VertexDesc>& b) const {
        double a_val = std::max(vert_fil[a[0]].item<double>(), vert_fil[a[1]].item<double>());
        double b_val = std::max(vert_fil[b[0]].item<double>(), vert_fil[b[1]].item<double>());
        if (a_val < b_val)
            return true;
        else if(std::abs(a_val - b_val) < EPS)
            return (vert_fil[a[1]] - vert_fil[b[1]]).item<int>();
        return false;
    }
};
vector<Tensor> compute_pd0(const Tensor & vertex_filtration,
                           const vector<Tensor> & boundary_info){
    auto num_nodes = vertex_filtration.size(0);
    union_find uf = union_find(num_nodes);
    Tensor tensor_edges = boundary_info[0];
    Tensor edge_val = std::get<0>(torch::max(vertex_filtration.index({tensor_edges}), 1));
    Tensor sorted_edge_indices = edge_val.argsort(-1,false);
    const Tensor sorted_edges = tensor_edges.index({sorted_edge_indices});
    edge_val = edge_val.index({sorted_edge_indices});
    auto num_edges = sorted_edges.size(0);
    vector<Tensor> pd_0;

    for(auto i = 0; i < num_edges; i++){
        auto e = sorted_edges[i];
        auto e_val = edge_val[i];
        int u = e[0].item<int>();
        int v = e[1].item<int>();
        int root_u = uf.find(u);
        int root_v = uf.find(v);
        if(root_u == root_v){
            continue;
        }
        int root = root_u;
        int merged = root_v;
        if (vertex_filtration[root].item<double>() > vertex_filtration[merged].item<double>())
            std::swap(root, merged);
        else if (std::abs(vertex_filtration[root].item<double>() - vertex_filtration[merged].item<double>()) < EPS) {
            if (root > merged)
                std::swap(root, merged);
        }
        auto merged_val = vertex_filtration[merged];
        //std::cout << "M: " << merged_val.item<double>() << " E: " << e_val.item<double>()<< std::endl;
        Tensor pd_pair = torch::stack({merged_val, e_val});
        pd_0.emplace_back(pd_pair);
        uf.link(root, merged);
    }
    return pd_0;

}

vector<vector<Tensor>> extended_filt_persistence_single(const Tensor & vertex_filtration,
                                                        const vector<Tensor> & boundary_info){
    vector<vector<Tensor>> pd;
    auto num_nodes = vertex_filtration.size(0);
    union_find uf = union_find(num_nodes);
    Graph g;
    vector<size_t> pos_edge_index;
    vector<Tensor> pd_0_up = compute_pd0(vertex_filtration, boundary_info);
    vector<Tensor> pd_0_down, pd_1_rel, pd_1_ext;
    Tensor tensor_edges = boundary_info[0];
    Tensor edge_val = std::get<0>(torch::min(vertex_filtration.index({tensor_edges}), 1));
    Tensor sorted_edge_indices = edge_val.argsort(-1, true);
    const Tensor sorted_edges = tensor_edges.index({sorted_edge_indices});
    edge_val = edge_val.index({sorted_edge_indices});
    auto num_edges = sorted_edges.size(0);
    for(auto i = 0; i < num_edges; i++){
        auto e = sorted_edges[i];
        auto e_val = edge_val[i];
        int u = e[0].item<int>();
        int v = e[1].item<int>();
        int root_u = uf.find(u);
        int root_v = uf.find(v);
        if(root_u == root_v){
            pos_edge_index.push_back(i);
            continue;
        }
        boost::add_edge(u, v, 1, g);
        int root = root_u;
        int merged = root_v;
        if (vertex_filtration[root].item<double>() < vertex_filtration[merged].item<double>())
            std::swap(root, merged);
        else if (std::abs(vertex_filtration[root].item<double>() - vertex_filtration[merged].item<double>()) < EPS) {
            if (root < merged)
                std::swap(root, merged);
        }
        auto merged_val = vertex_filtration[merged];
        Tensor pd_pair = torch::stack({merged_val, e_val});
        pd_0_down.emplace_back(pd_pair);
        uf.link(root, merged);
    }
    //pd.push_back(pd_0);
    Tensor min_max = torch::stack({vertex_filtration.min(), vertex_filtration.max()});
    pd_1_rel.push_back(min_max);
    CustomEdgeCompare cmp = CustomEdgeCompare(vertex_filtration);
    for(auto ii : pos_edge_index){
        auto pos_edge = sorted_edges[ii];
        auto pos_edge_val = edge_val[ii];
        int u = pos_edge[0].item<int>();
        int v = pos_edge[1].item<int>();
        PathType p = find_path(u, v, g);
        //std::cout << "Edge: " << u << " " << v << std::endl;
        //print_path(p);
        auto result = *std::max_element(p.begin(), p.end(), cmp);
        boost::remove_edge(result[0], result[1], g);
        //std::cout << "Removed edge: " << result[0] << " " << result[1] << std::endl;
        boost::add_edge(u, v, 1, g);
        auto cut_edge_tensor = torch::from_blob(result.data(), {2}, torch::TensorOptions().dtype(at::kLong));
        auto cut_edge_val = vertex_filtration.index({cut_edge_tensor}).max();
        //std::cout << "CE " << cut_edge_val.item<double>() << " AE " << pos_edge_val.item<double>() << std::endl;
        auto pers_pair = torch::stack({cut_edge_val, pos_edge_val});
        pd_1_ext.push_back(pers_pair);
    }
    //pd.push_back(pd_1);
    pd.push_back(pd_0_up);
    pd.push_back(pd_0_down);
    pd.push_back(pd_1_rel);
    pd.push_back(pd_1_ext);
    return pd;

}
vector<vector<vector<Tensor>>> extended_filt_persistence_batch(const vector<std::tuple<Tensor, vector<Tensor>>> & batch){
auto futures = vector<std::future<vector<vector<Tensor>>>>();
for (auto & arg: batch){

futures.push_back(
        async(std::launch::async,[=]{
                  return extended_filt_persistence_single(
                          std::get<0>(arg),
                          std::get<1>(arg)
                  );
              }
)
);
}
auto ret = vector<vector<vector<Tensor>>>();
for (auto & fut: futures){
ret.push_back(
        fut.get()
);
}

return ret;
}

PYBIND11_MODULE(TORCH_EXTENSION_NAME, m) {
m.def("extended_persistence_batch", &extended_filt_persistence_batch, "A function to compute extended_persistence in batches as C-hofer");
m.def("extended_persistence_single", &extended_filt_persistence_single, "A function to compute extended_persistence with (v, [e]) format");
}


//
//! Copyright © 2021
//! Brandon Kohn
//
//  Distributed under the Boost Software License, Version 1.0. (See
//  accompanying file LICENSE_1_0.txt or copy at
//  http://www.boost.org/LICENSE_1_0.txt)
//
#ifndef GEOMETRIX_CONVEX_POLYGON_POLYGON_INTERSECTION_HPP
#define GEOMETRIX_CONVEX_POLYGON_POLYGON_INTERSECTION_HPP
#pragma once

#include <geometrix/algorithm/intersection/segment_segment_intersection.hpp>
#include <geometrix/algorithm/orientation/point_segment_orientation.hpp>
#include <geometrix/primitive/point_sequence_traits.hpp>
#include <geometrix/primitive/polygon.hpp>
#include <geometrix/algorithm/point_in_polygon.hpp>
#include <geometrix/primitive/point.hpp>
#include <geometrix/arithmetic/arithmetic_promotion_policy.hpp>
#include <geometrix/utility/utilities.hpp>

#include <boost/container/flat_set.hpp>
#include <boost/container/small_vector.hpp>
#include <boost/container/new_allocator.hpp>
#include <boost/concept_check.hpp>

/////////////////////////////////////////////////////////////////////////////
//
// NAMESPACE
//
/////////////////////////////////////////////////////////////////////////////
namespace geometrix {
    enum class polygon_intersection_type
    {
        none = 0,
        face = 1,
        vertex = 2,
        overlapping = 3
    };

    //! From Computational Geometry in C, O'Rourke.
    template <typename Polygon1, typename Polygon2, typename Visitor, typename NumberComparisonPolicy>
    inline polygon_intersection_type convex_polygon_polygon_intersection(const Polygon1& pgon1, const Polygon2& pgon2, Visitor&& visitor, const NumberComparisonPolicy& cmp)
    {
        BOOST_CONCEPT_ASSERT((PointSequenceConcept<Polygon1>));
        BOOST_CONCEPT_ASSERT((PointSequenceConcept<Polygon2>));
        BOOST_CONCEPT_ASSERT((NumberComparisonPolicyConcept<NumberComparisonPolicy>));
        using access1 = point_sequence_traits<Polygon1>;
        using access2 = point_sequence_traits<Polygon2>;
        using length_t = typename select_arithmetic_type_from_sequences<typename access1::point_type, typename access2::point_type>::type;
        using area_t = decltype(std::declval<length_t>() * std::declval<length_t>());
        using point_t = point<length_t, 2>;
        using vector_t = vector<length_t, 2>;
        enum in_out_state
        {
            pgon1_in,
            pgon2_in,
            unknown
        };
		auto pointCmp = [&]( const point_t& lhs, const point_t& rhs )
		{
			return lexicographically_less_than( lhs, rhs, cmp );
		};
	#if BOOST_VERSION >= 107000
		using small_flat_set = boost::container::flat_set<point_t, decltype(pointCmp), boost::container::small_vector<point_t, 20, boost::container::new_allocator<point_t>>>;
		auto visited = small_flat_set(pointCmp);
    #else
		auto visited = boost::container::flat_set<point_t, decltype( pointCmp )>( pointCmp );
    #endif
		auto shouldVisit = [&]( const point_t& p )
		{
			auto it = visited.lower_bound( p );
			if( it != visited.end() && !( visited.key_comp()( p, *it ) ) )
				return;

			visited.insert( it, p );
			visitor( p );
		};
        auto get_point1 = [&](std::size_t i) { return access1::get_point(pgon1, i); };
        auto get_point2 = [&](std::size_t i) { return access2::get_point(pgon2, i); };
        auto advance = [](std::size_t& a, std::size_t& aa, std::size_t n) { ++aa; ++a %= n; };
        auto a = std::size_t {};
        auto b = std::size_t {};
        auto aa = std::size_t {};
        auto ba = std::size_t {};
        auto inflag = unknown;

        auto n = access1::size(pgon1);
        auto n2 = n * 2;
        auto m = access2::size(pgon2);
        auto m2 = m * 2;

        auto nVisited = 0;
        do
        {
            auto a1 = (a + n - 1) % n;
            auto b1 = (b + m - 1) % m;

            vector_t A = get_point1(a) - get_point1(a1);
            vector_t B = get_point2(b) - get_point2(b1);

            auto ABOrientation = get_orientation(A, B, cmp);
            auto aHB = get_orientation(get_point2(b1), get_point2(b), get_point1(a), cmp);
            auto bHA = get_orientation(get_point1(a1), get_point1(a), get_point2(b), cmp);

            point_t xpoints[2];
            auto iType = segment_segment_intersection(get_point1(a1), get_point1(a), get_point2(b1), get_point2(b), xpoints, cmp);
            if (iType == e_crossing || iType == e_endpoint)
            {
                shouldVisit(xpoints[0]);
                ++nVisited;

                if (inflag == unknown && nVisited == 1)
                    aa = ba = 0;
                if (aHB == oriented_left)
                    inflag = pgon1_in;
                else if (bHA == oriented_left)
                    inflag = pgon2_in;
            }
            else if (iType == e_overlapping && dot_product(A, B) < area_t {})
            {
                //++nVisited;
                shouldVisit(xpoints[0]);
                shouldVisit(xpoints[1]);
                return polygon_intersection_type::face;
            }

            if (ABOrientation == oriented_collinear && aHB == oriented_right && bHA == oriented_right)
                return polygon_intersection_type::none;
            else if (ABOrientation == oriented_collinear && aHB == oriented_collinear && bHA == oriented_collinear)
            {
                if (inflag == pgon1_in)
                    advance(b, ba, m);
                else
                    advance(a, aa, n);
            }
            else if (ABOrientation != oriented_right)
            {
                if (bHA == oriented_left)
                {
                    if (inflag == pgon1_in)
                    {
                        ++nVisited;
                        shouldVisit(get_point1(a));
                    }
                    advance(a, aa, n);
                }
                else
                {
                    if (inflag == pgon2_in)
                    {
                        ++nVisited;
                        shouldVisit(get_point2(b));
                    }
                    advance(b, ba, m);
                }
            }
            else
            {
                if (aHB == oriented_left)
                {
                    if (inflag == pgon2_in)
                    {
                        ++nVisited;
                        shouldVisit(get_point2(b));
                    }
                    advance(b, ba, m);
                }
                else
                {
                    if (inflag == pgon1_in)
                    {
                        ++nVisited;
                        shouldVisit(get_point1(a));
                    }
                    advance(a, aa, n);
                }
            }
        } while((aa < n || ba < m) && aa < n2 && ba < m2);

        if (inflag == unknown)
        {
            if (visited.size() == 1) {
				return polygon_intersection_type::vertex;
            }
            //! Check if one vertex of either is inside.
            if (point_in_polygon(get_point1(0), pgon2))
            {
                for (auto i = 0ULL; i < m; ++i)
                    shouldVisit(get_point1(i));
                return polygon_intersection_type::overlapping;
            }
            if (point_in_polygon(get_point2(0), pgon1))
            {
                for (auto i = 0ULL; i < m; ++i)
                    shouldVisit(get_point2(i));
                return polygon_intersection_type::overlapping;
            }
        }

        return nVisited == 0 ? polygon_intersection_type::none : polygon_intersection_type::overlapping;
    }

}//! namespace geometrix;

#endif//! GEOMETRIX_CONVEX_POLYGON_POLYGON_INTERSECTION_HPP


/**
 * @file   tevol_source_gca.hpp
 * @brief  ODE system for group SIS with group selection mechanism for transmission rates
 *
 * Source code. All parameters passed as arguments, but specify and compile to change precision or output format.
 * g++ -std=c++11 -O3 -o tevol_source_gca ./tevol_source_gca.cpp $(gsl-config --cflags) $(gsl-config --libs)
 *
 * @author  LHD
 * @since   2021-02-06
 */

#include <iostream>
#include <cmath>
#include <algorithm>
#include <vector>
#include <fstream>
#include <sstream>

#include <boost/multi_array.hpp>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_odeiv.h>

#include "dyn_gca.hpp"

double binomial(int n, int k, double p);

using namespace std;

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

	//Model parameters
	double beta = atof(argv[1]); //basic diffusion rate of innovation
	double gamma = atof(argv[2]); //recovery rate of innovation
	double rho = atof(argv[3]); //coupling between groups
	double b = atof(argv[4]); //collective benefit per adopter
	double c = atof(argv[5]); //cost per level of promotion
	double mu = atof(argv[6]); //mutation rate of adoption
	double epsilon = atof(argv[7]); //random initial condition
	int n = atoi(argv[8]); //group size
	int L = atoi(argv[9]); //number of levels
    bool timeseries = atoi(argv[10]); //1 for time series, 0 for final state

    if(argc<6) {cerr << "Requires bunch of parameters: basic diffusion rate (beta),\n recovery rate (gamma),"
                    << "\n group coupling (rho),\n adaptation benefit (b),\n promotion cost (c),"
                    << "\n initial adoption (epsilon),\n group size (n),\n number of levels (l_max), \n 1 for time series, 0 for final state.\n" << endl; return 0;}

    const int dim1 = L+1;
    const int dim2 = n+1;
    Sparam param = {beta, gamma, rho, b, c, mu, dim1, dim2};

    // Integrator parameters
    double t = 0;
    double dt = 1e-4;
    double t_step = 1.0;
    const double eps_abs = 1e-8;
    const double eps_rel = 1e-6;

    // Setting initial conditions
    typedef boost::multi_array<double,2> mat_type;
    typedef mat_type::index index;
    mat_type y(boost::extents[dim1][dim2]);
    fill(y.data(),y.data()+y.num_elements(),0.0);

    // Uniform initial conditions
    double lastI = 0.0;
    double lastIL = 0.0;
	for(int l=0; l<dim1; ++l) {
        for(int i=0; i<=n; ++i) {
            y[l][i] = (1.0/(1.0*(L+1)))*binomial(n,i,epsilon); //C_n,i
            lastI += 1.0*i*y[l][i];
        }
    }
    double newI = 0.0;
    double newIL = 0.0;
    double newS = 1.0;

    // Define GSL odeiv parameters
    const gsl_odeiv_step_type * step_type = gsl_odeiv_step_rkf45;
    gsl_odeiv_step * step = gsl_odeiv_step_alloc (step_type, dim1*dim2);
    gsl_odeiv_control * control = gsl_odeiv_control_y_new (eps_abs,eps_rel);
    gsl_odeiv_evolve * evolve = gsl_odeiv_evolve_alloc (dim1*dim2);
    gsl_odeiv_system sys = {dydt, NULL, dim1*dim2, &param};

	//Integration
    int status(GSL_SUCCESS);
    double diff = 1.0;
    double diffL = 1.0;
    for (double t_target = t+t_step; t_target < 10000 || diff > 1e-10; t_target += t_step ) { //stop by time and difference
        while (t < t_target) {
            status = gsl_odeiv_evolve_apply (evolve,control,step,&sys,&t,t_target,&dt,y.data());
            if (status != GSL_SUCCESS) {
				cout << "SNAFU" << endl;
                break;
			}
        } // end while

        //measure adoption
        vector<double> newIvec(dim1, 0.0); vector<double> popvec(dim1, 0.0); newI=0.0;
        for(int l=0; l<dim1; ++l) for(int i=0; i<=n; ++i) {
            newIvec[l] += 1.0*i*y[l][i]; popvec[l] += y[l][i]; newI += 1.0*i*y[l][i]; }

        //timeseries output
        if(timeseries) {
          cout << t;
		  for(int l=0; l<dim1; ++l) cout << " " << newIvec[l]/popvec[l] << " " << popvec[l];
          cout << "\n";
        }
        diff = abs(newIvec[0]/popvec[0] - lastI);
        diffL = abs(newIvec[dim1-1]/popvec[dim1-1] - lastIL);
        lastI=newIvec[0]/popvec[0];
        lastIL=newIvec[dim1-1]/popvec[dim1-1];
	} //end while

    //calculate fitnesses
    vector<double> Zvec_(dim1, 0.0); vector<double> popvec_(dim1, 0.0);
    for(int l=0; l<dim1; ++l) {
        for(int i=0; i<dim2; ++i) {
            Zvec_[l] += exp(b*i-c*l)*y[l][i]; //should this really be proportional to C_i,l?
            popvec_[l] += y[l][i];
        }
        Zvec_[l] /= popvec_[l];
    }
    double Z_ = accumulate(Zvec_.begin(), Zvec_.end(), 0.0);

    //final state output

    if(!timeseries) {
      vector<double> Ivec(dim1, 0.0); vector<double> pop(dim1, 0.0); newI=0.0;
      for(int l=0; l<dim1; ++l) for(int i=0; i<=n; ++i) {
        Ivec[l] += 1.0*i*y[l][i]; pop[l] += y[l][i]; newI += 1.0*i*y[l][i];
      }
      cout << beta << " " << b << " " << c << " " << rho;
      for(int l=0; l<dim1; ++l) cout << " " << Ivec[l]/pop[l] << " " << pop[l];
      for(int l=0; l<dim1; ++l) cout << " " << Zvec_[l]/Z_;
      cout << "\n";
    }

    //output fitness distribution
    /*for(int l=0; l<dim1; ++l) {
        cout << beta << " " << b << " " << c << " " << rho << " " << l;
        for(int i=0; i<dim2; ++i) {
            int g = 0;
            for(; g<int(1000*y[l][i]); ++g) cout << " " << exp(b*i-c*l);
            for(; g<1000; ++g) cout << " " << "nan";
        }
        cout << "\n";
    }*/

    // localization in cliques
    /*double xi = 0.0;
    double It = 0.0;
	for(int s=2; s<dim; ++s) for(int i=0; i<=s; ++i) It += 1.0*i*y[s][i];
    for(int s=2; s<dim; ++s) {
        double In = 0.0;
        for(int i=0; i<=s; ++i) In += 1.0*i*y[s][i];
        xi += (In/It)*(In/It);
    }*/

    /*cout << beta << " " << lastI;
    for(int n=dim-1; n>1; n-=10) {
        lastI=0.0;
        for(int i=0; i<=n; ++i) lastI += 1.0*norms*i*y[n][i]/(1.0*n*pow(1.0*n,-gammas));
	    cout << " " << lastI;
    }
    cout << "\n";
	*/
    cout.flush();

    // Free memory
    gsl_odeiv_evolve_free(evolve);
    gsl_odeiv_control_free(control);
    gsl_odeiv_step_free(step);

    return 0;
}

double binomial(int n, int k, double p)
	{
	//check
	if(k>n || k<0 || n<0) return 0.0;

	double result=0.L;
	double postfix = pow(p,k)*pow(1.L-p,n-k);
	if(n<2*k) k=n-k;

	if(k<=100&&n<=200)
		{
		double numerator=1;
		for(int i=0; i<k; i++) numerator = numerator*((double)(n-i));
		double denominator=1;
		for(int i=2; i<=k; i++) denominator = denominator*((double)i);
		result = (double)(numerator/denominator)*postfix;
		}
	else
		{
		if(k<100||n<300)
			{
			result=1.L;
			for(int i=1; i<=k; i++) result = result*((double)(n-k+i))/((double)i);
			result = result*postfix;
			}
		else if(n<1000)
			{
			double nd=(double)n, kd=(double)k, nkd=(double)(n-k);
			result = nd*log(nd)-nd+0.5L*log(2.L*M_PI*nd)+1.L/(12.L*nd)-1.L/(360.L*pow(nd,3))+1.L/(1260.L*pow(nd,5));
			result -= kd*log(kd)-kd + 0.5L*log(2.L*M_PI*kd) + 1.L/(12.L*kd) - 1.L/(360.L*pow(kd,3)) + 1.L/(1260.L*pow(kd,5));
			result -= nkd*log(nkd)-nkd + 0.5L*log(2.L*M_PI*nkd) + 1.L/(12.L*nkd) - 1.L/(360.L*pow(nkd,3)) + 1.L/(1260.L*pow(nkd,5));
			result = exp(result);
			result = result*postfix;
			}

		else result = (1.L/sqrt(2.L*M_PI*(double)n*p*(1.L-p)))*exp(-pow(k-n*p,2)/(2.L*(double)n*p*(1.L-p)));
		}
	if(result!=result) result=0.L;
	if(isinf(result)) result=0.L;
	return result;
	}


/* Copyright (c) 2018, Skolkovo Institute of Science and Technology (Skoltech)
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *
 *
 * optimizer.cpp
 *
 *  Created on: Jul 8, 2020
 *      Author: Gonzalo Ferrer
 *              g.ferrer@skoltech.ru
 *              Mobile Robotics Lab, Skoltech
 */

#include "mrob/optimizer.hpp"
#include <Eigen/LU> // for inverse and determinant
#include <iostream>

using namespace mrob;

Optimizer::Optimizer(matData_t solutionTolerance, matData_t lambda) :
        solutionTolerance_(solutionTolerance), max_iters_(1e2), lambda_(lambda)
{

}

Optimizer::~Optimizer()
{

}

 uint_t Optimizer::solve(optimMethod method, uint_t max_iters, double lambda)
{
    optimization_method_ = method;
    max_iters_ = max_iters;
    switch(method)
    {
      case NEWTON_RAPHSON:
          return optimize_newton_raphson();
      case LEVENBERG_MARQUARDT_SPHER:
      case LEVENBERG_MARQUARDT_ELLIP:
          lambda_ = lambda;
          return optimize_levenberg_marquardt();
    }
    return 0;
}

uint_t OptimizerDense::optimize_newton_raphson_one_iteration(bool useLambda)
{
    // 1) build problem: Gradient and Hessian and re-evaluates
    calculate_gradient_hessian();
    if (useLambda)
    {
        if (optimization_method_ == LEVENBERG_MARQUARDT_SPHER)
        {
            for (Eigen::Index i = 0; i < hessian_.diagonalSize() ; ++i)
                hessian_(i,i) += lambda_;
        }
        if (optimization_method_ == LEVENBERG_MARQUARDT_ELLIP)
        {
            for (Eigen::Index i = 0; i < hessian_.diagonalSize() ; ++i)
                hessian_(i,i) *= 1.0 + lambda_;
        }
    }

    // 2) dx = - h^-1 * grad XXX test for singularities?
    dx_ = - hessian_.inverse() * gradient_;
    // 3) update the solution
    this->update_state();

    return 1;
}

uint_t Optimizer::optimize_newton_raphson()
{
    uint_t iters = 0;
    // Calculate error also estimates planes, which are necessary for gradients. XXX this can cause bugs on the first iteration
    matData_t previous_error = this->calculate_error(), diff_error;
    do
    {
        this->optimize_newton_raphson_one_iteration(false);
        matData_t current_error = this->calculate_error();
        //std::cout << "iter " << iters << ", error = " << current_error <<std::endl;
        diff_error = previous_error - current_error;
        previous_error = current_error;
        iters++;
    }while(fabs(diff_error) > solutionTolerance_ && iters < max_iters_);

    return iters;
}

uint_t Optimizer::optimize_levenberg_marquardt()
{
    // LM trust region as described in Bertsekas (p.105)
    // sigma reference to the fidelity of the model at the proposed solution \in [0,1]
    matData_t sigma1(0.25), sigma2(0.8);// 0 < sigma1 < sigma2 < 1
    matData_t beta1(2.0), beta2(0.25); // lambda updates multiplier values, beta1 > 1 > beta2 >0
    uint_t iters = 0;
    matData_t previous_error = calculate_error();
    bool improvement; // variable for controlling when no update is done and number of iterations is exceeded.
    do
    {
        iters++;
        // 1) solve the current subproblem by Newton Raphson
        this->bookkeep_state();
        optimize_newton_raphson_one_iteration(true);
        auto current_error = calculate_error();
        //std::cout << "iter " << iters << ", error = " << current_error << ", lambda = "<< lambda_ << std::endl;
        auto diff_error = previous_error - current_error;
        improvement = true;

        // 2) Check for convergence, hillclimb
        if (diff_error < 0)
        {
            //std::cout << "no improvement\n";
            lambda_ *= beta1;
            this->update_state_from_bookkeep();
            improvement = false;
            continue;
        }
        previous_error = current_error;

        // 2.1) check for convergence, terminal
        if (diff_error < solutionTolerance_)
            return iters;

        // 3 Fidelity of the quadratized model vs non-linear error evaluation.
        // f = err(x_k) - err(x_k + dx)  ( >0 if upgrade)
        //     err(x_k) - m_k(dx)
        // where m_k is the quadratized model m_k(dx) = err(x_k) + dx'*Grad r + 0.5 dx'(Hessian + LM)dx
        // => f = d err / (-dx'*Grad r - 0.5 dx'(Hessian + LM)dx)
        //matData_t modelFidelity = diff_error / (-dx_.dot(gradient_) - 0.5*dx_.dot(hessian_* dx_));
        matData_t modelFidelity = calculate_model_fidelity(diff_error);

        // 4) update lambda
        if (modelFidelity < sigma1)
            lambda_ *= beta1;
        if (modelFidelity > sigma2)
            lambda_ *= beta2;

    }while(iters < max_iters_);

    if (!improvement)
    {
        this->update_state_from_bookkeep();//If no improvement shown, undo again
    }

    // output
    std::cout << "Optimizer::optimize_levenberg_marquardt: failed to converge after "
              << iters << " iterations and error " << calculate_error()
              << std::endl;

    return iters;
}

OptimizerDense::OptimizerDense(matData_t solutionTolerance, matData_t lambda):
        Optimizer(solutionTolerance, lambda)
{

}
OptimizerDense::~OptimizerDense()
{

}

matData_t OptimizerDense::calculate_model_fidelity(matData_t diff_error)
{

    return diff_error / (-dx_.dot(gradient_) - 0.5*dx_.dot(hessian_* dx_));
}


#include <shark/Algorithms/Trainers/LinearRegression.h>
#include <shark/Models/LinearModel.h>
#include <boost/archive/polymorphic_binary_iarchive.hpp>
#include <boost/archive/polymorphic_binary_oarchive.hpp>
using namespace shark;

double func(double x) {
  return 4. + 0.3 * x;  // line coeficients
}

std::pair<Data<RealVector>, Data<RealVector>> GenerateData(size_t n) {
  std::vector<RealVector> x_data(n);
  std::vector<RealVector> y_data(n);

  std::random_device rd;
  std::mt19937 re(rd());
  std::uniform_real_distribution<double> dist(-1.5, 1.5);

  // generate data
  RealVector x_v(1);  // it's a typdef to remora::vector<float>
  RealVector y_v(1);
  for (size_t i = 0; i < n; ++i) {
    x_v(0) = i;
    x_data[i] = x_v;

    y_v(0) = func(i) + dist(re);  // add noise
    y_data[i] = y_v;
  }

  return {createDataFromRange(x_data), createDataFromRange(y_data)};
}

int main() {
  {
    Data<RealVector> x;
    Data<RealVector> y;
    std::tie(x, y) = GenerateData(1000);
    RegressionDataset data(x, y);
    LinearModel<> model;
    LinearRegression trainer;
    trainer.train(model, data);

    std::ofstream ofs("shark-linear.dat");
    // boost::archive::polymorphic_text_oarchive oa(ofs);
    boost::archive::polymorphic_binary_oarchive oa(ofs);
    model.write(oa);
  }

  std::ifstream ifs("shark-linear.dat");
  // boost::archive::polymorphic_text_iarchive ia(ifs);
  boost::archive::polymorphic_binary_iarchive ia(ifs);
  LinearModel<> model;
  model.read(ia);

  std::cout << "Target values: \n";
  std::vector<RealVector> new_x_data;
  for (size_t i = 0; i < 5; ++i) {
    new_x_data.push_back({static_cast<double>(i)});
    std::cout << func(i) << std::endl;
  }
  auto prediction = model(createDataFromRange(new_x_data));
  std::cout << "Predictions: \n" << prediction << std::endl;

  return 0;
}


//File: homography.cc
//Author: Yuxin Wu <ppwwyyxx@gmail.com>

#include "homography.hh"

#include <Eigen/Dense>
#include <vector>

#include "lib/matrix.hh"
#include "lib/polygon.hh"
#include "match_info.hh"

using namespace std;

namespace {
inline Eigen::Map<Eigen::Matrix<double, 3, 3, Eigen::RowMajor>>
	to_eigenmap(const pano::Homography& m) {
		return Eigen::Map<Eigen::Matrix<double, 3, 3, Eigen::RowMajor>>(
				(double*)m.data, 3, 3);
	}
}

namespace pano {

Homography Homography::inverse(bool* succ) const {
	using namespace Eigen;
	Homography ret;
	auto res = to_eigenmap(ret),
			 input = to_eigenmap(*this);
	FullPivLU<Eigen::Matrix<double,3,3,RowMajor>> lu(input);
	if (succ == nullptr) {
		m_assert(lu.isInvertible());
	} else {
		*succ = lu.isInvertible();
		if (! *succ) return ret;
	}
	res = lu.inverse().eval();
	return ret;
}

Homography Homography::operator * (const Homography& r) const {
	Homography ret;
	auto m1 = to_eigenmap(*this),
			 m2 = to_eigenmap(r),
			 res = to_eigenmap(ret);
	res = m1 * m2;
	return ret;
}

std::vector<Vec2D> overlap_region(
		const Shape2D& shape1, const Shape2D& shape2,
		const Matrix& homo, const Homography& inv) {
	// use sampled edge points, rather than 4 corner, to deal with distorted homography
	// for distorted homography, the range of projected z coordinate contains 0
	// or equivalently, some point is projected to infinity
	const int NR_POINT_ON_EDGE = 100;
	Matrix edge_points(3, 4 * NR_POINT_ON_EDGE);
	float stepw = shape2.w * 1.0 / NR_POINT_ON_EDGE,
				steph = shape2.h * 1.0 / NR_POINT_ON_EDGE;
	REP(i, NR_POINT_ON_EDGE) {
		Vec2D p{-shape2.halfw() + i * stepw, -shape2.halfh()};
		edge_points.at(0, i * 4) = p.x, edge_points.at(1, i * 4) = p.y;
		p = Vec2D{-shape2.halfw() + i * stepw, shape2.halfh()};
		edge_points.at(0, i * 4 + 1) = p.x, edge_points.at(1, i * 4 + 1) = p.y;
		p = Vec2D{-shape2.halfw(), -shape2.halfh() + i * steph};
		edge_points.at(0, i * 4 + 2) = p.x, edge_points.at(1, i * 4 + 2) = p.y;
		p = Vec2D{shape2.halfw(), -shape2.halfh() + i * steph};
		edge_points.at(0, i * 4 + 3) = p.x, edge_points.at(1, i * 4 + 3) = p.y;
	}
	REP(i, 4 * NR_POINT_ON_EDGE)
		edge_points.at(2, i) = 1;
	auto transformed_pts = homo * edge_points;	//3x4n
	vector<Vec2D> pts2in1;
	REP(i, 4 * NR_POINT_ON_EDGE) {
		float denom = 1.0 / transformed_pts.at(2, i);
		Vec2D pin1{transformed_pts.at(0, i) * denom, transformed_pts.at(1, i) * denom};
		if (shape1.shifted_in(pin1))
			pts2in1.emplace_back(pin1);
	}

	// also add 4 corner of 1 to build convex hull, in case some are valid
	auto corners = shape1.shifted_corner();
	for (auto& c : corners) {
		Vec2D cin2 = inv.trans2d(c);
		if (shape2.shifted_in(cin2))
			pts2in1.emplace_back(c);
	}
	auto ret = convex_hull(pts2in1);
	return ret;
}

}


#include <CGAL/Cartesian.h>
#include <CGAL/Point_2.h>
#include <CGAL/Segment_2.h>
#include <CGAL/intersections.h>
#include <CGAL/tuple.h>
#include <CGAL/point_generators_2.h>
#include <CGAL/Join_input_iterator.h>
#include <CGAL/iterator.h>

#include <vector>
#include <functional>

#include <boost/variant.hpp>
#include <boost/optional.hpp>
#include <boost/any.hpp>
#include <boost/timer.hpp>
#include <boost/lexical_cast.hpp>

#include <tuple>
#include <functional>
#include <CGAL/Overload.h>

// Intersection_traits
template<typename, typename, typename>
struct Intersection_traits;

template<typename K>
struct Intersection_traits<K, typename K::Segment_2, typename K::Segment_2> {
  typedef typename boost::variant<typename K::Segment_2, typename K::Point_2 > variant_type;
  typedef typename boost::optional< variant_type > result_type;
};

template <class K, class OutputIterator>
OutputIterator intersect_do_iterator(const typename K::Segment_2 &seg1,
                           const typename K::Segment_2 &seg2,
                           const K&, OutputIterator o) {
  typedef CGAL::internal::Segment_2_Segment_2_pair<K> is_t;

  is_t ispair(&seg1, &seg2);
  switch (ispair.intersection_type()) {
  case is_t::NO_INTERSECTION:
  default:
    return o;
  case is_t::POINT:
    *o++ = ispair.intersection_point();
    return o;
  case is_t::SEGMENT:
    *o++ = ispair.intersection_segment();
    return o;
  }
}

template <class K>
boost::optional<
  boost::variant<typename K::Segment_2, typename K::Point_2>
  >
intersection_variant(const typename K::Segment_2 &seg1,
             const typename K::Segment_2 &seg2,
             const K&)
{
  typedef CGAL::internal::Segment_2_Segment_2_pair<K> is_t;

  typedef boost::variant<typename K::Segment_2, typename K::Point_2> Variant;
  typedef boost::optional<Variant> OptVariant;

  is_t ispair(&seg1, &seg2);
  switch (ispair.intersection_type()) {
  case is_t::NO_INTERSECTION:
  default:
    return OptVariant();
  case is_t::POINT:
    return OptVariant(ispair.intersection_point());
  case is_t::SEGMENT:
    return OptVariant(ispair.intersection_segment());
  }
}

template <class K>
boost::any intersection_any(const typename K::Segment_2 &seg1,
                            const typename K::Segment_2 &seg2,
                            const K&)
{
  typedef CGAL::internal::Segment_2_Segment_2_pair<K> is_t;

  is_t ispair(&seg1, &seg2);
  switch (ispair.intersection_type()) {
  case is_t::NO_INTERSECTION:
  default:
    return boost::any();
  case is_t::POINT:
    return boost::any(ispair.intersection_point());
  case is_t::SEGMENT:
    return boost::any(ispair.intersection_segment());
  }
}

using namespace CGAL;

typedef Cartesian<double>    K;
typedef K::Point_2           Point;
typedef Creator_uniform_2<double,Point>  Pt_creator;
typedef K::Segment_2         Segment;
typedef std::vector<Segment> Vector;

template<typename F>
void intersect_each(F f, const Vector& segs) {
  for(Vector::const_iterator it = segs.begin(); it != segs.end(); ++it) {
      const Segment& seg_1 = *it;
      for(Vector::const_iterator it2 = segs.begin(); it2 != segs.end(); ++it2) {
        f(seg_1, *it2);
      }
  }
}

struct Vec_holder {
  Vec_holder(std::vector<Point>* p, std::vector<Segment>* s) : p(p), s(s) { }
protected:
  std::vector<Point>* p;
  std::vector<Segment>* s;
};

struct Visitor : public boost::static_visitor<>, Vec_holder
{
  Visitor(std::vector<Point>* p, std::vector<Segment>* s) :
    Vec_holder(p, s) { }

  void operator()(const Point& point) { p->push_back(point);  }
  void operator()(const Segment& segment) { s->push_back(segment);  }
};

struct Variant_f {
  Variant_f(std::vector<Point>* p, std::vector<Segment>* s) : v(p, s)
    { }
  typedef Intersection_traits<K, Segment, Segment> Traits;
  typedef Traits::result_type result_type;

  Visitor v;
  void operator()(const Segment& s1, const Segment& s2) {
    result_type obj = intersection_variant(s1, s2, K());
    if(obj) {
      boost::apply_visitor(v, *obj);
    }
  }
};

struct Object_f : Vec_holder {
  Object_f(std::vector<Point>* p, std::vector<Segment>* s) :
    Vec_holder(p, s) { }

  void operator()(const Segment& s1, const Segment& s2) {
    Object obj = intersection(s1, s2);
      if (const Point * point = object_cast<Point>(&obj)) {
        p->push_back(*point);
      } else if (const Segment * segment = object_cast<Segment>(&obj)) {
        s->push_back(*segment);
      }
  }
};

struct Any_f : Vec_holder {
  Any_f(std::vector<Point>* p, std::vector<Segment>* s) :
    Vec_holder(p, s) { }

  void operator()(const Segment& s1, const Segment& s2) {
    boost::any obj = intersection_any(s1, s2, K());
    if (const Point * point = boost::any_cast<Point>(&obj)) {
       p->push_back(*point);
    } else if (const Segment * segment = boost::any_cast<Segment>(&obj)) {
      s->push_back(*segment);
    }
  }
};

struct Object_from_variant_f : Vec_holder {
  Object_from_variant_f(std::vector<Point>* p, std::vector<Segment>* s) :
    Vec_holder(p, s) { }

  void operator()(const Segment& s1, const Segment& s2) {
    Object obj = intersection_variant(s1, s2, K());
      if (const Point * point = object_cast<Point>(&obj)) {
        p->push_back(*point);
      } else if (const Segment * segment = object_cast<Segment>(&obj)) {
        s->push_back(*segment);
      }
  }
};

struct Do_f : Vec_holder {
  Do_f(std::vector<Point>* p, std::vector<Segment>* s) :
    Vec_holder(p, s) { }

  typedef typename std::back_insert_iterator< std::vector<Point> >   Iter1;
  typedef typename std::back_insert_iterator< std::vector<Segment> > Iter2;

  void operator()(const Segment& s1, const Segment& s2) {

    CGAL::Dispatch_or_drop_output_iterator<std::tuple<Point,Segment>,
                                           std::tuple<Iter1,Iter2>
                                           > do_it(std::back_inserter(*p), std::back_inserter(*s));

    intersect_do_iterator(s1, s2, K(), do_it);
  }
};

std::tuple<int, int, int> intersect_each_variant_overload(const Vector& segs) {
  std::tuple<int, int, int> ret = std::make_tuple(0, 0, 0);
  typedef Intersection_traits<K, Segment, Segment> Traits;
  typedef Traits::result_type result_type;

  // Calculate the intersections between each segment
  for(Vector::const_iterator it = segs.begin(); it != segs.end(); ++it) {
    const Segment& seg_1 = *it;
    for(Vector::const_iterator it2 = segs.begin(); it2 != segs.end(); ++it2) {
      result_type obj = intersection_variant(seg_1, *it2, K());
      if(obj) {
         // with c++0x
        auto v = make_overload(
          std::make_tuple(std::function<void(const Segment&)>(
                            [&ret](const Segment& s) { (void)s; ++(std::get<1>(ret)); }),
                          std::function<void(const Point&)>(
                            [&ret](const Point& p) { (void)p; ++(std::get<0>(ret)); })));

        boost::apply_visitor(v, *obj);
      } else {
        ++(std::get<2>(ret));
      }
    }
  }

  return ret;
}

int main(int argc, char* argv[]) {
  int repeats = 100;
  int seg_count = 200;

  if(argc > 1)
    repeats = boost::lexical_cast<int>(argv[1]);
  if(argc > 2)
    seg_count = boost::lexical_cast<int>(argv[2]);

  // Create test segment set. Prepare a vector for 200 segments.
  Vector segs;
  segs.reserve(200);

  // Prepare point generator for the horizontal segment, length 200.
  typedef Random_points_on_segment_2<Point,Pt_creator> P1;
  P1 p1( Point(-100,0), Point(100,0));

  // Prepare point generator for random points on circle, radius 250.
  typedef Random_points_on_circle_2<Point,Pt_creator> P2;
  P2 p2( 250);

  // Create segments.
  typedef Creator_uniform_2< Point, Segment> Seg_creator;
  typedef Join_input_iterator_2< P1, P2, Seg_creator> Seg_iterator;
  Seg_iterator g( p1, p2);
  CGAL::copy_n( g, seg_count, std::back_inserter(segs));

  std::vector<Point> points;
  std::vector<Segment> segments;
  points.clear(); segments.clear(); points.reserve(0); segments.reserve(0);
  //one run to get the size
  intersect_each(Variant_f(&points, &segments), segs);

  boost::timer timer;

  // variant vs object vs any

  points.clear(); segments.clear();
  timer.restart();

  for(int i = 0; i < repeats; ++i) {
    intersect_each(Object_f(&points, &segments), segs);
    points.clear(); segments.clear();
  }
  std::cout << "Time for object: " << timer.elapsed() << '\n';

  points.clear(); segments.clear();
  timer.restart();

  for(int i = 0; i < repeats; ++i) {
    intersect_each(Variant_f(&points, &segments), segs);
    points.clear(); segments.clear();
  }
  std::cout << "Time for variant: " << timer.elapsed() << '\n';

  points.clear(); segments.clear();
  timer.restart();

  for(int i = 0; i < repeats; ++i) {
    intersect_each(Any_f(&points, &segments), segs);
    points.clear(); segments.clear();
  }
  std::cout << "Time for any: " << timer.elapsed() << '\n';

  points.clear(); segments.clear();
  timer.restart();

  for(int i = 0; i < repeats; ++i) {
    intersect_each(Object_from_variant_f(&points, &segments), segs);
    points.clear(); segments.clear();
  }
  std::cout << "Time for object_from_variant: " << timer.elapsed() << '\n';

  points.clear(); segments.clear();
  timer.restart();

  for(int i = 0; i < repeats; ++i) {
    intersect_each(Do_f(&points, &segments), segs);
    points.clear(); segments.clear();
  }
  std::cout << "Time for dispatch_output: " << timer.elapsed() << '\n';

  std::cout << std::flush;
}


#include <mex.h>
#include <math.h>
#include <Eigen/Dense>
#include <Eigen/Sparse>
#include <iostream>
#include <vector>
#include <time.h>

using namespace Eigen;
using namespace std;

typedef Triplet<double> eigen_entry;
typedef vector<eigen_entry> entry_list;

double get_Fischer_Burmeister(VectorXd &x, VectorXd &pj, int num)
{

    double fb = 0;

    for (int i = 0; i < num; i++)
    {
        double ent = x[i] + pj[i] - sqrt(x[i] * x[i] + pj[i] * pj[i]);
        fb += ent * ent;
    }

    return sqrt(fb);

}

void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
{

    mxArray *output_mex;

    double *J_index, *JT_index, *J_value, *JT_value, *JTJ_info, *pu, *wu, *bu, *max_iter, *step_size;
    double *output;

    J_index = mxGetPr(prhs[0]);
    JT_index = mxGetPr(prhs[1]);
    J_value = mxGetPr(prhs[2]);
    JT_value = mxGetPr(prhs[3]);
    JTJ_info = mxGetPr(prhs[4]);
    pu = mxGetPr(prhs[5]);
    wu = mxGetPr(prhs[6]);
    bu = mxGetPr(prhs[7]);
    max_iter = mxGetPr(prhs[8]);
    step_size = mxGetPr(prhs[9]);

    // JTJ_info : dof_n, max_valence, tri_n, 3 * 2

    int J_row = JTJ_info[0];
    int J_col = JTJ_info[2];
    int max_it = max_iter[0];
    double step = step_size[0];

    output_mex = plhs[0] = mxCreateDoubleMatrix(J_row, 1, mxREAL);

    output = mxGetPr(output_mex);

    SparseMatrix<double> J_u = SparseMatrix<double>(J_row, J_col);
    SparseMatrix<double> JT_u = SparseMatrix<double>(J_col, J_row);
    VectorXd b_u = VectorXd::Zero(J_col);

    entry_list en_list;

    for(int i = 0; i < JTJ_info[0]; i++)
    {
        for(int j = 0; j < JTJ_info[1]; j++)
        {
            int index = i * JTJ_info[1] + j;

            if(J_index[index] >= 0)
            {
                en_list.push_back(eigen_entry(i, J_index[index], J_value[index]));
            }
        }
    }

    J_u.setFromTriplets(en_list.begin(), en_list.end());

    en_list.clear();

    for(int i = 0; i < JTJ_info[2]; i++)
    {
        for(int j = 0; j < JTJ_info[3]; j++)
        {
            int index = i * JTJ_info[3] + j;

            if(JT_index[index] >= 0)
            {
                en_list.push_back(eigen_entry(i, JT_index[index], JT_value[index]));
            }
        }
    }

    JT_u.setFromTriplets(en_list.begin(), en_list.end());

    VectorXd p_u = VectorXd::Zero(J_row);

    for(int i = 0; i < J_row; i++)
    {
        p_u[i] = pu[i];
    }

    for(int i = 0; i < J_col; i++)
    {
        b_u[i] = bu[i];
    }

    VectorXd b = b_u + JT_u * p_u;

    VectorXd lambda = VectorXd::Zero(J_col);

    VectorXd pi;

    double pre_FB, post_FB;

    int iteration = -1;

    ///////////////////////////////////////////////////////////////////////////////////////////////

    pi = JT_u * (J_u * lambda) + b;

    post_FB = get_Fischer_Burmeister(lambda, pi, J_col);

    for(int outer = 0; outer < max_it; outer++)
    {
        iteration = outer;

        pre_FB = post_FB;

        for(int iter = 0; iter < J_col; iter++)
        {
            lambda[iter] = fmax(lambda[iter] - step * pi[iter] / wu[iter], 0.0);
        }

        pi = JT_u * (J_u * lambda) + b;

        post_FB = get_Fischer_Burmeister(lambda, pi, J_col);

				if(abs(post_FB) < 1e-6)
				{
					break;
				}

        if(abs(pre_FB - post_FB) < 1e-3 * abs(pre_FB))
        {
            break;
        }
    }

    VectorXd p_mod = J_u * lambda;

    for(int i = 0; i < J_row; i++)
    {
        output[i] = p_mod[i];
    }

    return;

}


#include <Eigen/Core>
#include <Eigen/Eigenvalues>

    #include <iostream>

    #include <iomanip>
    #include "riemann_filter.h"

    bool debug=0;
    bool debug2=0;

    #define halfpi 3.141593f/2.f
    #define c_speed  299792458
    #define max_nop 8

using namespace std;
using namespace Eigen;

namespace {

constexpr float b = 1.f;
constexpr float d = 1.e4f;
int nop;

typedef Matrix<float, Dynamic, Dynamic, 0, max_nop, max_nop> MatrixNf;
typedef Matrix<float, Dynamic, Dynamic, 0, 2*max_nop, 2*max_nop> Matrix2Nf;
typedef Matrix<float, Dynamic, Dynamic, 0, 3*max_nop, 3*max_nop> Matrix3Nf;
typedef Matrix<float, Dynamic, 1, 0, max_nop, 1> VectorNf;
typedef Matrix<float, Dynamic, 1, 0, 2*max_nop, 1> Vector2Nf;
typedef Matrix<float, Dynamic, 1, 0, 3*max_nop, 1> Vector3Nf;
typedef Matrix<float, 2, Dynamic, 0, 2, max_nop> Matrix2xNf;
typedef Matrix<float, 3, Dynamic, 0, 3, max_nop> Matrix3xNf;
typedef Matrix<float, 5, 5> Matrix5f;
typedef Matrix<float, 5, 1> Vector5f;

struct circle_fit{
Vector3f par;
Matrix3f cov;
int charge;
float chi2;
};

struct line_fit{
Vector2f par;
Matrix2f cov;
float chi2;
};

struct helix_fit{
Vector5f par;
Matrix5f cov;
int charge;
float chi2;
};

struct scatter{
    float p;
    float theta;
    float X;
};

inline float sqr(float a){
return a*a;
}

//only barrel ! TO FIX
MatrixNf Scatter_cov_rad (Matrix2xNf p2D, scatter MS){
    VectorNf rad = (p2D.row(0).array().square() + p2D.row(1).array().square()).sqrt();
    MatrixNf scatter_cov_rad = MatrixXf::Zero(nop,nop);
    float sig2 = sqr(0.015/MS.p *sqr(MS.X/cos(MS.theta))*(1+0.038*log(MS.X/cos(MS.theta))));
    for(int k=0; k<nop; k++){
        for(int l=k; l<nop; l++){
            for(int i=0; i<min(k,l); i++){
                scatter_cov_rad(k,l) += (rad(k)-rad(i))*(rad(l)-rad(i))*sig2/sqr(sin(MS.theta));
                scatter_cov_rad(l,k) = scatter_cov_rad(k,l);
            }
        }
    }
    return scatter_cov_rad;
}

Matrix2Nf cov_radtocart (Matrix2xNf p2D, MatrixNf cov_rad){
    Matrix2Nf cov_cart=MatrixXf::Zero(2*nop,2*nop);
    VectorNf rad = (p2D.row(0).array().square() + p2D.row(1).array().square()).sqrt();
    for(int k=0; k<nop; k++){
        for(int l=k; l<nop; l++){
            cov_cart(k,l) = cov_rad(k,l)*p2D(1,k)/rad(k)*p2D(1,l)/rad(l);
            cov_cart(k+nop,l+nop) = cov_rad(k,l)*p2D(0,k)/rad(k)*p2D(0,l)/rad(l);
            cov_cart(k,l+nop) = -cov_rad(k,l)*p2D(1,k)/rad(k)*p2D(0,l)/rad(l);
            cov_cart(k+nop,l) = -cov_rad(k,l)*p2D(0,k)/rad(k)*p2D(1,l)/rad(l);
            cov_cart(l,k) = cov_cart(k,l);
            cov_cart(l+nop,k+nop) = cov_cart(k+nop,l+nop);
            cov_cart(l+nop,k) = cov_cart(k,l+nop);
            cov_cart(l,k+nop) = cov_cart(k+nop,l);
        }
    }
    return cov_cart;
}

MatrixNf cov_carttorad (Matrix2xNf p2D, Matrix2Nf cov_cart){ //only diagonal terms
    MatrixNf cov_rad=MatrixXf::Zero(nop,nop);
    VectorNf rad2 = p2D.row(0).array().square() + p2D.row(1).array().square();
    for(int i=0; i<nop; i++){
        cov_rad(i,i) = (cov_cart(i,i)*sqr(p2D(1,i)) + cov_cart(i+nop,i+nop)*sqr(p2D(0,i))
            - 2.f*cov_cart(i,i+nop)*p2D(0,i)*p2D(1,i))/rad2(i);
        if(rad2(i)<1e-4) cov_rad(i,i) = cov_cart(i,i); //TO FIX
    }
    return cov_rad;
}

VectorNf Weight(Matrix2xNf p2D, MatrixNf cov_rad_inv){
    VectorNf weight(nop);
    for(int i=0; i<nop; i++){
        weight(i) = cov_rad_inv.col(i).sum();
    }
    return weight;
}

float chi2_circle(Matrix2xNf p2D, Matrix2Nf V, Vector3f par_uvr){
    float chi2 = 0;
    for(int i=0; i<nop; i++){
        float x_ = p2D(0,i)-par_uvr(0);
        float y_ = p2D(1,i)-par_uvr(1);
        float x_2 = sqr(x_);
        float y_2 = sqr(y_);
        chi2 += sqr(sqrt(x_2+y_2)-par_uvr(2))/((V(i,i)*x_2+V(i+nop,i+nop)*y_2+2*V(i,i+nop)*x_*y_)/(x_2+y_2));
    }
    return chi2;
}

float chi2_line(Matrix2xNf p2D, VectorNf y_err2, Vector2f par_line){
    float chi2 = 0;
        for(int i=0; i<nop; i++){
        chi2 += sqr(p2D(1,i)-p2D(0,i)*par_line(0)-par_line(1)) / (1+sqr(par_line(0)))
                / (y_err2(i)*sqr(cos(atan(par_line(0)))) + y_err2(i)*sqr(sin(atan(par_line(0)))));
    }
    return chi2;
}

inline int Charge (Matrix2xNf p2D, Vector3f par_uvr) { //error to be computed TO FIX
    float dir = (p2D(0,1)-p2D(0,0))*(par_uvr(1)-p2D(1,0))-(p2D(1,1)-p2D(1,0))*(par_uvr(0)-p2D(0,0));
    return (dir > 0) ? -1 : 1;
}

Vector3f par_transformation(Vector3f par_uvr, int charge, float B_field){
    Vector3f par_pak;
    float phi = (charge > 0) ? atan2(par_uvr(0), -par_uvr(1)) : atan2(-par_uvr(0), par_uvr(1));
    par_pak <<  phi,
                charge * (sqrt(sqr(par_uvr(0))+sqr(par_uvr(1)))-par_uvr(2)),
                par_uvr(2)*B_field;
    return par_pak;
}

// return the eigenvector associated to the minimum eigenvalue
Vector3f min_eigen3D(Matrix3f A){
    EigenSolver<Matrix3f> solver(A); // evaluate eigenvalues and eigenvector
    Vector3cf lambdac = solver.eigenvalues(); //why can't I cast here ??
    Matrix3cf eigenvectc = solver.eigenvectors();
    Vector3f lambda = lambdac.real().cast<float>(); //check if real ! TO FIX
    Matrix3f eigenvect = eigenvectc.real().cast<float>();
    int minindex =0;
    lambda.minCoeff(&minindex);
    Vector3f n = eigenvect.col(minindex);
    return n;
}

Vector2f min_eigen2D(Matrix2f A){
    EigenSolver<Matrix2f> solver(A); // evaluate eigenvalues and eigenvector
    Vector2cf lambdac = solver.eigenvalues(); //why can't I cast here ??
    Matrix2cf eigenvectc = solver.eigenvectors();
    Vector2f lambda = lambdac.real().cast<float>(); //check if real ! TO FIX
    Matrix2f eigenvect = eigenvectc.real().cast<float>();
    int minindex =0;
    lambda.minCoeff(&minindex);
    Vector2f n = eigenvect.col(minindex);
    return n;
}

//     a       ||   0|   1|   2|   3|   4|   5
// nu(a)=(i,j) || 0,0| 0,1| 0,2| 1,1| 1,2| 2,2
inline int nu(int a, int* i, int* j){
    *i = (a==0 || a==1 || a==2) ? 0 : (a==3 || a==4) ? 1 : (a==5) ? 2 : 3;
    *j = (a==2 || a==4 || a==5) ? 2 : (a==1 || a==3) ? 1 : (a==0) ? 0 : 3;
    if (*i == 3) return 1;
    return 0;
}

circle_fit Circle_fit(Matrix2xNf p2D, Matrix2Nf V, float B_field,
                      bool return_err = true, bool scattering=true, scatter* MS=nullptr){

    //INITIALIZATION

    //SORTING !! TO FIX
    Matrix2xNf p2D_ = p2D;
    Matrix3xNf p3D(3,nop);

    MatrixNf cov_rad = cov_carttorad(p2D_, V);
    if(scattering && MS != nullptr){
        MatrixNf scatter_cov_rad = Scatter_cov_rad(p2D_, *MS);
        V += cov_radtocart(p2D_, scatter_cov_rad);
        cov_rad += scatter_cov_rad;
    }
    MatrixNf G = cov_rad.inverse();
    G /= G.sum();
    VectorNf weight = Weight(p2D_, G);
    if(debug){
        cout << "cov_rad:\n" << cov_rad << endl << endl;
        cout << "G:\n" << G << endl << endl;
        cout << "weight:\n" << weight.transpose() << endl;
    }

    //CENTER & SCALE 2D POINTS
    float umean = p2D.row(0).mean();
    float vmean = p2D.row(1).mean();
    p2D.row(0) = p2D.row(0).array() - umean;
    p2D.row(1) = p2D.row(1).array() - vmean;
    Vector2Nf mc(2*nop);
    mc << p2D.row(0).transpose(), p2D.row(1).transpose(); //useful for error propagation
    float q = p2D.array().square().sum();
    float s = b*sqrt(nop/q); //scaling factor (b is an arbitrary constant)
    p2D *= s;

    //CALCULATE TRASFORMED POINTS IN 3D
    p3D.block(0,0,2,nop) = p2D;
    p3D.row(2) = p2D.row(0).array().square() + p2D.row(1).array().square();

    //CALCULATE & MINIMIZE COST FUNCTION
    Matrix3f A = Matrix3f::Zero();
    Vector3f r0 = p3D * weight;

    Matrix3xNf temp = p3D - r0*RowVectorXf::Constant(nop,1.f);
    A = temp*G*temp.transpose();

    Vector3f n = min_eigen3D(A);
    n *= (n(2)>0) ? 1 : -1;
    float c = -n.transpose()*r0;

    //CALCULATE CIRCUMFERENCE PARAMETER
    Vector3f par_uvr_;
        par_uvr_ << -n(0)/(2.f*n(2)),
                    -n(1)/(2.f*n(2)),
                    sqrt((1.f-sqr(n(2))-4.f*c*n(2))/(4.f*sqr(n(2))));
    Vector3f par_uvr;
        par_uvr <<  par_uvr_(0)/s + umean, par_uvr_(1)/s + vmean, par_uvr_(2)/s;

    circle_fit circle;
    circle.charge = Charge(p2D_, par_uvr);
    circle.par = par_transformation(par_uvr, circle.charge, B_field);
    if(!scattering || (scattering && MS!=nullptr))
        circle.chi2 = chi2_circle(p2D_, V, par_uvr);

    return circle;
}

line_fit Line_fit (Matrix3xNf p3D, Matrix3Nf V, circle_fit circle,
                   float B_field, bool return_err=true){

    //INITIALIZATION
    Matrix2xNf p2D(2,nop);

    //VectorNf x_err2 = X_err(p3D, V, circle);
    VectorNf y_err2 = V.block(2*nop,2*nop,nop,nop).diagonal();
    //float k = x_err2.array().sqrt().mean()/y_err2.array().sqrt().mean();
    //if(debug) cout << "k:  " <<  k <<endl;

    VectorNf weight = 1.f/(y_err2).array();
    weight /= weight.sum();

    //CALCULATE TRASFORMED POINTS IN 2D
    p2D.row(1) = p3D.row(2);
    //p2D.row(0) = p3D.row(1);
    Matrix<float, 1, Dynamic> ciccio=((p3D.row(0).array() - cos(circle.par(0)-halfpi)*circle.par(1)).square()
        + (p3D.row(1).array() - sin(circle.par(0)-halfpi)*circle.par(1)).square()).sqrt()
        / (circle.par(2)/B_field*2.f);
        for(int i=0; i<nop; i++){
            if(ciccio(i) < -1) ciccio(i)=-1.f;
            else if(ciccio(i) >1) ciccio(i)=1.f;
        }

    p2D.row(0) = 2.f*asin(ciccio.array())*(circle.par(2)/B_field);

    //CALCULATE & MINIMIZE COST FUNCTION
    Matrix2f A = Matrix2f::Zero();
    Vector2f r0 = p2D * weight;

    for (int i=0; i<nop; i++) A += weight(i)*((p2D.col(i)-r0)*(p2D.col(i)-r0).transpose());

    Vector2f n = min_eigen2D(A);
    float c = -n.transpose()*r0;

    //CALCULATE LINE PARAMETER
    line_fit line;
    line.par << -n(0)/n(1), -c*sqrt(sqr(n(0))+sqr(n(1)))/n(1);
    line.chi2 = chi2_line(p2D, y_err2, line.par);

    //ERROR PROPAGATION
    if(return_err){
        //auxiliary quantities
        float sig2 = y_err2.mean(); //TO FIX
        float S = (A(0,0) + A(1,1))*nop;
        float n0_2 = sqr(n(0));
        float n1_2 = sqr(n(1));
        float sqrt_ = sqrt(n1_2+n0_2);
        float x_ =  p2D.row(0).sum()/nop;
        float y_ =  p2D.row(1).sum()/nop;
        float corr = sqr(1.131); //TO FIX
        float C13 = sig2*n(1)*(n(0)*y_-n(1)*x_)/S;
        float C23 =-sig2*n(0)*(n(0)*y_-n(1)*x_)/S;
        float C33 = corr*sig2*sqr(1/nop + n(0)*y_-n(1)*x_)/S;
        Matrix3f C;
        C <<    sig2*n1_2/S, -sig2*n(0)*n(1)/S, C13,
                -sig2*n(0)*n(1)/S, sig2*n0_2/S, C23,
                C13, C23, C33;

        Matrix<float, 2, 3> J;
        J <<    -1.f/n(1), n(0)/n1_2, 0,
                -c*n(0)/(n(1)*sqrt_), n0_2*c/(n1_2*sqrt_), -sqrt_/n(1);

        line.cov = J * C * J.transpose();
    }

    return line;
}

helix_fit Helix_fit(Matrix3xNf hits, Matrix3Nf hits_cov, float B_field,
                    bool return_err=true, bool scattering=true){
    nop = hits.cols();
    circle_fit circle;
    line_fit line;
    scatter MS;
    if(scattering){
        circle = Circle_fit(hits.block(0,0,2,nop), hits_cov.block(0,0,2*nop,2*nop),
                            B_field, false, true, nullptr);
        line = Line_fit(hits, hits_cov, circle, B_field, false);
        MS.theta = atan(1/line.par(0));
        MS.p = circle.par(2)/cos(MS.theta);
        MS.X = 0.04f;
    }
    circle = Circle_fit(hits.block(0,0,2,nop), hits_cov.block(0,0,2*nop,2*nop),
                        B_field, return_err, scattering, &MS);
    line = Line_fit(hits, hits_cov, circle, B_field, return_err);
    helix_fit helix;
    helix.par << circle.par, line.par;
    helix.cov = MatrixXf::Zero(5,5);
    if(return_err){
        helix.cov.block(0,0,3,3) = circle.cov;
        helix.cov.block(3,3,2,2) = line.cov;
    }
    helix.charge = circle.charge;
    helix.chi2 = circle.chi2 + line.chi2;
    return helix;
}

}

bool havesamephi(Vector3f p1, Vector3f p2, float phiCut){
    Vector2f p1_ = p1.block(0,0,2,1);
    Vector2f p2_ = p2.block(0,0,2,1);
    return (((p1_.transpose()*p2_)/(p1_.norm()*p2_.norm())).norm() > phiCut);
}

bool areAlignedRZ(const float r1, const float z1,const float r2,const float z2,const float r3,const float z3, float ptmin, float thetaCut)
    {
        float radius_diff = std::abs(r1 - r3);
        float distance_13_squared = radius_diff*radius_diff + (z1 - z3)*(z1 - z3);

        float pMin = ptmin*std::sqrt(distance_13_squared); //this needs to be divided by radius_diff later

        float tan_12_13_half_mul_distance_13_squared = fabs(z1 * (r2 - r3) + z2 * (r3 - r1) + z3 * (r1 - r2)) ;
        return tan_12_13_half_mul_distance_13_squared * pMin <= thetaCut * distance_13_squared * radius_diff;
    }

void findTps(const vector< event_t>& events, vector < vector <vector <array <int,4>>>>& tracking_particles){
    for (int event_id=0; event_id <events.size(); event_id++){
        for(unsigned int offset=0; offset<4; offset++){
            int first_lid = offset;
            for(unsigned int i=0; i<events[event_id][first_lid].size(); i++){
                if(events[event_id][first_lid][i].inner_tp == events[event_id][first_lid][i].outer_tp){
                    for(unsigned int j=0; j<events[event_id][first_lid+1].size(); j++){
                        if( events[event_id][first_lid][i].outer_tp == events[event_id][first_lid+1][j].inner_tp &&
                            events[event_id][first_lid+1][j].inner_tp == events[event_id][first_lid+1][j].outer_tp){
                            for(unsigned int k=0; k<events[event_id][first_lid+2].size(); k++){
                                if(events[event_id][first_lid+1][j].outer_tp == events[event_id][first_lid+2][k].inner_tp &&
                                    events[event_id][first_lid+2][k].inner_tp == events[event_id][first_lid+2][k].outer_tp){
                                        tracking_particles[event_id][first_lid].emplace_back(array<int, 4>{{events[event_id][first_lid][i].inner_tp, i, j, k}});
                                    //cout << tracking_particles[event_id][first_lid].back()[0] << endl;
                                }
                            }
                        }
                    }
                }
            }
        }
    }
}

void track_part(const vector <event_t> &events,  vector <vector <vector <array<int,4>>>> &tracking_particles, ofstream & myfile){
    findTps(events, tracking_particles);
    for (int ev_id=0; ev_id<tracking_particles.size(); ev_id++){

        for(unsigned int offset=0; offset<4; offset++){
            int first_lid = offset;
             sort(tracking_particles[ev_id][offset].begin(),tracking_particles[ev_id][offset].end(),[] (const array<int,4>& a, const array<int,4>& b ) { return a[0]< b[0]; } );
             tracking_particles[ev_id][offset].erase(unique(tracking_particles[ev_id][offset].begin(),tracking_particles[ev_id][offset].end(),
                                                            [] (const array<int,4>& a, const array<int,4>& b ) { return a[0]== b[0]; } ),
                                                    tracking_particles[ev_id][offset].end());
                    }
                }

    for (int ev_id=0; ev_id<tracking_particles.size(); ev_id++){
        for(unsigned int offset=0; offset<4; offset++){
            int first_lid = offset;
            for(unsigned int tp=0; tp<tracking_particles[ev_id][offset].size(); tp++){
             myfile << ev_id << "  " << first_lid << " " << tracking_particles[ev_id][offset][tp][0] << endl;
            }
        }
    }
}

int main(){
    ofstream myfile("tracking_particles.txt", ofstream::out);
    ofstream comecazzovuoi("fileditesto.txt", ofstream::out);
    vector <event_t> events;
    vector <vector <vector <array<int,4>>>> tracking_particles;
    ciccio("data.csv", events);
    Matrix<float, 3, 7> hit;
    Matrix<float, 21, 21> cov=MatrixXf::Zero(21,21);
    Vector3f zero; zero << 0,0,0;
    unsigned long long int counter=0;
    tracking_particles.resize(events.size());
    for(auto& tp:tracking_particles){
        tp.resize(4);
    }

    track_part(events, tracking_particles, myfile);

 for (auto& ev: events){

        for(unsigned int offset=0; offset<4; offset++){
            int first_lid = offset;
            //int first_lid=0;
            //int i=3194; int j=1363; int k=576;
            for(unsigned int i=0; i<ev[first_lid].size(); i++){
                if(i%10==0){
                unsigned long long int index = (unsigned long long int)i*ev[first_lid+1].size()*ev[first_lid+2].size();
                cout << "processed: " << counter << "    " <<  index << endl;
                }
                float r1 = sqrt(sqr(ev[first_lid][i].point1(0))+sqr(ev[first_lid][i].point1(1)));
                for(unsigned int j=0; j<ev[first_lid+1].size(); j++){
                    if(havesamephi(ev[first_lid][i].point1,ev[first_lid+1][j].point1,0.9f)){
                        float r2 = sqrt(sqr(ev[first_lid+1][j].point1(0))+sqr(ev[first_lid+1][j].point1(1)));

                        for(unsigned int k=0; k<ev[first_lid+2].size(); k++){
                            if(havesamephi(ev[first_lid+1][j].point1,ev[first_lid+2][k].point1,0.9f)){
                                float r3 = sqrt(sqr(ev[first_lid+2][k].point1(0))+sqr(ev[first_lid+2][k].point1(1)));

                                if(areAlignedRZ(r1, ev[first_lid][i].point1(2), r2, ev[first_lid+1][j].point1(2),r3, ev[first_lid+2][k].point1(2),0.5f, 0.01)){

                                    counter++;
                                    hit <<  zero, ev[first_lid][i].point1, ev[first_lid][i].point2, ev[first_lid+1][j].point1,
                                            ev[first_lid+1][j].point2,ev[first_lid+2][k].point1, ev[first_lid+2][k].point2;

                                    cov(0,0)=0.1;
                                    cov(1,1)=0.1;
                                    cov(2,2)=5;
                                    for(int z=0; z < 3; z++){
                                         cov(6*z+3,6*z+3) = ev[first_lid][i].err_point1(z);
                                         cov(6*z+4,6*z+4) = ev[first_lid][i].err_point2(z);
                                         cov(6*z+5,6*z+5) = ev[first_lid+1][j].err_point1(z);
                                         cov(6*z+6,6*z+6) = ev[first_lid+1][j].err_point2(z);
                                         cov(6*z+7,6*z+7) = ev[first_lid+2][k].err_point1(z);
                                         cov(6*z+8,6*z+8) = ev[first_lid+2][k].err_point2(z);
                                    }
                                    cov.array().square();

                                    helix_fit helix = Helix_fit(hit, cov, 3.8*c_speed/pow(10,9)/100, false, false);
                                    //cout << i << " " << j << "  " << k << endl;
                                    if(helix.chi2 < 100){
                                        //cout << ev[first_lid][i].inner_tp   << "   " << helix.chi2 << endl;
                                        comecazzovuoi  << ev[first_lid][i].inner_tp   << "   "
                                                << ev[first_lid][i].outer_tp    << "   "
                                                << ev[first_lid+1][j].inner_tp  << "   "
                                                << ev[first_lid+1][j].outer_tp  << "   "
                                                << ev[first_lid+2][k].inner_tp  << "   "
                                                << ev[first_lid+2][k].outer_tp  << "   " << helix.chi2 << endl;
                                    }
                                }
                            }
                        }
                    }
                }
            }
        }
    }

comecazzovuoi.close();
myfile.close();

    return 0;
}