// Copyright Nick Thompson, 2017
// Use, modification and distribution are 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)

#define BOOST_TEST_MODULE Gauss Kronrod_quadrature_test

#include <complex>
#include <boost/config.hpp>
#include <boost/detail/workaround.hpp>

#if !defined(BOOST_NO_CXX11_DECLTYPE) && !defined(BOOST_NO_CXX11_TRAILING_RESULT_TYPES) && !defined(BOOST_NO_SFINAE_EXPR)

#include <boost/math/concepts/real_concept.hpp>
#include <boost/test/included/unit_test.hpp>
#include <boost/test/tools/floating_point_comparison.hpp>
#include <boost/math/quadrature/gauss_kronrod.hpp>
#include <boost/math/special_functions/sinc.hpp>
#include <boost/multiprecision/cpp_bin_float.hpp>
#include <boost/multiprecision/cpp_dec_float.hpp>
#include <boost/multiprecision/debug_adaptor.hpp>

#ifdef BOOST_HAS_FLOAT128
#include <boost/multiprecision/complex128.hpp>
#endif

#if !defined(TEST1) && !defined(TEST1A) && !defined(TEST2) && !defined(TEST3)
#  define TEST1
#  define TEST1A
#  define TEST2
#  define TEST3
#endif

#ifdef _MSC_VER
#pragma warning(disable:4127)  // Conditional expression is constant
#endif

using std::expm1;
using std::atan;
using std::tan;
using std::log;
using std::log1p;
using std::asinh;
using std::atanh;
using std::sqrt;
using std::isnormal;
using std::abs;
using std::sinh;
using std::tanh;
using std::cosh;
using std::pow;
using std::exp;
using std::sin;
using std::cos;
using std::string;
using boost::math::quadrature::gauss_kronrod;
using boost::math::constants::pi;
using boost::math::constants::half_pi;
using boost::math::constants::two_div_pi;
using boost::math::constants::two_pi;
using boost::math::constants::half;
using boost::math::constants::third;
using boost::math::constants::half;
using boost::math::constants::third;
using boost::math::constants::catalan;
using boost::math::constants::ln_two;
using boost::math::constants::root_two;
using boost::math::constants::root_two_pi;
using boost::math::constants::root_pi;
using boost::multiprecision::cpp_bin_float_quad;
using boost::multiprecision::cpp_dec_float_50;
using boost::multiprecision::debug_adaptor;
using boost::multiprecision::number;

//
// Error rates depend only on the number of points in the approximation, not the type being tested,
// define all our expected errors here:
//

enum
{
   test_ca_error_id,
   test_ca_error_id_2,
   test_three_quad_error_id,
   test_three_quad_error_id_2,
   test_integration_over_real_line_error_id,
   test_right_limit_infinite_error_id,
   test_left_limit_infinite_error_id
};

template <unsigned Points>
double expected_error(unsigned)
{
   return 0; // placeholder, all tests will fail
}

template <>
double expected_error<15>(unsigned id)
{
   switch (id)
   {
   case test_ca_error_id:
      return 1e-7;
   case test_ca_error_id_2:
      return 2e-5;
   case test_three_quad_error_id:
      return 1e-8;
   case test_three_quad_error_id_2:
      return 3.5e-3;
   case test_integration_over_real_line_error_id:
      return 6e-3;
   case test_right_limit_infinite_error_id:
   case test_left_limit_infinite_error_id:
      return 1e-5;
   }
   return 0;  // placeholder, all tests will fail
}

template <>
double expected_error<17>(unsigned id)
{
   switch (id)
   {
   case test_ca_error_id:
      return 1e-7;
   case test_ca_error_id_2:
      return 2e-5;
   case test_three_quad_error_id:
      return 1e-8;
   case test_three_quad_error_id_2:
      return 3.5e-3;
   case test_integration_over_real_line_error_id:
      return 6e-3;
   case test_right_limit_infinite_error_id:
   case test_left_limit_infinite_error_id:
      return 1e-5;
   }
   return 0;  // placeholder, all tests will fail
}

template <>
double expected_error<21>(unsigned id)
{
   switch (id)
   {
   case test_ca_error_id:
      return 1e-12;
   case test_ca_error_id_2:
      return 3e-6;
   case test_three_quad_error_id:
      return 2e-13;
   case test_three_quad_error_id_2:
      return 2e-3;
   case test_integration_over_real_line_error_id:
      return 6e-3;  // doesn't get any better with more points!
   case test_right_limit_infinite_error_id:
   case test_left_limit_infinite_error_id:
      return 5e-8;
   }
   return 0;  // placeholder, all tests will fail
}

template <>
double expected_error<31>(unsigned id)
{
   switch (id)
   {
   case test_ca_error_id:
      return 6e-20;
   case test_ca_error_id_2:
      return 3e-7;
   case test_three_quad_error_id:
      return 1e-19;
   case test_three_quad_error_id_2:
      return 6e-4;
   case test_integration_over_real_line_error_id:
      return 6e-3;  // doesn't get any better with more points!
   case test_right_limit_infinite_error_id:
   case test_left_limit_infinite_error_id:
      return 5e-11;
   }
   return 0;  // placeholder, all tests will fail
}

template <>
double expected_error<41>(unsigned id)
{
   switch (id)
   {
   case test_ca_error_id:
      return 1e-26;
   case test_ca_error_id_2:
      return 1e-7;
   case test_three_quad_error_id:
      return 3e-27;
   case test_three_quad_error_id_2:
      return 3e-4;
   case test_integration_over_real_line_error_id:
      return 5e-5;  // doesn't get any better with more points!
   case test_right_limit_infinite_error_id:
   case test_left_limit_infinite_error_id:
      return 1e-15;
   }
   return 0;  // placeholder, all tests will fail
}

template <>
double expected_error<51>(unsigned id)
{
   switch (id)
   {
   case test_ca_error_id:
      return 5e-33;
   case test_ca_error_id_2:
      return 1e-8;
   case test_three_quad_error_id:
      return 1e-32;
   case test_three_quad_error_id_2:
      return 3e-4;
   case test_integration_over_real_line_error_id:
      return 1e-14;
   case test_right_limit_infinite_error_id:
   case test_left_limit_infinite_error_id:
      return 3e-19;
   }
   return 0;  // placeholder, all tests will fail
}

template <>
double expected_error<61>(unsigned id)
{
   switch (id)
   {
   case test_ca_error_id:
      return 5e-34;
   case test_ca_error_id_2:
      return 5e-9;
   case test_three_quad_error_id:
      return 4e-34;
   case test_three_quad_error_id_2:
      return 1e-4;
   case test_integration_over_real_line_error_id:
      return 1e-16;
   case test_right_limit_infinite_error_id:
   case test_left_limit_infinite_error_id:
      return 3e-23;
   }
   return 0;  // placeholder, all tests will fail
}

template<class Real, unsigned Points>
void test_linear()
{
    std::cout << "Testing linear functions are integrated properly by gauss_kronrod on type " << boost::typeindex::type_id<Real>().pretty_name() << "\n";
    Real tol = boost::math::tools::epsilon<Real>() * 10;
    Real error;
    auto f = [](const Real& x)->Real
    {
       return 5*x + 7;
    };
    Real L1;
    Real Q = gauss_kronrod<Real, Points>::integrate(f, (Real) 0, (Real) 1, 0, 0, &error, &L1);
    BOOST_CHECK_CLOSE_FRACTION(Q, 9.5, tol);
    BOOST_CHECK_CLOSE_FRACTION(L1, 9.5, tol);

    Q = gauss_kronrod<Real, Points>::integrate(f, (Real) 1, (Real) 0, 0, 0, &error, &L1);
    BOOST_CHECK_CLOSE_FRACTION(Q, -9.5, tol);
    BOOST_CHECK_CLOSE_FRACTION(L1, 9.5, tol);

    Q = gauss_kronrod<Real, Points>::integrate(f, (Real) 0, (Real) 0, 0, 0, &error, &L1);
    BOOST_CHECK_CLOSE(Q, Real(0), tol);
}

template<class Real, unsigned Points>
void test_quadratic()
{
    std::cout << "Testing quadratic functions are integrated properly by Gauss Kronrod on type " << boost::typeindex::type_id<Real>().pretty_name() << "\n";
    Real tol = boost::math::tools::epsilon<Real>() * 10;
    Real error;

    auto f = [](const Real& x)->Real { return 5*x*x + 7*x + 12; };
    Real L1;
    Real Q = gauss_kronrod<Real, Points>::integrate(f, 0, 1, 0, 0, &error, &L1);
    BOOST_CHECK_CLOSE_FRACTION(Q, (Real) 17 + half<Real>()*third<Real>(), tol);
    BOOST_CHECK_CLOSE_FRACTION(L1, (Real) 17 + half<Real>()*third<Real>(), tol);
}

// Examples taken from
//http://crd-legacy.lbl.gov/~dhbailey/dhbpapers/quadrature.pdf
template<class Real, unsigned Points>
void test_ca()
{
    std::cout << "Testing integration of C(a) on type " << boost::typeindex::type_id<Real>().pretty_name() << "\n";
    Real tol = expected_error<Points>(test_ca_error_id);
    Real L1;
    Real error;

    auto f1 = [](const Real& x)->Real { return atan(x)/(x*(x*x + 1)) ; };
    Real Q = gauss_kronrod<Real, Points>::integrate(f1, 0, 1, 0, 0, &error, &L1);
    Real Q_expected = pi<Real>()*ln_two<Real>()/8 + catalan<Real>()*half<Real>();
    BOOST_CHECK_CLOSE_FRACTION(Q, Q_expected, tol);
    BOOST_CHECK_CLOSE_FRACTION(L1, Q_expected, tol);

    auto f2 = [](Real x)->Real { Real t0 = x*x + 1; Real t1 = sqrt(t0); return atan(t1)/(t0*t1); };
    Q = gauss_kronrod<Real, Points>::integrate(f2, 0 , 1, 0, 0, &error, &L1);
    Q_expected = pi<Real>()/4 - pi<Real>()/root_two<Real>() + 3*atan(root_two<Real>())/root_two<Real>();
    BOOST_CHECK_CLOSE_FRACTION(Q, Q_expected, tol);
    BOOST_CHECK_CLOSE_FRACTION(L1, Q_expected, tol);

    tol = expected_error<Points>(test_ca_error_id_2);
    auto f5 = [](Real t)->Real { return t*t*log(t)/((t*t - 1)*(t*t*t*t + 1)); };
    Q = gauss_kronrod<Real, Points>::integrate(f5, 0, 1, 0);
    Q_expected = pi<Real>()*pi<Real>()*(2 - root_two<Real>())/32;
    BOOST_CHECK_CLOSE_FRACTION(Q, Q_expected, tol);
}

template<class Real, unsigned Points>
void test_three_quadrature_schemes_examples()
{
    std::cout << "Testing integral in 'A Comparison of Three High Precision Quadrature Schemes' on type " << boost::typeindex::type_id<Real>().pretty_name() << "\n";
    Real tol = expected_error<Points>(test_three_quad_error_id);
    Real Q;
    Real Q_expected;

    // Example 1:
    auto f1 = [](const Real& t)->Real { return t*boost::math::log1p(t); };
    Q = gauss_kronrod<Real, Points>::integrate(f1, 0 , 1, 0);
    Q_expected = half<Real>()*half<Real>();
    BOOST_CHECK_CLOSE_FRACTION(Q, Q_expected, tol);

    // Example 2:
    auto f2 = [](const Real& t)->Real { return t*t*atan(t); };
    Q = gauss_kronrod<Real, Points>::integrate(f2, 0 , 1, 0);
    Q_expected = (pi<Real>() -2 + 2*ln_two<Real>())/12;
    BOOST_CHECK_CLOSE_FRACTION(Q, Q_expected, 2 * tol);

    // Example 3:
    auto f3 = [](const Real& t)->Real { return exp(t)*cos(t); };
    Q = gauss_kronrod<Real, Points>::integrate(f3, 0, half_pi<Real>(), 0);
    Q_expected = boost::math::expm1(half_pi<Real>())*half<Real>();
    BOOST_CHECK_CLOSE_FRACTION(Q, Q_expected, tol);

    // Example 4:
    auto f4 = [](Real x)->Real { Real t0 = sqrt(x*x + 2); return atan(t0)/(t0*(x*x+1)); };
    Q = gauss_kronrod<Real, Points>::integrate(f4, 0 , 1, 0);
    Q_expected = 5*pi<Real>()*pi<Real>()/96;
    BOOST_CHECK_CLOSE_FRACTION(Q, Q_expected, tol);

    tol = expected_error<Points>(test_three_quad_error_id_2);
    // Example 5:
    auto f5 = [](const Real& t)->Real { return sqrt(t)*log(t); };
    Q = gauss_kronrod<Real, Points>::integrate(f5, 0 , 1, 0);
    Q_expected = -4/ (Real) 9;
    BOOST_CHECK_CLOSE_FRACTION(Q, Q_expected, tol);

    // Example 6:
    auto f6 = [](const Real& t)->Real { return sqrt(1 - t*t); };
    Q = gauss_kronrod<Real, Points>::integrate(f6, 0 , 1, 0);
    Q_expected = pi<Real>()/4;
    BOOST_CHECK_CLOSE_FRACTION(Q, Q_expected, tol);
}

