#include <Eigen/Core>
#include <Eigen/SparseCore>
#include <iostream>
#include <random> // Requires C++ 11

#include <Spectra/SymGEigsSolver.h>
#include <Spectra/MatOp/DenseSymMatProd.h>
#include <Spectra/MatOp/DenseCholesky.h>
#include <Spectra/MatOp/SparseSymMatProd.h>
#include <Spectra/MatOp/SparseCholesky.h>

using namespace Spectra;

#define CATCH_CONFIG_MAIN
#include "catch.hpp"

typedef Eigen::MatrixXd Matrix;
typedef Eigen::VectorXd Vector;
typedef Eigen::SparseMatrix<double> SpMatrix;

// Traits to obtain operation type from matrix type
template <typename MatType>
struct OpTypeTrait
{
    typedef DenseSymMatProd<double> OpType;
};

template <>
struct OpTypeTrait<SpMatrix>
{
    typedef SparseSymMatProd<double> OpType;
};

template <typename MatType>
struct BOpTypeTrait
{
    typedef DenseCholesky<double> OpType;
};

template <>
struct BOpTypeTrait<SpMatrix>
{
    typedef SparseCholesky<double> OpType;
};

// Generate random sparse matrix
SpMatrix sprand(int size, double prob = 0.5)
{
    SpMatrix mat(size, size);
    std::default_random_engine gen;
    gen.seed(0);
    std::uniform_real_distribution<double> distr(0.0, 1.0);
    for(int i = 0; i < size; i++)
    {
        for(int j = 0; j < size; j++)
        {
            if(distr(gen) < prob)
                mat.insert(i, j) = distr(gen) - 0.5;
        }
    }
    return mat;
}

// Generate data for testing
void gen_dense_data(int n, Matrix& A, Matrix& B)
{
    Matrix M = Eigen::MatrixXd::Random(n, n);
    A = M + M.transpose();
    B = M.transpose() * M;
    // To make sure B is positive definite
    B.diagonal() += Eigen::VectorXd::Random(n).cwiseAbs();
}

void gen_sparse_data(int n, SpMatrix& A, SpMatrix& B)
{
    // Eigen solver only uses the lower triangle of A,
    // so we don't need to make A symmetric here.
    A = sprand(n, 0.1);
    B = A.transpose() * A;
    // To make sure B is positive definite
    for(int i = 0; i < n; i++)
        B.coeffRef(i, i) += 0.1;
}

template <typename MatType, int SelectionRule>
void run_test(const MatType& A, const MatType& B, int k, int m, bool allow_fail = false)
{
    typedef typename OpTypeTrait<MatType>::OpType OpType;
    typedef typename BOpTypeTrait<MatType>::OpType BOpType;
    OpType op(A);
    BOpType Bop(B);
    // Make sure B is positive definite and the decomposition is successful
    REQUIRE( Bop.info() == SUCCESSFUL );

    SymGEigsSolver<double, SelectionRule, OpType, BOpType, GEIGS_CHOLESKY> eigs(&op, &Bop, k, m);
    eigs.init();
    int nconv = eigs.compute(100); // maxit = 100 to reduce running time for failed cases
    int niter = eigs.num_iterations();
    int nops  = eigs.num_operations();

    if(allow_fail)
    {
        if( eigs.info() != SUCCESSFUL )
        {
            WARN( "FAILED on this test" );
            std::cout << "nconv = " << nconv << std::endl;
            std::cout << "niter = " << niter << std::endl;
            std::cout << "nops  = " << nops  << std::endl;
            return;
        }
    } else {
        INFO( "nconv = " << nconv );
        INFO( "niter = " << niter );
        INFO( "nops  = " << nops );
        REQUIRE( eigs.info() == SUCCESSFUL );
    }

    Vector evals = eigs.eigenvalues();
    Matrix evecs = eigs.eigenvectors();

    Matrix resid = A.template selfadjointView<Eigen::Lower>() * evecs -
                   B.template selfadjointView<Eigen::Lower>() * evecs * evals.asDiagonal();
    const double err = resid.array().abs().maxCoeff();

    INFO( "||AU - BUD||_inf = " << err );
    REQUIRE( err == Approx(0.0).margin(1e-9) );
}

template <typename MatType>
void run_test_sets(const MatType& A, const MatType& B, int k, int m)
{
    SECTION( "Largest Magnitude" )
    {
        run_test<MatType, LARGEST_MAGN>(A, B, k, m);
    }
    SECTION( "Largest Value" )
    {
        run_test<MatType, LARGEST_ALGE>(A, B, k, m);
    }
    SECTION( "Smallest Magnitude" )
    {
        run_test<MatType, SMALLEST_MAGN>(A, B, k, m, true);
    }
    SECTION( "Smallest Value" )
    {
        run_test<MatType, SMALLEST_ALGE>(A, B, k, m);
    }
    SECTION( "Both Ends" )
    {
        run_test<MatType, BOTH_ENDS>(A, B, k, m);
    }
}

TEST_CASE("Generalized eigensolver of symmetric real matrix [10x10]", "[geigs_sym]")
{
    std::srand(123);

    Matrix A, B;
    gen_dense_data(10, A, B);
    int k = 3;
    int m = 6;

    run_test_sets(A, B, k, m);
}

TEST_CASE("Generalized eigensolver of symmetric real matrix [100x100]", "[geigs_sym]")
{
    std::srand(123);

    Matrix A, B;
    gen_dense_data(100, A, B);
    int k = 10;
    int m = 20;

    run_test_sets(A, B, k, m);
}

TEST_CASE("Generalized eigensolver of symmetric real matrix [1000x1000]", "[geigs_sym]")
{
    std::srand(123);

    Matrix A, B;
    gen_dense_data(1000, A, B);
    int k = 20;
    int m = 50;

    run_test_sets(A, B, k, m);
}

TEST_CASE("Generalized eigensolver of sparse symmetric real matrix [10x10]", "[geigs_sym]")
{
    std::srand(123);

    // Eigen solver only uses the lower triangle
    SpMatrix A, B;
    gen_sparse_data(10, A, B);
    int k = 3;
    int m = 6;

    run_test_sets(A, B, k, m);
}

TEST_CASE("Generalized eigensolver of sparse symmetric real matrix [100x100]", "[geigs_sym]")
{
    std::srand(123);

    // Eigen solver only uses the lower triangle
    SpMatrix A, B;
    gen_sparse_data(100, A, B);
    int k = 10;
    int m = 20;

    run_test_sets(A, B, k, m);
}

TEST_CASE("Generalized eigensolver of sparse symmetric real matrix [1000x1000]", "[geigs_sym]")
{
    std::srand(123);

    // Eigen solver only uses the lower triangle
    SpMatrix A, B;
    gen_sparse_data(1000, A, B);
    int k = 20;
    int m = 50;

    run_test_sets(A, B, k, m);
}


//
// Created by chen-tian on 7/24/17.
//

#include "pose_estimate_3d3d.h"
#include <iostream>
#include <Eigen/Core>
#include <Eigen/SVD>
#include <opencv2/core/core.hpp>
#include <opencv2/features2d/features2d.hpp>

using namespace std;
using namespace cv;

void pose_estimate_3d3d::pose_estimation_3d3d
        (const vector<Point3f> &pts1,
         const vector<Point3f> &pts2,
         Mat &R, Mat &t)
{
    Point3f p1, p2;  //center of mass
    int N = pts1.size();
    for ( int i = 0; i < N; i++ )
    {
        p1 += pts1[i];
        p2 += pts2[i];
    }

    p1 /= N;
    p2 /= N;
    vector<Point3f> q1(N), q2(N);//remove the center
    for ( int i = 0; i < N; i++ )
    {
        q1[i] = pts1[i] - p1;
        q2[i] = pts2[i] - p2;
    }

    //compute q1*q2^T
    Eigen::Matrix3d W = Eigen::Matrix3d::Zero();
    for ( int i = 0; i<N; i++ )
    {
        W += Eigen::Vector3d( q1[i].x, q1[i].y, q1[i].z )* Eigen::Vector3d( q2[i].x, q2[i].y, q2[i].z ).transpose();
    }
    cout<<"W="<<W<<endl;

    //SVD ON W
    Eigen::JacobiSVD<Eigen::Matrix3d> svd(W, Eigen::ComputeFullU|Eigen::ComputeFullV);
    Eigen::Matrix3d U = svd.matrixU();
    Eigen::Matrix3d V = svd.matrixV();
    cout<<"U="<<U<<endl;
    cout<<"V="<<V<<endl;

    Eigen::Matrix3d R_ = U*(V.transpose() );
    Eigen::Vector3d t_ = Eigen::Vector3d( p1.x, p1.y, p1.z ) - R_* Eigen::Vector3d( p2.x, p2.y, p2.z );

    //convert to cv::Mat
    R = (Mat_<double>(3,3)<<
                          R_(0,0), R_(0,1),R_(0,2),
            R_(1,0),R_(1,1),R_(1,2),
            R_(2,0),R_(2,1),R_(2,2) );
    t = ( Mat_<double>(3,1)<< t_(0,0),t_(1,0),t_(2,0));
}


#include <boost/test/unit_test.hpp>
#include "matrix.h"

namespace
{

template<int N> void
test_axis_angle_i(math::vec<3> const &src, math::vec<3> const &axis, math::scalar angle, math::vec<3> const &res)
{
	math::matrix<N,N> m;
	m.rotation(axis, angle);

	math::vec<3> result = src * m;

	BOOST_REQUIRE ((result.length() - src.length()) < math::EPSILON);
	BOOST_REQUIRE ((res - result).length() < math::EPSILON);
}

void test_axis_angle(math::vec<3> const &src, math::vec<3> const &axis, math::scalar angle, math::vec<3> const &res)
{
	test_axis_angle_i<3>(src, axis, angle, res);
	test_axis_angle_i<4>(src, axis, angle, res);

	test_axis_angle_i<3>(res, axis, -angle, src);
	test_axis_angle_i<4>(res, axis, -angle, src);
}

template<int N> void
test_rotation_i(math::vec<3> const &src, math::vec<3> const &angles, math::vec<3> const &res)
{
	math::matrix<N,N> m;
	m.rotation(angles);

	math::vec<3> result = src * m;

	BOOST_REQUIRE ((result.length() - src.length()) < math::EPSILON);
	BOOST_REQUIRE ((res - result).length() < math::EPSILON);
}

void test_rotation(math::vec<3> const &src, math::vec<3> const &angles, math::vec<3> const &res)
{
	test_rotation_i<3>(src, angles, res);
	test_rotation_i<4>(src, angles, res);

	test_rotation_i<3>(res, -angles, src);
	test_rotation_i<4>(res, -angles, src);
}

}

BOOST_AUTO_TEST_SUITE (test_matrix)

BOOST_AUTO_TEST_CASE (test_ijk)
{
	math::vec<3> const i(1, 0, 0), j(0, 1, 0), k(0, 0, 1);

	test_axis_angle(i, j, -math::PI / 2, k);
	test_rotation(i, -math::PI / 2 * j, k);

	test_axis_angle(j, k, -math::PI / 2, i);
	test_rotation(j, -math::PI / 2 * k, i);

	test_axis_angle(k, i, -math::PI / 2, j);
	test_rotation(k, -math::PI / 2 * i, j);
}

BOOST_AUTO_TEST_CASE (test_pi4)
{
	{
		math::vec<3> src(1, 0, 0);
		math::vec<3> res = math::normalize(math::vec<3>(1, -1, 0));

		test_rotation(src, math::vec<3>(0, 0, -math::PI / 4), res);
		test_axis_angle(src, math::vec<3>(0, 0, 1), -math::PI / 4, res);
	}

	{
		math::vec<3> src(0, 1, 0);
		math::vec<3> res = math::normalize(math::vec<3>(1, 1, 0));

		test_rotation(src, math::vec<3>(0, 0, -math::PI / 4), res);
		test_axis_angle(src, math::vec<3>(0, 0, 1), -math::PI / 4, res);
	}
}

BOOST_AUTO_TEST_CASE (test_pi4_number_2)
{
	{
		math::vec<3> src(1, 0, 0);
		math::vec<3> res = math::normalize(math::vec<3>(1, 1, 0));

		test_rotation(src, math::vec<3>(0, 0, math::PI / 4), res);
		test_axis_angle(src, math::vec<3>(0, 0, 1), math::PI / 4, res);
	}

	{
		math::vec<3> src(0, 1, 0);
		math::vec<3> res = math::normalize(math::vec<3>(-1, 1, 0));

		test_rotation(src, math::vec<3>(0, 0, math::PI / 4), res);
		test_axis_angle(src, math::vec<3>(0, 0, 1), math::PI / 4, res);
	}
}

BOOST_AUTO_TEST_CASE (test_matrix_inverse_4x4_1)
{
	math::matrix<4,4> M, I, R, U;

	M.scaling(math::scalar(rand() + 1), math::scalar(rand() + 1), math::scalar(rand() + 1));

	M.inverse(I);

	R = M * I;
	U.identity();

	BOOST_REQUIRE(math::equal(R, U));
}

BOOST_AUTO_TEST_CASE (test_matrix_inverse_4x4_2)
{
	math::matrix<4,4> M, I, R, U;

	M.rotation(rand() * math::PI / RAND_MAX , rand() * math::PI / RAND_MAX, rand() * math::PI / RAND_MAX);

	M.inverse(I);

	R = M * I;
	U.identity();

	BOOST_REQUIRE(math::equal(R, U));
}

BOOST_AUTO_TEST_CASE (test_matrix_inverse_4x4_3)
{
	math::matrix<4,4> M, I, R, U;

	M.translation(rand() * 10.0f / RAND_MAX, rand() * 10.0f / RAND_MAX, rand() * 10.0f / RAND_MAX);

	M.inverse(I);

	R = M * I;
	U.identity();

	BOOST_REQUIRE(math::equal(R, U));
}