template<class Real, unsigned Points>
void test_integration_over_real_line()
{
    std::cout << "Testing integrals over entire real line in 'A Comparison of Three High Precision Quadrature Schemes' on type " << boost::typeindex::type_id<Real>().pretty_name() << "\n";
    Real tol = expected_error<Points>(test_integration_over_real_line_error_id);
    Real Q;
    Real Q_expected;
    Real L1;
    Real error;

    auto f1 = [](const Real& t)->Real { return 1/(1+t*t);};
    Q = gauss_kronrod<Real, Points>::integrate(f1, -boost::math::tools::max_value<Real>(), boost::math::tools::max_value<Real>(), 0, 0, &error, &L1);
    Q_expected = pi<Real>();
    BOOST_CHECK_CLOSE_FRACTION(Q, Q_expected, tol);
    BOOST_CHECK_CLOSE_FRACTION(L1, Q_expected, tol);
}

template<class Real, unsigned Points>
void test_right_limit_infinite()
{
    std::cout << "Testing right limit infinite for Gauss Kronrod in 'A Comparison of Three High Precision Quadrature Schemes' on type " << boost::typeindex::type_id<Real>().pretty_name() << "\n";
    Real tol = expected_error<Points>(test_right_limit_infinite_error_id);
    Real Q;
    Real Q_expected;
    Real L1;
    Real error;

    // Example 11:
    auto f1 = [](const Real& t)->Real { return 1/(1+t*t);};
    Q = gauss_kronrod<Real, Points>::integrate(f1, 0, boost::math::tools::max_value<Real>(), 0, 0, &error, &L1);
    Q_expected = half_pi<Real>();
    BOOST_CHECK_CLOSE(Q, Q_expected, 100*tol);

    auto f4 = [](const Real& t)->Real { return 1/(1+t*t); };
    Q = gauss_kronrod<Real, Points>::integrate(f4, 1, boost::math::tools::max_value<Real>(), 0, 0, &error, &L1);
    Q_expected = pi<Real>()/4;
    BOOST_CHECK_CLOSE(Q, Q_expected, 100*tol);
}

template<class Real, unsigned Points>
void test_left_limit_infinite()
{
    std::cout << "Testing left limit infinite for Gauss Kronrod in 'A Comparison of Three High Precision Quadrature Schemes' on type " << boost::typeindex::type_id<Real>().pretty_name() << "\n";
    Real tol = expected_error<Points>(test_left_limit_infinite_error_id);
    Real Q;
    Real Q_expected;

    // Example 11:
    auto f1 = [](const Real& t)->Real { return 1/(1+t*t);};
    Q = gauss_kronrod<Real, Points>::integrate(f1, -boost::math::tools::max_value<Real>(), Real(0), 0);
    Q_expected = half_pi<Real>();
    BOOST_CHECK_CLOSE(Q, Q_expected, 100*tol);
}

template<class Complex>
void test_complex_lambert_w()
{
    std::cout << "Testing that complex-valued integrands are integrated correctly by Gaussian quadrature on type " << boost::typeindex::type_id<Complex>().pretty_name() << "\n";
    typedef typename Complex::value_type Real;
    Real tol = 10e-9;
    using boost::math::constants::pi;
    Complex z{2, 3};
    auto lw = [&z](Real v)->Complex {
      using std::cos;
      using std::sin;
      using std::exp;
      Real sinv = sin(v);
      Real cosv = cos(v);

      Real cotv = cosv/sinv;
      Real cscv = 1/sinv;
      Real t = (1-v*cotv)*(1-v*cotv) + v*v;
      Real x = v*cscv*exp(-v*cotv);
      Complex den = z + x;
      Complex num = t*(z/pi<Real>());
      Complex res = num/den;
      return res;
    };

    //N[ProductLog[2+3*I], 150]
    boost::math::quadrature::gauss_kronrod<Real, 61> integrator;
    Complex Q = integrator.integrate(lw, (Real) 0, pi<Real>());
    BOOST_CHECK_CLOSE_FRACTION(Q.real(), boost::lexical_cast<Real>("1.09007653448579084630177782678166964987102108635357778056449870727913321296238687023915522935120701763447787503167111962008709116746523970476893277703"), tol);
    BOOST_CHECK_CLOSE_FRACTION(Q.imag(), boost::lexical_cast<Real>("0.530139720774838801426860213574121741928705631382703178297940568794784362495390544411799468140433404536019992695815009036975117285537382995180319280835"), tol);
}

BOOST_AUTO_TEST_CASE(gauss_quadrature_test)
{
#ifdef TEST1
    std::cout << "Testing 15 point approximation:\n";
    test_linear<double, 15>();
    test_quadratic<double, 15>();
    test_ca<double, 15>();
    test_three_quadrature_schemes_examples<double, 15>();
    test_integration_over_real_line<double, 15>();
    test_right_limit_infinite<double, 15>();
    test_left_limit_infinite<double, 15>();

    //  test one case where we do not have pre-computed constants:
    std::cout << "Testing 17 point approximation:\n";
    test_linear<double, 17>();
    test_quadratic<double, 17>();
    test_ca<double, 17>();
    test_three_quadrature_schemes_examples<double, 17>();
    test_integration_over_real_line<double, 17>();
    test_right_limit_infinite<double, 17>();
    test_left_limit_infinite<double, 17>();
    test_complex_lambert_w<std::complex<double>>();
    test_complex_lambert_w<std::complex<long double>>();
#endif
#ifdef TEST1A
    std::cout << "Testing 21 point approximation:\n";
    test_linear<cpp_bin_float_quad, 21>();
    test_quadratic<cpp_bin_float_quad, 21>();
    test_ca<cpp_bin_float_quad, 21>();
    test_three_quadrature_schemes_examples<cpp_bin_float_quad, 21>();
    test_integration_over_real_line<cpp_bin_float_quad, 21>();
    test_right_limit_infinite<cpp_bin_float_quad, 21>();
    test_left_limit_infinite<cpp_bin_float_quad, 21>();

    std::cout << "Testing 31 point approximation:\n";
    test_linear<cpp_bin_float_quad, 31>();
    test_quadratic<cpp_bin_float_quad, 31>();
    test_ca<cpp_bin_float_quad, 31>();
    test_three_quadrature_schemes_examples<cpp_bin_float_quad, 31>();
    test_integration_over_real_line<cpp_bin_float_quad, 31>();
    test_right_limit_infinite<cpp_bin_float_quad, 31>();
    test_left_limit_infinite<cpp_bin_float_quad, 31>();
#endif
#ifdef TEST2
    std::cout << "Testing 41 point approximation:\n";
    test_linear<cpp_bin_float_quad, 41>();
    test_quadratic<cpp_bin_float_quad, 41>();
    test_ca<cpp_bin_float_quad, 41>();
    test_three_quadrature_schemes_examples<cpp_bin_float_quad, 41>();
    test_integration_over_real_line<cpp_bin_float_quad, 41>();
    test_right_limit_infinite<cpp_bin_float_quad, 41>();
    test_left_limit_infinite<cpp_bin_float_quad, 41>();

    std::cout << "Testing 51 point approximation:\n";
    test_linear<cpp_bin_float_quad, 51>();
    test_quadratic<cpp_bin_float_quad, 51>();
    test_ca<cpp_bin_float_quad, 51>();
    test_three_quadrature_schemes_examples<cpp_bin_float_quad, 51>();
    test_integration_over_real_line<cpp_bin_float_quad, 51>();
    test_right_limit_infinite<cpp_bin_float_quad, 51>();
    test_left_limit_infinite<cpp_bin_float_quad, 51>();
#endif
#ifdef TEST3
    // Need at least one set of tests with expression templates turned on:
    std::cout << "Testing 61 point approximation:\n";
    test_linear<cpp_dec_float_50, 61>();
    test_quadratic<cpp_dec_float_50, 61>();
    test_ca<cpp_dec_float_50, 61>();
    test_three_quadrature_schemes_examples<cpp_dec_float_50, 61>();
    test_integration_over_real_line<cpp_dec_float_50, 61>();
    test_right_limit_infinite<cpp_dec_float_50, 61>();
    test_left_limit_infinite<cpp_dec_float_50, 61>();
#ifdef BOOST_HAS_FLOAT128
    test_complex_lambert_w<boost::multiprecision::complex128>();
#endif
#endif
}

#else

int main() { return 0; }

#endif


/*
 * Copyright (c) 2013-2019 Masahide Kashiwagi (kashi@waseda.jp)
 */

#ifndef ODE_MAFFINE_HPP
#define ODE_MAFFINE_HPP

//
// ODE using Affine and Mean Value Form
//
//  (2018/11/28) ode-maffine0 and ode-maffine are integrated by
//   porting maffine's algorithm to ode-autodif.hpp .
//
//  use -DODE_AUTODIF_NEW=0 to come back the behaviour of maffine0.
//

#include <iostream>
#include <list>
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/io.hpp>
// #include <boost/tuple/tuple.hpp>
#include <kv/interval.hpp>
#include <kv/rdouble.hpp>
#include <kv/interval-vector.hpp>
#include <kv/affine.hpp>
#include <kv/ode.hpp>
#include <kv/ode-autodif.hpp>
#include <kv/ode-param.hpp>
#include <kv/ode-callback.hpp>

namespace kv {

namespace ub = boost::numeric::ublas;

template <class T, class F>
int
ode_maffine(F f, ub::vector< affine<T> >& init, const interval<T>& start, interval<T>& end, ode_param<T> p = ode_param<T>() , ub::matrix< interval<T> >* mat = NULL, ub::vector< psa< interval<T> > >* result_psa = NULL)
{
	int n = init.size();
	int i, j;

	ub::vector< interval<T> > c;
	ub::vector< interval<T> > fc;
	ub::vector< interval<T> > I;
	ub::vector< autodif< interval<T> > > Iad;

	ub::vector< interval<T> > result_i;
	ub::matrix< interval<T> > result_d;

	ub::vector< affine<T> > result;

	int maxnum_save;

	int r;

	int ret_val;
	interval<T> end2 = end;

	I.resize(n);
	c.resize(n);
	for (i=0; i<n; i++) {
		I(i) = to_interval(init(i));
		c(i) = mid(I(i));
	}

	Iad = autodif< interval<T> >::init(I);
	// NOTICE: below must be autodif version of ode
	r = ode(f, Iad, start, end2, p, result_psa);
	if (r == 0) return 0;
	ret_val = r;
	autodif< interval<T> >::split(Iad, result_i, result_d);

	fc = c;
	// Step size should be same as above ode call.
	// Because above ode call is with autodif and interval input and
	// below ode call is without autodif and point input,
	// below ode call is supposed to be easier to succeed than above.
	// If below ode call fails, force success by increasing order.
	ode_param<T> p2 = p;
	p2.set_autostep(false);
	while (true) {
		r = ode(f, fc, start, end2, p2);
		if (r != 0) break;
		p2.order++;
		if (p.verbose == 1) {
			std::cout << "ode_maffine: increase order: " << p.order << "\n";
		}
	}

	if (p.ep_reduce == 0) {
		maxnum_save = affine<T>::maxnum();
	}

	result = fc + prod(result_d, init - c);

	if (p.ep_reduce == 0) {
		epsilon_reduce2(result, maxnum_save);
	} else {
		epsilon_reduce(result, p.ep_reduce, p.ep_reduce_limit);
	}

	init = result;
	if (ret_val == 1) end = end2;
	if (mat != NULL) *mat = result_d;

	return ret_val;
}

template <class T, class F>
int
odelong_maffine(
	F f,
	ub::vector< affine<T> >& init,
	const interval<T>& start,
	interval<T>& end,
	ode_param<T> p = ode_param<T>(),
	const ode_callback<T>& callback = ode_callback<T>(),
	ub::matrix< interval<T> >* mat = NULL
) {

	int s = init.size();
	ub::vector< affine<T> > x, x1;
	interval<T> t, t1;
	int ret_ode;
	ub::matrix< interval<T> > M, M_tmp;
	ub::matrix< interval<T> >* M_p;
	int ret_val = 0;
	bool ret_callback;

	ub::vector< psa< interval<T> > > result_tmp;

	if (mat == NULL) {
		M_p = NULL;
	} else {
		M_p = &M_tmp;
		M = ub::identity_matrix< interval<T> >(s);
	}

	x = init;
	t = start;
	p.set_autostep(true);

	while (true) {
		x1 = x;
		t1 = end;

		ret_ode = ode_maffine(f, x1, t, t1, p, M_p, &result_tmp);
		if (ret_ode == 0) {
			if (ret_val == 1) {
				init = x1;
				if (mat != NULL) *mat = M;
				end = t;
			}
			return ret_val;
		}
		ret_val = 1;
		if (mat != NULL) M = prod(M_tmp, M);
		#if 0
		if (result_psa != NULL) {
			(*result_psa).push_back(boost::make_tuple(t, t1, result_tmp));
		}
		#endif
		if (p.verbose == 1) {
			std::cout << "t: " << t1 << "\n";
			std::cout << to_interval(x1) << "\n";
		}

		ret_callback = callback(t, t1, to_interval(x), to_interval(x1), result_tmp);

		if (ret_callback == false) {
			init = x1;
			if (mat != NULL) *mat = M;
			end = t1;
			return 3;
		}

		if (ret_ode == 2) {
			init = x1;
			if (mat != NULL) *mat = M;
			return 2;
		}

		t = t1;
		x = x1;
	}
}

template <class T, class F>
int
odelong_maffine(
	F f,
	ub::vector< interval<T> >& init,
	const interval<T>& start,
	interval<T>& end,
	ode_param<T> p = ode_param<T>(),
	const ode_callback<T>& callback = ode_callback<T>()
) {
	int s = init.size();
	int i;
	ub::vector< affine<T> > x;
	int maxnum_save;
	int r;

	maxnum_save = affine<T>::maxnum();
	affine<T>::maxnum() = 0;

	x = init;

	r = odelong_maffine(f, x, start, end, p, callback);

	affine<T>::maxnum() = maxnum_save;

	if (r == 0) return 0;

	for (i=0; i<s; i++) init(i) = to_interval(x(i));

	return r;
}

template <class T, class F>
int
odelong_maffine(
	F f,
	ub::vector< autodif< interval<T> > >& init,
	const interval<T>& start,
	interval<T>& end,
	ode_param<T> p = ode_param<T>(),
	const ode_callback<T>& callback = ode_callback<T>()
) {
	int s = init.size();
	int i, j;
	ub::vector< interval<T> > xi;
	ub::vector< affine<T> > x;
	ub::matrix< interval<T> > M, M_tmp;
	int maxnum_save;
	int r;

	autodif< interval<T> >::split(init, xi, M);
	int s2 = M.size2();

	maxnum_save = affine<T>::maxnum();
	affine<T>::maxnum() = 0;

	x = xi;

	r = odelong_maffine(f, x, start, end, p, callback, &M_tmp);

	affine<T>::maxnum() = maxnum_save;

	if (r == 0) return 0;

	M = prod(M_tmp, M);

	for (i=0; i<s; i++) {
		init(i).v = to_interval(x(i));
		init(i).d.resize(s2);
		for (j=0; j<s2; j++) {
			init(i).d(j) = M(i, j);
		}
	}

	return r;
}

} // namespace kv

#endif // ODE_MAFFINE_HPP


/**
    @file .cpp

    @author Terence Henriod

    Project Name

    @brief This program...

    @version Original Code 1.00 (10/29/2013) - T. Henriod
*/

/*
bool extensionIsGood( const string& file_name, const string& extension )
{
    return ( file_name.find( extension, file_name.length() - 5 ) !=
             string::npos );
}
 */

/*==============================================================================
=======     HEADER FILES     ===================================================
==============================================================================*/
#include <cstdlib>
#include <iostream>
#include <fstream>
#include <cstdio>
#include <cmath>
#include <ctime>
#include <string>
#include <vector>

#include <Eigen/Dense>  // -I /home/thenriod/Desktop/cpp_libs/Eigen_lib
#include "my_stopwatch.h"

using namespace std;
using namespace Eigen;

/*==============================================================================
=======     GLOBAL CONSTANTS     ===============================================
==============================================================================*/
#define DEBUG 1

const int STD_STR_LEN = 100;

const char MATCH = 'X';
const char NO_MATCH = '-';

const string TRAINING_RESULTS_DIRECTORY = "/training_results/";

enum Status
{
    NO_ERRORS = 0,
    ERROR
};

enum Mode
{
    TRAINING_MODE = 0,
    TESTING_MODE = 1,
    INVALID = 666
};

/*==============================================================================

=======     USER DEFINED TYPES     =============================================
==============================================================================*/

/**
@struct

Description

@var
*/
typedef struct
{
    char** training_image_names;
    int num_training_images;
    char** training_eigenweight_names;
    int num_training_eigenweights;
    char** testing_image_names;
    int num_testing_images;
    int pixels_per_image;
    int width_in_pixels;
    int height_in_pixels;
} JobInfo;

typedef struct
{
    char tested_image_name[ STD_STR_LEN ];
    int test_image_index;
    char matched_image_name[ STD_STR_LEN ];
    int matched_image_index;
    double distance_between_images;
    char is_match_by_test;
    char is_actual_match;
} MatchResult;

/*==============================================================================
=======     GLOBAL VARIABLES     ===============================================
==============================================================================*/

/*==============================================================================
=======     FUNCTION PROTOTYPES     ============================================
==============================================================================*/
/**
FunctionName

A short description

@param

@return

@pre
-#

@post
-#

@detail @bAlgorithm
-#

@exception

@code
@endcode
*/
int processCommandLineArguments( const int argc, char** argv,
                                 JobInfo& info );

int trainingMode( JobInfo& info );

int findSampleImageSize( int& image_width, int& image_height,
                         const char* file_name );

int allocateArrays( double**& face_data_matrix, double*& mean_face, const JobInfo& info );

int loadTrainingData( Matrix<double, Dynamic, Dynamic>& face_data, const JobInfo& info );

int readPgmToVector( VectorXd& image_vector, const char* file_name, const JobInfo& info );

int trainMeanFace( VectorXd& mean_face,  const Matrix<double, Dynamic, Dynamic>& face_data, const JobInfo& info );

int writeVectorToFile( const char* file_name, const VectorXd& data_vector );

int centerFaceData( Matrix<double, Dynamic, Dynamic>& centered_data, Matrix<double, Dynamic, Dynamic>& face_data, const VectorXd& mean_face, const JobInfo& info );

int findFaceCovariance( Matrix<double, Dynamic, Dynamic>& face_covariance,
                        const Matrix<double, Dynamic, Dynamic>& centered_data,
                        const JobInfo& info );

int findEigenValuesVectors( VectorXd& eigenvalues, MatrixXd& eigenvectors,
                            const MatrixXd& A_matrix, const JobInfo& info );

bool compareEigenvalIndexPairs( const pair< double, int >& one,
                                const pair< double, int >& other );

double getVectorMagnitude( const VectorXd the_vector );

int writePgmImage( const string& file_name, const VectorXd& image_data,
                   int image_width, int image_height, int image_shades );

VectorXd scaleImageToPgm( const VectorXd& image_data, const JobInfo& info );

int storeEigenData( const VectorXd& eigenvalues, const MatrixXd& eigenvectors );

int promptForNumEigens( const VectorXd& eigenvalues );

int storeEigenProjections( const int eigens_to_keep, const MatrixXd& centered_data, const MatrixXd& face_eigenvectors, const JobInfo& info );

int writeEigenweightFile( const char* file_name, const VectorXd& eigen_weights );

int testingMode( JobInfo& info );

int loadTrainingOutcomes( VectorXd& mean_face, VectorXd& eigenvalues, MatrixXd& eigenvectors, MatrixXd& training_eigenweights, const JobInfo& info );

int readDataVectorFromTxt( VectorXd& data_vector, const char* file_name );

int readMatrixFromTxt( MatrixXd& data_matrix, const char* file_name );

int loadTestingData( Matrix<double, Dynamic, Dynamic>& face_data, const JobInfo& info );

int centerTestData( MatrixXd& centered_data, MatrixXd& face_data, const VectorXd& mean_face, const JobInfo& info );

int projectDataInEigenspace( MatrixXd& eigenweights, const MatrixXd& data, const MatrixXd& eigenvectors );

VectorXd projectImageInEigenspace( const VectorXd& centered_image, const MatrixXd& eigenvectors  );

double promptForThreshold();

int matchAllImages( vector< MatchResult >& matching_results,
                 const MatrixXd& training_eigenweights,
                 const MatrixXd& testing_eigenweights,
                 const MatrixXd& eigenvalues,
                 const double threshold,
                 const int num_eigens_to_use,
                 const JobInfo& info );

int matchImage( MatchResult& result, const VectorXd& test_weights, const MatrixXd& training_weights );

int matchImage( MatchResult& result, const VectorXd& test_weights,
                const MatrixXd& training_weights, const VectorXd& eigenvalues,
                const int num_eigens_to_use, const double threshold,
                const JobInfo& info );

double computeMahalanobisDistance( const VectorXd& test_weights,
                                   const VectorXd& training_weights,
                                   const VectorXd& eigenvalues,
                                   const int num_eigens_to_use );

int outputDataSummary( vector< MatchResult >& results, double threshold, int num_eigens_to_use, bool explicit_summary );

char* stripFilePath( char* file_name );

/*==============================================================================
=======     MAIN FUNCTION     ==================================================
==============================================================================*/

/**
FunctionName

A short description

@param

@return

@pre
-#

@post
-#

@detail @bAlgorithm
-#

@exception

@code
@endcode
*/
int main( int argc, char** argv )
{
    // variables
    int program_status = NO_ERRORS;
    int program_mode = TRAINING_MODE;
    JobInfo info;

    // process the command line arguments
    program_mode = processCommandLineArguments( argc, argv, info );

info.pixels_per_image = 2880; // 48 x 60 for hi-res samples

    // case: the program is running in training mode
    if( program_mode == TRAINING_MODE )
    {
        // indicate that the program is running in training mode
        cout << char( 0x0C )
             << "    =====================" << endl
             << "    |   TRAINING MODE   |" << endl
             << "    =====================" << endl << endl;

        // run program in training mode
        trainingMode( info );
    }
    // case: the program is running in "testing" mode
    else if( program_mode == TESTING_MODE )
    {
        // indicate that the program is running in training mode
        cout << char( 0x0C )
             << "       ====================" << endl
             << "       |   TESTING MODE   |" << endl
             << "       ====================" << endl << endl;

        testingMode( info );
    }
    // case: bad command line arguments were used
    else
    {
        puts( "Bad command line arguments result in program termination.\n" );
    }

    // return a program operation status signal
    return program_status;
}

/*==============================================================================
=======     FUNCTION IMPLEMENTATIONS     =======================================
==============================================================================*/
int processCommandLineArguments( const int argc, char** argv,
                                 JobInfo& info )
{
    // variables
    int program_mode = INVALID;

    // case: a large enough number of command line arguments were given
    if( argc >= 3 )
    {
        // collect the numerical value of the first command line argument
        program_mode = atoi( argv[1] );

        // case: the given mode is not a valid one
        if( !( program_mode == TRAINING_MODE ||
               program_mode == TESTING_MODE    ) )
        {
            // set the mode to indicate an invalid mode
            program_mode = INVALID;
        }

        if( program_mode == TRAINING_MODE )
        {
            // set up the job information
            info.num_training_images = argc - 2;
            info.training_image_names = argv + 2;
        }
        else if( program_mode == TESTING_MODE )
        {
            // set up the job information
            info.training_image_names = argv + 2;
            info.num_training_images = 0;
            while( strstr( info.training_image_names[ info.num_training_images ], ".pgm" ) != NULL )
            {
                info.num_training_images++;
            }

            info.training_eigenweight_names = info.training_image_names + info.num_training_images;
            info.num_training_eigenweights = 0;
            while( strstr( info.training_eigenweight_names[ info.num_training_eigenweights ], ".txt" ) != NULL )
            {
                info.num_training_eigenweights++;
            }

            info.testing_image_names = info.training_eigenweight_names + info.num_training_eigenweights;
            info.num_testing_images = 0;
            while( ( info.num_testing_images < 1196 ) &&
                   ( strstr( info.testing_image_names[ info.num_testing_images ], ".pgm" ) != NULL ) )
            {
                info.num_testing_images++;
            }
        }
    }
    // case: too few arguments were used
    else
    {
        // give a stern message
        cout << "Something's wrong there guy... You need more arguments."
             << endl << endl;
    }

    // return the mode the program will be running in
    return program_mode;
}

int trainingMode( JobInfo& info )
{
    // variables
    int training_success = NO_ERRORS;

    VectorXd mean_face;
    VectorXd face_eigenvalues;
    Matrix< double, Dynamic, Dynamic > face_eigenvectors;
    Matrix< double, Dynamic, Dynamic > face_data;
    Matrix< double, Dynamic, Dynamic > centered_data; // A matrix
    Matrix< double, Dynamic, Dynamic > face_covariance;
    Matrix< double, Dynamic, Dynamic > eigen_faces;
    pair< VectorXd, MatrixXd > eigen_values_vectors;

    int eigens_to_keep = 1;

    info.pixels_per_image = findSampleImageSize( info.width_in_pixels,
                                                 info.height_in_pixels,
                                                 info.training_image_names[0] );

    puts( "Loading data...\n" );
    loadTrainingData( face_data, info );

    puts( "Training mean...\n" );
    trainMeanFace( mean_face, face_data, info );

    puts( "\"Centering\" faces...\n" );
    centerFaceData( centered_data, face_data, mean_face, info );

/*    puts( "Training covariance matrix...\n" );
    findFaceCovariance( face_covariance, centered_data, info );
 */

    // compute the Eigen values/vectors
    puts( "Finding Eigenvalues/Eigenvectors...\n" );
    findEigenValuesVectors( face_eigenvalues, face_eigenvectors,
                            centered_data, info );

    // create data/coefficients file
    puts( "Storing the found eigen-data...\n" );
    storeEigenData( face_eigenvalues, face_eigenvectors );

    // prompt user for the amount of information to keep
    eigens_to_keep = promptForNumEigens( face_eigenvalues );

    // create the reduced dimensionality eigenfaces, store them to a file
    puts( "Storing the training faces projected into eigenspace...\n" );
    storeEigenProjections( eigens_to_keep, centered_data, face_eigenvectors, info );

    // return a signal as to whether or not training was successful
    return training_success;
}

int findSampleImageSize( int& image_width, int& image_height,
                         const char* file_name )
{
    ifstream fin;
    string magic_number;
    int width = 0;
    int height = 0;

    fin.clear();
    fin.open( file_name );
    fin >> magic_number >> image_width >> image_height;
    fin.close();

    return ( image_width * image_height );
}