BOOST_AUTO_TEST_CASE (test_matrix_inverse_4x4_4)
{
	math::matrix<4,4> M, I, R, U;

	M.translation(rand() * 10.0f / RAND_MAX, rand() * 10.0f / RAND_MAX, rand() * 10.0f / RAND_MAX);
	M.rotate(rand() * math::PI / RAND_MAX , rand() * math::PI / RAND_MAX, rand() * math::PI / RAND_MAX);

	M.inverse(I);

	R = M * I;
	U.identity();

	BOOST_REQUIRE(math::equal(R, U));
}

BOOST_AUTO_TEST_SUITE_END()


// Boost.Geometry (aka GGL, Generic Geometry Library)
// QuickBook Example

// Copyright (c) 2011-2012 Barend Gehrels, Amsterdam, the Netherlands.

// Use, modification and distribution is subject to 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)

//[correct
//` Shows how to correct a polygon with respect to its orientation and closure

#include <iostream>

#include <boost/geometry.hpp>
#include <boost/geometry/geometries/polygon.hpp>
#include <boost/geometry/geometries/adapted/boost_tuple.hpp>

BOOST_GEOMETRY_REGISTER_BOOST_TUPLE_CS(cs::cartesian)

#include <boost/assign.hpp>

int main()
{
    using boost::assign::tuple_list_of;

    typedef boost::geometry::model::polygon
        <
            boost::tuple<int, int>
        > clockwise_closed_polygon;

    clockwise_closed_polygon cwcp;

    // Fill it counterclockwise (so wrongly), forgetting the closing point
    boost::geometry::exterior_ring(cwcp) = tuple_list_of(0, 0)(10, 10)(0, 9);

    // Add a counterclockwise closed inner ring (this is correct)
    boost::geometry::interior_rings(cwcp).push_back(tuple_list_of(1, 2)(4, 6)(2, 8)(1, 2));

    // Its area should be negative (because of wrong orientation)
    //     and wrong (because of omitted closing point)
    double area_before = boost::geometry::area(cwcp);

    // Correct it!
    boost::geometry::correct(cwcp);

    // Check its new area
    double area_after = boost::geometry::area(cwcp);

    // And output it
    std::cout << boost::geometry::dsv(cwcp) << std::endl;
    std::cout << area_before << " -> " << area_after << std::endl;

    return 0;
}

//]

//[correct_output
/*`
Output:
[pre
(((0, 0), (0, 9), (10, 10), (0, 0)), ((1, 2), (4, 6), (2, 8), (1, 2)))
-7 -> 38
]
*/
//]


/*
*   Copyright 2017-2018 Simon Raschke
*
*   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.
*/

#pragma once

#include <cmath>
#include <cstdint>
#include <sstream>
#include <exception>
#if __has_include(<Eigen/Core>)
#include <Eigen/Core>
#elif __has_include(<eigen3/Eigen/Core>)
#include <eigen3/Eigen/Core>
#endif

namespace enhance
{
    // number of digits
    template <typename T>
    std::uint16_t numDigits(T number)
    {
        std::uint16_t digits = 0;
        if (number < 0) digits = 1; // remove this line if '-' counts as a digit
        std::uint32_t helper = static_cast<int>(number);
        while (helper)
        {
            helper /= 10;
            digits++;
        }
        return digits;
    }

    // angle between two eigen vectors
    // vectors dont have to be normalized
    // will give signed result
    template<typename DERIVED1, typename DERIVED2>
    constexpr float directed_angle(const DERIVED1& v1, const DERIVED2& v2)
    {
        assert(std::isfinite(std::atan2(v2.normalized()(1), v2.normalized()(0)) - std::atan2(v1.normalized()(1), v1.normalized()(0))));
        return std::atan2(v2.normalized()(1), v2.normalized()(0)) - std::atan2(v1.normalized()(1), v1.normalized()(0));
    }

    // will give unsigned result
    template<typename DERIVED1, typename DERIVED2>
    constexpr float absolute_angle(const DERIVED1& v1, const DERIVED2& v2)
    {
        return std::abs(directed_angle(v1,v2));
    }

    //directed angle normalized to 0 to 360°
    template<typename DERIVED1, typename DERIVED2>
    constexpr float normalized_angle(const DERIVED1& v1, const DERIVED2& v2)
    {
        const float angle = directed_angle(v1,v2);
        return angle < 0.f ? angle+M_PI : angle;
    }

    // calculate rad from given deg
    // expects floating pioint type
    template<typename T, typename ENABLER = typename std::enable_if<std::is_floating_point<T>::value>::type>
    constexpr T deg_to_rad(const T& __deg) noexcept
    {
        return __deg*M_PI/180;
    }

    // calculate deg from given rad
    // expects floating pioint type
    template<typename T, typename ENABLER = typename std::enable_if<std::is_floating_point<T>::value>::type>
    constexpr T rad_to_deg(const T& __rad) noexcept
    {
        return __rad/M_PI*180;
    }

    // calculates the volume of a sphere given its radius
    // may throw if radius is negative
    template<typename T>
    constexpr T sphere_volume(const T& __rad)
    {
        if(__rad < 0) throw std::logic_error("radius must not be negative");
        return M_PI*__rad*__rad*__rad*4.f/3.f;
    }

    // calculates the surface of a sphere given its radius
    // may throw if radius is negative
    template<typename T>
    constexpr T sphere_surface(const T& __rad)
    {
        if(__rad < 0) throw std::logic_error("radius must not be negative");
        return M_PI*__rad*__rad*4.f;
    }

    // calculates the area of a circle given its radius
    // may throw if radius is negative
    template<typename T>
    constexpr T circle_area(const T& __rad)
    {
        if(__rad < 0) throw std::logic_error("radius must not be negative");
        return M_PI*__rad*__rad;
    }

    // calculates the radius of a circle given its area
    // may throw if area is negative
    template<typename T>
    constexpr T circle_area_to_radius(const T& __area)
    {
        if(__area < 0) throw std::logic_error("area must not be negative");
        return std::sqrt( __area/M_PI );
    }

    // calculates the volume of a cone given its base radius and heigt
    // may throw if area is negative
    // may throw if height is negative
    template<typename T>
    constexpr T cone_volume(const T& __rad, const T& __height)
    {
        if(__rad < 0) throw std::logic_error("radius must not be negative");
        if(__height < 0) throw std::logic_error("height must not be negative");
        return enhance::circle_area(__rad)*__height/(static_cast<T>(3));
    }

    template<std::size_t N, typename T>
    constexpr typename std::enable_if<std::is_floating_point<T>::value,T>::type nth_root(const T& __val )
    {
        return std::pow(__val, 1.0/N);
    }
}


//==================================================================================================
/*!
  @file

  @copyright 2016 NumScale SAS
  @copyright 2016 J.T. Lapreste

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_FUNCTION_LOGSPACE_SUB_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_LOGSPACE_SUB_HPP_INCLUDED

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{

 /*!

    @ingroup group-exponential
    Function object implementing logspace_sub capabilities

     Compute the log of a sum from logs of terms
     properly compute \f$\log (\exp (\log x) - \exp (\log y))\f$

    @par Semantic:

    For every parameters of floating type T:

    @code
    T r = logspace_sub(x, y);
    @endcode

    is similar to:

    @code
    T r =  log(exp(log(x)) - exp(log(y)));
    @endcode

  **/
  const boost::dispatch::functor<tag::logspace_sub_> logspace_sub = {};
} }
#endif

#include <boost/simd/function/scalar/logspace_sub.hpp>
#include <boost/simd/function/simd/logspace_sub.hpp>

#endif


#pragma once

#include <vpp/vpp.hh>
#include <list>
#include <iostream>
#include <iterator>
#include <random>

#include <opencv2/highgui.hpp>
#include <opencv2/videoio.hpp>
#include <vpp/vpp.hh>
#include <vpp/utils/opencv_bridge.hh>
#include <Eigen/Core>

using namespace vpp;
using namespace std;
using namespace cv;
using namespace Eigen;

// Inhomogeneous coordinates
class point_3d
{
public: double x,y,z;

    point_3d()
    {x=y=z=0.0;}

    point_3d(double xx, double yy, double zz)
    {x=xx;y=yy;z=zz;}

    friend ostream & operator << (ostream & os, const point_3d p)
    {
        os<<"["<<p.x<<","<<p.y<<","<<p.z<<"]";
        return os;
    }
};

class Plucker{
public: double c[6];

    Plucker(point_3d p1, point_3d p2)
    {
        c[0]=p2.x-p1.x;
        c[1]=p2.y-p1.y;
        c[2]=p2.z-p1.z;
        c[3]=p1.y*p2.z-p1.z*p2.y;
        c[4]=p2.x*p1.z-p1.x*p2.z;
        c[5]=p1.x*p2.y-p2.x*p1.y;
    }

    friend ostream & operator << (ostream & os, const Plucker l)
    {
        os<<"Plucker Line:["<< l.c[0] <<"," << l.c[1] <<","<<l.c[2]<<","<<l.c[3]<<","<<l.c[4]<<","<<l.c[5]<<"]";
        return os;
    }

}; // Homogeneous coordinates

double plucker_dot_product(Plucker l1, Plucker l2)
{
    double s=0;

    for(int i=0;i<6;i++)
        s+=l1.c[i]*l2.c[(3+i)%6];

    return s;
}

// The same primitive rewritten differently
double plucker_line_intersect(Plucker l1, Plucker l2)
{
    double s;

    s=l1.c[0]*l2.c[3]+l2.c[0]*l1.c[3] +\
            l1.c[1]*l2.c[4]+l2.c[1]*l1.c[4]+\
            l1.c[2]*l2.c[5]+l2.c[2]*l1.c[5];

    return s;
}

double plucker_point_on_line(Plucker l, point_3d p)
{
    double rx, ry, rz, rw;

    rx=l.c[3]*p.x+l.c[4]*p.y+l.c[5]*p.z;
    ry=-l.c[2]*p.y+l.c[1]*p.z+l.c[3];
    rz=-l.c[2]*p.x+l.c[0]*p.z-l.c[4];
    rw=-l.c[1]*p.x+l.c[0]*p.y+l.c[5];

    //cout<<rx<<" "<<ry<< " "<<rz<<" "<<rw<<" "<<endl;
    return fabs(rx)+fabs(ry)+fabs(rz)+fabs(rw);
}


#include "problemes.h"
#include "arithmetique.h"

#include <fstream>

#include <boost/range/adaptor/reversed.hpp>

typedef unsigned long long nombre;
typedef std::vector<nombre> vecteur;

namespace {
    nombre cycle(nombre p, nombre m) {
        nombre n = p, i = 1;
        while (n != 1) {
            ++i;
            n = (n * p) % m;
        }
        return i;
    }
}

ENREGISTRER_PROBLEME(188, "The hyperexponentiation of a number") {
    // The hyperexponentiation or tetration of a number a by a positive integer b, denoted by a↑↑b or ba,
    // is recursively defined by:
    //
    // a↑↑1 = a,
    // a↑↑(k+1) = a(a↑↑k).
    //
    // Thus we have e.g. 3↑↑2 = 33 = 27, hence 3↑↑3 = 327 = 7625597484987 and 3↑↑4 is roughly
    // 10^(3.6383346400240996*10^12).
    //
    // Find the last 8 digits of 1777↑↑1855.
    vecteur masques{100000000};
    while (masques.back() > 2)
        masques.push_back(cycle(1777, masques.back()));

    nombre resultat = 1;
    for (const auto &m : boost::adaptors::reverse(masques))
        resultat = puissance::puissance_modulaire<nombre>(1777, resultat, m);

    return std::to_string(resultat);
}


#include <cstdlib>
#include <cmath>
#include <Eigen/Dense>
#include "nv_core.h"
#include "nv_ml_mlp.h"

// 多層パーセプトロン
// 2 Layer

static float nv_mlp_sigmoid(float a) {
	return 1.0F / (1.0F + expf(-a));
}

// クラス分類

int nv_mlp_predict_label(const nv_mlp_t *mlp, const Eigen::Ref<Eigen::Matrix<float, NV_FACE_HAARLIKE_DIM, 1> > x)
{
	Eigen::Map<Eigen::VectorXf> input_bias(mlp->input_bias->v, mlp->hidden);
	Eigen::Map<Eigen::Matrix<float, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> > input_w(mlp->input_w->v, mlp->hidden, mlp->input);
	Eigen::Map<Eigen::VectorXf> hidden_bias(mlp->hidden_bias->v, mlp->output);
	Eigen::Map<Eigen::Matrix<float, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> > hidden_w(mlp->hidden_w->v, mlp->output, mlp->hidden);
	Eigen::VectorXf y = hidden_w*(input_w*x + input_bias).unaryExpr(&nv_mlp_sigmoid) + hidden_bias;
	int l;
	y.maxCoeff(&l);
	return (y[l] > 0.F) ? l : -1;
}

double nv_mlp_predict_d(const nv_mlp_t *mlp, const Eigen::Ref<Eigen::Matrix<float, NV_FACE_HAARLIKE_DIM, 1> > x)
{
	Eigen::Map<Eigen::VectorXf> input_bias(mlp->input_bias->v, mlp->hidden);
	Eigen::Map<Eigen::Matrix<float, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> > input_w(mlp->input_w->v, mlp->hidden, mlp->input);
	float hidden_bias = *mlp->hidden_bias->v;
	Eigen::Map<Eigen::RowVectorXf > hidden_w(mlp->hidden_w->v, mlp->hidden);
	return 1./(1.+exp(-hidden_w.dot((input_w*x + input_bias).unaryExpr(&nv_mlp_sigmoid)) - hidden_bias));
}

double nv_mlp_bagging_predict_d(const nv_mlp_t **mlp, int nmlp, const Eigen::Ref<Eigen::Matrix<float, NV_FACE_HAARLIKE_DIM, 1> > x)
{
	double p = 0.0F;
	double factor = 1.0 / nmlp;
	int i;

	for (i = 0; i < nmlp; ++i) {
		p += factor * nv_mlp_predict_d(mlp[i], x);
	}

	return p;
}

// 非線形重回帰

void nv_mlp_regression(const nv_mlp_t *mlp, const Eigen::Ref<Eigen::Matrix<float, NV_FACE_HAARLIKE_DIM, 1> >  x, nv_matrix_t *out)
{
	Eigen::Map<Eigen::VectorXf> input_bias(mlp->input_bias->v, mlp->hidden);
	Eigen::Map<Eigen::Matrix<float, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> > input_w(mlp->input_w->v, mlp->hidden, mlp->input);
	Eigen::Map<Eigen::VectorXf> hidden_bias(mlp->hidden_bias->v, mlp->output);
	Eigen::Map<Eigen::Matrix<float, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> > hidden_w(mlp->hidden_w->v, mlp->output, mlp->hidden);
	Eigen::Map<Eigen::VectorXf> y(out->v, out->n);
	y = hidden_w*(input_w*x + input_bias).unaryExpr(&nv_mlp_sigmoid) + hidden_bias;
}


// TestMatrixExcel.cpp
//
// Test output of a matrix in Excel. Here we
// use the Excel Driver object directly.
//
// The output is in cell/numeric format.
//
// (C) Datasim Education BV 2006-2017
//

#include "ExcelDriverlite.hpp"
#include "Utilities.hpp"

#include <boost/numeric/ublas/matrix.hpp>
#include "UtilitiesDJD/VectorsAndMatrices/NestedMatrix.hpp"

#include <string>
#include <vector>
#include <list>

double rho = 0.5;
double NormalPdf2d(double x, double y)
{ // Bivariate normal density function

	double fac = 1.0 / (2.0 * 3.14159265359 * std::sqrt(1.0 - rho*rho));
	double t = x*x - 2.0*rho*x*y + y*y;
	t /= 2.0 * (1.0 - rho*rho);

	return fac * std::exp(-t);
}

template <typename Matrix>
	Matrix DiscreteNormalPdf2d(const std::vector<double>& x, const std::vector<double>& y)
{
	return CreateDiscreteFunction2d<Matrix>(x, y, NormalPdf2d);
}

int main()
{
		//	using NumericMatrix = boost::numeric::ublas::matrix<double>;
		// C++11 syntax
		//using NumericMatrix = NestedMatrix<double>;
		// using Vector = std::vector<double>
		typedef NestedMatrix<double> NumericMatrix;
		typedef std::vector<double> Vector;

		std::size_t N = 10; std::size_t M = 6; // rows and columns
		NumericMatrix matrix(N + 1, M + 1);
		for (std::size_t i = 0; i < matrix.size1(); ++i)
		{
			for (std::size_t j = 0; j < matrix.size2(); ++j)
			{
				matrix(i, j) = static_cast<double>(i + j);
			}
		}

		// Start Excel
		ExcelDriver& excel = ExcelDriver::Instance();
		excel.MakeVisible(true);		// Default is INVISIBLE!

		// Call Excel print function
		std::string sheetName("Test Case 101 Matrix");
		long row = 4; long col = 2;
	//	excel.AddMatrix<NumericMatrix>(matrix, sheetName, row, col);

		std::string sheetName2("Matrix Labels Case");

	//	ExcelDriver& excel = ExcelDriver::Instance();
	//	excel.MakeVisible(true);		// Default is INVISIBLE!

		// Labels for rows and columns of the Excel matrix.
		// Only labelled values are printed!!
		std::list<std::string> rowLabels; // C++11: {"A","B","C","D","E", "F","K","L"};
		rowLabels.push_back("A");
		rowLabels.push_back("B");
		rowLabels.push_back("C");
		rowLabels.push_back("D");
		rowLabels.push_back("E");
		rowLabels.push_back("F");
		rowLabels.push_back("K");
		rowLabels.push_back("L");

		std::list<std::string> colLabels; // C++11: {"C1", "C2", "C3", "C4","C5"};
		colLabels.push_back("C1");
		colLabels.push_back("C2");
		colLabels.push_back("C3");
		colLabels.push_back("C4");
		colLabels.push_back("C5");

		long rowPos = 4; long colPos = 3;
	//	excel.AddMatrix<NumericMatrix>(matrix, sheetName2, rowLabels, colLabels,
		//										rowPos, colPos);

		{
			// Using mapping continuous space to discrete space
			std::size_t N = 20; std::size_t M = 10;
			auto x = CreateMesh(N, -4.0, 4.0);
			auto y = CreateMesh(M, -4.0, 4.0);

			NumericMatrix matrix = DiscreteNormalPdf2d<NumericMatrix>(x, y);

		//	ExcelDriver& excel = ExcelDriver::Instance();
			std::string sheetName("Bivariate Normal pdf");
			long row = 1; long col = 1;
			excel.AddMatrix<NumericMatrix>(matrix, sheetName, row, col);
	}

	return 0;
}


#include <toynet/math.h>
#include <toynet/stlio.h>
#include <toynet/ublas/convert.h>
#include <toynet/ublas/io.h>
#include <toynet/ublas/test.h>
#include <boost/test/unit_test.hpp>

using namespace toynet;

void help_test_softmax_size_0(ublas::vector<double> (*fn)(const ublas::vector<double>&))
{
    const ublas::vector<double> v;
    const ublas::vector<double> expected;
    const ublas::vector<double> got = (*fn)(v);
    check_close_vectors(expected, got);
}

void help_test_softmax_size_1(ublas::vector<double> (*fn)(const ublas::vector<double>&))
{
    const ublas::vector<double> v = convert({5.0});
    const ublas::vector<double> expected = convert({1});
    const ublas::vector<double> got = (*fn)(v);
    check_close_vectors(expected, got);
}

void help_test_softmax_size_1_sum_0(ublas::vector<double> (*fn)(const ublas::vector<double>&))
{
    // Make sure we don't divide by 0!
    const ublas::vector<double> v = convert({0.0});
    const ublas::vector<double> expected = convert({1.0});
    const ublas::vector<double> got = (*fn)(v);
    check_close_vectors(expected, got);
}

void help_test_softmax_size_2(ublas::vector<double> (*fn)(const ublas::vector<double>&))
{
    const ublas::vector<double> v = convert({1.0, 2.0});
    const ublas::vector<double> expected = convert({0.26894142, 0.73105858});  // numpy 1.15.4
    const ublas::vector<double> got = (*fn)(v);
    check_close_vectors(expected, got);
}

void help_test_softmax_numerical_stability(ublas::vector<double> (*fn)(const ublas::vector<double>&))
{
    // https://ogunlao.github.io/2020/04/26/you_dont_really_know_softmax.html#numerical-stability-of-softmax
    const ublas::vector<double> v = convert({10, 2, 10000, 4});
    const ublas::vector<double> expected = convert({0, 0, 1, 0});
    const ublas::vector<double> got = (*fn)(v);
    check_close_vectors(expected, got);
}

BOOST_AUTO_TEST_CASE(test_softmax)
{
    help_test_softmax_size_0(softmax);
    help_test_softmax_size_1(softmax);
    help_test_softmax_size_1_sum_0(softmax);
    help_test_softmax_size_2(softmax);
    help_test_softmax_numerical_stability(softmax);
}

BOOST_AUTO_TEST_CASE(test_naive_softmax)
{
    help_test_softmax_size_0(naive_softmax);
    help_test_softmax_size_1(naive_softmax);
    help_test_softmax_size_1_sum_0(naive_softmax);
    help_test_softmax_size_2(naive_softmax);
}

BOOST_AUTO_TEST_CASE(test_stable_softmax)
{
    help_test_softmax_size_0(stable_softmax);
    help_test_softmax_size_1(stable_softmax);
    help_test_softmax_size_1_sum_0(stable_softmax);
    help_test_softmax_size_2(stable_softmax);
    help_test_softmax_numerical_stability(stable_softmax);
}

void help_test_magnitude_size_0(double (*fn)(const ublas::vector<double>&))
{
    const ublas::vector<double> v;
    const double expected = 0.0;
    const double got = (*fn)(v);
    BOOST_CHECK_CLOSE(expected, got, 0.000001);
}

void help_test_magnitude_size_1(double (*fn)(const ublas::vector<double>&))
{
    const ublas::vector<double> v = convert({6});
    const double expected = 6;
    const double got = (*fn)(v);
    BOOST_CHECK_CLOSE(expected, got, 0.000001);
}

void help_test_magnitude_size_2(double (*fn)(const ublas::vector<double>&))
{
    const ublas::vector<double> v = convert({4, 3});
    const double expected = 5;
    const double got = (*fn)(v);
    BOOST_CHECK_CLOSE(expected, got, 0.000001);
}

BOOST_AUTO_TEST_CASE(test_magnitude)
{
    help_test_magnitude_size_0(magnitude);
    help_test_magnitude_size_1(magnitude);
    help_test_magnitude_size_2(magnitude);
}

BOOST_AUTO_TEST_CASE(test_naive_magnitude)
{
    help_test_magnitude_size_0(naive_magnitude);
    help_test_magnitude_size_1(naive_magnitude);
    help_test_magnitude_size_2(naive_magnitude);
}

BOOST_AUTO_TEST_CASE(test_ublas_magnitude)
{
    help_test_magnitude_size_0(ublas_magnitude);
    help_test_magnitude_size_1(ublas_magnitude);
    help_test_magnitude_size_2(ublas_magnitude);
}

void help_test_dot_product_size_0(double (*fn)(const ublas::vector<double>& v1, const ublas::vector<double>& v2))
{
    const ublas::vector<double> v1;
    const ublas::vector<double> v2;
    const double expected = 0.0;
    const double got = (*fn)(v1, v2);
    BOOST_CHECK_EQUAL(expected, got);
}

void help_test_dot_product_size_1(double (*fn)(const ublas::vector<double>& v1, const ublas::vector<double>& v2))
{
    const ublas::vector<double> v1 = convert({1.5});
    const ublas::vector<double> v2 = convert({-2.0});
    const double expected = -3.0;
    const double got = (*fn)(v1, v2);
    BOOST_CHECK_EQUAL(expected, got);
}

void help_test_dot_product_size_2(double (*fn)(const ublas::vector<double>& v1, const ublas::vector<double>& v2))
{
    const ublas::vector<double> v1 = convert({1.5, 10});
    const ublas::vector<double> v2 = convert({-2.0, 0.25});
    const double expected = -0.5;
    const double got = (*fn)(v1, v2);
    BOOST_CHECK_EQUAL(expected, got);
}

BOOST_AUTO_TEST_CASE(test_dot_product)
{
    help_test_dot_product_size_0(dot_product);
    help_test_dot_product_size_1(dot_product);
    help_test_dot_product_size_2(dot_product);
}

BOOST_AUTO_TEST_CASE(test_naive_dot_product)
{
    help_test_dot_product_size_0(naive_dot_product);
    help_test_dot_product_size_1(naive_dot_product);
    help_test_dot_product_size_2(naive_dot_product);
}

BOOST_AUTO_TEST_CASE(test_ublas_dot_product)
{
    help_test_dot_product_size_0(ublas_dot_product);
    help_test_dot_product_size_1(ublas_dot_product);
    help_test_dot_product_size_2(ublas_dot_product);
}

BOOST_AUTO_TEST_CASE(cosine_distance_a)
{
    // https://stackoverflow.com/a/1750187
    const ublas::vector<double> v1 = convert({2, 0, 1, 1, 0, 2, 1, 1});
    const ublas::vector<double> v2 = convert({2, 1, 1, 0, 1, 1, 1, 1});
    const double expected = 0.822;
    const double got12 = cosine_distance(v1, v2);
    const double got21 = cosine_distance(v2, v1);
    const double got11 = cosine_distance(v1, v1);
    const double got22 = cosine_distance(v2, v2);
    BOOST_CHECK_CLOSE(expected, got12, 0.1);
    BOOST_CHECK_EQUAL(got12, got21);
    BOOST_CHECK_CLOSE(1.0, got11, 1e-12);
    BOOST_CHECK_CLOSE(1.0, got22, 1e-12);
}

BOOST_AUTO_TEST_CASE(cosine_distance_b)
{
    // https://stackoverflow.com/a/14038820
    const ublas::vector<double> v1 = convert({-1, -1, 0});
    const ublas::vector<double> v2 = convert({-1, 0, -1});
    const double expected = 0.5;
    const double got12 = cosine_distance(v1, v2);
    const double got21 = cosine_distance(v2, v1);
    const double got11 = cosine_distance(v1, v1);
    const double got22 = cosine_distance(v2, v2);
    BOOST_CHECK_CLOSE(expected, got12, 0.1);
    BOOST_CHECK_EQUAL(got12, got21);
    BOOST_CHECK_CLOSE(1.0, got11, 1e-12);
    BOOST_CHECK_CLOSE(1.0, got22, 1e-12);
}

BOOST_AUTO_TEST_CASE(nearest_neighbors_size_0)
{
    const ublas::vector<double> v = convert({-1, -1, 0});
    const std::vector<ublas::vector<double>> points{};
    const std::vector<int> expected{};
    const std::vector<int> got = nearest_neighbors(v, points);
    BOOST_CHECK_EQUAL(expected, got);
}

BOOST_AUTO_TEST_CASE(nearest_neighbors_size_1_equal)
{
    const ublas::vector<double> v = convert({-1, -1, 0});
    const std::vector<ublas::vector<double>> points{convert({-1, -1, 0})};
    const std::vector<int> expected{0};
    const std::vector<int> got = nearest_neighbors(v, points);
    BOOST_CHECK_EQUAL(expected, got);
}

BOOST_AUTO_TEST_CASE(nearest_neighbors_size_1_not_equal)
{
    const ublas::vector<double> v = convert({-1, -1, 0});
    const std::vector<ublas::vector<double>> points{convert({-1, 0, -1})};
    const std::vector<int> expected{0};
    const std::vector<int> got = nearest_neighbors(v, points);
    BOOST_CHECK_EQUAL(expected, got);
}

BOOST_AUTO_TEST_CASE(nearest_neighbors_size_2)
{
    const ublas::vector<double> v = convert({-1, -1, 0});
    const std::vector<ublas::vector<double>> points{
        convert({-1, 0, -1}),
        convert({-1, -1, 0})
    };
    const std::vector<int> expected{1, 0};
    const std::vector<int> got = nearest_neighbors(v, points);
    BOOST_CHECK_EQUAL(expected, got);
}