int loadTrainingData( Matrix<double, Dynamic, Dynamic>& face_data, const JobInfo& info )
{
    // variables
    int image_num = 0;
    VectorXd temp;

    face_data.resize( info.pixels_per_image, info.num_training_images );

    // for every image
    for( image_num = 0; image_num < info.num_training_images; image_num++ )
    {
        readPgmToVector( temp, info.training_image_names[image_num], info );
        face_data.col( image_num ) = temp;
    }

#if 0
writePgmImage( info.training_image_names[20], face_data.col( 20 ), info.width_in_pixels,
               info.height_in_pixels, 255 );
#endif

}

int readPgmToVector( VectorXd& image_vector, const char* file_name, const JobInfo& info )
{
    int read_success = NO_ERRORS;
    int pixel_num = 0;
    ifstream fin;
    string magic_number;
    int width;
    int height;
    int granularity;
    unsigned char pixel_byte;

    // open the file
    fin.clear();
    fin.open( file_name );

    // size the vector appropriately
    image_vector.resize( info.pixels_per_image );

    // eat the header
    fin >> magic_number >> width >> height >> granularity;
    fin.get(); // and an endline char

    // read in the data to a column vector
    for( pixel_num = 0; pixel_num < info.pixels_per_image; pixel_num++ )
    {
        // read in the byte, convert to float
        pixel_byte = fin.get();
        image_vector( pixel_num ) = (double) pixel_byte;
    }

    // close the file
    fin.close();

    return read_success;
}

int trainMeanFace( VectorXd& mean_face,  const Matrix<double, Dynamic, Dynamic>& face_data, const JobInfo& info )
{
    int mean_success = NO_ERRORS;
    int image_num = 0, pixel_num = 0;

    mean_face.resize( info.pixels_per_image );

    for( pixel_num = 0; pixel_num < info.pixels_per_image; pixel_num++ )
    {
        for( image_num = 0, mean_face( pixel_num ) = 0;
             image_num < info.num_training_images;
             image_num++ )
        {
            mean_face( pixel_num ) += face_data( pixel_num, image_num );
        }

        mean_face( pixel_num ) /= info.num_training_images;
    }

#if 1
writePgmImage( "mean_training_face.pgm", mean_face, info.width_in_pixels,
               info.height_in_pixels, 255 );

writeVectorToFile( "mean_training_face_data.txt", mean_face );
#endif

    return mean_success;
}

int writeVectorToFile( const char* file_name, const VectorXd& data_vector )
{
    int write_success = NO_ERRORS;
    ofstream fout;
    int i = 0;

    fout.clear();
    fout.open( file_name );
    fout << data_vector.size() << endl;
    fout<< data_vector << endl;

    return write_success;
}

int centerFaceData( Matrix<double, Dynamic, Dynamic>& centered_data, Matrix<double, Dynamic, Dynamic>& face_data, const VectorXd& mean_face, const JobInfo& info )
{
    int centering_success = NO_ERRORS;
    int image_number = 0, pixel_number = 0;

    centered_data.resize( face_data.rows(), face_data.cols() );

    for( image_number = 0; image_number < info.num_training_images; image_number++ )
    {
        for( pixel_number = 0; pixel_number < info.pixels_per_image; pixel_number++ )
        {
            centered_data( pixel_number, image_number ) = face_data( pixel_number, image_number ) - mean_face( pixel_number );
        }
    }

#if 0
writePgmImage( "test_center.pgm", centered_data.col( 5 ), info.width_in_pixels,
               info.height_in_pixels, 255 );
#endif

    return centering_success;
}

int findFaceCovariance( Matrix<double, Dynamic, Dynamic>& face_covariance,
                        const Matrix<double, Dynamic, Dynamic>& centered_data,
                        const JobInfo& info )
{
    int covariance_success = NO_ERRORS;
    double inverse_num_training_images = 1.0 / info.num_training_images;

    face_covariance.resize( info.pixels_per_image, info.pixels_per_image );

    face_covariance = inverse_num_training_images *
                      ( centered_data * centered_data.transpose().eval() );

    return covariance_success;
}

bool compareEigenvalIndexPairs( const pair< double, int >& one,
                                const pair< double, int >& other )
{
    // this makes for a high to low sort when used with std::sort
    return ( one.first > other.first );
}

double getVectorMagnitude( const VectorXd the_vector )
{
    double magnitude = 0.0;
    int i = 0;

    for( i = 0; i < the_vector.size(); i++ )
    {
        magnitude += ( the_vector( i ) * the_vector( i ) );
    }

    return sqrt( magnitude );
}

int writePgmImage( const string& file_name, const VectorXd& image_data, int image_width, int image_height, int image_shades )
{
    int write_success = NO_ERRORS;
    ofstream fout;
    int pixel_num = 0;
    int row_sizer = 0;
    int num_pixels = image_width * image_height;

    fout.clear();
    fout.open( file_name.c_str() );

    // write header with PGM "magic number" for binary type
    fout << "P5" << '\n' << image_width << ' '
         << image_height << '\n' << image_shades << '\n';

    // write the data
    for( pixel_num = 0; pixel_num < num_pixels; pixel_num++ )
    {
        fout << (unsigned char) ((unsigned int)image_data( pixel_num ) % (image_shades + 1));
    }

#if 0
    // write header with PGM "magic number" for ASCII type
    fout << "P2" << ' ' << image_width << ' '
         << image_height << ' ' << image_shades << '\n';

    // write the data
    for( pixel_num = 0; pixel_num < num_pixels; pixel_num++ )
    {
        fout << (unsigned int) image_data( pixel_num ) % image_shades;
        row_sizer++;
        if( row_sizer < image_width )
        {
            fout << ' ';
        }
        else
        {
            row_sizer = 0;
            fout << '\n';
        }
    }
#endif
    fout.close();

    return write_success;
}

int findEigenValuesVectors( VectorXd& eigenvalues, MatrixXd& eigenvectors, const MatrixXd& A_matrix, const JobInfo& info )
{
    EigenSolver<MatrixXd> eigen_solver;
    Matrix<double, Dynamic, Dynamic> At_A_matrix;   // A^t * A matrix
    Matrix<double, Dynamic, Dynamic> interim_eigenvectors;
    vector< pair< double, int> > eigen_indices;
    pair< double, int > temp;
    int i = 0;
    char buffer[100];

    // compute the A^t * A matrix to save on computations vs A * A^t
    At_A_matrix = A_matrix.transpose().eval() * A_matrix;

    // compute the eigen-stuff of the matrix
    eigen_solver.compute( At_A_matrix );

    // identify the strictly real eigen values and vectors
    for( i = 0; i < eigen_solver.eigenvalues().size(); i++ )
    {
        if( ( eigen_solver.eigenvalues().imag()( i ) == 0.0 ) )
        {
            temp.first = eigen_solver.eigenvalues().real()( i );
            temp.second = i;
            eigen_indices.push_back( temp );
        }
    }

    // sort the eigen stuff in descending order by eigen value
    sort( eigen_indices.begin(), eigen_indices.end(),
          compareEigenvalIndexPairs );

    // store the eigen-stuff in usable objects
    eigenvalues.resize( eigen_indices.size() );
    interim_eigenvectors.resize( A_matrix.cols(), eigen_indices.size() );
    eigenvectors.resize( A_matrix.rows(), eigen_indices.size() );
    for( i = 0; i < eigen_indices.size(); i++ )
    {
        eigenvalues( i ) = eigen_indices[i].first;
        interim_eigenvectors.col( i ) = eigen_solver.eigenvectors().col( eigen_indices[i].second ).real();
    }

    // get from Av to u for actual eigenvectors
    for( i = 0; i < eigenvectors.cols(); i++ )
    {
        // get the u vector
        eigenvectors.col( i ) = (A_matrix * interim_eigenvectors.col( i ));

        // make it a unit u vector
        eigenvectors.col( i ) /= getVectorMagnitude( eigenvectors.col( i ) );
    }

    for( i = 0; i < eigenvalues.size(); i++ )
    {
        sprintf( buffer, "eigenfaces/eigenface_%d.pgm", i );

        writePgmImage( buffer, scaleImageToPgm( eigenvectors.col( i ), info ), info.width_in_pixels,
               info.height_in_pixels, 255 );
    }

    return 0;
}

VectorXd scaleImageToPgm( const VectorXd& image_data, const JobInfo& info )
{
    VectorXd scaled_vector;
    double minimum_value = image_data( 0 );
    double maximum_value = image_data( 0 );
    int pixel_num = 0;

    scaled_vector.resize( image_data.size() );
    scaled_vector = image_data;

    // find the minimum and maximum values for scaling
    for( pixel_num = 0; pixel_num < image_data.size(); pixel_num++ )
    {
        if( image_data( pixel_num ) < minimum_value )
        {
            minimum_value = image_data( pixel_num );
        }

        if( image_data( pixel_num ) > maximum_value )
        {
            maximum_value = image_data( pixel_num );
        }
    }

    // scale the pixel values properly
    for( pixel_num = 0; pixel_num < image_data.size(); pixel_num++ )
    {
        scaled_vector( pixel_num ) = image_data( pixel_num ) - minimum_value;
    }
    maximum_value -= minimum_value;
    scaled_vector *= ( 255.0 / (maximum_value ));//- minimum_value ));

    return scaled_vector;
}

int storeEigenData( const VectorXd& eigenvalues, const MatrixXd& eigenvectors )
{
    int storage_success = NO_ERRORS;
    ofstream fout;
    int i = 0;
    int j = 0;

    fout.clear();
    fout.open( "EIGENVALUES.txt" );
    fout << eigenvalues.size() << endl;
    for( i = 0; i < eigenvalues.size(); i++ )
    {
        fout << eigenvalues( i ) << endl;
    }
    fout.close();

    fout.clear();
    fout.open( "EIGENVECTORS.txt" );
    fout << eigenvectors.rows() << ' ' << eigenvectors.cols() << endl;

    fout << eigenvectors << endl;

    fout.close();

    return storage_success;
}

int promptForNumEigens( const VectorXd& eigenvalues )
{
    int num_eigens_to_keep = 0;
    double eigenvalue_sum = 0;
    double kept_eigenvalue_sum = 0;
    char response;
    bool keep_prompting = true;
    int i = 0;

    for( i = 0; i < eigenvalues.size(); i++ )
    {
        eigenvalue_sum += eigenvalues( i );
    }

    while( keep_prompting )
    {
        cout << "How many eigen values (faces) should be kept? ("
             << eigenvalues.size() << " available): ";
        cin >> num_eigens_to_keep;
 // use 16 for ~80%

        for( kept_eigenvalue_sum = 0, i = 0; i < num_eigens_to_keep; i++ )
        {
            kept_eigenvalue_sum += eigenvalues( i );
        }

        cout << ( kept_eigenvalue_sum * 100 / eigenvalue_sum )
             << "% of the information will be kept. Is this acceptable? (y/n): ";
        cin >> response;

        if( response == 'y' )
        {
            keep_prompting = false;
        }
    }

    return num_eigens_to_keep;
}

int storeEigenProjections( const int eigens_to_keep, const MatrixXd& centered_data, const MatrixXd& eigenvectors, const JobInfo& info )
{
    int eigenprojection_success = NO_ERRORS;
    VectorXd projected_face;
    MatrixXd eigen_weights;
    char buffer[100];
    ofstream fout;
    int i = 0;
    int j = 0;

    // initial setup and resizing
    eigen_weights.resize( eigens_to_keep, centered_data.cols() );
    projected_face.resize( info.pixels_per_image );

    for( j = 0; j < centered_data.cols(); j++ )
    {
       // eigen_weights.col( j ) = projectImageInEigenspace( centered_data.col( j ), eigenvectors );

        for( i = 0; i < eigens_to_keep; i++ )
        {
            eigen_weights( i, j ) = eigenvectors.col( i ).transpose().eval() * centered_data.col( j );
        }

    }

    for( j = 0; j < centered_data.cols(); j++ )
    {
        projected_face *= 0;

        for( i = 0; i < eigens_to_keep; i++ )
        {
            projected_face += eigen_weights( i, j ) * eigenvectors.col( i );
        }

        sprintf( buffer, "training_eigenprojections/projected_face_%d_coefficients.txt", j );
        writeEigenweightFile( buffer, eigen_weights.col( j ) );

        sprintf( buffer, "training_eigenprojections/projected_face_%d.pgm", j );
        writePgmImage( buffer, scaleImageToPgm( projected_face, info ), info.width_in_pixels,
               info.height_in_pixels, 255 );
    }

    return eigenprojection_success;
}

int writeEigenweightFile( const char* file_name, const VectorXd& eigen_weights )
{
    int write_success = NO_ERRORS;
    ofstream fout;

    fout.clear();
    fout.open( file_name );

    fout << eigen_weights.size() << endl;
    fout << eigen_weights << endl;

    fout.close();

    return write_success;
}