BOOST_AUTO_TEST_CASE(nearest_neighbors_size_4)
{
    const ublas::vector<double> v = convert({-1, -1, 0});
    const std::vector<ublas::vector<double>> points{
        convert({-2, -1, 0}),
        convert({-1, 0, -1}),
        convert({-1, -1, 0}),
        convert({-3, -1, 0})
    };
    const std::vector<int> expected{2, 0, 3, 1};
    const std::vector<int> got = nearest_neighbors(v, points);
    BOOST_CHECK_EQUAL(expected, got);
}


#include <chrono>
#include <cstdlib>
#include <iostream>
#include <iterator>

#include <boost/multiprecision/mpfr.hpp>

namespace mp = boost::multiprecision;
using float_huge = mp::number<mp::mpfr_float_backend<PRECISION_DIGITS>>;
using timer_value_type = float;
using std::chrono::high_resolution_clock;

template <typename T>
struct scoped_timer
{
    explicit scoped_timer(T& t) : t_(t), start_(high_resolution_clock::now()) { }
    ~scoped_timer()
    {
        t_ = std::chrono::duration<T,
           std::chrono::milliseconds::period>(high_resolution_clock::now() -
                   start_).count();
    }

private:
    T& t_;
    high_resolution_clock::time_point start_;
};

float_huge pi_gauss_legendre()
{
    float_huge a = static_cast<float_huge>(1);
    float_huge b = static_cast<float_huge>(1) /
        mp::sqrt(static_cast<float_huge>(2));
    float_huge t = static_cast<float_huge>(.25);
    float_huge p = static_cast<float_huge>(1);

    for (int correct_digits = 3;
        correct_digits < std::numeric_limits<float_huge>::digits;
        correct_digits *= 2)
    {
        float_huge a_n = (a + b) / static_cast<float_huge>(2);
        b = mp::sqrt(a * b);
        t -= p * mp::pow(a - a_n, static_cast<float_huge>(2));
        p *= static_cast<float_huge>(2);
        a = a_n;
    }

    float_huge pi = mp::pow(a + b, 2) / (static_cast<float_huge>(4) * t);

    return pi;
}

int main()
{
    timer_value_type t = 0;
    {
        scoped_timer<timer_value_type> timer(t);

        std::cout <<
            std::setprecision(std::numeric_limits<float_huge>::max_digits10)
            << "pi = " << pi_gauss_legendre() << '\n';
    }
    std::cout << "Gauss-Legendre took " << std::fixed << std::setprecision(4)
              << t << "(ms)\n";

    return 0;
}
/*
Partial Output (Windows, x64, Release, VC 15):
About to run Gauss-Legendre, t is 0(ms)
pi = 3.1415926535897932384626433832795028841971693993751058209749445923078164062...
Gauss-Legendre took 6674.4258(ms)
*/


#include <boost/test/unit_test.hpp>

#include "LinearEquation.h"

BOOST_AUTO_TEST_CASE( LinearEquationTest )
{
    LinearEquation noSlope(0.0,0.0);

    BOOST_CHECK( noSlope.yVal(0.0) == 0.0);
}

BOOST_AUTO_TEST_CASE( LinearEquation_Intersection )
{

	// Example taken from: https://www.evernote.com/shard/s3/sh/da1d5daa-2e24-404a-b26c-f84d9edb1aa3/4c4e4dd48d11085f49629933e0600aa2
    LinearEquation equation1(3.0,2.0);
    LinearEquation equation2(2.0,-1.0);

    XYCoord intercept = equation1.intercept(equation2);

    BOOST_CHECK( intercept.x() == -3.0);
    BOOST_CHECK( intercept.y() == -7.0);
}

BOOST_AUTO_TEST_CASE( LinearEquation_InterceptEndpoints )
{

    BOOST_TEST_MESSAGE("LinearEquation_InterceptEndpoints: checking intercept used in WedgeScannerEngine_SynthesizedPattern test case");

   LinearEquation upperTrendline(XYCoord(3,100),XYCoord(11,98));
    LinearEquation lowerTrendline(XYCoord(7,92),XYCoord(15,94));

    XYCoord intercept = upperTrendline.intercept(lowerTrendline);

    BOOST_TEST_MESSAGE("LinearEquation_InterceptEndpoints: intercept: " << intercept);

    BOOST_CHECK_CLOSE( intercept.x(),21,0.001);
    BOOST_CHECK_CLOSE( intercept.y(),95.5,0.001);
}


/**
 * Copyright (C) Omar Thor <omarthoro@gmail.com> - All Rights Reserved
 * Unauthorized copying of this file, via any medium is strictly prohibited
 * Proprietary and confidential
 *
 * Written by Omar Thor <omarthoro@gmail.com>, 2017
 */

//#include "sp/util/timing.hpp"
//#include "sp/util/rand.hpp"
//#include "sp/util/typename.hpp"

#include <iostream>
#include <iomanip>
#define BOOST_TEST_MODULE sp_algo_nn
#include <boost/test/unit_test.hpp>
#include "sp/algo/nn.hpp"
#include "sp/algo/nn/gradient_check.hpp"
#include "assert_matrix.hpp"

using namespace sp::algo::nn;
using namespace sp::testing;

BOOST_AUTO_TEST_CASE(test_pooling_layer_mean_fprop_stride) {

    using input_dims = volume_dims<1, 4, 4>;
    using k_params = pooling_kernel_params<2, 2>;

    using mean_pooling_layer_type = mean_pooling_layer<input_dims, k_params>;

    mean_pooling_layer_type layer;

    layer.weight_initializer = fixed_weight_initializer(1);
    layer.bias_initializer = fixed_weight_initializer(0);
    layer.configure(1, true);

    /* Layer weight and bias override */
    tensor_4 input(1, 1, 4, 4);
    input.setValues({{{
        {0,  1,  2,  3},
        {8,  7,  5,  6},
        {4,  3,  1,  2},
        {0, -1, -2, -3}
    }}});

    tensor_4 result(1, 1, 2, 2);

    tensor_4 expected_result(1, 1, 2, 2);

    expected_result.setValues({{{
        { 4,    4  },
        { 1.5, -0.5},
    }}});

    layer.forward_prop(input, result);

    assert_tensor_equals(expected_result, result);

}

BOOST_AUTO_TEST_CASE(test_pooling_layer_mean_fprop_stride_with_weights_and_biases) {

    using input_dims = volume_dims<1, 4, 4>;
    using k_params = pooling_kernel_params<2, 2>;

    using mean_pooling_layer_type = mean_pooling_layer<input_dims, k_params>;

    mean_pooling_layer_type layer;

    layer.weight_initializer = vector_weight_initializer({1.25});
    layer.bias_initializer = vector_weight_initializer({0.5});
    layer.configure(1, true);

    /* Layer weight and bias override */
    tensor_4 input(1, 1, 4, 4);
    input.setValues({{{
        {-0.948148,  0.863082,   0.0993249, 0.895461},
        {-0.129355, -0.0305018, -0.159264, -0.358927},
        {-0.33933,  -0.691147,  -0.590703,  0.397725},
        {0.238542,  -0.760099,  -0.400691, -0.0296482}
    }}});

    tensor_4 result(1, 1, 2, 2);

    tensor_4 expected_result(1, 1, 2, 2);

    expected_result.setValues({{{
        { 0.423461625, 0.64893590625},
        { 0.014989375, 0.305213375}
    }}});

    layer.forward_prop(input, result);
    assert_tensor_equals(expected_result, result);

}

BOOST_AUTO_TEST_CASE(test_pooling_layer_mean_bprop_stride) {

    using input_dims = volume_dims<1, 4, 4>;
    using k_params = pooling_kernel_params<2, 2>;

    using avg_pooling_layer_type = mean_pooling_layer<input_dims, k_params>;

    avg_pooling_layer_type layer;

    layer.configure(1, true);

    /* Layer weight and bias override */
    tensor_4 prev_out(1, 1, 4, 4);
    tensor_4 curr_out(1, 1, 2, 2); curr_out.setZero();
    tensor_4 prev_delta(1, 1, 4, 4); prev_delta.setZero();
    tensor_4 expected_prev_delta(1, 1, 4, 4);
    tensor_4 curr_delta(1, 1, 2, 2);

    prev_out.setValues({{{
        {0,  1,  2,  3},
        {8,  7,  5,  6},
        {4,  3,  1,  2},
        {0, -1, -2, -3}
    }}});
    curr_delta.setValues({{{
        {1, 2},
        {3, 4},
    }}});
    expected_prev_delta.setValues({{{
        {1, 1, 2, 2},
        {1, 1, 2, 2},
        {3, 3, 4, 4},
        {3, 3, 4, 4}
    }}});
    expected_prev_delta = expected_prev_delta * (1 / 4.0f);

    tensor_4 result(1, 1, 2, 2);

    layer.forward_prop(prev_out, result);

    layer.backward_prop(prev_out, prev_delta, curr_out, curr_delta);

    assert_tensor_equals(expected_prev_delta, prev_delta);

}

BOOST_AUTO_TEST_CASE(test_pooling_layer_max_bprop_stride) {

    using input_dims = volume_dims<1, 4, 4>;
    using k_params = pooling_kernel_params<2, 2>;
    using max_pooling_layer_type = max_pooling_layer<input_dims, k_params>;

    max_pooling_layer_type layer;
    layer.weight_initializer = fixed_weight_initializer(1.0f);
    layer.configure(1, true);

    tensor_4 prev_out(1, 1, 4, 4);
    tensor_4 curr_out(1, 1, 2, 2); curr_out.setZero();
    tensor_4 prev_delta(1, 1, 4, 4); prev_delta.setZero();
    tensor_4 expected_prev_delta(1, 1, 4, 4);
    tensor_4 curr_delta(1, 1, 2, 2);

    prev_out.setValues({{{
        {0,  1,  2,  3},
        {8,  7,  5,  6},
        {4,  3,  1,  2},
        {0, -1, -2, -3}
    }}});
    curr_delta.setValues({{{
        {1, 2},
        {3, 4},
    }}});
    expected_prev_delta.setValues({{{
        {0, 0, 0, 0},
        {1, 0, 0, 2},
        {3, 0, 0, 4},
        {0, 0, 0, 0}
    }}});

    layer.forward_prop(prev_out, curr_out);

    layer.backward_prop(prev_out, prev_delta, curr_out, curr_delta);

    assert_tensor_equals(expected_prev_delta, prev_delta);

}
BOOST_AUTO_TEST_CASE(test_pooling_layer_mean_gradient_check) {

    /** FIX SEED FOR TESTING, REPRODUCIBLE RESULTS< NOT TO BE COMMITTED! */
    random_generator::get().seed(2);
    using input_dims = volume_dims<1, 4, 4>;
    using k_params = pooling_kernel_params<2, 2>;

    constexpr float_t epsilon = 1e-2f;
    constexpr size_t batch_size = 1;

    using avg_pooling_layer_type = mean_pooling_layer<input_dims, k_params>;
    avg_pooling_layer_type layer;

    layer.weight_initializer = gauss_weight_initializer(-1.0f, 1.0f);
    layer.bias_initializer = gauss_weight_initializer(-1.0f, 1.0f);

    /* setup weights and dimensions */
    layer.configure(batch_size, true);

    auto in = generate_inputs_for(layer, batch_size);

    for(size_t i = 0; i < 1; ++i) {
        auto in_selected  = gradient_random_input(layer);
        auto out_selected = gradient_random_output(layer);

        /* Perform numerical and analytics gradient */
        /* estimate */
        auto n = numerical_gradient (layer, in, in_selected, out_selected);
        /* actual */
        auto a = analytical_gradient(layer, in, in_selected, out_selected);

        /* Validate result */
        BOOST_CHECK_MESSAGE(std::abs(a-n) <= epsilon, "Gradient check |" << std::setprecision(15) << a << " - " << n << "| < " << epsilon);
    }
}


//==================================================================================================
/**
  EVE - Expressive Vector Engine
  Copyright : EVE Contributors & Maintainers
  SPDX-License-Identifier: MIT
**/
//==================================================================================================
#include "test.hpp"
#include <eve/module/special.hpp>
#include <boost/math/special_functions/factorials.hpp>

//==================================================================================================
// Types tests
//==================================================================================================
EVE_TEST_TYPES( "Check return types of eve::double_factorial"
              , eve::test::simd::unsigned_integers)
<typename T>(eve::as<T>)
{
  using v_t = eve::element_type_t<T>;
  using d_t = eve::wide<double, eve::cardinal_t<T>>;
  TTS_EXPR_IS( eve::double_factorial(T())                       , d_t);
  TTS_EXPR_IS( eve::double_factorial(v_t())                     , double );
};

//==================================================================================================
// Test for corner-cases values
//==================================================================================================
EVE_TEST_TYPES( "Check corner-cases behavior of eve::double_factorial on wide"
        , eve::test::simd::unsigned_integers
         )
  <typename T>(eve::as<T>)
{
  using eve::as;
  using d_t = eve::wide<double, eve::cardinal_t<T>>;
  TTS_ULP_EQUAL(eve::double_factorial(T(10))  , d_t(boost::math::double_factorial<double>(10)), 0.5);
  TTS_ULP_EQUAL(eve::double_factorial(T( 5))  , d_t(boost::math::double_factorial<double>( 5)), 0.5);
  TTS_ULP_EQUAL(eve::double_factorial(T(180)) , d_t(boost::math::double_factorial<double>( 180)), 1.);
  TTS_ULP_EQUAL(eve::double_factorial(T(181)) , d_t(boost::math::double_factorial<double>( 181)), 1.5);

  if constexpr(sizeof(eve::element_type_t<T>) > 1)
  {
    TTS_ULP_EQUAL(eve::double_factorial(T(300)), d_t(boost::math::double_factorial<double>( 300)), 1.0);
    TTS_ULP_EQUAL(eve::double_factorial(T(301)), eve::inf(eve::as<d_t>()), 0);
    TTS_ULP_EQUAL(eve::double_factorial(T(302)), eve::inf(eve::as<d_t>()), 0);
  }
};