int testingMode( JobInfo& info )
{
    int testing_success = NO_ERRORS;
    VectorXd mean_face;
    VectorXd eigenvalues;
    MatrixXd eigenvectors;
    MatrixXd testing_faces;
    MatrixXd centered_test_faces;
    MatrixXd training_eigenweights;
    MatrixXd testing_eigenweights;
    int num_eigens_to_use = 0;
    vector< MatchResult > matching_results;
    double threshold = 1.0;
    ofstream fout;
    int i = 0;
    int true_positive = 0, false_positive = 0, total_positive = 0, total_negative = 0;

    info.pixels_per_image = findSampleImageSize( info.width_in_pixels,
                                                 info.height_in_pixels,
                                                 info.training_image_names[0] );

    puts( "Loading training outcomes...\n" );
    loadTrainingOutcomes( mean_face, eigenvalues, eigenvectors, training_eigenweights, info );

    puts( "Projecting inputs onto eigenspace...\n" );
    loadTestingData( testing_faces, info );
    centerTestData( centered_test_faces, testing_faces, mean_face, info );
    projectDataInEigenspace( testing_eigenweights, centered_test_faces, eigenvectors );

    puts( "Matching inputs to training images...\n" );
    fout.clear()
    fout.open( "ROC.txt" );
    fout << "True positives, total positives, false negatives, total negatives" << endl;
    for( threshold = 0.0001, true_positive = 0, false_positive = 0, total_positive = 0, total_negative = 0;
           threshold < 0.004; threshold += 0.0005 )
    {
        num_eigens_to_use = 114; //promptForNumEigens( eigenvalues );
        // threshold = 1.5; // promptForThreshold();
        matchAllImages( matching_results, training_eigenweights, testing_eigenweights, eigenvalues, threshold, num_eigens_to_use, info );

        puts( "Outputting matching data...\n" );
        outputDataSummary( matching_results, threshold, num_eigens_to_use, false );

        for( i = 0; i < matching_results.size(); i++ );

    }

    return testing_success;
}

int loadTrainingOutcomes( VectorXd& mean_face, VectorXd& eigenvalues, MatrixXd& eigenvectors, MatrixXd& training_eigenweights, const JobInfo& info )
{
    int load_success = NO_ERRORS;
    int i = 0;
    VectorXd temp;

    readDataVectorFromTxt( mean_face, "mean_training_face_data.txt" );
    readDataVectorFromTxt( eigenvalues, "EIGENVALUES.txt" );
    readMatrixFromTxt( eigenvectors, "EIGENVECTORS.txt" );

cout << eigenvalues.size() << endl;

    training_eigenweights.resize( 1204, eigenvectors.cols() );
    for( i = 0; i < info.num_training_eigenweights; i++ )
    {
        readDataVectorFromTxt( temp, info.training_eigenweight_names[i] );
        training_eigenweights.col( i ) = temp;
    }

    return load_success;
}

int readDataVectorFromTxt( VectorXd& data_vector, const char* file_name )
{
    int read_success = NO_ERRORS;
    ifstream fin;
    int size;
    double value;
    int i;

    fin.clear();
    fin.open( file_name );

    if( fin.good() )
    {
        fin >> size;
        data_vector.resize( size );

        for( i = 0; i < size; i++ )
        {
            fin >> value;
            data_vector( i ) = value;
        }
    }

    return read_success;
}

int readMatrixFromTxt( MatrixXd& data_matrix, const char* file_name )
{
    int read_success = NO_ERRORS;
    ifstream fin;
    int rows, cols;
    double value;
    int i = 0, j = 0;

    fin.clear();
    fin.open( file_name );

    if( fin.good() )
    {
        fin >> rows >> cols;
        data_matrix.resize( rows, cols );

        for( i = 0; i < rows; i++ )
        {
            for( j = 0; j < cols; j++ )
            {
                fin >> value;
                data_matrix( i, j ) = value;
            }
        }
    }

    return read_success;
}

int loadTestingData( Matrix<double, Dynamic, Dynamic>& face_data, const JobInfo& info )
{
    // variables
    int image_num = 0;
    VectorXd temp;

    face_data.resize( info.pixels_per_image, info.num_testing_images );

    // for every image
    for( image_num = 0; image_num < info.num_testing_images; image_num++ )
    {
        readPgmToVector( temp, info.testing_image_names[image_num], info );
        face_data.col( image_num ) = temp;
    }

#if 1
writePgmImage( info.training_image_names[20], face_data.col( 20 ), info.width_in_pixels,
               info.height_in_pixels, 255 );
#endif
}

int centerTestData( MatrixXd& centered_data, MatrixXd& face_data, const VectorXd& mean_face, const JobInfo& info )
{
    int centering_success = NO_ERRORS;
    int image_number = 0, pixel_number = 0;

    centered_data.resize( face_data.rows(), face_data.cols() );

    for( image_number = 0; image_number < info.num_testing_images; image_number++ )
    {
        for( pixel_number = 0; pixel_number < info.pixels_per_image; pixel_number++ )
        {
            centered_data( pixel_number, image_number ) = face_data( pixel_number, image_number ) - mean_face( pixel_number );
        }
    }

#if 0

writePgmImage( "test_center.pgm", centered_data.col( 5 ), info.width_in_pixels,
               info.height_in_pixels, 255 );
#endif

    return centering_success;
}

int projectDataInEigenspace( MatrixXd& eigenweights, const MatrixXd& data, const MatrixXd& eigenvectors )
{
    int eigenprojection_success = NO_ERRORS;
    int j = 0;

    // initial setup and resizing
    eigenweights.resize( eigenvectors.cols(), data.cols() );

    for( j = 0; j < data.cols(); j++ )  // for each image
    {
        eigenweights.col( j ) = projectImageInEigenspace( data.col( j ), eigenvectors );

/*
        for( i = 0; i < eigenvectors.cols(); i++ ) // for each feature/weight
        {
            eigenweights( i, j ) = eigenvectors.col( i ).transpose().eval() * data.col( j );
        }
*/
    }

    return eigenprojection_success;
}

VectorXd projectImageInEigenspace( const VectorXd& centered_image, const MatrixXd& eigenvectors  )
{
    VectorXd weight_vector;
    int i = 0;

    weight_vector.resize( eigenvectors.cols() );

    for( i = 0; i < eigenvectors.cols(); i++ ) // for every eigenvector
    {
        weight_vector( i ) = eigenvectors.col( i ).transpose().eval() * centered_image;
    }

    return weight_vector;
}

double promptForThreshold()
{
    double threshold;

    cout << "Enter the threshold value to be used: ";
    cin >> threshold;

    return threshold;
}

int matchAllImages( vector< MatchResult >& matching_results,
                 const MatrixXd& training_eigenweights,
                 const MatrixXd& testing_eigenweights,
                 const MatrixXd& eigenvalues,
                 const double threshold,
                 const int num_eigens_to_use,
                 const JobInfo& info )
{
    int matching_success;
    int i = 0;
    int j = 0;
    double temp_distance = 0;
    double distance = 0;
    MatchResult temp;

    for( i = 0; i < testing_eigenweights.cols(); i++ )
    {
        strcpy( temp.tested_image_name, stripFilePath( info.testing_image_names[i] ) );

        matchImage( temp, testing_eigenweights.col( i ), training_eigenweights, eigenvalues, num_eigens_to_use, threshold, info );

        matching_results.push_back( temp );
    }

    return matching_success;
}

int matchImage( MatchResult& result, const VectorXd& test_weights,
                const MatrixXd& training_weights, const VectorXd& eigenvalues,
                const int num_eigens_to_use, const double threshold,
                const JobInfo& info )
{
    int match_function_success = NO_ERRORS;
    int i = 0;
    double temp_distance = 2E64;
    int CMC_counter = 0;

    // check against all other sets of weights
    for( i = 0, result.distance_between_images = 2E64; i < training_weights.cols(); i++ )
    {
        // find the distance between the image and a training one
        temp_distance = computeMahalanobisDistance( test_weights, training_weights.col( i ), eigenvalues, num_eigens_to_use );

        // case: the distance is better than what was thought to be the previous best
        if( temp_distance < result.distance_between_images )
        {
            // update the index of the match and the new, lower distance
            result.distance_between_images = temp_distance;
            result.matched_image_index = i;
        }

        if( atoi(result.tested_image_name) == atoi( stripFilePath( info.training_image_names[ i ] ) )
            && temp_distance < threshold )
        {
            CMC_counter++;
        }

    }

cout << "CMC" << CMC_counter << endl;

    // indicate which image the test one is closest to
    strcpy( result.matched_image_name,
            stripFilePath( info.training_image_names[ result.matched_image_index ] ) );

    // case: the best distance could be considered a match
    if( result.distance_between_images < threshold )
    {
        // indicate that there was a match, get matching image's name
        result.is_match_by_test = MATCH;
    }
    // case: the distance does not indicate a match
    else
    {
        result.is_match_by_test = NO_MATCH;
    }

    // determine if the images are an actual match
    // NOTE: I expect the atoi to read just the first segment of the image name, up to the first underscore
    if( atoi( result.tested_image_name ) == atoi( result.matched_image_name ) )
    {
        result.is_actual_match = MATCH;

cout << result.tested_image_name << "  " << result.matched_image_name << "  " << result.is_actual_match << ' ' <<  MATCH << endl;

    }
    else
    {
        result.is_actual_match = NO_MATCH;
    }

    return match_function_success;
}

double computeMahalanobisDistance( const VectorXd& test_weights,
                                   const VectorXd& training_weights,
                                   const VectorXd& eigenvalues,
                                   const int num_eigens_to_use )
{
    double mahalanobis_distance = 0;
    double euclidean_component;
    int i = 0;

    for( i = 0; i < num_eigens_to_use; i++ )
    {
        euclidean_component = ( test_weights( i ) - training_weights( i ) );
        euclidean_component *= euclidean_component;
        mahalanobis_distance += euclidean_component / eigenvalues( i );
    }

    return mahalanobis_distance;
}

int outputDataSummary( vector< MatchResult >& results, double threshold, int num_eigens_to_use, bool explicit_summary )
{
    ofstream fout;
    int i = 0;

    if( explicit_summary )
    {
        fout.clear();
        fout.open( "RESULTS.txt" );

        fout << "Test Image Name,"
                "Matched Image Name,"
                "Distance Between Images,"
                "Is Match By Test,"
                "Is Actually A Match,"
             << endl;

        for( i = 0; i < results.size(); i++ )
        {
            fout << results[i].tested_image_name << ", "
                 << results[i].matched_image_name << ", "
                 << results[i].distance_between_images << ", "
                 << results[i].is_match_by_test << ", "
                 << results[i].is_actual_match << endl;
        }
    }
}

char* stripFilePath( char* file_name )
{
    char* stripped_string = file_name;
    char* temp;

    temp = strstr( stripped_string, "/" );
    while( temp != NULL )
    {
        stripped_string = temp + 1;
        temp = strstr( stripped_string, "/" );
    }

    return stripped_string;
}


#include <Eigen/Dense>

#include <ancse/cfl_condition.hpp>
#include <ancse/config.hpp>
#include <ancse/fvm_rate_of_change.hpp>
#include <ancse/snapshot_writer.hpp>
#include <ancse/time_loop.hpp>

template <class F>
Eigen::VectorXd ic(const F &f, const Grid &grid) {
    Eigen::VectorXd u0(grid.n_cells);
    for (int i = 0; i < grid.n_cells; ++i) {
        u0[i] = f(cell_center(grid, i));
    }

    return u0;
}

TimeLoop make_fvm(const Grid &grid) {
    auto config = get_global_config();
    double t_end = config["t_end"];
    double cfl_number = config["cfl_number"];

    auto n_ghost = grid.n_ghost;
    auto n_cells = grid.n_cells;

    auto model = Model{};

    auto simulation_time = std::make_shared<SimulationTime>(t_end);
    auto fvm_rate_of_change
        = make_fvm_rate_of_change(grid, model, simulation_time);
    auto boundary_condition
        = make_boundary_condition(n_ghost, config["boundary_condition"]);
    auto time_integrator
        = make_runge_kutta(fvm_rate_of_change, boundary_condition, n_cells);
    auto cfl_condition = make_cfl_condition(grid, model, cfl_number);
    auto snapshot_writer = std::make_shared<JSONSnapshotWriter>(
        grid, simulation_time, std::string(config["output"]));

    return TimeLoop(
        simulation_time, time_integrator, cfl_condition, snapshot_writer);
}

template <class F>
void run_test(const F &f) {
    auto config = get_global_config();

    int n_ghost = config["n_ghost"];
    int n_cells = int(config["n_interior_cells"]) + n_ghost * 2;

    auto grid = Grid({0.0, 1.0}, n_cells, n_ghost);
    auto u0 = ic(f, grid);

    auto fvm = make_fvm(grid);
    fvm(u0);
}

void smooth_sine_test() {
    run_test([](double x) { return std::sin(2.0 * M_PI * x); });
}

void jump_test() {
    run_test([](double x) { return (x < 0.5 ? 1.0 : 0.0); });
}

void backward_jump_test() {
    run_test([](double x) { return (x < 0.5 ? -1.0 : 1.0); });
}

void ex2() {
    auto config = get_global_config();

    int n_ghost = config["n_ghost"];
    int n_cells = int(config["n_interior_cells"]) + n_ghost * 2;

    auto grid = Grid({-1.5, 3.0}, n_cells, n_ghost);

    auto f = [](double x) {
        if (x < -1.0) {
            return 0.0;
        } else if (x < 0.0) {
            return 1.0;
        } else if (x < 1.0) {
            return 1.0 - x;
        } else {
            return 0.0;
        }
    };
    auto u0 = ic(f, grid);

    auto fvm = make_fvm(grid);
    fvm(u0);
}