#include <stan/math/rev/scal.hpp>
#include <gtest/gtest.h>
#include <boost/math/special_functions/digamma.hpp>
#include <test/unit/math/rev/scal/fun/nan_util.hpp>
#include <test/unit/math/rev/scal/util.hpp>

TEST(AgradRev,log_rising_factorial_var_double) {
  double a(1);
  AVAR b(4.0);
  AVAR f = stan::math::log_rising_factorial(b,a);
  EXPECT_FLOAT_EQ(std::log(4.0),f.val());

  AVEC x = createAVEC(a,b);
  VEC g;
  f.grad(x,g);
  EXPECT_FLOAT_EQ(0, g[0]);
  EXPECT_FLOAT_EQ(boost::math::digamma(5) - boost::math::digamma(4),g[1]);

  double eps = 1e-6;
  EXPECT_FLOAT_EQ((stan::math::log_rising_factorial(4.0 + eps, 1.0)
                  - stan::math::log_rising_factorial(4.0 - eps, 1.0))
                  / (2 * eps), g[1]);
}

TEST(AgradRev, log_rising_factorial_exceptions) {
  double a(1);
  AVAR b(-3.0);
  EXPECT_THROW(stan::math::log_rising_factorial(b,a), std::domain_error);
  EXPECT_THROW(stan::math::log_rising_factorial(b,b), std::domain_error);
}

TEST(AgradRev, log_rising_factorial_double_var) {
  double a(5.0);
  AVAR b(4.0);
  AVAR f = stan::math::log_rising_factorial(a,b);
  EXPECT_FLOAT_EQ(std::log(5*6*7*8), f.val());
  AVEC x = createAVEC(a,b);
  VEC g;
  f.grad(x,g);
  EXPECT_FLOAT_EQ(0, g[0]);
  EXPECT_FLOAT_EQ(boost::math::digamma(9), g[1]);

  double eps = 1e-6;
  EXPECT_FLOAT_EQ((stan::math::log_rising_factorial(5.0, 4.0 + eps)
                  - stan::math::log_rising_factorial(5.0, 4.0 - eps))
                  / (2 * eps), g[1]);
}

TEST(AgradRev, log_rising_factorial_var_var) {
  AVAR c(5.0);
  AVAR b(4.0);
  AVAR f = stan::math::log_rising_factorial(b,c);
  EXPECT_FLOAT_EQ(std::log(4*5*6*7*8), f.val());
  AVEC x = createAVEC(b,c);
  VEC g;
  f.grad(x,g);
  EXPECT_FLOAT_EQ(boost::math::digamma(9.0) - boost::math::digamma(4.0), g[0]);
  EXPECT_FLOAT_EQ(boost::math::digamma(9), g[1]);

  double eps = 1e-6;
  EXPECT_FLOAT_EQ((stan::math::log_rising_factorial(4.0 + eps, 5.0)
                  - stan::math::log_rising_factorial(4.0 - eps, 5.0))
                  / (2 * eps), g[0]);
  EXPECT_FLOAT_EQ((stan::math::log_rising_factorial(4.0, 5.0 + eps)
                  - stan::math::log_rising_factorial(4.0, 5.0 - eps))
                  / (2 * eps), g[1]);
}

struct log_rising_factorial_fun {
  template <typename T0, typename T1>
  inline
  typename stan::return_type<T0,T1>::type
  operator()(const T0& arg1,
             const T1& arg2) const {
    return log_rising_factorial(arg1,arg2);
  }
};

TEST(AgradRev, log_rising_factorial_nan) {
  log_rising_factorial_fun log_rising_factorial_;
  test_nan(log_rising_factorial_,3.0,5.0,false,true);
}

TEST(AgradRev, check_varis_on_stack) {
  AVAR a(1.0);
  AVAR b(4.0);
  test::check_varis_on_stack(stan::math::log_rising_factorial(b, a));
  test::check_varis_on_stack(stan::math::log_rising_factorial(b, 1.0));
  test::check_varis_on_stack(stan::math::log_rising_factorial(4.0, a));
}


/*
(c) 2019 M. Werner - Part of the GIS++ tutorial
- https://www.martinwerner.de/teaching/spatial-cpp
- https://github.com/mwernerds/spatial-cpp

Program: Hello world in C
Compile: g++ -Wall -std=c++17  -o 05-algo 05-algo.cpp
*/

#include<iostream>
#include<vector>
#include<list>
#include<map>
#include<unordered_map>

#include<algorithm>
#include<numeric>

#include<chrono>

#include <boost/range/adaptor/indexed.hpp>
#include <boost/assign.hpp>
#include <boost/assign/list_inserter.hpp> // for 'insert()'

// you will need this header for the C++17 parallel version, but it is not yet available everywhere.
//#include<execution>

using  boost::adaptors::indexed;
using namespace boost::assign;

template <typename T>
std::ostream &operator<< (std::ostream &os, const std::vector<T>  & data)
{
    os << "(@" <<  (void *) &data[0] << "): ";
    std::copy(data.begin(), data.end(), std::ostream_iterator<int>(os, " "));
    return os;
}

int main(int argc, char **argv)
{
    // some algorithm templates in use

    std::vector<int> v;
    v += 1,2,3,4,5,6,7;
    std::cout << v << std::endl;
    // let us randomize v
    std::random_shuffle(v.begin(), v.end());
    std::cout << v << std::endl;
    // sort
    std::sort(v.begin(), v.end());
    // in c++ 20, you can see a vector as a range and do
    // std::ranges::sort(v)
    std::cout << v << std::endl;
    // show only even entries
    std::random_shuffle(v.begin(), v.end());

    auto uneven = std::count_if(v.begin(), v.end(), [](int a) {return (a%2 == 1);});
    std::cout << "Found " << uneven << " uneven numbers." << std::endl;

    // Okay, but how does this help us? Let us search for a number in an ordered list in three ways
    // and do some timing on simple, classical, and C++ style solutions.

    std::vector<long> many(1024*1024);
    std::iota(many.begin(), many.end(),0);
//    std::cout << many << std::endl;

    {
    unsigned long sink;
    auto start = std::chrono::high_resolution_clock::now();
    auto it = many.begin();
    for (sink=0; it != many.end(); ++it){  sink += *it;};

    auto end = std::chrono::high_resolution_clock::now();
     std::chrono::duration<double> diff = end-start;
     std::cout << "Summed up to " << sink << " in " << diff.count() << "seconds" << std::endl;
    }

    // faster (!) and easier to read
    {
    auto start = std::chrono::high_resolution_clock::now();
    auto sink = std::accumulate(many.begin(), many.end(), 0ul);
    auto end = std::chrono::high_resolution_clock::now();
     std::chrono::duration<double> diff = end-start;
     std::cout << "Summed up to " << sink << " in " << diff.count() << "seconds" << std::endl;
    }

    // similar: let us search for the location of the value 12345

    {
    auto start = std::chrono::high_resolution_clock::now();
    size_t i;
    for(i=0; i<many.size();i++)
    {
	if (many[i] == 123456)
	   break;
    }

    auto end = std::chrono::high_resolution_clock::now();
     std::chrono::duration<double> diff = end-start;
     std::cout << "Found it at " << i << " in " << diff.count() << "seconds" << std::endl;
    }

    // algorithm version (using sorted property, that is binary search)
    {
    auto start = std::chrono::high_resolution_clock::now();
    auto it = std::lower_bound(many.begin(), many.end(),123456);
    auto end = std::chrono::high_resolution_clock::now();
     std::chrono::duration<double> diff = end-start;
     std::cout << "Found it at " << std::distance(many.begin(),it) << " in " << diff.count() << "seconds" << std::endl;
    }

    // in c++17: MapReduce?!
    // Map:
    std::transform(many.begin(), many.end(), many.begin(), [](int v) -> int {return v*2;});

    std::cout << "transform: ";
    std::copy_n(many.begin(), 10 , std::ostream_iterator<int>(std::cout, " "));
    std::cout << "..." << std::endl;

    // accumulate (e.g., sequential)
     {
	auto start = std::chrono::high_resolution_clock::now();
	auto sink = std::accumulate(many.begin(), many.end(), 0ul);
	auto end = std::chrono::high_resolution_clock::now();
	 std::chrono::duration<double> diff = end-start;
	 std::cout << "Summed up to " << sink << " in " << diff.count() << "seconds" << std::endl;
    }

    // reduce (e.g. parallel, accumulate out of order, c++17, specified execution strategy)
/*
this is C++17 and not yet widely available. But how cool is this: just tell it to be parallel and it does ;-)

    {
	auto start = std::chrono::high_resolution_clock::now();
	auto sink = std::reduce(std::execution::par, many.begin(), many.end(), 0ul);
	auto end = std::chrono::high_resolution_clock::now();
	 std::chrono::duration<double> diff = end-start;
	 std::cout << "Summed up to " << sink << " in " << diff.count() << "seconds" << std::endl;
    }

// and a complete example of the simple algorithm std::sort in C++17:

using namespace std;
vector<int> v = ...

// standard sequential sort
sort(v.begin(), v.end());

// explicitly sequential sort
sort(sequential, v.begin(), v.end());

// permitting parallel execution
sort(par, v.begin(), v.end());

// permitting vectorization as well
sort(par_vec, v.begin(), v.end());
*/

    return 0;
}


// Boost.Geometry (aka GGL, Generic Geometry Library)

// Copyright (c) 2019 Tinko Bartels, Berlin, Germany.

// Contributed and/or modified by Tinko Bartels,
//   as part of Google Summer of Code 2019 program.

// Use, modification and distribution is subject to 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 BOOST_GEOMETRY_EXTENSIONS_TRIANGULATION_STRATEGIES_CARTESIAN_SIDE_ROBUST_HPP
#define BOOST_GEOMETRY_EXTENSIONS_TRIANGULATION_STRATEGIES_CARTESIAN_SIDE_ROBUST_HPP

#include <boost/geometry/util/select_most_precise.hpp>
#include <boost/geometry/util/select_calculation_type.hpp>
#include <boost/geometry/extensions/triangulation/strategies/cartesian/detail/precise_math.hpp>

namespace boost { namespace geometry
{

namespace strategy { namespace side
{

/*!
\brief Adaptive precision predicate to check at which side of a segment a point lies:
    left of segment (>0), right of segment (< 0), on segment (0).
\ingroup strategies
\tparam CalculationType \tparam_calculation (numeric_limits<ct>::epsilon() and numeric_limits<ct>::digits must be supported for calculation type ct)
\tparam Robustness std::size_t value from 0 (fastest) to 3 (default, guarantees correct results).
\details This predicate determines at which side of a segment a point lies using an algorithm that is adapted from orient2d as described in "Adaptive Precision Floating-Point Arithmetic and Fast Robust Geometric Predicates" by Jonathan Richard Shewchuk ( https://dl.acm.org/citation.cfm?doid=237218.237337 ). More information and copies of the paper can also be found at https://www.cs.cmu.edu/~quake/robust.html . It is designed to be adaptive in the sense that it should be fast for inputs that lead to correct results with plain float operations but robust for inputs that require higher precision arithmetics.
 */
template
<
    typename CalculationType = void,
    std::size_t Robustness = 3
>
struct side_robust
{
public:
    //! \brief Computes double the signed area of the CCW triangle p1, p2, p
    template
    <
        typename PromotedType,
        typename P1,
        typename P2,
        typename P
    >
    static inline PromotedType side_value(P1 const& p1, P2 const& p2,
        P const& p)
    {
        typedef ::boost::geometry::detail::precise_math::vec2d<PromotedType> vec2d;
        vec2d pa { get<0>(p1), get<1>(p1) };
        vec2d pb { get<0>(p2), get<1>(p2) };
        vec2d pc { get<0>(p), get<1>(p) };
        return ::boost::geometry::detail::precise_math::orient2d
            <PromotedType, Robustness>(pa, pb, pc);
    }

#ifndef DOXYGEN_SHOULD_SKIP_THIS
    template
    <
        typename P1,
        typename P2,
        typename P
    >
    static inline int apply(P1 const& p1, P2 const& p2, P const& p)
    {
        typedef typename select_calculation_type_alt
            <
                CalculationType,
                P1,
                P2,
                P
            >::type coordinate_type;
        typedef typename select_most_precise
            <
                coordinate_type,
                double
            >::type promoted_type;

        promoted_type sv = side_value<promoted_type>(p1, p2, p);
        return sv > 0 ? 1
            : sv < 0 ? -1
            : 0;
    }
#endif

};

}} // namespace strategy::side

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_EXTENSIONS_TRIANGULATION_STRATEGIES_CARTESIAN_SIDE_ROBUST_HPP


//
// OpenTissue, A toolbox for physical based simulation and animation.
// Copyright (C) 2007 Department of Computer Science, University of Copenhagen
//
#include <OpenTissue/configuration.h>

#include <OpenTissue/core/math/math_basic_types.h>
#include <OpenTissue/core/math/math_eigen_system_decomposition.h>
#include <OpenTissue/core/math/math_polar_decomposition.h>

#define BOOST_AUTO_TEST_MAIN
#include <OpenTissue/utility/utility_push_boost_filter.h>
#include <boost/test/auto_unit_test.hpp>
#include <boost/test/unit_test_suite.hpp>
#include <boost/test/floating_point_comparison.hpp>
#include <boost/test/test_tools.hpp>
#include <OpenTissue/utility/utility_pop_boost_filter.h>

#include <cmath>
#include <iostream>

using namespace OpenTissue;

BOOST_AUTO_TEST_SUITE(opentissue_math_polar_decomposition);

  BOOST_AUTO_TEST_CASE(eigen_method)
  {
    typedef OpenTissue::math::BasicMathTypes<double, size_t> math_types;
    typedef math_types::vector3_type                         vector3_type;
    typedef math_types::matrix3x3_type                       matrix3x3_type;
    typedef math_types::real_type                            real_type;
    typedef math_types::index_type                           index_type;
    typedef math_types::value_traits                         value_traits;

    real_type epsilon = 10e-7;
    matrix3x3_type A,R,S,D;

    for(index_type i=0;i<100000;++i)
    {
      OpenTissue::math::random(S);
      S = OpenTissue::math::trans(S)*S;
      OpenTissue::math::random(A);
      R = OpenTissue::math::ortonormalize( A );
      A = R*S;
      R = OpenTissue::math::diag(1.0);
      S = OpenTissue::math::diag(1.0);
      bool success = OpenTissue::math::polar_decomposition::eigen(A,R,S);
      if(success)
      {
        bool right_handed = det(R) > value_traits::zero();
        BOOST_CHECK( right_handed );

        D = A - R*S;
        real_type maximum_deviation =  max_value(  fabs(D) );

        //BOOST_CHECK_CLOSE( maximum_deviation, value_traits::zero(), tol);
        BOOST_CHECK( maximum_deviation<epsilon );
      }
    }
  }

BOOST_AUTO_TEST_SUITE_END();


// Software License for MTL
//
// Copyright (c) 2007 The Trustees of Indiana University.
//               2008 Dresden University of Technology and the Trustees of Indiana University.
//               2010 SimuNova UG (haftungsbeschränkt), www.simunova.com.
// All rights reserved.
// Authors: Peter Gottschling and Andrew Lumsdaine
//
// This file is part of the Matrix Template Library
//
// See also license.mtl.txt in the distribution.

#include <iostream>
#include <cmath>
#include <complex>
#include <boost/numeric/mtl/mtl.hpp>

using namespace std;

template <typename Matrix>
void test(Matrix& , const char* name)
{
    typedef typename mtl::Collection<Matrix>::value_type value_type;
    typedef mtl::dense_vector<value_type>                vector_type;
    value_type  ar[][3] = {{3., 9., 0.},
			   {1., 2., 7.},
			   {9., 6., 8.}};
    const Matrix A(ar);

    // trans(A)[0][1]= 11.0;

    cout << name << ":\n trans(A)[0][1]= " << trans(A)[0][1] << "\n";

    MTL_THROW_IF(trans(A)[0][1] != value_type(1.), mtl::runtime_error("constant transposing wrong"));

    vector_type v(3), vcomp(3), w(3);
    w= 4, 7, 8;
    vcomp= 91,98,113;

    v= trans(A) * w;
    // cout << "trans(A) * w = " << v << '\n';
    MTL_THROW_IF(one_norm(vector_type(v - vcomp)) > 0.01, mtl::runtime_error("Error in trans(A) * w"));
}

template <typename Matrix>
void mutable_test(Matrix&, const char* name)
{
    typedef typename mtl::Collection<Matrix>::value_type value_type;
    value_type  ar[][3] = {{3., 9., 0.},
			   {1., 2., 7.},
			   {9., 6., 8.}};
    Matrix A(ar);

    trans(A)[0][1]= 11.0;

    cout << name << ":\n trans(A)[0][1]= " << trans(A)[0][1] << "\n";

    MTL_THROW_IF(trans(A)[0][1] != value_type(11.), mtl::runtime_error("transposing wrong"));
}

int main(int argc, char* argv[])
{
    using namespace mtl;

    unsigned size= 3;
    if (argc > 1) size= atoi(argv[1]);

    dense2D<double>                                      dr(size, size);
    dense2D<double, mat::parameters<col_major> >      dc(size, size);
    morton_dense<double, recursion::morton_z_mask>       mzd(size, size);
    morton_dense<double, recursion::doppled_2_row_mask>  d2r(size, size);
    compressed2D<double>                                 cr(size, size);
    compressed2D<double, mat::parameters<col_major> > cc(size, size);

    dense2D<complex<double> >                            drc(size, size);
    compressed2D<complex<double> >                       crc(size, size);

    test(dr, "Dense row major");
    test(dc, "Dense column major");
    test(mzd, "Morton Z-order");
    test(d2r, "Hybrid 2 row-major");
    test(cr, "Compressed row major");
    test(cc, "Compressed column major");
    test(drc, "Dense row major complex");
    test(crc, "Compressed row major complex");

    mutable_test(dr, "Dense row major");
    mutable_test(dc, "Dense column major");
    mutable_test(mzd, "Morton Z-order");
    mutable_test(d2r, "Hybrid 2 row-major");
    mutable_test(drc, "Dense row major complex");

    return 0;
}


#include "fem_solve.hpp"
#include "writer.hpp"
#include <Eigen/Core>
#include <sstream>

double f_square(double x, double y) {
	return 2 * M_PI * M_PI * sin(M_PI * x) * sin(M_PI * y);
}

int main(int, char **) {
	try {
		Vector u;

		Eigen::MatrixXd vertices;
		Eigen::MatrixXi triangles;
		Eigen::MatrixXi tetrahedra;

		igl::readMESH(NPDE_DATA_PATH "square_5.mesh", vertices, tetrahedra, triangles);

		solveFiniteElement(u, vertices, triangles, f_square);

		writeToFile("square_5_values.txt", u);
		writeMatrixToFile("square_5_vertices.txt", vertices);
		writeMatrixToFile("square_5_triangles.txt", triangles);

	} catch (std::runtime_error &e) {
		std::cerr << "An error occurred. Error message: " << std::endl;
		std::cerr << "    \"" << e.what() << "\"" << std::endl;
		return EXIT_FAILURE;
	} catch (...) {
		std::cerr << "An unknown error occurred." << std::endl;
		throw;
	}

	return EXIT_SUCCESS;
}


#include "KMC/integrals.hpp"
#include "KMC/lookup_table.hpp"
#include "KMC/lut_filler_edep.hpp"
#include "KMC/macros.hpp"
#include "catch.hpp"
#include "test_helpers.hpp"

#include <cmath>
#include <cstdio>
#include <iostream>
#include <string>

#include <boost/math/quadrature/gauss_kronrod.hpp>

TEST_CASE("Lookup table test SOFT spring ", "[lookup_soft]") {
    constexpr double errTol = 1e-3;
    const double D = 0.024;
    const double alpha = 0.1 / (2 * 0.00411);
    const double freelength = 0.05;
    const double M = alpha * D * D;
    const double ell0 = freelength / D;

    LUTFillerEdep lut_filler(256, 256);
    lut_filler.Init(alpha, freelength, D);
    LookupTable LUT(&lut_filler);

    double distPerp = 0;
    distPerp = 0.2;
    // ("distPerp = 0.2 > D+ell0, single peaked")
    for (double sbound = 0; sbound < 20; sbound += 0.5) {
        CHECK(LUT.Lookup(distPerp, sbound * D) ==
              Approx(D * integral(distPerp / D, 0, sbound, M, ell0))
                  .epsilon(errTol));
    }
    // ("distPerp = 0.1 > D+ell0, single peaked")
    distPerp = 0.1;
    for (double sbound = 0; sbound < 20; sbound += 0.5) {
        CHECK(LUT.Lookup(distPerp, sbound * D) ==
              Approx(D * integral(distPerp / D, 0, sbound, M, ell0))
                  .epsilon(errTol));
    }
    // ("distPerp = 0.06 < D+ell0, double peaked")
    distPerp = 0.06;
    for (double sbound = 0; sbound < 20; sbound += 0.5) {
        CHECK(LUT.Lookup(distPerp, sbound * D) ==
              Approx(D * integral(distPerp / D, 0, sbound, M, ell0))
                  .epsilon(errTol));
    }
}

TEST_CASE("Lookup table test MEDIUM spring ", "[lookup_med]") {
    constexpr double errTol = 1e-3;
    const double D = 0.024;
    const double alpha = 1.0 / (2 * 0.00411);
    const double freelength = 0.05;
    const double M = alpha * D * D;
    const double ell0 = freelength / D;

    LUTFillerEdep lut_filler(256, 256);
    lut_filler.Init(alpha, freelength, D);
    LookupTable LUT(&lut_filler);

    double distPerp = 0;
    // ("distPerp = 0.2 > D+ell0, single peaked")
    distPerp = 0.2;
    for (double sbound = 0; sbound < 20; sbound += 0.5) {
        CHECK(LUT.Lookup(distPerp, sbound * D) ==
              Approx(D * integral(distPerp / D, 0, sbound, M, ell0))
                  .epsilon(errTol));
    }
    // ("distPerp = 0.1 > D+ell0, single peaked")
    distPerp = 0.1;
    for (double sbound = 0; sbound < 20; sbound += 0.5) {
        CHECK(LUT.Lookup(distPerp, sbound * D) ==
              Approx(D * integral(distPerp / D, 0, sbound, M, ell0))
                  .epsilon(errTol));
    }
    // ("distPerp = 0.06 < D+ell0, double peaked")
    distPerp = 0.06;
    for (double sbound = 0; sbound < 20; sbound += 0.5) {
        CHECK(LUT.Lookup(distPerp, sbound * D) ==
              Approx(D * integral(distPerp / D, 0, sbound, M, ell0))
                  .epsilon(errTol));
    }
}

TEST_CASE("Lookup table test STIFF spring ", "[lookup_stiff]") {
    constexpr double absTol = 1e-5;
    const double D = 0.024;
    const double alpha = 10.0 / (2 * 0.00411);
    const double freelength = 0.05 + D;
    const double M = alpha * D * D;
    const double ell0 = freelength / D;

    LUTFillerEdep lut_filler(256, 256);
    lut_filler.Init(alpha, freelength, D);
    LookupTable LUT(&lut_filler);

    double distPerp = 0;
    // ("distPerp = 0.2 > D+ell0, single peaked")
    distPerp = 0.2;
    for (double sbound = 0; sbound < 20; sbound += 0.5) {
        // CHECK(errorPass(LUT.Lookup(distPerp, sbound * D),
        // D * integral(distPerp / D, 0, sbound, M, ell0)));
        CHECK(LUT.Lookup(distPerp, sbound * D) ==
              Approx(D * integral(distPerp / D, 0, sbound, M, ell0))
                  .margin(absTol));
    }
    // ("distPerp = 0.1 > D+ell0, single peaked")
    distPerp = 0.1;
    for (double sbound = 0; sbound < 20; sbound += 0.5) {
        // CHECK(errorPass(LUT.Lookup(distPerp, sbound * D),
        // D * integral(distPerp / D, 0, sbound, M, ell0)));
        CHECK(LUT.Lookup(distPerp, sbound * D) ==
              Approx(D * integral(distPerp / D, 0, sbound, M, ell0))
                  .margin(absTol));
    }
    // ("distPerp = 0.06 < D+ell0, double peaked")
    distPerp = 0.06;
    for (double sbound = 0; sbound < 20; sbound += 0.5) {
        CHECK(errorPass(LUT.Lookup(distPerp, sbound * D),
                        D * integral(distPerp / D, 0, sbound, M, ell0)));
        CHECK(LUT.Lookup(distPerp, sbound * D) ==
              Approx(D * integral(distPerp / D, 0, sbound, M, ell0))
                  .margin(absTol));
    }
}

TEST_CASE("Lookup table test manual medium spring REL error", "[lookup]") {
    // integrated by mathematica
    const double D = 0.024;
    constexpr double tol = 1e-4;

    LUTFillerEdep lut_filler(256, 256);
    lut_filler.Init(1.0 / (2 * 0.00411), 0.05 + D, D);
    LookupTable LUT(&lut_filler);

    double distPerp = 0;

    distPerp = 0.2;
    // ("distPerp = 0.2 > D+ell0, single peaked")
    CHECK(LUT.Lookup(distPerp, 0.5 * D) / D == Approx(0.0722077).epsilon(tol));
    CHECK(LUT.Lookup(distPerp, 1.0 * D) / D == Approx(0.1428390).epsilon(tol));
    CHECK(LUT.Lookup(distPerp, 1.5 * D) / D == Approx(0.2104120).epsilon(tol));
    CHECK(LUT.Lookup(distPerp, 2.0 * D) / D == Approx(0.2736230).epsilon(tol));
    CHECK(LUT.Lookup(distPerp, 3.0 * D) / D == Approx(0.3830390).epsilon(tol));
    CHECK(LUT.Lookup(distPerp, 4.0 * D) / D == Approx(0.4663750).epsilon(tol));
    CHECK(LUT.Lookup(distPerp, 5.0 * D) / D == Approx(0.5238890).epsilon(tol));
    CHECK(LUT.Lookup(distPerp, 6.0 * D) / D == Approx(0.5596700).epsilon(tol));

    distPerp = 0.08;
    // "distPerp = 0.08 > D+ell0, single peaked"/D,
    CHECK(LUT.Lookup(distPerp, 0.5 * D) / D == Approx(0.497588).epsilon(tol));
    CHECK(LUT.Lookup(distPerp, 1.0 * D) / D == Approx(0.993608).epsilon(tol));
    CHECK(LUT.Lookup(distPerp, 1.5 * D) / D == Approx(1.485540).epsilon(tol));
    CHECK(LUT.Lookup(distPerp, 2.0 * D) / D == Approx(1.969290).epsilon(tol));
    CHECK(LUT.Lookup(distPerp, 3.0 * D) / D == Approx(2.887840).epsilon(tol));
    CHECK(LUT.Lookup(distPerp, 4.0 * D) / D == Approx(3.692500).epsilon(tol));
    CHECK(LUT.Lookup(distPerp, 5.0 * D) / D == Approx(4.333200).epsilon(tol));
    CHECK(LUT.Lookup(distPerp, 6.0 * D) / D == Approx(4.789860).epsilon(tol));

    distPerp = 0.06;
    // "distPerp = 0.06 < D+ell0, double peaked"/D,
    CHECK(LUT.Lookup(distPerp, 0.5 * D) / D == Approx(0.488864).epsilon(tol));
    CHECK(LUT.Lookup(distPerp, 1.0 * D) / D == Approx(0.981139).epsilon(tol));
    CHECK(LUT.Lookup(distPerp, 1.5 * D) / D == Approx(1.478150).epsilon(tol));
    CHECK(LUT.Lookup(distPerp, 2.0 * D) / D == Approx(1.977880).epsilon(tol));
    CHECK(LUT.Lookup(distPerp, 3.0 * D) / D == Approx(2.960520).epsilon(tol));
    CHECK(LUT.Lookup(distPerp, 4.0 * D) / D == Approx(3.858570).epsilon(tol));
    CHECK(LUT.Lookup(distPerp, 5.0 * D) / D == Approx(4.598640).epsilon(tol));
    CHECK(LUT.Lookup(distPerp, 6.0 * D) / D == Approx(5.140580).epsilon(tol));
}

TEST_CASE("REVERSE Lookup table test manual medium spring REL error",
          "[REVERSE lookup]") {
    // integrated by mathematica
    const double D = 0.024;

    double distPerp = 0;
    LUTFillerEdep lut_filler(256, 256);
    lut_filler.Init(1.0 / (2 * 0.00411), 0.05 + D, D);
    LookupTable LUT(&lut_filler);
    // LUT.Init(&lut_filler);

    // double tol = RELTOL * REVERSEFAC;
    const double tol = 1e-4;

    distPerp = 0.1;
    // ("distPerp = 0.1 > D+ell0, single peaked")
    CHECK(LUT.ReverseLookup(distPerp, D * 0) / D == Approx(0.0).epsilon(tol));
    CHECK(LUT.ReverseLookup(distPerp, D * 0.0460519) / D ==
          Approx(.05).epsilon(tol));
    CHECK(LUT.ReverseLookup(distPerp, D * 0.3221280) / D ==
          Approx(.35).epsilon(tol));
    CHECK(LUT.ReverseLookup(distPerp, D * 0.4598240) / D ==
          Approx(0.5).epsilon(tol));
    CHECK(LUT.ReverseLookup(distPerp, D * 0.9153560) / D ==
          Approx(1.0).epsilon(tol));
    CHECK(LUT.ReverseLookup(distPerp, D * 1.3619600) / D ==
          Approx(1.5).epsilon(tol));
    CHECK(LUT.ReverseLookup(distPerp, D * 1.7944600) / D ==
          Approx(2.0).epsilon(tol));
    CHECK(LUT.ReverseLookup(distPerp, D * 2.2071800) / D ==
          Approx(2.5).epsilon(tol));
    CHECK(LUT.ReverseLookup(distPerp, D * 3.2701500) / D ==
          Approx(4.0).epsilon(tol));
    CHECK(LUT.ReverseLookup(distPerp, D * 3.7911500) / D ==
          Approx(5.0).epsilon(tol));
    CHECK(LUT.ReverseLookup(distPerp, D * 4.1524200) / D ==
          Approx(6.0).epsilon(tol));
    // CHECK(relError(LUT.ReverseLookup(distPerp / D, 4.37561), 7.0) < tol);
}

TEST_CASE("REVERSE Lookup table test soft spring ", "[REVERSE lookup]") {
    const double tol = 1e-2;
    const double D = 0.024;
    const double alpha = 0.1 / (2 * 0.00411);
    const double freelength = 0.05;
    const double M = alpha * D * D;
    const double ell0 = freelength / D;

    LUTFillerEdep lut_filler(256, 256);
    lut_filler.Init(alpha, freelength, D);
    LookupTable LUT(&lut_filler);

    double distPerp = 0;
    distPerp = 0.2;
    // ("distPerp = 0.2 > D+ell0, single peaked")
    for (double sbound = 0; sbound < LUT.getNonDsbound() / 2; sbound += 0.2) {
        double val = integral(distPerp / D, 0, sbound, M, ell0);
        CHECK(LUT.ReverseLookup(distPerp, val * D) ==
              Approx(sbound * D).epsilon(tol));
    }
    // ("distPerp = 0.1 > D+ell0, single peaked")
    distPerp = 0.1;
    for (double sbound = 0; sbound < LUT.getNonDsbound() / 2; sbound += 0.2) {
        double val = integral(distPerp / D, 0, sbound, M, ell0);
        CHECK(LUT.ReverseLookup(distPerp, val * D) ==
              Approx(sbound * D).epsilon(tol));
    }
    // ("distPerp = 0.06 < D+ell0, double peaked")
    distPerp = 0.06;
    for (double sbound = 0; sbound < LUT.getNonDsbound() / 2; sbound += 0.2) {
        double val = integral(distPerp / D, 0, sbound, M, ell0);
        CHECK(LUT.ReverseLookup(distPerp, val * D) ==
              Approx(sbound * D).epsilon(tol));
    }
}

TEST_CASE("REVERSE Lookup table test medium spring ", "[REVERSE lookup]") {
    const double tol = 1e-2;
    const double D = 0.024;
    const double alpha = 1.0 / (2 * 0.00411);
    const double freelength = 0.05;
    const double M = alpha * D * D;
    const double ell0 = freelength / D;

    LUTFillerEdep lut_filler(256, 256);
    lut_filler.Init(alpha, freelength, D);
    LookupTable LUT(&lut_filler);

    double distPerp = 0;
    distPerp = 0.2;
    // ("distPerp = 0.2 > D+ell0, single peaked")
    for (double sbound = 0; sbound < LUT.getNonDsbound() / 3; sbound += 0.2) {
        double val = integral(distPerp / D, 0, sbound, M, ell0);
        // CHECK(errorPass(LUT.ReverseLookup(distPerp, val * D), sbound * D));
        CHECK(LUT.ReverseLookup(distPerp, val * D) ==
              Approx(sbound * D).epsilon(tol));
    }
    // ("distPerp = 0.1 > D+ell0, single peaked")
    distPerp = 0.1;
    for (double sbound = 0; sbound < LUT.getNonDsbound() / 2; sbound += 0.2) {
        double val = integral(distPerp / D, 0, sbound, M, ell0);
        // CHECK(errorPass(LUT.ReverseLookup(distPerp, val * D), sbound * D));
        CHECK(LUT.ReverseLookup(distPerp, val * D) ==
              Approx(sbound * D).epsilon(tol));
    }
    // ("distPerp = 0.06 < D+ell0, double peaked")
    distPerp = 0.06;
    for (double sbound = 0; sbound < LUT.getNonDsbound() / 2; sbound += 0.2) {
        double val = integral(distPerp / D, 0, sbound, M, ell0);
        // CHECK(errorPass(LUT.ReverseLookup(distPerp, val * D), sbound * D));
        CHECK(LUT.ReverseLookup(distPerp, val * D) ==
              Approx(sbound * D).epsilon(tol));
    }
}

TEST_CASE("REVERSE Lookup table test stiff spring ", "[REVERSE lookup]") {
    const double tol = 1e-2;

    const double D = 0.024;
    const double alpha = 10.0 / (2 * 0.00411);
    const double freelength = 0.05;
    const double M = alpha * D * D;
    const double ell0 = freelength / D;

    LUTFillerEdep lut_filler(256, 256);
    lut_filler.Init(alpha, freelength, D);
    LookupTable LUT(&lut_filler);

    double distPerp = 0;
    // ("distPerp = 0.2 > D+ell0, single peaked")
    // WARNING: This reverse lookup fails because function is too flat
    // distPerp = 0.2;
    // for (double sbound = 0; sbound < LUT.getNonDsbound() / 8; sbound += 0.1)
    // {
    //    double val = integral(distPerp / D, 0, sbound, M, ell0);
    //    double scalc = LUT.ReverseLookup(distPerp, val * D);
    //    printf("scalc = %f\n", scalc);
    //    printf("sbound = %f\n", sbound * D);
    //    CHECK(errorPass(scalc, sbound * D, REVERSEFAC));
    //}

    // ("distPerp = 0.1 > D+ell0, single peaked")
    distPerp = 0.1;
    for (double sbound = 0; sbound < LUT.getNonDsbound() / 2; sbound += 0.1) {
        double val = integral(distPerp / D, 0, sbound, M, ell0);
        CHECK(LUT.ReverseLookup(distPerp, val * D) ==
              Approx(sbound * D).margin(tol));
        // CHECK(errorPass(LUT.ReverseLookup(distPerp, val * D), sbound * D,
        // REVERSEFAC));
    }
    // ("distPerp = 0.06 < D+ell0, double peaked")
    distPerp = 0.06;
    for (double sbound = 0; sbound < LUT.getNonDsbound() / 2; sbound += 0.1) {
        double val = integral(distPerp / D, 0, sbound, M, ell0);
        CHECK(LUT.ReverseLookup(distPerp, val * D) ==
              Approx(sbound * D).epsilon(tol));
        // CHECK(errorPass(LUT.ReverseLookup(distPerp, val * D), sbound * D,
        // REVERSEFAC));
    }
}

TEST_CASE("REVERSE binary Lookup with different springs", "[REVERSE binary]") {

    const double D = 0.024;
    const double freelength = 0.05;
    const double ell0 = freelength / D;
    double alpha, M;

    LUTFillerEdep lut_filler(256, 256);

    double rowIndexMax = lut_filler.getDistPerpGridNum();
    double colIndexMax = lut_filler.getDistParaGridNum();
    double distPerpSpacing;

    SECTION("Soft spring") {
        alpha = 0.1 / (2 * 0.00411);
        M = alpha * D * D;

        lut_filler.Init(alpha, freelength, D);
        LookupTable LUT(&lut_filler);

        distPerpSpacing = LUT.perp_spacing_;
        for (int i = 0; i < rowIndexMax - 2; ++i) {
            double C0 = LUT.table_[LUT.getTableIndex(i, colIndexMax - 1)] * D;
            double C1 =
                LUT.table_[LUT.getTableIndex(i + 1, colIndexMax - 1)] * D;
            double Cavg = .5 * (C0 + C1);
            double distPerpAvg = distPerpSpacing * (i + .5) * D;
            double sbound = LUT.ReverseLookup(distPerpAvg, Cavg);
            double Cintegral =
                integral(distPerpAvg / D, 0, sbound / D, M, ell0) * D;
            CHECK(Cintegral == Approx(Cavg).epsilon(RELTOL));
        }
    }

    SECTION("Medium spring") {
        alpha = 1.0 / (2 * 0.00411);
        M = alpha * D * D;

        lut_filler.Init(alpha, freelength, D);
        LookupTable LUT(&lut_filler);

        distPerpSpacing = LUT.perp_spacing_;
        for (int i = 0; i < rowIndexMax - 2; ++i) {
            double C0 = LUT.table_[LUT.getTableIndex(i, colIndexMax - 1)] * D;
            double C1 =
                LUT.table_[LUT.getTableIndex(i + 1, colIndexMax - 1)] * D;
            double Cavg = .5 * (C0 + C1);
            double distPerpAvg = distPerpSpacing * (i + .5) * D;
            double sbound = LUT.ReverseLookup(distPerpAvg, Cavg);
            double Cintegral =
                integral(distPerpAvg / D, 0, sbound / D, M, ell0) * D;
            CHECK(Cintegral == Approx(Cavg).epsilon(RELTOL));
        }
    }

    SECTION("Stiff spring") {
        alpha = 10. / (2 * 0.00411);
        M = alpha * D * D;

        lut_filler.Init(alpha, freelength, D);
        LookupTable LUT(&lut_filler);

        distPerpSpacing = LUT.perp_spacing_;
        for (int i = 0; i < rowIndexMax - 2; ++i) {
            double C0 = LUT.table_[LUT.getTableIndex(i, colIndexMax - 1)] * D;
            double C1 =
                LUT.table_[LUT.getTableIndex(i + 1, colIndexMax - 1)] * D;
            double Cavg = .5 * (C0 + C1);
            double distPerpAvg = distPerpSpacing * (i + .5) * D;
            double sbound = LUT.ReverseLookup(distPerpAvg, Cavg);
            double Cintegral =
                integral(distPerpAvg / D, 0, sbound / D, M, ell0) * D;
            CHECK(Cintegral == Approx(Cavg).margin(1e-5));
        }
    }
}

TEST_CASE("Test the calculation of binding volume.", "[bind volume]") {
    const double D = 0.024;
    const double freelength = 0.05;
    double alpha = 1. / (2 * 0.00411);
    const double ell0 = freelength / D;
    double M = alpha * D * D;

    LUTFillerEdep lut_filler(256, 256);

    for (double i = 0.1; i < 1.0; i += .1) {
        lut_filler.Init(alpha * i, freelength, D);
        double bind_vol =
            bind_vol_integral(lut_filler.getUpperBound(), i * M, ell0);
        REQUIRE(lut_filler.getBindingVolume() ==
                Approx(bind_vol).epsilon(1e-8));
    }
}


#include <CGAL/Exact_predicates_inexact_constructions_kernel.h>
#include <CGAL/Delaunay_triangulation_2.h>
#include <CGAL/boost/graph/graph_traits_Delaunay_triangulation_2.h>

#include <boost/graph/kruskal_min_spanning_tree.hpp>
#include <boost/graph/filtered_graph.hpp>
#include <fstream>

typedef CGAL::Exact_predicates_inexact_constructions_kernel K;
typedef K::Point_2 Point;

typedef CGAL::Delaunay_triangulation_2<K> Triangulation;

// As we only consider finite vertices and edges
// we need the following filter

template <typename T>
struct Is_finite {