int main() {
    auto config = get_global_config();

    std::string ic_key = config["initial_conditions"];
    if (ic_key == "sine") {
        smooth_sine_test();
    } else if (ic_key == "jump") {
        jump_test();
    } else if (ic_key == "backward-jump") {
        backward_jump_test();
    } else if (ic_key == "ex2") {
        ex2();
    }

    return 0;
}


// NormalGenerator.hpp
//
// A class hierarchy for generating random numbers,
// vectors and matrices.
//
// This hierarchy uses the Template Method Pattern and it
// delegates to a Strategy pattern for generating uniform
// random numbers.
//
// The solution is object-oriented and uses run-time polymorphic
// functions. In another chapter we use policy classes and templates.
//
// 2012-17 DD restrict to Boost
//
// (C) Datasim Education BV 2008-2012
//

#ifndef NormalGenerator_HPP
#define NormalGenerator_HPP

// boost
#include <boost/random.hpp>
#include <boost/random/mersenne_twister.hpp>
#include <boost/random/normal_distribution.hpp>
#include <boost/random/variate_generator.hpp>

class NormalGenerator
{

public:

	// Empty at the moment
	virtual double getNormal() const = 0;
};

class BoostNormal : public NormalGenerator
{
private:

	boost::lagged_fibonacci607 rng;
	boost::normal_distribution<> nor;
//
	boost::variate_generator<boost::lagged_fibonacci607&, boost::normal_distribution<> >* myRandom;

public:
	BoostNormal();	// NB no uniform parameters

	// Implement (variant) hook function
	double getNormal() const;

	~BoostNormal();
};

#endif

Level_9/VI.4 Monte Carlo Simulation/VI.4 Monte Carlo Simulation/RNG/NormalGenerator.hpp


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

  @copyright 2016 NumScale SAS

  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_CONSTANT_EULER_HPP_INCLUDED
#define BOOST_SIMD_CONSTANT_EULER_HPP_INCLUDED

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{
  /*!

    @ingroup group-constant

    GeneratesEuler constant.

    @par Header <boost/simd/constant/euler.hpp>

    @par Semantic:
    The Euler constant can be defined as \f$\displaystyle \lim_{n \rightarrow \infty} \left(\sum_1^n \frac1n -\log n\right)\f$

    @code
    T r = Euler<T>();
    @endcode

    is similar to:

    @code
      r =  T(0.577215664901532860606512090082402431042159335939923598805767234884867726777664670936947063291746749);
    @endcode

**/
  template<typename T> T Euler();

  namespace functional
  {
    /*!
      @ingroup group-callable-constant

      GeneratesEuler constant.

      Generate the  constant euler.

      @return The Euler constant for the proper type
    **/
    Value Euler();
  }
} }
#endif

#include <boost/simd/constant/scalar/euler.hpp>
#include <boost/simd/constant/simd/euler.hpp>

#endif


/* -*- mode: c++; tab-width: 4; indent-tabs-mode: nil; c-basic-offset: 4 -*- */

/*
 Copyright (C) 2016 Sebastian Schlenkrich

*/

#ifndef quantlib_templateauxilliaries_choleskyfactorisation_hpp
#define quantlib_templateauxilliaries_choleskyfactorisation_hpp

//#include <ql/types.hpp>
//#include <boost/function.hpp>

#include <stdlib.h>
#include <math.h>

namespace TemplateAuxilliaries {

    // This method is unsafe! Memory allocation need to be ensured by user
    // A(i,j) (and L) is stored row-wise as A[i*n+j]
    // L(i,j) is lower triangular matrix with L L^T = A
    template <typename Type>
    inline void cholesky(std::vector< Type >& A, std::vector< Type >& L, size_t n) {
        for (size_t i = 0; i < n; ++i) {
            for (size_t j = 0; j < (i+1); ++j) {
                Type s = 0;
                for (size_t k = 0; k < j; ++k) s += L[i * n + k] * L[j * n + k];
                if (i==j) {
                    if (A[i * n + i] < s) throw std::exception();
                    L[i * n + j] = sqrt(A[i * n + i] - s);
                }
                else L[i * n + j] = (1.0 / L[j * n + j] * (A[i * n + j] - s));
            }
        }
    }

    template <typename Type>
    std::vector< std::vector< Type > > cholesky(const std::vector< std::vector< Type > >& A) {
        std::vector<Type> arrayA(A.size()*A.size());
        std::vector<Type> arrayL(A.size()*A.size());
        for (size_t i = 0; i < A.size(); ++i) {
            if (A.size()!=A[i].size()) throw std::exception();
            for (size_t j = 0; j < A[i].size(); ++j) arrayA[i*A[i].size() + j] = A[i][j];
        }
        cholesky(arrayA, arrayL, A.size());
        std::vector< std::vector< Type > > L(A.size(), std::vector< Type >(A.size(), 0.0));
        for (size_t i = 0; i < L.size(); ++i) {
            for (size_t j = 0; j <= i; ++j) L[i][j] = arrayL[i*L[i].size() + j];
        }
        return L;
    }

    // alternative implementation of Cholesky decomposition
    template<class T>
    void performCholesky(std::vector< std::vector<T> >& matrix, size_t dimIn, bool flexible) {
        size_t dim = dimIn;
        for (size_t i = 0; i < dim; i++) {
            for (size_t j = i; j < dim; j++) {
                if (abs(matrix[i][j] - matrix[j][i])>QL_EPSILON)
                    QL_FAIL(std::string("A symmetrix matrix is necessary to apply Cholesky Decomposition"));
            }
        }

        //Perform Decomposition as described in script of Trottenberg (S. 47):
        //Because script delivers Upper right matrix, we interchange indizes in order to end up with lower left matrix.

        //First step:
        matrix[0][0] = sqrt(matrix[0][0]);
        if (abs(matrix[0][0]) < QL_EPSILON && !flexible)
            QL_FAIL("No positive definite Correlation Matrix because rank is not full.");
        for (size_t i = 1; i < dim; i++) {
            matrix[i][0] = abs(matrix[0][0]) < QL_EPSILON ? 0.0 : matrix[i][0] / matrix[0][0];
        }

        //Now iterate:
        for (size_t i = 1; i < dim; i++) {
            for (size_t k = 0; k < i; k++) {
                matrix[i][i] = matrix[i][i] - matrix[i][k] * matrix[i][k];
            }
            matrix[i][i] = sqrt(matrix[i][i]);
            if (abs(matrix[i][i]) < QL_EPSILON && !flexible)
                QL_FAIL(std::string("No positive definite Correlation Matrix because rank is not full."));
            if (matrix[i][i] != matrix[i][i] && !flexible) {
                QL_FAIL(std::string("No positive definite Correlation Matrix as diagonal square entry is negative."));
            }
            matrix[i][i] = (matrix[i][i] != matrix[i][i]) ? 0.0 : matrix[i][i];
            for (size_t j = 0; j < dim; j++) {
                if (j >= i + 1) {
                    for (size_t k = 0; k < i; k++) {
                        matrix[j][i] = matrix[j][i] - matrix[i][k] * matrix[j][k];
                    }
                    matrix[j][i] = (abs(matrix[i][i]) < QL_EPSILON || matrix[i][i] != matrix[i][i]) ? 0.0 : matrix[j][i] / matrix[i][i];
                }
                else if (j<i) {
                    matrix[j][i] = 0;
                }
                else {
                    //Nothing because diagonale.
                }
            }
        }
    }

}

#endif  /* ifndef quantlib_templateauxilliaries_qrfactorisation_hpp */


#include <iostream>
#include <Eigen/Dense>

int main()
{
using Eigen::Quaterniond;
using Eigen::Vector3d;
Eigen::Quaterniond q(2, 0, 1, -3);

  std::cout << "This quaternion consists of a scalar " << q.w() << " and a vector " << std::endl << q.vec() << std::endl;

  q.normalize();

  std::cout << "To represent rotation, we need to normalize it such that its length is " << q.norm() << std::endl;

  Eigen::Vector3d v(1, 2, -1);

  Eigen::Quaterniond p;

  p.w() = 0;

  p.vec() = v;

  Eigen::Quaterniond rotatedP = q * p * q.inverse();

  Eigen::Vector3d rotatedV = rotatedP.vec();

  std::cout << "We can now use it to rotate a vector " << std::endl << v << " to " << std::endl << rotatedV << std::endl;

  Eigen::Matrix3d R = q.toRotationMatrix(); // convert a quaternion to a 3x3 rotation matrix

  std::cout << "Compare with the result using an rotation matrix " << std::endl << R * v << std::endl;

  Eigen::Quaterniond a = Eigen::Quaterniond::Identity();

  Eigen::Quaterniond b = Eigen::Quaterniond::Identity();

  Eigen::Quaterniond c; // Adding two quaternion as two 4x1 vectors is not supported by the EIgen API. That is, c = a + b is not allowed. We have to do this in a hard way

  c.w() = a.w() + b.w();

  c.x() = a.x() + b.x();

  c.y() = a.y() + b.y();

  c.z() = a.z() + b.z();
}


#include <iostream>
#include <boost/math/special_functions/bessel.hpp>

#include "gauss_lobatto.hpp"

double f(double x)
{
    return std::exp(std::sin(M_PI*x));
}

bool test_gauss_lobatto()
{
    quad_rule a = gauss_lobatto<double>(9);

    double I = 0;
    for (int i=0; i < a.x.size(); ++i)
        I += f(a.x[i])*a.w[i];

    double e1 = std::abs(I - 2*boost::math::cyl_bessel_i(0,1.0));

    quad_rule b = gauss_lobatto<double>(10);

    I = 0;
    for (int i=0; i < b.x.size(); ++i)
        I += f(b.x[i]) * b.w[i];

    double e2 = std::abs(I - 2*boost::math::cyl_bessel_i(0,1.0));

    bool passed = true;

    if (e1 > 1e-4)
    {
        std::cout << "gauss_lobatto() failed accuracy test.\n";
        passed = false;
    }

    if (e2 > e1)
    {
        std::cout << "gauss_lobatto() failed convergence test.\n";
        passed = false;
    }

    return passed;
}


#include <iostream>
#include <armadillo>
#include <ctime>

using namespace std;
using namespace arma;

const double pi  =3.141592653589793238462;
int n,k;  // nrow(X),ncol(X)
mat z,X,y,XXi,Xt;
const double a=1;
const double b=1;
const int B=pow(10,5);
const double s2 = 10;

double ll(mat be, double sig2) {
  mat c(1,k), out(k,1);

  c = y-X*be;
  out = (c.t()*c / sig2 + n*log(sig2))/-2;

  return as_scalar(out);
}

double lpb(mat be) {
  return as_scalar(-be.t()*XXi*be/(2*s2));
}

double lps(double sig2) {
  return (a-1) * log(sig2) - sig2/b;
}

mat mvrnorm(mat M, mat S) {
  int n = M.n_rows;
  mat e = randn(n);
  return M + chol(S).t()*e;
}

int main(int argc, char** argv) {
  mat mle;

  //posteriors:
  mat bb;
  mat ss;

  // candidate sigma:
  mat csb;
  const double css = 1;

  //candidate values:
  mat candb;
  double cands;

  //acceptance rates:
  int accb = 0;
  int accs = 0;

  //metropolis ratio:
  double q;

  z.load("../data/dat.txt");
  n = z.n_rows;
  k = z.n_cols-1;

  y = z.col(0);
  X = z.cols(1,k); // columns 2 to k+1 of z
  Xt = X.t();
  XXi = (Xt*X).i();
  csb = 4*XXi;
  mle = XXi * Xt * y;

  bb.set_size(B,k);
  ss.set_size(B,1);
  bb.zeros();
  ss.ones();

  //current vals:
  mat bc = bb.row(0);
  double sc = 1.0;

  cout << "Starting Metropolis:" <<endl;
  clock_t t1 = clock();
  for (int i=1; i<B; i++) {
    // Set Initial Values:
    bb.row(i) = bb.row(i-1);
    ss.at(i,0) = sc;
    bc = bb.row(i).t();

    //Update Beta:
    candb = mvrnorm(bc,csb);
    q = ll(candb,sc)+lpb(candb) -ll(bc,sc)-lpb(bc);
    if (q>log(randu())) {
      bc = candb;
      bb.row(i) = bc.t();
      accb++;
    }

    //Update sigma2:
    cands = randn()*sqrt(css)+sc;
    if (cands>0){
      q = ll(bc,cands)+lps(cands) -ll(bc,sc)-lps(sc);
      if (q>log(randu())) {
        sc = cands;
        accs++;
      }
    }

    cout << "\r" << i*100/B <<"%";
  }
  clock_t t2 = clock();
  double elapsed = double(t2-t1) / CLOCKS_PER_SEC;
  cout << "Elapsed Time: " <<elapsed<<"s. \n"<<endl;

  cout << "Posterior Means Beta: \n" <<
          mean(bb.rows(90000,100000-1)).t()<<endl;
  cout << "Posterior Mean Sigma2: \n" <<
          mean(ss.rows(90000,100000-1))<<endl;
  cout << "Beta Acceptance:   "<< 100*accb/B <<"%"<<endl;
  cout << "Sigma2 Acceptance: "<< 100*accs/B <<"%"<<endl;
  cout <<endl;
  //ss.save("s2.txt",raw_ascii);

  return 0;
}