  const T* t_;

  Is_finite()
    : t_(NULL)
  {}

  Is_finite(const T& t)
    : t_(&t)
  { }

  template <typename VertexOrEdge>
  bool operator()(const VertexOrEdge& voe) const {
    return ! t_->is_infinite(voe);
  }
};

typedef Is_finite<Triangulation> Filter;
typedef boost::filtered_graph<Triangulation,Filter,Filter> Finite_triangulation;
typedef boost::graph_traits<Finite_triangulation>::vertex_descriptor vertex_descriptor;
typedef boost::graph_traits<Finite_triangulation>::vertex_iterator vertex_iterator;
typedef boost::graph_traits<Finite_triangulation>::edge_descriptor edge_descriptor;

// The BGL makes use of indices associated to the vertices
// We use a std::map to store the index
typedef std::map<vertex_descriptor,int> VertexIndexMap;
VertexIndexMap vertex_id_map;

// A std::map is not a property map, because it is not lightweight
typedef boost::associative_property_map<VertexIndexMap> VertexIdPropertyMap;
VertexIdPropertyMap vertex_index_pmap(vertex_id_map);

int
main(int argc,char* argv[])
{
  const char* filename = (argc > 1) ? argv[1] : "data/points.xy";
  std::ifstream input(filename);
  Triangulation t;
  Filter is_finite(t);
  Finite_triangulation ft(t, is_finite, is_finite);

  Point p ;
  while(input >> p){
    t.insert(p);
  }

  vertex_iterator vit, ve;
  // Associate indices to the vertices
  int index = 0;
  // boost::tie assigns the first and second element of the std::pair
  // returned by boost::vertices to the variables vit and ve
  for(boost::tie(vit,ve)=boost::vertices(ft); vit!=ve; ++vit ){
    vertex_descriptor  vd = *vit;
    vertex_id_map[vd]= index++;
    }

  // We use the default edge weight which is the squared length of the edge
  // This property map is defined in graph_traits_Triangulation_2.h

  // In the function call you can see a named parameter: vertex_index_map
   std::list<edge_descriptor> mst;
   boost::kruskal_minimum_spanning_tree(ft,
					std::back_inserter(mst),
					vertex_index_map(vertex_index_pmap));

   std::cout << "The edges of the Euclidean mimimum spanning tree:" << std::endl;

   for(std::list<edge_descriptor>::iterator it = mst.begin(); it != mst.end(); ++it){
     edge_descriptor ed = *it;
     vertex_descriptor svd = source(ed,t);
     vertex_descriptor tvd = target(ed,t);
     Triangulation::Vertex_handle sv = svd;
     Triangulation::Vertex_handle tv = tvd;
     std::cout << "[ " << sv->point() << "  |  " << tv->point() << " ] " << std::endl;
   }

   return 0;
}


/// @file simple.hpp Sampling for various simple base distributions

#ifndef BIGGLES_SAMPLING_SIMPLE_HPP__
#define BIGGLES_SAMPLING_SIMPLE_HPP__

#include <numeric> // std::partial_sum
#include <vector>
#include <boost/tuple/tuple.hpp>

#include "../detail/random.hpp"
#include "../detail/fun.hpp"
#include "../observation.hpp"
#include "../observation_collection.hpp"

namespace biggles {

/// @brief Utility sampling methods and functions.
namespace sampling
{

/// @brief Sample an integer from a uniform distribution on the interval [\p first, \p last).
///
/// @note This function will never return an integer equal to \p last.
///
/// @param first
/// @param last
int uniform_int(int first, int last);

/// @brief Sample a real uniformly from the interval [\p first, \p last).
///
/// @param first
/// @param last
float uniform_real(float first = 0.f, float last = 1.f);

/// @brief Sample a floating point value from the exponential distribution with rate parameter \p lambda.
///
/// @param lambda
float exponential(float lambda = 1.f);

/// @brief Sample a Poisson variate from a distribution with mean \p lambda.
///
/// @param lambda
int poisson(float lambda);

/// @brief Sample an iterator uniformly from the range [\p first, \p last).
///
/// If \p first = \p last, then return \p last.
///
/// @note This is currently only implemented for \c InputIterator types. Really there should be a specialisation for
/// a \c RandomAccessIterator. This is a FIXME for someone enthused to learn about iterator traits.
///
/// @tparam InputIterator
/// @param first
/// @param last
template<typename InputIterator>
InputIterator from_range(InputIterator first, InputIterator last);

/// @brief Sample an iterator uniformly from the range [\p first, \p last).
///
/// If \p first = \p last, then return \p last. The reference \p log_prob is only written to when \p first != \p last.
///
/// @note This is currently only implemented for \c InputIterator types. Really there should be a specialisation for
/// a \c RandomAccessIterator. This is a FIXME for someone enthused to learn about iterator traits.
///
/// @tparam InputIterator
/// @param first
/// @param last
/// @param[out] log_prob write the log probability of having sampled the return value to this reference
template<typename InputIterator>
InputIterator from_range(InputIterator first, InputIterator last, float& log_prob);

/*
/// @brief Sample an observation from within a light cone.
///
/// Given the definition of a light cone as in biggles::observations_within_light_cone(), sample an observation from it
/// with the following procedure:
///
/// - sample uniformly some time stamp within the light cone
/// - sample uniformly from the observations with that time stamp
///
/// The log probability of drawing the sample is returned in \p output_log_prob and the sample itself is returned in \p
/// output_obs.
///
/// @note The sampling procedure may sometimes fail to sample an observation. In this case the output references are
/// untouched and the function returns \p false.
///
/// @sa observations_within_light_cone()
/// @sa log_prob_of_sampling_from_light_cone()
///
/// @param observations
/// @param start
/// @param speed_of_light
/// @param first_time_stamp
/// @param last_time_stamp
/// @param[out] output_obs
/// @param[out] output_log_prob
///
/// @return \c true iff a sample could be drawn
bool observation_from_light_cone(const observation_collection& observations,
                                 const observation& start,
                                 float speed_of_light,
                                 time_stamp first_time_stamp,
                                 time_stamp last_time_stamp,
                                 observation& output_obs,
                                 float& output_log_prob);
                                 */

/// @brief Return the log probability of having sampled an observation from a light cone.
///
/// This is the distribution from which observation_from_light_cone() actually samples. If \p o could never have
/// been sampled, this returns \c -FLT_MAX as a place holder for negative infinity.
///
/// The parameters are as those in observation_from_light_cone().
///
/// @sa observations_within_light_cone()
/// @sa observation_from_light_cone()
///
/// @param o
/// @param observations
/// @param start
/// @param speed_of_light
/// @param first_time_stamp
/// @param last_time_stamp
float log_prob_of_sampling_from_light_cone(const observation& o,
                                           const observation_collection& observations,
                                           const observation& start,
                                           float speed_of_light,
                                           time_stamp first_time_stamp,
                                           time_stamp last_time_stamp);

/// @brief Sample an observation from an observation collection
///
/// Sample an observation from \p oc using the following procedure:
///
/// - sample a time stamp uniformly from the range covered by \p oc;
/// - sample an observation uniformly from those in \p oc with that timestamp
///
/// It is possible for this procedure to fail to sample an observation in which case this function returns \c false. If
/// the sampling succeeded, the sample is written to \p output_obs and the log probability of having sampled it is
/// written to \p output_log_prob.
///
/// @sa log_prob_of_sampling_observation()
///
/// @param oc Sample an observation from this collection
/// @param[out] output_obs
/// @param[out] output_log_prob
///
/// @return \c true iff an observation was sampled
bool observation_from_collection(const observation_collection& oc,
                                 observation& output_obs,
                                 float& output_log_prob);

/// @brief The distribution from which observation() samples.
///
/// This is the distribution from which observation() actually samples. If \p o could never have been sampled,
/// this returns \c -FLT_MAX as a place holder for negative infinity.
///
/// The parameters are as those in observation().
///
/// @param observation The observation sampled
/// @param oc The collection from which it was sampled
float log_prob_of_sampling_observation(const observation& observation,
                                       const observation_collection& oc);

/// @brief samples from a range of \p items with probabilities proporational to \p weights
///
/// @param items_begin the iterator pointing to the begin of \p items
/// @param weights_begin the iterator pointing to the begin of \p weights
/// @param weights_end the iterator pointing to the end of \p weights
///
/// @return the sampled item
template <class ITEMITER, class WEIGHTITER>
typename ITEMITER::value_type
weighted_choice(ITEMITER items_begin, WEIGHTITER weights_begin, WEIGHTITER weights_end) {
    std::vector<float> partsum(std::distance(weights_begin, weights_end));
    std::partial_sum(weights_begin, weights_end, partsum.begin());
    size_t i = std::distance( partsum.begin(),
        std::lower_bound( partsum.begin(), partsum.end(), uniform_real(0.f, partsum.back()) )
    );
    return *(items_begin+i);
}

/// @brief samples from a range of \p items with probabilities proporational to \p weights
///
/// @param items_begin the iterator pointing to the begin of \p items
/// @param weights_begin the iterator pointing to the begin of \p weights
/// @param weights_end the iterator pointing to the end of \p weights
/// @param log_prob the log-probability to sample the returned item
///
/// @return the sampled item
template <class ITEMITER, class WEIGHTITER>
typename ITEMITER::value_type
weighted_choice(ITEMITER items_begin, WEIGHTITER weights_begin, WEIGHTITER weights_end, float& log_prob) {
    std::vector<float> partsum(std::distance(weights_begin, weights_end));
    float total = std::accumulate(weights_begin, weights_end, 0.0);
    std::partial_sum(weights_begin, weights_end, partsum.begin());
    size_t i = std::distance( partsum.begin(),
        std::lower_bound( partsum.begin(), partsum.end(), uniform_real(0.f, partsum.back()) )
    );
    log_prob = logf(*(weights_begin+i)/total);
    std::advance(items_begin, i);
    return *items_begin;
}

/// @brief samples from a range of \p items with probabilities proporational to weights given by \em fun
///
/// @param items_begin the iterator pointing to the begin of \p items
/// @param items_end the iterator pointing to the end of \p items
/// @param fun the the weight functor
/// @param log_prob the log-probability to sample the returned item
///
/// @return the iterator to the sampled item
template <class ITEMITER, class FUN>
ITEMITER weighted_choice(ITEMITER items_begin, ITEMITER items_end, FUN& fun , float& log_prob) {
    std::vector<float> partsum(std::distance(items_begin, items_end));
    fun_partial_sum(items_begin, items_end, partsum.begin(), fun, 0.f);
    if (partsum.back() == 0) return items_end;
    size_t i = std::distance( partsum.begin(),
        std::lower_bound( partsum.begin(), partsum.end(), uniform_real(0.f, partsum.back()) )
    );
    log_prob = std::log((i == 0 ? *partsum.begin() : partsum[i] - partsum[i-1]) / partsum.back()) ;
    std::advance(items_begin, i);
    return items_begin;
}

template <class WEIGHTITER>
size_t weighted_choice_index(WEIGHTITER weights_begin, WEIGHTITER weights_end, float& log_prob) {
    std::vector<float> partsum(std::distance(weights_begin, weights_end));
    float total = std::accumulate(weights_begin, weights_end, 0.f);
    std::partial_sum(weights_begin, weights_end, partsum.begin());
    size_t i = std::distance( partsum.begin(),
        std::lower_bound( partsum.begin(), partsum.end(), uniform_real(0.f, partsum.back()) )
    );
    log_prob = logf(*(weights_begin+i)/total);
    return i;
}

/// @brief  sample inverse wishart
Eigen::Matrix2f sample_inverse_wishart(const Eigen::Matrix2f& Phi, size_t s);
/// @brief  sample wishart
Eigen::Matrix2f sample_wishart(const Eigen::Matrix2f& Phi, size_t s);

/// @brief A simple functor yielding a uniformly sampled real from the interval [0, 1).
struct uniform_real_functor
{
    typedef float result_type;
    float operator () () const { return sampling::uniform_real(); }
};

float sample_normal();

/// @brief A default uniform real generator.
///
/// This is a convenience functor struct which can be used to get a uniform variate on the interval [0,1]. It is the
/// default source of randomness for metropolis_hastings::sampler<> unless otherwise specified.
///
/// It is implemented using an internal boost Mersenne twister random number generator and a uniform real
/// distribution.
struct boost_uniform
{
    boost_uniform() : dist_(0.,1.), uni_real_(biggles::detail::random_generator, dist_) { }
    boost_uniform(const boost_uniform&) : dist_(0.,1.), uni_real_(biggles::detail::random_generator, dist_) { }
    const boost_uniform& operator = (const boost_uniform&) { return *this; }