// Test ../include/LinAlg/UpperHessenbergEigen.h and
//      ../include/LinAlg/TridiagEigen.h
#include <LinAlg/UpperHessenbergEigen.h>
#include <LinAlg/TridiagEigen.h>
#include <Eigen/Eigenvalues>
#include <ctime>

using namespace Spectra;

#define CATCH_CONFIG_MAIN
#include "catch.hpp"

using Eigen::MatrixXd;
using Eigen::VectorXd;
using Eigen::MatrixXcd;
using Eigen::VectorXcd;

TEST_CASE("Eigen decomposition of upper Hessenberg matrix", "[Eigen]")
{
    std::srand(123);
    int n = 100;
    MatrixXd m = MatrixXd::Random(n, n);
    m.array() -= 0.5;
    MatrixXd H = m.triangularView<Eigen::Upper>();
    H.diagonal(-1) = m.diagonal(-1);

    UpperHessenbergEigen<double> decomp(H);
    VectorXcd evals = decomp.eigenvalues();
    MatrixXcd evecs = decomp.eigenvectors();

    MatrixXcd err = H * evecs - evecs * evals.asDiagonal();

    INFO( "||HU - UD||_inf = " << err.cwiseAbs().maxCoeff() );
    REQUIRE( err.cwiseAbs().maxCoeff() == Approx(0.0) );

    clock_t t1, t2;
    t1 = clock();
    for(int i = 0; i < 100; i++)
    {
        UpperHessenbergEigen<double> decomp(H);
        VectorXcd evals = decomp.eigenvalues();
        MatrixXcd evecs = decomp.eigenvectors();
    }
    t2 = clock();
    std::cout << "elapsed time for UpperHessenbergEigen: "
              << double(t2 - t1) / CLOCKS_PER_SEC << " secs\n";

    t1 = clock();
    for(int i = 0; i < 100; i++)
    {
        Eigen::EigenSolver<MatrixXd> decomp(H);
        VectorXcd evals = decomp.eigenvalues();
        MatrixXcd evecs = decomp.eigenvectors();
    }
    t2 = clock();
    std::cout << "elapsed time for Eigen::EigenSolver: "
              << double(t2 - t1) / CLOCKS_PER_SEC << " secs\n";
}

TEST_CASE("Eigen decomposition of symmetric tridiagonal matrix", "[Eigen]")
{
    std::srand(123);
    int n = 100;
    MatrixXd m = MatrixXd::Random(n, n);
    m.array() -= 0.5;
    MatrixXd H = MatrixXd::Zero(n, n);
    H.diagonal() = m.diagonal();
    H.diagonal(-1) = m.diagonal(-1);
    H.diagonal(1) = m.diagonal(-1);

    TridiagEigen<double> decomp(H);
    VectorXd evals = decomp.eigenvalues();
    MatrixXd evecs = decomp.eigenvectors();

    MatrixXd err = H * evecs - evecs * evals.asDiagonal();

    INFO( "||HU - UD||_inf = " << err.cwiseAbs().maxCoeff() );
    REQUIRE( err.cwiseAbs().maxCoeff() == Approx(0.0) );

    clock_t t1, t2;
    t1 = clock();
    for(int i = 0; i < 100; i++)
    {
        TridiagEigen<double> decomp(H);
        VectorXd evals = decomp.eigenvalues();
        MatrixXd evecs = decomp.eigenvectors();
    }
    t2 = clock();
    std::cout << "elapsed time for TridiagEigen: "
              << double(t2 - t1) / CLOCKS_PER_SEC << " secs\n";

    t1 = clock();
    for(int i = 0; i < 100; i++)
    {
        Eigen::SelfAdjointEigenSolver<MatrixXd> decomp(H);
        VectorXd evals = decomp.eigenvalues();
        MatrixXd evecs = decomp.eigenvectors();
    }
    t2 = clock();
    std::cout << "elapsed time for Eigen::SelfAdjointEigenSolver: "
              << double(t2 - t1) / CLOCKS_PER_SEC << " secs\n";
}


#ifndef _COCONUT_PULP_MATH_ANGLE_HPP_
#define _COCONUT_PULP_MATH_ANGLE_HPP_

#include <iosfwd>

#include <boost/operators.hpp>

namespace coconut {
namespace pulp {
namespace math {

const float PI = static_cast<float>(3.14159264f);

class Angle :
	boost::less_than_comparable<Angle,
	boost::equality_comparable<Angle,
	boost::additive<Angle,
	boost::multiplicative<Angle, float>>>>
{
public:

	static const Angle RIGHT;

	static const Angle HALF_FULL;

	static const Angle FULL;

	constexpr float radians() const noexcept {
		return radians_;
	}

	constexpr float degrees() const noexcept {
		return radians_ * (180.0f / PI);
	}

	constexpr bool operator==(const Angle& rhs) const noexcept {
		return radians_ == rhs.radians_;
	}

	constexpr bool operator<(const Angle& rhs) const noexcept {
		return radians_ < rhs.radians_;
	}

	Angle& operator+=(const Angle& rhs) noexcept {
		radians_ += rhs.radians_;
		return *this;
	}

	Angle& operator-=(const Angle& rhs) noexcept {
		radians_ -= rhs.radians_;
		return *this;
	}

	Angle& operator*=(float rhs) noexcept {
		radians_ *= rhs;
		return *this;
	}

	Angle& operator/=(float rhs) noexcept {
		radians_ /= rhs;
		return *this;
	}

	Angle operator-() const noexcept {
		return -1.0f * (*this);
	}

	friend const Angle radians(float radians) noexcept;

	friend const Angle degrees(float degrees) noexcept;

private:

	constexpr explicit Angle(float radians) noexcept :
		radians_(radians)
	{
	}

private:

	float radians_;

};

static_assert(sizeof(Angle) == sizeof(float), "Angle should have no extra data");

std::ostream& operator<<(std::ostream& os, const Angle& angle);

inline const Angle radians(float radians) noexcept {
	return Angle(radians);
}

inline const Angle degrees(float degrees) noexcept {
	return Angle(degrees * (PI / 180.0f));
}

inline const Angle operator""_rad(long double r) noexcept {
	return radians(static_cast<float>(r));
}

inline const Angle operator""_deg(long double d) noexcept {
	return degrees(static_cast<float>(d));
}

} // namespace math

using math::PI;
using math::Angle;
using math::radians;
using math::degrees;

namespace math_literals {

using math::operator ""_deg;
using math::operator ""_rad;

} // namespace math_literals

} // namespace pulp
} // namespace coconut

#endif /* _COCONUT_PULP_MATH_ANGLE_HPP_ */


#include <iostream>
#include <armadillo>
#include <cmath>

double calculate_anisotropy(const arma::mat &m) {

    const double iso = arma::mean(arma::eig_sym(m));
    double aniso = arma::accu(m % m);
    aniso = std::sqrt(std::abs(1.5*(aniso - (3.0*iso*iso))));

    return aniso;
}

int main() {

    arma::mat tensor(3, 3);
    tensor(0, 0) = 71.96979730;
    tensor(0, 1) = -7.19617988;
    tensor(0, 2) = 1.01470321;
    tensor(1, 0) = -7.19617833;
    tensor(1, 1) = 65.74361807;
    tensor(1, 2) = -5.62003645;
    tensor(2, 0) = 1.01469269;
    tensor(2, 1) = -5.62002592;
    tensor(2, 2) = 65.05385723;

    // arma::cx_vec principal_components_cx;
    // arma::cx_mat orientation_cx;

    // arma::eig_gen(principal_components_cx, orientation_cx, tensor);
    // principal_components_cx.print("principal components (complex)");
    // orientation_cx.print("orientation (complex)");

    arma::vec principal_components;
    arma::mat orientation;

    arma::eig_sym(principal_components, orientation, tensor);

    principal_components.print("principal components (real)");
    orientation.print("orientation (real)");

    double isotropic = arma::mean(principal_components);
    std::cout << "isotropic  : " << isotropic << std::endl;
    double anisotropic = calculate_anisotropy(tensor);
    std::cout << "anisotropic: " << anisotropic << std::endl;

    return 0;
}


//-----------------------------------------------------------------------------
// Copyright (c) 2015-2018 Benjamin Buch
//
// https://github.com/bebuch/mitrax
//
// Distributed under the Boost Software License, Version 1.0. (See accompanying
// file LICENSE_1_0.txt or copy at https://www.boost.org/LICENSE_1_0.txt)
//-----------------------------------------------------------------------------
#define BOOST_TEST_MODULE mitrax convolution
#include <boost/test/unit_test.hpp>

#include <mitrax/convolution.hpp>

#include <iostream>

using boost::typeindex::type_id;
using boost::typeindex::type_id_runtime;
using namespace mitrax;
using namespace mitrax::literals;

constexpr auto image = make_matrix< int >(5_DS, {
	{1, 2, 3, 4, 5},
	{2, 3, 4, 5, 6},
	{3, 4, 5, 6, 7},
	{4, 5, 6, 7, 8},
	{5, 6, 7, 8, 9}
});

constexpr auto sobel_x = make_matrix< int >(3_DS, {
	{1, 0, -1},
	{2, 0, -2},
	{1, 0, -1}
});

BOOST_AUTO_TEST_SUITE(suite_convolution)

BOOST_AUTO_TEST_CASE(test_convolution){
	constexpr auto m = convolution(image, sobel_x);

	auto eq =
		m.cols() == 3_CS &&
		m.rows() == 3_RS &&
		m(0_c, 0_r) == -8 &&
		m(1_c, 0_r) == -8 &&
		m(2_c, 0_r) == -8 &&
		m(0_c, 1_r) == -8 &&
		m(1_c, 1_r) == -8 &&
		m(2_c, 1_r) == -8 &&
		m(0_c, 2_r) == -8 &&
		m(1_c, 2_r) == -8 &&
		m(2_c, 2_r) == -8;

	BOOST_TEST(eq);

	BOOST_TEST(type_id_runtime(m) == (type_id< std_matrix< int, 3_C, 3_R > >()));
}

BOOST_AUTO_TEST_SUITE_END()


#include <Eigen/Core>
#include <iostream>
#include <vector>

#include "crossprod.h"

int main() {
  double T = 1.;
  int N = 1;

  Eigen::Vector3d y0(0.1, 0.2, 0.4);

  auto f = [](Eigen::Vector3d y) -> Eigen::Vector3d {
    return Eigen::Vector3d(y(0) * y(1), y(1) * y(2), y(2) - y(0));
  };

  auto Jf = [](Eigen::Vector3d y) -> Eigen::Matrix3d {
    Eigen::Matrix3d J;
    J << y(1), y(0), 0, 0, y(2), y(1), -1, 0, 1;
    return J;
  };
  // test implicit midpoint
  std::vector<Eigen::VectorXd> test_imp =
      CrossProd::solve_imp_mid(f, Jf, T, y0, N);
  std::cout << "Implicit midpoint:\n"
            << test_imp.back() << std::endl
            << std::endl;

  // test linear implicit midpoint
  std::vector<Eigen::VectorXd> test_lin =
      CrossProd::solve_lin_mid(f, Jf, T, y0, N);
  std::cout << "Implicit linear midpoint:\n"
            << test_lin.back() << std::endl
            << std::endl;

  // CrossProd::tab_crossprod();

  return 0;
}


#include "drake/math/quadratic_form.h"

#include <algorithm>

#include <Eigen/Cholesky>
#include <Eigen/Eigenvalues>

#include "drake/math/matrix_util.h"

namespace drake {
namespace math {
Eigen::MatrixXd DecomposePSDmatrixIntoXtransposeTimesX(
    const Eigen::Ref<const Eigen::MatrixXd>& Y, double zero_tol) {
  if (Y.rows() != Y.cols()) {
    throw std::runtime_error("Y is not square.");
  }
  if (zero_tol < 0) {
    throw std::runtime_error("zero_tol should be non-negative.");
  }
  Eigen::LLT<Eigen::MatrixXd> llt_Y(Y);
  if (llt_Y.info() == Eigen::Success) {
    return llt_Y.matrixU();
  } else {
    // TODO(hongkai.dai) Switch to use robust Choleskly decomposition instead
    // of Eigen value decomposition, when the bug in
    // http://eigen.tuxfamily.org/bz/show_bug.cgi?id=1479 is fixed.
    Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> es_Y(Y);
    if (es_Y.info() == Eigen::Success) {
      Eigen::MatrixXd X(Y.rows(), Y.cols());
      int X_row_count = 0;
      for (int i = 0; i < es_Y.eigenvalues().rows(); ++i) {
        if (es_Y.eigenvalues()(i) < -zero_tol) {
          throw std::runtime_error("Y is not positive definite.");
        } else if (es_Y.eigenvalues()(i) > zero_tol) {
          X.row(X_row_count++) = std::sqrt(es_Y.eigenvalues()(i)) *
                                 es_Y.eigenvectors().col(i).transpose();
        }
      }
      return X.topRows(X_row_count);
    }
  }
  throw std::runtime_error("Y is not PSD.");
}

std::pair<Eigen::MatrixXd, Eigen::MatrixXd> DecomposePositiveQuadraticForm(
    const Eigen::Ref<const Eigen::MatrixXd>& Q,
    const Eigen::Ref<const Eigen::VectorXd>& b, double c, double tol) {
  if (Q.rows() != Q.cols()) {
    throw std::runtime_error("Q should be a square matrix.");
  }
  if (b.rows() != Q.rows()) {
    throw std::runtime_error("b does not have the right size.");
  }
  // The quadratic form xᵀQx + bᵀx + c can also be written as
  // [x]ᵀ * [Q   b/2] * [x]
  // [1]    [b/2   c]   [1]
  // We will call the matrix in the middle as M
  Eigen::MatrixXd M(Q.rows() + 1, Q.rows() + 1);
  // clang-format on
  M << (Q + Q.transpose()) / 2, b / 2,
       b.transpose() / 2, c;
  // clang-format off

  const Eigen::MatrixXd A = DecomposePSDmatrixIntoXtransposeTimesX(M, tol);
  Eigen::MatrixXd R = A.leftCols(Q.cols());
  Eigen::VectorXd d = A.col(Q.cols());
  return std::make_pair(R, d);
}

Eigen::MatrixXd BalanceQuadraticForms(
    const Eigen::Ref<const Eigen::MatrixXd>& S,
    const Eigen::Ref<const Eigen::MatrixXd>& P) {
  const double tolerance = 1e-8;
  const int n = S.rows();
  DRAKE_THROW_UNLESS(P.rows() == n);
  DRAKE_THROW_UNLESS(IsPositiveDefinite(S, tolerance));
  DRAKE_THROW_UNLESS(IsSymmetric(P, tolerance));

  const Eigen::MatrixXd R =
      S.llt().matrixL().solve(Eigen::MatrixXd::Identity(n, n));

  const Eigen::JacobiSVD<Eigen::MatrixXd> svd(R * P * R.transpose(),
                                              Eigen::ComputeThinU);
  // Check that P was full rank (hence RPR' full-rank).
  DRAKE_THROW_UNLESS(svd.singularValues()(svd.singularValues().size()-1) >=
                         tolerance*std::max(1., svd.singularValues()(0)));

  const Eigen::VectorXd sigmaRootN4 =
      svd.singularValues().array().pow(-0.25).matrix();
  return R.transpose() * svd.matrixU() * sigmaRootN4.asDiagonal();
}

}  // namespace math
}  // namespace drake


#include <Eigen/Dense>
#include <iostream>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/videoio/videoio.hpp>

Eigen::Affine2d get_svd_transform(const Eigen::Matrix<double, -1, 2> &src, const Eigen::Matrix<double, -1, 2> &tgt)
{

  assert(src.rows() == tgt.rows());
  Eigen::Affine2d result = Eigen::Affine2d::Identity();
  Eigen::Matrix<double, -1, 2> src_ = src;
  Eigen::Matrix<double, -1, 2> tgt_ = tgt;
  Eigen::Vector2d centroid_src, centroid_tgt;
  // std::cout << "getting mean" << std::endl;

  centroid_src = src.colwise().mean();
  centroid_tgt = tgt.colwise().mean();

  std::cout << centroid_src << std::endl
            << centroid_tgt << std::endl;

  src_.colwise() -= centroid_src;
  tgt_.colwise() -= centroid_tgt;
  // std::cout << "getting svd" << std::endl;
  Eigen::Matrix2d mat = tgt_.transpose() * src_;
  Eigen::JacobiSVD<Eigen::Matrix2d> svd(mat, Eigen::ComputeFullU | Eigen::ComputeFullV);
  Eigen::Matrix2d u, v;
  u = svd.matrixU();
  v = svd.matrixV();
  // std::cout << "getting rot" << std::endl;
  result.linear() = u * v.transpose();
  if (result.linear().determinant() < 0)
  {
    Eigen::Matrix<double, 2, 2> inv;
    inv << 1, 0,
           0, -1;
    std::cout << "special case" << std::endl;
    // v.transpose().rowwise() *= v;
    v = v * inv;
    result.linear() = u * v.transpose();
  }
  // std::cout << "getting trans" << std::endl;
  result.translation() = centroid_tgt - result.linear() * centroid_src;
  return result;
}

// Return l2 distance of transformed points
double compare_point_sets(const Eigen::Matrix<double, -1, 2> &src, const Eigen::Matrix<double, -1, 2> &tgt, Eigen::Affine2d &result)
{
  int rows = src.rows();

  // std::cout << "src: \n" << src << std::endl;
  // std::cout << "tgt: \n" << tgt << std::endl;
  // std::cout << "getting tf" << std::endl;

  result = get_svd_transform(src, tgt);
  std::cout << "rot:" << std::endl
            << result.linear() << std::endl;
  std::cout << "trans:" << std::endl
            << result.translation() << std::endl;

  Eigen::Matrix<double, -1, 2> transformed_src = (result * src.transpose()).transpose();
  // transformed_src.resize(rows, 2);
  // for(int i=0; i<rows; i++){
  //   transformed_src.row(i) = result * src.row(i);
  // }

  std::cout << "before \n"
            << src << std::endl;
  std::cout << "after \n"
            << transformed_src << std::endl;

  Eigen::Matrix<double, -1, 2> diff = tgt - transformed_src.topLeftCorner(rows, 2);
  Eigen::Matrix<double, -1, 1> err = diff.rowwise().norm();

  cv::Mat before = cv::Mat::zeros(cv::Size(640, 480), CV_8UC3);
  cv::Mat after = cv::Mat::zeros(cv::Size(640, 480), CV_8UC3);
  for (int i = 0; i < src.rows(); i++)
  {
    cv::circle(before, cv::Point(src(i, 0), src(i, 1)), 2, cv::Scalar(0, 0, 255), 2);
  }

  for (int i = 0; i < tgt.rows(); i++)
  {
    cv::circle(before, cv::Point(tgt(i, 0), tgt(i, 1)), 2, cv::Scalar(255, 255, 0), 2);
    cv::circle(after, cv::Point(tgt(i, 0), tgt(i, 1)), 2, cv::Scalar(255, 255, 0), 2);
  }

  for (int i = 0; i < transformed_src.rows(); i++)
  {
    cv::circle(after, cv::Point(transformed_src(i, 0), transformed_src(i, 1)), 2, cv::Scalar(0, 0, 255), 2);
  }

  cv::imshow("before", before);
  cv::imshow("after", after);
  cv::waitKey(2);
  return err.sum();
}


#include <Eigen/Core>
#include <iostream>
#include <sys/stat.h>
#include <igl/readPLY.h>
#include <igl/writePLY.h>
#include <igl/point_mesh_squared_distance.h>
#include <igl/slice.h>
#include <igl/barycentric_coordinates.h>

#include <stdio.h>
#include <stdlib.h>

igl::AABB<Eigen::MatrixXd,3> tree;

/*
float icp_step(Eigen::MatrixXd *Vmov, Eigen::MatrixXd Vref, Eigen::MatrixXi Fref)
{
  using namespace Eigen;
  using namespace std;
  using namespace igl;

  int i;
  float dist = 0;

  // compute closest points
  VectorXd sqrD;
  VectorXi I;
  MatrixXd Vout;

  igl::point_mesh_squared_distance(*Vmov, Vref, Fref, sqrD, I, Vout);

  // compute rotation and translation
  MatrixXd C = Matrix3d::Constant(0);
  VectorXd oout = Vout.colwise().sum()/Vout.rows();
  VectorXd omov = (*Vmov).colwise().sum()/(*Vmov).rows();

  for(i=0;i<(*Vmov).rows();i++)
    C += ((*Vmov).row(i) - omov.transpose()).transpose()
         * (Vout.row(i) - oout.transpose());

  JacobiSVD<MatrixXd> svd(C, ComputeThinU|ComputeThinV);
  Matrix3d U = svd.matrixU();
  Matrix3d V = svd.matrixV().transpose();

  MatrixXd R = U*V;
  MatrixXd t = omov - R*oout;

  // apply rotation and translation
  (*Vmov)*=R;
  (*Vmov).transpose().colwise() -= t.col(0);

  // check convergence
  // angle_dist = arccos((trace(P*Q')-1)/2), P, Q rotation matrices
  // (here, Q=I)
  // http://www.boris-belousov.net/2016/12/01/quat-dist/
  dist = acos((R.trace()-1)/2.0);
  dist = dist + t.norm();

  return dist;
}

int icp(Eigen::MatrixXd Vref, Eigen::MatrixXi Fref,
        Eigen::MatrixXd *Vmov, Eigen::MatrixXi Fmov,
        float *diff)
{
  using namespace Eigen;
  using namespace std;
  using namespace igl;

  int i;
  float dist;
  int maxiter = 100;
  float tol = 1e-6;

  // init distance AABB tree
  tree.init(Vref, Fref);

  float dist0=-1;

  for(i=0;i<maxiter;i++)
  {
    dist = icp_step(Vmov, Vref, Fref);
    if(dist0<0)
    {
        *diff = dist;
        printf("initial distance: %g\n", *diff);
    }
    else
        *diff = fabs(dist-dist0);

    if(*diff < tol)
        break;

    dist0=dist;
  }
  printf("final distance: %g (after %i iterations)\n", *diff, i);

  return i;
}
*/

void read_vertices(char *path, Eigen::MatrixXd *V, Eigen::MatrixXi *E)
{
  using namespace Eigen;
  using namespace std;

  FILE *f;
  int i, nv, ne;
  float x, y, z;
  int a, b;

  f=fopen(path,"r");
  fscanf(f," %i %*i %i ", &nv, &ne);
  cout<<"verts: "<<nv<<endl;
  MatrixXd X(nv, 3);
  for(i=0;i<nv;i++)
  {
    fscanf(f, " %f %f %f ", &x, &y, &z);
    X.row(i)<<x,y,z;
  }
  MatrixXi Y(ne, 2);
  for(i=0;i<ne;i++)
  {
    fscanf(f, " %i %i ", &a, &b);
    Y.row(i)<<a,b;
  }
  (*V) = X;
  (*E) = Y;
}

void save_vertices(char *path, Eigen::MatrixXd V, Eigen::MatrixXi E)
{
  using namespace Eigen;
  using namespace std;

  FILE *f;
  int i;

  f=fopen(path,"w");
  fprintf(f,"%i 0 %i\n", (int)V.rows(), (int)E.rows());
  for(i=0;i<V.rows();i++)
  {
    fprintf(f, "%f %f %f\n", V(i,0), V(i,1), V(i,2));
  }
  for(i=0;i<E.rows();i++)
  {
    fprintf(f, "%i %i\n", E(i,0), E(i,1));
  }
  fclose(f);
}

int main(int argc, char * argv[])
{
  using namespace Eigen;
  using namespace std;
  using namespace igl;

  /*
    Transform fold curves to sphere
  */
  char path_orig[] = "/Users/roberto/Documents/annex-foldgraph/data/raw/baboon/both.ply";
  char path_sph[] = "/Users/roberto/Documents/annex-foldgraph/data/derived/skeleton/baboon/both_spherical.ply";
  char path_verts[] = "/Users/roberto/Documents/annex-foldgraph/data/derived/skeleton/baboon/both_skel_curves.txt";
  char path_out[] = "/Users/roberto/Documents/annex-foldgraph/data/derived/skeleton/baboon/test.txt";

  struct stat info;
  if(stat(path_orig, &info))
      printf("File %s not found\n", path_orig);
  if(stat(path_sph, &info))
      printf("File %s not found\n", path_sph);
  if(stat(path_verts, &info))
      printf("File %s not found\n", path_verts);

  MatrixXd Vorig, Vsph;
  MatrixXi Forig, Fsph;
  MatrixXd TMP1, TMP2;

  igl::readPLY(path_orig, Vorig, Forig, TMP1, TMP2);
  cout<<"Original nv, nt: "<<Vorig.rows()<<", "<<Forig.rows()<<endl;

  igl::readPLY(path_sph, Vsph, Fsph, TMP1, TMP2);
  cout<<"Spherical nv, nt: "<<Vsph.rows()<<", "<<Fsph.rows()<<endl;

  MatrixXd V;
  MatrixXi E;
  Vector3d t;
  t<<32.5,38.74,23.92;
  read_vertices(path_verts, &V, &E);
  V*=0.26;
  V.transpose().colwise() -=t;
  cout<<"Verts: "<<V.rows()<<endl;

  // init distance AABB tree
  tree.init(Vorig, Forig);

  // compute closest points
  VectorXd sqrD;
  VectorXi I;
  MatrixXd Vout;
  igl::point_mesh_squared_distance(V, Vorig, Forig, sqrD, I, Vout);

  // get barycentric coordinates
  MatrixXd Vx, Vy, Vz, B;
  Vector3d xyz;
  xyz<<0,1,2;
  MatrixXd VV(Vout.rows(),3);
  MatrixXi FF;
  slice(Forig, I, xyz, FF);
  slice(Vorig, FF.col(0), xyz, Vx);
  slice(Vorig, FF.col(1), xyz, Vy);
  slice(Vorig, FF.col(2), xyz, Vz);
  barycentric_coordinates(Vout, Vx, Vy, Vz, B);

  // get coordinates in Vsph space
  slice(Vsph, FF.col(0), xyz, Vx);
  slice(Vsph, FF.col(1), xyz, Vy);
  slice(Vsph, FF.col(2), xyz, Vz);
  int i;
  for(i=0;i<Vout.rows();i++)
    VV.row(i) = Vx.row(i)*B(i, 0) + Vy.row(i)*B(i, 1) + Vz.row(i)*B(i, 2);

  // save result
  save_vertices(path_out, VV, E);

  return 0;
}