    float operator() () { return uni_real_(); }
protected:
    boost::uniform_real<float> dist_;
    boost::variate_generator<boost::mt19937&, boost::uniform_real<float> > uni_real_;
};

/// @brief Sample a Beta variate from a distribution with shape parameters \p alpha and \p beta.
///
/// @param alpha
/// @param beta
float sample_beta(float alpha, float beta);

/** \brief Sample a truncated Beta variate from a distribution with shape parameters \p alpha and \p beta.
 * and limits \p lo_limit and \p hi_limit
 *
 * The truncated beta is defined by:
 * * p(x; alpha, beta) = B(x; alpha, beta)/B([lo_limit, hi_limit]; alpha, beta) if \b x is in [lo_limit, hi_limit]
 * * p(x; alpha, beta) = 0 if \b x is not in  [lo_limit, hi_limit]
 *
 */
float sample_truncated_beta(float alpha, float beta, float lo_limit, float hi_limit);

/// @brief Sample a Gamma variate from a distribution with shape parmater \p k and scale parameter \p theta.
///
/// @param k shape
/// @param theta scale
float sample_gamma(unsigned int k, float theta);

/// @brief Sample a Gamma variate from a distribution with shape parmater \p k and scale parameter \p theta.
///
/// Ensures that the result is > 0.0f
/// @param k shape
/// @param theta scale
float sample_gamma0(unsigned int k, float theta);

template<typename Real, int Dim>
Eigen::Matrix<Real, Dim, 1> sample_multivariate_gaussian(const Eigen::Matrix<Real, Dim, 1>& mean,
                                                         const Eigen::Matrix<Real, Dim, Dim>& covariance)
{
    // this uses http://en.wikipedia.org/wiki/Multivariate_normal_distribution#Drawing_values_from_the_distribution

    typedef Eigen::Matrix<Real, Dim, Dim> Matrix;
    typedef Eigen::Matrix<Real, Dim, 1> Vector;

    Vector sample;

    for(int i=0; i<Dim; ++i)
        sample(i) = sample_normal();

    Matrix cholesky_decomposition = covariance.llt().matrixL();

    return mean + cholesky_decomposition * sample;
}

}

}

#define WITHIN_BIGGLES_SAMPLING_SIMPLE_HPP__
#include "simple.tcc"
#undef WITHIN_BIGGLES_SAMPLING_SIMPLE_HPP__

#endif // BIGGLES_SAMPLING_SIMPLE_HPP__