#include <iostream>

#include <Eigen/Core>
#include <Eigen/Dense>

#include "tyco.hpp"

namespace cs
{
struct world {};
struct camera {};
};

template<typename Cs>
using point3 = tyco::P3::column_vector<double, Cs>;

template<typename Cs>
using plane3 = tyco::P3::row_vector<double, Cs>;

template<typename CsLeft, typename CsRight>
using homography3 = tyco::P3::homography<double, CsLeft, CsRight>;

int main()
{
    using namespace std;
    cout << "TEST" << endl;

    auto point_world  = point3<cs::world>{{1, 1, 1, 1}};
    auto point_camera = point3<cs::camera>{{1, 1, 1, 0}};

    auto plane_world  = plane3<cs::world>();
    auto plane_camera = plane3<cs::camera>();

    auto camera_from_world  = homography3<cs::camera, cs::world>();
    auto world_from_camera  = homography3<cs::world,  cs::camera>();
    auto world_from_world   = homography3<cs::world,  cs::world>();
    auto camera_from_camera = homography3<cs::camera, cs::camera>();

    world_from_camera = inverse(camera_from_world);
    camera_from_world = inverse(world_from_camera);

    point_camera = camera_from_world * point_world;
    point_world = world_from_camera * point_camera;

    plane_camera = plane_world * world_from_camera;
    plane_world = plane_camera * camera_from_world;

    world_from_world = world_from_camera * camera_from_world;
    camera_from_camera = camera_from_world * world_from_camera;
}


//==============================================================================
//         Copyright 2003 - 2012   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2012   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#define NT2_UNIT_MODULE "nt2 optimize toolbox - levenberg"

#include <iostream>
#include <nt2/include/functions/levenberg.hpp>

#include <nt2/sdk/unit/tests.hpp>
#include <nt2/sdk/unit/module.hpp>
#include <boost/fusion/tuple.hpp>
#include <boost/lambda/lambda.hpp>
#include <boost/bind.hpp>
#include <nt2/include/functions/sqr.hpp>
#include <nt2/include/functions/exp.hpp>
#include <nt2/include/functions/zeros.hpp>
#include <nt2/include/functions/globalsum.hpp>
#include <nt2/include/functions/globalmax.hpp>
#include <nt2/include/functions/ones.hpp>
#include <nt2/include/constants/half.hpp>
#include <nt2/include/constants/eps.hpp>
#include <nt2/include/constants/four.hpp>
#include <nt2/table.hpp>

template < class Tabout >
struct fpp
{
  template < class Tabin> inline
  Tabout operator()(const Tabin & x ) const
  {
    typedef typename Tabin::value_type value_type;
//    Tabout r = (nt2::sqr(x)-value_type(3))*x+nt2::_(value_type(1), value_type(numel(x)));
    Tabout r = (nt2::sqr((x-nt2::_(value_type(1), value_type(numel(x))))));
    return r;
  }
};

template<class Tabout, class Tabin >  Tabout f1(const Tabin & x )
{
    typedef typename Tabin::value_type value_type;
    Tabout r = (nt2::sqr(x)-value_type(3))*x;
    return r;
}

// NT2_TEST_CASE_TPL( levenberg_function_ptr, NT2_REAL_TYPES )
// {
//   using nt2::levenberg;
//   using nt2::optimization::output;
//   typedef nt2::table<T> tab_t;
//   typedef typename nt2::meta::as_logical<T>::type lT;
//   typedef nt2::table<T> ltab_t;
//   tab_t x0 = nt2::zeros(nt2::of_size(1, 3), nt2::meta::as_<T>());
//   ltab_t h = nt2::is_nez(nt2::ones (nt2::of_size(1, 3), nt2::meta::as_<T>())*nt2::Half<T>());
//   tab_t r = nt2::ones (nt2::of_size(1, 3), nt2::meta::as_<T>());
//   output<tab_t,T> res = levenberg(&f1<tab_t, tab_t>, x0, h);

//   std::cout << "Minimum : f(" << res.minimum << ") = " << res.value
//             << " after " << res.iterations_count <<  " iterations\n";

//   NT2_TEST(res.successful);
//   NT2_TEST_LESSER_EQUAL(nt2::globalmax(nt2::abs(res.minimum()-r)), nt2::Sqrteps<T>());
// }

NT2_TEST_CASE_TPL( levenberg_functor, NT2_REAL_TYPES )
{
  using nt2::levenberg;
  using nt2::options;
  using nt2::optimization::output;
  typedef nt2::table<T> tab_t;
  typedef typename nt2::meta::as_logical<T>::type lT;
  typedef nt2::table<T> ltab_t;
  tab_t x0 = nt2::zeros(nt2::of_size(1, 3), nt2::meta::as_<T>());
  ltab_t h = nt2::is_nez(nt2::ones (nt2::of_size(1, 3), nt2::meta::as_<T>())*nt2::Half<T>());
  tab_t r = nt2::_(T(1), T(3));
  output<tab_t,T> res = levenberg(fpp<tab_t>(), x0, h,
                                  options [ nt2::iterations_ = 100,
                                            nt2::tolerance::absolute_ = nt2::Eps<T>()
                                    ]);

  std::cout << "Minimum : f(" << res.minimum << ") = " << res.value
            << " after " << res.iterations_count <<  " iterations\n";

  NT2_TEST(res.successful);
  NT2_TEST_LESSER_EQUAL(nt2::globalmax(nt2::abs(res.minimum()-r)), nt2::Four<T>()*nt2::Sqrteps<T>());

}

// NT2_TEST_CASE_TPL( levenberg_function, (double)(float) )
// {
//   using nt2::levenberg;
//   using nt2::optimization::output;

//   output<T,T> res = levenberg<T>( f1, 0, 0.5, 2 );

//   std::cout << "Minimum : f(" << res.minimum << ") = " << res.value
//             << " after " << res.iterations_count <<  " iterations\n";

//   NT2_TEST(res.successful);
//   NT2_TEST_LESSER_EQUAL(nt2::abs(res.minimum - 1.f), nt2::Sqrteps<float>());
// }

// NT2_TEST_CASE_TPL( levenberg_functor, (double)(float) )
// {
//   using nt2::levenberg;
//   using nt2::optimization::output;

//   output<T,T> res = levenberg<T>( f2(), 0, 0.5, 2 );

//   std::cout << "Minimum : f(" << res.minimum << ") = " << res.value
//             << " after " << res.iterations_count <<  " iterations\n";

//   NT2_TEST(res.successful);
//   NT2_TEST_LESSER_EQUAL(nt2::abs(res.minimum - T(1)), nt2::Sqrteps<T>());
// }

// NT2_TEST_CASE_TPL( levenberg_bind, (double)(float) )
// {
//   using nt2::levenberg;
//   using nt2::optimization::output;

//   output<T,T> res = levenberg<T>( boost::bind(f3, _1, 3., 4.), 0, 0.5, 2 );

//   std::cout << "Minimum : f(" << res.minimum << ") = " << res.value
//             << " after " << res.iterations_count <<  " iterations\n";

//   NT2_TEST(res.successful);
//   NT2_TEST_LESSER_EQUAL(nt2::abs(res.minimum - T(1)), nt2::Sqrteps<T>());
// }

// NT2_TEST_CASE_TPL( levenberg_lambdda, (double)(float) )
// {
//   using nt2::levenberg;
//   using nt2::optimization::output;
//   namespace bl = boost::lambda;

//   output<T,T> res = levenberg<T>( bl::_1*bl::_1*bl::_1 - 3*bl::_1 + 4
//                             , 0, 0.5, 2
//                             );

//   std::cout << "Minimum : f(" << res.minimum << ") = " << res.value
//             << " after " << res.iterations_count <<  " iterations\n";

//   NT2_TEST(res.successful);
//   NT2_TEST_LESSER_EQUAL(nt2::abs(res.minimum - T(1)), nt2::Sqrteps<T>());
// }

// NT2_TEST_CASE_TPL( levenberg_tie, (double)(float) )
// {
//   using nt2::levenberg;
//   using boost::fusion::tie;
//   using boost::fusion::ignore;

//   std::size_t i;
//   T x,y,fx;
//   bool convergence_successful;

//   tie(x,fx,i,convergence_successful) = levenberg<T>( &f0<T>, 0, 0.5, 2 );
//   std::cout << "Minimum : f(" << x << ") = " << fx
//             << " after " << i <<  " iterations\n";

//   NT2_TEST(convergence_successful);
//   NT2_TEST_LESSER_EQUAL(nt2::abs(x - T(1)), nt2::Sqrteps<T>());

//   tie(y,ignore,ignore,ignore) = levenberg<T>( &f0<T>, 0, 0.5, 2 );
//   std::cout << "Minimum is at y = " << y << "\n";

//   NT2_TEST_LESSER_EQUAL(nt2::abs(y - T(1)), nt2::Sqrteps<T>());
// }

// NT2_TEST_CASE_TPL( levenberg_option, (double)(float) )
// {
//   using nt2::levenberg;
//   using nt2::options;
//   using nt2::optimization::output;

//   output<T,T> res = levenberg<T>( f1, 0, 0.5, 2
//                             , options [ nt2::iterations_ = 10
//                                       , nt2::tolerance::absolute_ = T(1e-3)
//                                       ]
//                             );

//   std::cout << "Minimum : f(" << res.minimum << ") = "  << res.value
//             << " after "      << res.iterations_count   <<  " iterations\n";

//   NT2_TEST(res.successful);
//   NT2_TEST_LESSER_EQUAL(nt2::abs(res.minimum - 1.f), 1e-3);

//   res = levenberg<T>( f1, 0,0.5,2, options [ nt2::iterations_ = 1 ] );
//   NT2_TEST(!res.successful);
// }

// double f1(const matrix < double >& x )       {
//      return x*x*x-3*x+4;
// }

// int main(int argc, char* argv[])
// {
//   ffp fff;
//   matrix < double >  b =  0.5*ones(1);
//   matrix < double >  h =  ones(1);
//   levenberg < matrix < double > > hjm;
//   cout << hjm.optimize(fff, b, h) << endl; ;
//   cout << b << endl;
//   cout << "minimum " << hjm.optimize(fff, b, h) <<  " au point " <<  b << " en " << hjm.getNbIteration() <<  " iterations" << endl;

// }


/*****************************************************************************/
/*                                                                           */
/* Best-fit                                                                  */
/*                                                                           */
/* Copyright 2014                                                            */
/* Alasdair Craig                                                            */
/* ac@acraig.za.net                                                          */
/* License: Code Project Open License 1.02                                   */
/* http://www.codeproject.com/info/cpol10.aspx                               */
/*                                                                           */
/*****************************************************************************/

#ifdef _MSC_VER
#pragma warning( disable : 4244 ) // suppress possible loss-of-data warning arising from boost
#endif

#include "BestFit.h"
#include "cholesky.h"

#include <limits>
#include <cassert>
#include <stdexcept>
#include <iomanip>
#include <boost/numeric/ublas/lu.hpp>
#include <boost/numeric/ublas/io.hpp>

#define CONVERGENCE_CRITERIA 0.000000001
#define MAX_ITERATIONS 50

BestFit::BestFit(int unknowns)
	: m_verbosity(1)
	, m_oStream(m_nullStream)
	, m_solution(unknowns,1)
	, m_provisionals(unknowns,1)
	, m_numObs(0)
	, m_numUnknowns(unknowns)
	, m_minx( std::numeric_limits<double>::max())
	, m_maxx(-std::numeric_limits<double>::max())
	, m_miny( std::numeric_limits<double>::max())
	, m_maxy(-std::numeric_limits<double>::max())
{
}

BestFit::BestFit(int unknowns, std::ostream &oStream)
	: m_verbosity(1)
	, m_oStream(oStream)
	, m_solution(unknowns,1)
	, m_provisionals(unknowns,1)
	, m_numObs(0)
	, m_numUnknowns(unknowns)
	, m_minx( std::numeric_limits<double>::max())
	, m_maxx(-std::numeric_limits<double>::max())
	, m_miny( std::numeric_limits<double>::max())
	, m_maxy(-std::numeric_limits<double>::max())
{
}

BestFit::~BestFit()
{
}

// Set the level of detail required for output to stdout
void BestFit::SetVerbosity(int verbosity)
{
	m_verbosity = verbosity;
}

bool BestFit::Compute(BestFitIO &in, BestFitIO &out)
{
	SetVerbosity(in.verbosity);

	if (!in.points || in.numPoints == 0)
		{
		m_oStream << "No solution. No input points." << std::endl;
		return false;
		}
	if (in.numPoints < m_numUnknowns + 1)
		{
		m_oStream << "No solution. Too few input points, need " << m_numUnknowns + 1 - in.numPoints << " or more." << std::endl;
		return false;
		}

	m_numObs = in.numPoints;

	ResizeMatrices();

	for (int i = 0; i < m_numObs; ++i)
	{
		double x = in.points[i * 2 + 0];
		double y = in.points[i * 2 + 1];
		AddObservation(i, x, y);
	}

	Compute();

	FillOutput(out);

	return true;
}

void BestFit::ResizeMatrices()
{
	m_residuals.resize(m_numObs, 1);
	m_design.resize(m_numObs, m_numUnknowns);
	m_l.resize(m_numObs, 1);
	m_qweight.resize(m_numObs, m_numObs, 0, 0);
	m_observations.resize(m_numObs, 2);
	m_b.resize(m_numObs, m_numObs * 2);
}

// Add another observable (coordinate) to the computation object
void BestFit::AddObservation(int count, double x, double y)
{
	m_observations(count, 0) = x;
	m_observations(count, 1) = y;

	m_minx = std::min<double>(m_minx, x);
	m_maxx = std::max<double>(m_maxx, x);
	m_miny = std::min<double>(m_miny, y);
	m_maxy = std::max<double>(m_maxy, y);
}

// Do the least-squares adjustment
bool BestFit::Compute()
{
	if (m_verbosity > 1)
		m_oStream << "Observations:    " << m_observations << std::endl;

	GenerateProvisionals();

	bool successful = true;

	int iteration = 0;

	while (true)
		{
		FormulateMatrices();

		if (m_verbosity > 1)
			{
			m_oStream << "Provisionals:    " << m_provisionals << std::endl;
			m_oStream << "l-matrix:        " << m_l << std::endl;
			}

		// evaluate the unknowns - small corrections to be applied to the provisional unknowns
		if (EvaluateUnknowns())
			{
			++iteration;

			// add the solution to the provisional unknowns
			EvaluateAdjustedUnknowns();

			if (HasConverged())
				break;
			if (IsDegenerate(iteration))
				break;
			}
		else
			{
			successful = false;
			break;
			}
		}

	successful = successful && (iteration > 0 && iteration < MAX_ITERATIONS);
	if (successful)
		{
		// Give ellipse chance to normalise it's axes.
		NormaliseAdjustedUnknowns();

		// evaluate the residuals
		EvaluateResiduals();

		// add the residuals to the provisional observations
		EvaluateAdjustedObservations();

		// Check that the adjusted unknowns and adjusted observations satisfy
		// the original line/circle/ellipse equation.
		GlobalCheck();

		// Subsequent error analysis and statistical output
		ErrorAnalysis(iteration);

		if (0 == m_verbosity)
			OutputSimpleSolution();
		}

	return successful;
}

// Matrix inversion routine using LU decomposition
bool BestFit::InvertMatrix(const ublas::matrix<double> &input, ublas::matrix<double> &inverse)
{
	// create a working copy of the input
	ublas::matrix<double> A(input);

	// create a permutation matrix for the LU-factorization
	ublas::permutation_matrix<double> pm(A.size1());

	// perform LU-factorization
	auto res = ublas::lu_factorize(A, pm);
	if (res != 0)
		return false;

	// create identity matrix of "inverse"
	inverse.assign(ublas::identity_matrix<double>(A.size1()));

	try
		{
		// back-substitute to get the inverse
		ublas::lu_substitute(A, pm, inverse);
		}
	catch (std::logic_error &)
		{
		return false;
		}

	return true;
}

// Determine solution vector
bool BestFit::EvaluateUnknowns()
{
	ublas::matrix<double> pa = ublas::prod(m_qweight, m_design);
	ublas::matrix<double> atpa = ublas::prod(ublas::trans(m_design), pa);

	ublas::matrix<double> inverse(atpa.size1(), atpa.size2());
	//if (CholeskyInversion(atpa, inverse))
	if (InvertMatrix(atpa, inverse))
		{
		ublas::matrix<double> pl = ublas::prod(m_qweight, m_l);
		ublas::matrix<double> atpl = ublas::prod(ublas::trans(m_design), pl);

		m_solution = ublas::prod(inverse, atpl);

		if (m_verbosity > 2)
			m_oStream << "Iter. solution   " << m_solution << std::endl;
		return true;
		}
	else
		m_oStream << "No solution. Cannot invert matrix." << std::endl;
	return false;
}

// Add solution to the provisionals
void BestFit::EvaluateAdjustedUnknowns()
{
	m_provisionals += m_solution;

	if (m_verbosity > 2)
		m_oStream << "Iter adj unknowns" << m_provisionals << std::endl;
}

// Residuals to the initial observations
void BestFit::EvaluateResiduals()
{
	// v = Ax-l, quasi-residuals in the case of circle and ellipse
	m_residuals = ublas::prod(m_design, m_solution) - m_l;

	if (m_verbosity > 1)
		m_oStream << "Residuals:       " << m_residuals << std::endl;
}

void BestFit::EvaluateFinalResiduals(int point, double &vxi, double &vyi) const
{
	// In the case of the quasi-parametric case (circle and ellipse) the actual
	// residuals for both the x and y coordinate need to be extracted.
	vxi = m_design(point, 0) * m_qweight(point, point) * m_residuals(point, 0);
	vyi = m_design(point, 1) * m_qweight(point, point) * m_residuals(point, 0);

	//// Method 1: ref (2:35)
	//ublas::matrix<double> bt = ublas::trans(m_b);
	//ublas::matrix<double> pv = ublas::prod(m_qweight, m_residuals);
	//ublas::matrix<double> btpv = ublas::prod(bt, pv);
	//double avxi = -btpv(point * 2 + 0,0);
	//double avyi = -btpv(point * 2 + 1,0);
	//m_oStream << "New residuals (Method 1) differ by: " << avxi - vxi << "," << avyi - vyi << std::endl;

	//// Method 2: ref (2:28)
	//double dx = m_observations(point, 0) - m_provisionals(0, 0);
	//double dy = m_observations(point, 1) - m_provisionals(1, 0);
	//double r = m_provisionals(2, 0);
	//double quasiv = -2.0 * dx * m_solution(0, 0) - 2.0 * dy * m_solution(1, 0) - 2.0 * r * m_solution(2, 0) - m_l(point, 0);
	//double bvxi = - (quasiv * 2.0 * dx) / (4.0 * (dx * dx + dy * dy));
	//double bvyi = - (quasiv * 2.0 * dy) / (4.0 * (dx * dx + dy * dy));
	//m_oStream << "New residuals (Method 2) differ by: " << bvxi - vxi << "," << bvyi - vyi << std::endl;
}

// Add residuals to initial observations
void BestFit::EvaluateAdjustedObservations()
{
	for (int i = 0; i < m_numObs; ++i)
		{
		double vxi = 0.0;
		double vyi = 0.0;
		EvaluateFinalResiduals(i, vxi, vyi); // overridden for normal (non-quasi-parametric) BestFitLine

		m_observations(i, 0) += vxi;
		m_observations(i, 1) += vyi;
		}

	if (m_verbosity > 1)
		m_oStream << "Adj. observations" << m_observations << std::endl;
}

// Global check on the quality of the adjustment
void BestFit::GlobalCheck()
{
	ublas::vector<double> global(m_numObs, 0);
	bool pass = (m_numObs > 0);

	for (int i = 0; i < m_numObs; ++i)
		{
		double x = m_observations(i, 0);
		double y = m_observations(i, 1);

		global(i) = SolveAt(x, y); // should be zero
		pass = pass && fabs(global(i)) < 0.01; // TODO: Is this too lax?
		}

	if (m_verbosity > 1)
		m_oStream << "Global check:    " << global << std::endl;
	if (m_verbosity > 0)
		m_oStream << "Global check of the adjustment     ***" << (pass ? "PASSES***" : "FAILS***") << std::endl;

	// Secondary test is to check that aTPv is zero too, really only neccessary
	// if the above global check fails.
	ublas::matrix<double> atp = ublas::prod(ublas::trans(m_design), m_qweight);
	ublas::matrix<double> atpv = ublas::prod(atp, m_residuals);

	pass = true;
	for (int j = 0; j < m_numUnknowns; ++j)
		pass = pass && Double::IsZero(atpv(j, 0));

	if (!pass && m_verbosity > 1)
		m_oStream << "aTPv             " << atpv << std::endl;
	if (m_verbosity > 0)
		m_oStream << "Check of the evaluated unknowns    ***" << (pass ? "PASSES***" : "FAILS***") << std::endl;
}

// Predicate for seeing whether the solution has become sufficiently small
bool BestFit::HasConverged() const
{
	for (int i = 0; i < m_numUnknowns; ++i)
		{
		if (fabs(m_solution(i, 0)) > CONVERGENCE_CRITERIA)
			return false;
		}
	return true;
}

// Predicate for seeing whether the solution is diverging?
bool BestFit::IsDegenerate(int iteration) const
{
	bool degenerate = (iteration >= MAX_ITERATIONS);
	if (degenerate)
		m_oStream << "No solution. Does not converge." << std::endl;

	return degenerate;
}

// Output stats
// TODO: Variance-covariance matrices
void BestFit::ErrorAnalysis(int iterations)
{
	ublas::matrix<double> pv = ublas::prod(m_qweight, m_residuals);
	ublas::matrix<double> vtpv = ublas::prod(ublas::trans(m_residuals), pv);
	int degreesFreedom = m_numObs - m_numUnknowns;
	double variance = vtpv(0, 0) / degreesFreedom;
	double stddev = sqrt(variance);

	if (m_verbosity > 0)
		{
		m_oStream << std::setprecision(6) << std::setiosflags(std::ios::fixed)
			<< "Number of observations             " << m_numObs << std::endl
			<< "Number of unknowns                 " << m_numUnknowns << std::endl
			<< "Degrees of freedom                 " << degreesFreedom << std::endl
			<< "Iterations until convergence       " << iterations << std::endl
			<< "Variance                           " << variance << std::endl
			<< "Std. dev. observation unit weight  " << stddev << std::endl;

		m_oStream << "***********************************" << std::endl;

		OutputAdjustedUnknowns(m_oStream);
		}
}

// Output at it's very simplest
void BestFit::OutputSimpleSolution() const
{
	for (int i = 0; i < m_numUnknowns; ++i)
		m_oStream << m_provisionals(i, 0) << std::endl;
}

// Return selected values back to caller
void BestFit::FillOutput(BestFitIO &out) const
{
	out.numPoints = m_numObs;
	out.numOutputFields = m_numUnknowns;
	out.verbosity = m_verbosity;

	//memset(out.outputFields, 0, sizeof(outputFields));
	for (int i = 0; i < m_numUnknowns; ++i)
		out.outputFields[i] = m_provisionals(i, 0);

	if (out.wantResiduals)
		out.residuals = new double[m_numObs];

	for (int j = 0; j < m_numObs; ++j)
	{
		if (out.wantAdjustedObs)
		{
			out.points[j * 2 + 0] = m_observations(j, 0);
			out.points[j * 2 + 1] = m_observations(j, 1);
		}
		if (out.wantResiduals)
			out.residuals[j] = m_residuals(j, 0);
	}
}

void BestFit::LowerTriangularModifyInversion(const ublas::matrix<double> &l, ublas::matrix<double> &m)
{
	typedef ublas::matrix<double>::size_type INDEX;
	INDEX i, j, n = l.size1();

	for (i = 0; i < n; ++i)
		m(i, i) = 1.0 / l(i, i);

	for (j = 0; j < n; ++j)
	{
		for (i = j + 1; i < n; ++i)
		{
			assert(i > j);

			double sum = 0.0;
			for (INDEX k = j; k <= i - 1; ++k)
				sum += l(i, k) * m(k, j);

			m(i, j) = -m(i, i) * sum;
		}
	}
}

bool BestFit::CholeskyInversion(const ublas::matrix<double> &input, ublas::matrix<double> &inverse)
{
	ublas::matrix<double> l(input.size1(), input.size2());
	ublas::matrix<double> m(input.size1(), input.size2(), 0.0);

	if (0 == cholesky_decompose(input, l))
	{
		LowerTriangularModifyInversion(l, m);

		inverse = ublas::prod(ublas::trans(m), m);

		return true;
	}

	return false;
}


#ifndef MATMETHODS_HPP
#define MATMETHODS_HPP
#include "mkl.h"
#include "mkl_vsl.h"
#include <boost/random/mersenne_twister.hpp>
#include <boost/random/uniform_int_distribution.hpp>
#include <algorithm>
#include "armadillo"

using namespace arma;
using namespace std;

mat CorMats(const mat &A, const mat &B){
  mat C = cor(A,B);
  C.elem(find_nonfinite(C)).zeros();
  return(C);
}

mat CVCorMAD(const mat &Est, const mat &TestA, const mat &TestB){
  mat testC = CorMats(TestA,TestB);
  mat Errmat = abs(Est-testC);
  return(Errmat);
}

umat GenBoot (size_t colsize, size_t bootstrapnumber){

  umat samplemat = randi<umat>(bootstrapnumber,colsize,distr_param(0,colsize-1));

  return(samplemat);
}

mat BootstrapSample(mat &A,umat &Bootmat,int i){

  return(A.rows(Bootmat.row(i)));
}

void Mdim(const mat &cmat,const string matname){
  cout<<matname<<" has "<<cmat.n_rows<<"rows and "<<cmat.n_cols<<" cols"<<endl;
}

mat BootCorMedian(mat &A, mat &B,cube &C,cube &quants,umat &BootMat,int bsi){

  int status;
  mat medians(A.n_cols,B.n_cols);
  if(A.n_rows!=BootMat.n_cols){
    cerr<<"Boot(incorrect indices):  A.n_rows= "<<A.n_rows<<" BM.n_cols= "<<BootMat.n_cols<<endl;
    throw 15;
  }
  int n= bsi;
  int samplenum = C.n_slices;
  VSLSSTaskPtr task;
  MKL_INT q_order_n=2;
  double q_order[q_order_n];
  double params;
  MKL_INT p,nparams,xstorage;
  p = C.n_rows*C.n_cols;
  xstorage = VSL_SS_MATRIX_STORAGE_COLS;
  params = 0.001;
  nparams = VSL_SS_SQUANTS_ZW_PARAMS_N;
  q_order[0] = 0.5;
  q_order[1] = 0.5+(double)(1)/((double)n);

  status = vsldSSNewTask(&task,&p,&samplenum,&xstorage,C.memptr(),0,NULL);
  status = vsldSSEditStreamQuantiles(task,&q_order_n,q_order,quants.memptr(),&nparams,&params);

  cerr<<"Starting Bootstrap:"<<bsi<<endl;
  //First iteration of bootstrap
  mat tA(A.n_rows,A.n_cols);
  mat tB(B.n_rows,B.n_cols);
  int m=0;
  for(int i=0; i<bsi; ++i){
    tA = BootstrapSample(A,BootMat,i);
    tB = BootstrapSample(B,BootMat,i);

    m = i%C.n_slices;
    C.slice(m) = CorMats(tA,tB);

    if(m==(C.n_slices-1)){
      status = vsldSSCompute(task,VSL_SS_STREAM_QUANTS,VSL_SS_METHOD_SQUANTS_ZW_FAST);
    }
  }
  samplenum=0;
  status = vsldSSCompute(task,VSL_SS_STREAM_QUANTS,VSL_SS_METHOD_SQUANTS_ZW);
  status = vslSSDeleteTask(&task);
  if(n%2==0){
    medians = 0.5*quants(span(0,0),span(),span())+0.5*quants(span(1,1),span(),span());
  }else{
    medians = quants(span(0,0),span(),span());
  }
  cerr<<"Bootstrap finished"<<endl;
  return(medians);
}

uvec trainindex(const size_t totalsize,const int ksize,const int k){
  uvec returnvec(totalsize-ksize);
  int j=-1;
  for(int i=0; i<totalsize; i++){
    if(i <(k*ksize)||i>((k+1)*ksize)-1){
      j++;
      returnvec(j)=i;
    }
  }
  if(j+1!=totalsize-ksize){
    cerr<<"j="<<j<<" returnsize= "<<totalsize-ksize-1<<endl;
  }
  return(returnvec);
}

uvec testindex(const size_t totalsize,const  int ksize,const  int k){
  uvec returnvec(ksize);
  int j=-1;
  for( int i=0; i<totalsize; i++){
    if(i>=(k*ksize)&&i<=((k+1)*ksize-1)){
      j++;
      returnvec(j)=i;

    }
  }
  if(j+1!=ksize){
    cerr<<"j="<<j<<" ksize= "<<ksize<<endl;
  }
  return(returnvec);
}

void KfoldCV (const mat &A,const mat &B, const int kfold, const int chunknum, const int bsi,const string outfilename,const int bsichunksize){

  int kfoldIterations = kfold;
  int iter_k = (int) floor((double)A.n_rows/(double)kfold);
  if(kfoldIterations<1){
    cerr<<"Must have at least one iteration of cross-validation, not "<<kfoldIterations<<endl;
    throw 12;
  }

  cout<<"Begin "<<iter_k<<"-fold cross validation ("<<kfoldIterations<<" iterations). There are "<<A.n_rows<<" samples"<<endl;
  uvec testi = testindex(A.n_rows,iter_k,0);
  uvec traini = trainindex(A.n_rows,iter_k,0);

  mat testA,testB,trainA,trainB;

  cout<<"Initiating variables."<<endl;
  mat Point(A.n_cols,B.n_cols);
  cube C(A.n_cols,B.n_cols,bsichunksize);
  cube quants(2,C.n_rows,C.n_cols,fill::zeros);
  cout<<"Generating Bootmat (Dimensions should be "<<bsi<<"x"<<traini.n_elem<<")"<<endl;
  umat BootMat = GenBoot(traini.n_elem,bsi);
  cout<<"Starting kfold: 0"<<endl;
  mat MedianMat(A.n_cols,B.n_cols,fill::zeros);
  mat BootMAD(A.n_cols,B.n_cols,fill::zeros);
  mat pointMAD(A.n_cols,B.n_cols,fill::zeros);

  for(int i=0; i<kfoldIterations; i++){
    cout<<"Starting kfold: "<<i<<endl;
    testi = testindex(A.n_rows,iter_k,i);
    traini = trainindex(A.n_rows,iter_k,i);
    testA = A.rows(testi);
    testB = B.rows(testi);
    trainA = A.rows(traini);
    trainB = B.rows(traini);

    Point = CorMats(trainA,trainB);

    MedianMat = BootCorMedian(trainA,trainB,C,quants,BootMat,bsi);

    BootMAD=BootMAD+CVCorMAD(MedianMat,testA,testB);

    pointMAD += CVCorMAD(Point,testA,testB);

  }
  pointMAD = pointMAD/kfoldIterations;
  BootMAD/=kfoldIterations;

  double pointSumMAD = accu(pointMAD)/pointMAD.n_elem;
  double SumMAD = accu(BootMAD)/BootMAD.n_elem;
  ofstream outputfile;

  outputfile.open(outfilename.c_str(),std::ofstream::out|std::ofstream::app);
  outputfile.setf(ios::fixed,ios::floatfield);

  outputfile.precision(10);
  if(chunknum==0){
    outputfile<<"Chunk\tSize\tbsi\tPointMAD\tBootMAD"<<endl;
  }
  outputfile<<chunknum<<"\t"<<Point.n_elem<<"\t"<<bsi<<"\t"<<pointSumMAD<<"\t"<<SumMAD<<"\t"<<endl;

  outputfile.close();
}

#endif


#include "mtf/SM/PF.h"
#include "mtf/Utilities/miscUtils.h"
#include <boost/random/random_device.hpp>
#include <boost/random/seed_seq.hpp>
#include "opencv2/highgui/highgui.hpp"

//! OpenMP scheduler
#ifndef PF_OMP_SCHD
#define PF_OMP_SCHD auto
#endif

_MTF_BEGIN_NAMESPACE

template <class AM, class SSM>
PF<AM, SSM >::PF(const ParamType *pf_params,
	const AMParams *am_params, const SSMParams *ssm_params) :
	SearchMethod<AM, SSM>(am_params, ssm_params),
	params(pf_params), max_wt_id(0),
	enable_adaptive_resampling(false), min_eff_particles(0){
	printf("\n");
	printf("Using Particle Filter SM with:\n");
	printf("max_iters: %d\n", params.max_iters);
	printf("n_particles: %d\n", params.n_particles);
	printf("epsilon: %f\n", params.epsilon);
	printf("dynamic_model: %s\n", PFParams::toString(params.dynamic_model));
	printf("update_type: %s\n", PFParams::toString(params.update_type));
	printf("likelihood_func: %s\n", PFParams::toString(params.likelihood_func));
	printf("resampling_type: %s\n", PFParams::toString(params.resampling_type));
	printf("adaptive_resampling_thresh: %f\n", params.adaptive_resampling_thresh);
	printf("mean_type: %s\n", PFParams::toString(params.mean_type));
	printf("reset_to_mean: %d\n", params.reset_to_mean);
	ssm_state_size = ssm.getStateSize();
	if(params.pix_sigma.empty() || params.pix_sigma[0] <= 0){
		if(params.ssm_sigma.empty()){
			throw utils::InvalidArgument("Sigma must be provided for at least one sampler");
		}
		using_pix_sigma = false;
	} else{
		using_pix_sigma = true;
		printf("pix_sigma: %f\n", params.pix_sigma[0]);
	}
	printf("measurement_sigma: %f\n", params.measurement_sigma);
	printf("show_particles: %d\n", params.show_particles);
	printf("enable_learning: %d\n", params.enable_learning);
	printf("debug_mode: %d\n", params.debug_mode);
	printf("appearance model: %s\n", am.name.c_str());
	printf("state space model: %s\n", ssm.name.c_str());
	printf("ssm_state_size: %d\n", ssm_state_size);
	printf("\n");

	name = "pf";
	log_fname = "log/pf_debug.txt";
	time_fname = "log/pf_times.txt";
	frame_id = 0;

	const double pi = 3.14159265358979323846;
	measurement_factor = 1.0 / sqrt(2 * pi * params.measurement_sigma);

	for(int set_id = 0; set_id < 2; ++set_id){
		particle_states[set_id].resize(params.n_particles);
		particle_ar[set_id].resize(params.n_particles);
		for(int particle_id = 0; particle_id < params.n_particles; ++particle_id){
			particle_states[set_id][particle_id].resize(ssm_state_size);
			particle_ar[set_id][particle_id].resize(ssm_state_size);
		}
	}
	curr_set_id = 0;
	particle_wts.resize(params.n_particles);
	particle_cum_wts.setZero(params.n_particles);

	perturbed_state.resize(ssm_state_size);
	perturbed_ar.resize(ssm_state_size);
	mean_state.resize(ssm_state_size);
	state_sigma.resize(ssm_state_size);
	state_mean.resize(ssm_state_size);

	if(!using_pix_sigma){
		if(params.ssm_sigma[0].size() < ssm_state_size){
			throw utils::InvalidArgument(
				cv::format("SSM sigma has invalid size: %d",
				params.ssm_sigma[0].size()));
		}
		if(params.ssm_mean.empty()){
			state_mean = VectorXd::Zero(ssm_state_size);
		} else{
			if(params.ssm_mean[0].size() < ssm_state_size){
				throw utils::InvalidArgument(
					cv::format("SSM ssm_mean has invalid size: %d",
					params.ssm_mean[0].size()));
			}
			state_mean = Map<const VectorXd>(params.ssm_mean[0].data(), ssm_state_size);
		}
		state_sigma = Map<const VectorXd>(params.ssm_sigma[0].data(), ssm_state_size);
		utils::printMatrix(state_sigma.transpose(), "state_sigma");
	}
	// initialize random number generators for resampling and measurement function
	boost::random_device r;
	boost::random::seed_seq measurement_seed{ r(), r(), r(), r(), r(), r(), r(), r() };

	measurement_gen = RandGenT(measurement_seed);
	measurement_dist = MeasureDistT(0, params.measurement_sigma);

	boost::random::seed_seq resample_seed{ r(), r(), r(), r(), r(), r(), r(), r() };
	resample_gen = RandGenT(resample_seed);
	resample_dist = ResampleDistT(0, 1);

	if(params.adaptive_resampling_thresh > 0 && params.adaptive_resampling_thresh <= 1){
		printf("Using adaptive resampling\n");
		enable_adaptive_resampling = true;
		min_eff_particles = params.adaptive_resampling_thresh*params.n_particles;
	}

	if(params.debug_mode){
		fclose(fopen(log_fname, "w"));
		resample_ids.resize(params.n_particles);
		uniform_rand_nums.resize(params.n_particles);
	}

#ifdef ENABLE_PARALLEL
	for(int particle_id = 0; particle_id < params.n_particles; particle_id++){
		am_vec.push_back(AMPTr(new AM(am_params)));
		ssm_vec.push_back(SSMPTr(new SSM(ssm_params)));
	}
#endif
#if defined ENABLE_OMP
printf(" ******* Parallelization is enabled using OpenMP ******* \n");
#endif
}

template <class AM, class SSM>
void PF<AM, SSM >::initialize(const cv::Mat &corners){

	ssm.initialize(corners, am.getNChannels());

	if(using_pix_sigma){
		//! estimate SSM parameter sigma from pixel sigma
		ssm.estimateStateSigma(state_sigma, params.pix_sigma[0]);
		state_mean = VectorXd::Zero(ssm_state_size);
	}
	//! initialize SSM sampler with the first distribution
	ssm.initializeSampler(state_sigma, state_mean);

	state_sigma = ssm.getSamplerSigma();
	state_mean = ssm.getSamplerMean();

	am.initializePixVals(ssm.getPts());
	am.initializeSimilarity();
	max_similarity = am.getSimilarity();

	if(params.debug_mode){
		//! print the sigma and mean for the SSM distributions
		utils::printMatrix(state_sigma.transpose(), "state_sigma");
		utils::printMatrix(state_mean.transpose(), "state_mean");
		utils::printScalar(max_similarity, "max_similarity");
	}
	initializeParticles();

	prev_corners = ssm.getCorners();
	ssm.getCorners(cv_corners_mat);
#ifdef ENABLE_PARALLEL
	for(int particle_id = 0; particle_id < params.n_particles; particle_id++){
		ssm_vec[particle_id]->initialize(corners, am.getNChannels());
		ssm_vec[particle_id]->initializeSampler(state_sigma, VectorXd::Zero(ssm_state_size));
		am_vec[particle_id]->initializePixVals(ssm.getPts());
		am_vec[particle_id]->initializeSimilarity();
	}
#endif
}

template <class AM, class SSM>
void PF<AM, SSM >::initializeParticles(){
	double init_wt = 1.0 / params.n_particles;
	for(int particle_id = 0; particle_id < params.n_particles; particle_id++){
		particle_states[curr_set_id][particle_id] = ssm.getState();
		particle_wts[particle_id] = init_wt;
		if(particle_id > 0){
			particle_cum_wts[particle_id] = particle_wts[particle_id] + particle_cum_wts[particle_id - 1];
		} else{
			particle_cum_wts[particle_id] = particle_wts[particle_id];
		}
		particle_ar[curr_set_id][particle_id].setZero();
	}
}

template <class AM, class SSM>
void PF<AM, SSM >::update(){
	++frame_id;
	am.setFirstIter();
	int pause_after_show = 1;
	for(int i = 0; i < params.max_iters; i++){
		if(params.show_particles){
			am.getCurrImg().convertTo(curr_img_uchar, CV_8UC1);
		}
		double max_wt = std::numeric_limits<double>::lowest();
#ifdef ENABLE_OMP
#pragma omp parallel for schedule(PF_OMP_SCHD)
#endif
		for(int particle_id = 0; particle_id < params.n_particles; ++particle_id){
#ifdef ENABLE_PARALLEL
			SSM &ssm = *ssm_vec[particle_id];
			AM &am = *am_vec[particle_id];
#endif
			switch(params.dynamic_model){
			case DynamicModel::AutoRegression1:
				switch(params.update_type){
				case UpdateType::Additive:
					ssm.additiveAutoRegression1(perturbed_state, perturbed_ar,
						particle_states[curr_set_id][particle_id], particle_ar[curr_set_id][particle_id]);
					break;
				case UpdateType::Compositional:
					ssm.compositionalAutoRegression1(perturbed_state, perturbed_ar,
						particle_states[curr_set_id][particle_id], particle_ar[curr_set_id][particle_id]);
					break;
				}
				particle_ar[curr_set_id][particle_id] = perturbed_ar;
				break;
			case DynamicModel::RandomWalk:
				switch(params.update_type){
				case UpdateType::Additive:
					ssm.additiveRandomWalk(perturbed_state, particle_states[curr_set_id][particle_id]);
					break;
				case UpdateType::Compositional:
					ssm.compositionalRandomWalk(perturbed_state, particle_states[curr_set_id][particle_id]);
					break;
				}
				break;
			}
			particle_states[curr_set_id][particle_id] = perturbed_state;

			ssm.setState(particle_states[curr_set_id][particle_id]);
			am.updatePixVals(ssm.getPts());
			am.updateSimilarity(false);

			// a positive number that measures the dissimilarity between the
			// template and the patch corresponding to this particle
			double measuremnt_val = max_similarity - am.getSimilarity();

			// convert this dissimilarity to a likelihood proportional to the dissimilarity
			switch(params.likelihood_func){
			case LikelihoodFunc::AM:
				measurement_likelihood = am.getLikelihood();
				break;
			case LikelihoodFunc::Gaussian:
				measurement_likelihood = measurement_factor * exp(-0.5*measuremnt_val / params.measurement_sigma);
				break;
			case LikelihoodFunc::Reciprocal:
				measurement_likelihood = 1.0 / (1.0 + measuremnt_val);
				break;
			}
			if(params.show_particles){
				cv::Point2d corners[4];
				ssm.getCorners(corners);
				utils::drawCorners(curr_img_uchar, corners,
					cv::Scalar(0, 0, 255), cv::format("%d: %5.3e", particle_id + 1, measurement_likelihood));
				//printf("measurement_likelihood: %e\n", measurement_likelihood);
				if((particle_id + 1) % params.show_particles == 0){
					cv::imshow("Particles", curr_img_uchar);
					int key = cv::waitKey(1 - pause_after_show);
					if(key == 27){
						cv::destroyWindow("Particles");
						params.show_particles = 0;
					} else if(key == 32){
						pause_after_show = 1 - pause_after_show;
					}
					am.getCurrImg().convertTo(curr_img_uchar, CV_8UC1);
				}
			}
			particle_wts[particle_id] = measurement_likelihood;
			if(particle_id > 0){
				particle_cum_wts[particle_id] = particle_wts[particle_id] + particle_cum_wts[particle_id - 1];
			} else{
				particle_cum_wts[particle_id] = particle_wts[particle_id];
			}
			if(particle_wts[particle_id] >= max_wt){
				max_wt = particle_wts[particle_id];
				max_wt_id = particle_id;
			}
		}
		if(params.debug_mode){
			utils::printMatrixToFile(particle_wts.transpose(), "particle_wts", log_fname, "%e");
			utils::printMatrixToFile(particle_cum_wts.transpose(), "particle_cum_wts", log_fname, "%e");
		}
		bool perform_resampling = true;
		if(enable_adaptive_resampling){
			double n_eff_particles = (particle_wts / particle_wts.sum()).squaredNorm();
			n_eff_particles = n_eff_particles == 0 ? 0 :
				1.0 / n_eff_particles;
			if(n_eff_particles > min_eff_particles){
				perform_resampling = false;
			}
		}
		if(perform_resampling){
			switch(params.resampling_type){
			case ResamplingType::None:
				break;
			case ResamplingType::BinaryMultinomial:
				binaryMultinomialResampling();
				break;
			case ResamplingType::LinearMultinomial:
				linearMultinomialResampling();
				break;
			case ResamplingType::Residual:
				residualResampling();
				break;
			}
		}
		switch(params.mean_type){
		case MeanType::None:
			//! set the SSM state to that of the highest weighted particle
			ssm.setState(particle_states[curr_set_id][max_wt_id]);
			break;
		case MeanType::SSM:
			ssm.estimateMeanOfSamples(mean_state, particle_states[curr_set_id], params.n_particles);
			ssm.setState(mean_state);
			break;
		case MeanType::Corners:
			updateMeanCorners();
			ssm.setCorners(mean_corners);
		}

		double update_norm = (prev_corners - ssm.getCorners()).squaredNorm();
		prev_corners = ssm.getCorners();
		if(update_norm < params.epsilon){
			if(params.debug_mode){
				printf("n_iters: %d\n", i + 1);
			}
			break;
		}
		am.clearFirstIter();
	}
	if(params.reset_to_mean){
		initializeParticles();
	}
	if(params.enable_learning){
		am.updateModel(ssm.getPts());
	}
	ssm.getCorners(cv_corners_mat);
}

// uses binary search to find the particle with the smallest
// index whose cumulative weight is greater than the provided
// random number supposedly drawn from a uniform distribution
// between 0 and max cumulative weight
template <class AM, class SSM>
void PF<AM, SSM >::binaryMultinomialResampling(){
	// change the range of the uniform distribution used for resampling instead of normalizing the weights
	//resample_dist.param(ResampleDistParamT(0, particle_cum_wts[params.n_particles - 1]));

	// normalize the cumulative weights and leave the uniform distribution range to (0, 1]
	particle_cum_wts /= particle_cum_wts[params.n_particles - 1];
	if(params.debug_mode){
		utils::printMatrixToFile(particle_cum_wts.transpose(), "normalized particle_cum_wts", log_fname, "%e");
	}
	double max_wt = std::numeric_limits<double>::lowest();
	for(int particle_id = 0; particle_id < params.n_particles; ++particle_id){
		double uniform_rand_num = resample_dist(resample_gen);
		int lower_id = 0, upper_id = params.n_particles - 1;
		int resample_id = (lower_id + upper_id) / 2;
		int iter_id = 0;
		while(upper_id > lower_id){
			if(particle_cum_wts[resample_id] >= uniform_rand_num){
				upper_id = resample_id;
			} else{
				lower_id = resample_id + 1;
			}
			resample_id = (lower_id + upper_id) / 2;
			if(params.debug_mode){
				printf("iter_id: %d upper_id: %d lower_id: %d resample_id: %d\n", iter_id, upper_id,
					lower_id, resample_id);
			}
			++iter_id;
		}

		if(params.debug_mode){
			printf("particle_id: %d resample_id: %d\n", particle_id, resample_id);
			resample_ids[particle_id] = resample_id;
			uniform_rand_nums[particle_id] = uniform_rand_num;
		}

		// place the resampled particle states into the other set so as not to overwrite the current one
		particle_states[1 - curr_set_id][particle_id] = particle_states[curr_set_id][resample_id];
		particle_ar[1 - curr_set_id][particle_id] = particle_ar[curr_set_id][resample_id];
		if(particle_wts[resample_id] >= max_wt){
			max_wt = particle_wts[resample_id];
			max_wt_id = particle_id;
		}
	}
	if(params.debug_mode){
		utils::printMatrixToFile(resample_ids.transpose(), "resample_ids", log_fname, "%d");
		utils::printMatrixToFile(uniform_rand_nums.transpose(), "uniform_rand_nums", log_fname, "%e");
	}
	// make the other particle set the current one
	curr_set_id = 1 - curr_set_id;
}

template <class AM, class SSM>
void PF<AM, SSM >::linearMultinomialResampling(){
	// change the range of the uniform distribution used for resampling instead of normalizing the weights
	//resample_dist.param(ResampleDistParamT(0, particle_cum_wts[params.n_particles - 1]));

	// normalize the cumulative weights and leave the uniform distribution range to (0, 1]
	particle_cum_wts /= particle_cum_wts[params.n_particles - 1];
	if(params.debug_mode){
		utils::printMatrix(particle_cum_wts.transpose(), "normalized particle_cum_wts");
	}
	double max_wt = std::numeric_limits<double>::lowest();
	for(int particle_id = 0; particle_id < params.n_particles; ++particle_id){
		double uniform_rand_num = resample_dist(resample_gen);
		int resample_id = 0;
		while(particle_cum_wts[resample_id] < uniform_rand_num){ ++resample_id; }

		if(params.debug_mode){
			utils::printScalar(uniform_rand_num, "uniform_rand_num");
			utils::printScalar(resample_id, "resample_id", "%d");
		}

		// place the resampled particle states into the other set so as not to overwrite the current one
		particle_states[1 - curr_set_id][particle_id] = particle_states[curr_set_id][resample_id];
		particle_ar[1 - curr_set_id][particle_id] = particle_ar[curr_set_id][resample_id];
		if(particle_wts[resample_id] >= max_wt){
			max_wt = particle_wts[resample_id];
			max_wt_id = particle_id;
		}
	}
	// make the other particle set the current one
	curr_set_id = 1 - curr_set_id;
}

template <class AM, class SSM>
void PF<AM, SSM >::residualResampling() {
	// normalize the weights
	particle_wts /= particle_cum_wts[params.n_particles - 1];
	// vector of particle indies
	VectorXi particle_idx = VectorXi::LinSpaced(params.n_particles, 0, params.n_particles - 1);
	if(params.debug_mode){
		utils::printMatrix(particle_wts.transpose(), "normalized particle_wts");
		utils::printMatrix(particle_idx.transpose(), "particle_idx", "%d");
	}
	// sort, with highest weight first
	std::sort(particle_idx.data(), particle_idx.data() + params.n_particles - 1,
		[&](int a, int b){
		return particle_wts[a] > particle_wts[b];
	});
	if(params.debug_mode){
		utils::printMatrix(particle_idx.transpose(), "sorted particle_idx", "%d");
	}

	// now we append
	int particles_found = 0;
	for(int particle_id = 0; particle_id < params.n_particles; ++particle_id) {
		int resample_id = particle_idx[particle_id];
		int particle_copies = static_cast<int>(round(particle_wts[resample_id] * params.n_particles));
		for(int copy_id = 0; copy_id < particle_copies; ++copy_id) {
			particle_states[1 - curr_set_id][particles_found] = particle_states[curr_set_id][resample_id];
			particle_ar[1 - curr_set_id][particles_found] = particle_ar[curr_set_id][resample_id];
			if(++particles_found == params.n_particles) { break; }
		}
		if(particles_found == params.n_particles) { break; }
	}
	int resample_id = particle_idx[0];
	for(int particle_id = particles_found; particle_id < params.n_particles; ++particle_id) {
		// duplicate particle with highest weight to get exactly same number again
		particle_states[1 - curr_set_id][particle_id] = particle_states[curr_set_id][resample_id];
		particle_ar[1 - curr_set_id][particle_id] = particle_ar[curr_set_id][resample_id];
	}
	curr_set_id = 1 - curr_set_id;
	max_wt_id = particle_idx[0];
}

template <class AM, class SSM>
void PF<AM, SSM >::updateMeanCorners(){
	mean_corners.setZero();
	for(int particle_id = 0; particle_id < params.n_particles; ++particle_id) {
		// compute running average of corners corresponding to the resampled particle states
		ssm.setState(particle_states[curr_set_id][particle_id]);
		mean_corners += (ssm.getCorners() - mean_corners) / (particle_id + 1);
	}
}
template <class AM, class SSM>
void PF<AM, SSM >::setRegion(const cv::Mat& corners){
	ssm.setCorners(corners);
	ssm.getCorners(cv_corners_mat);
	initializeParticles();
}

_MTF_END_NAMESPACE

#ifndef HEADER_ONLY_MODE
#include "mtf/Macros/register.h"
_REGISTER_TRACKERS(PF);
#endif


//
//  cal_slip_sliprate_angle.hpp
//  hybrid_fem_bie
//
//  Created by Max on 2/8/18.
//
//

#ifndef cal_slip_sliprate_angle_hpp
#define cal_slip_sliprate_angle_hpp

#include <stdio.h>
#include <Eigen/Eigen>
#include "maplocal.hpp"

using namespace Eigen;
void cal_slip_slip_rate_angle(VectorXd &u_n, VectorXd &v_n, ArrayXi &top_surf_index, ArrayXi &bot_surf_index, int Ndofn, int nx, ArrayXd &delt_u_n, ArrayXd &delt_v_n,double fault_angle);

#endif /* cal_slip_sliprate_angle_hpp */


#include "Camera.h"

#include <Eigen>
#include <vector>
#include <iostream>
#include <fstream>
#include <algorithm>
#include <math.h>
#include "Shape.h"
#include "Intersection.h"

#define HEIGHT 600
#define WIDTH 800
#define FLENGTH 1.0
#define PWIDTH 4.0
#define PHEIGHT 3.0
#define MAXRECDEPTH 4
#define INF 100000000

using namespace Eigen;
using namespace std;

float clamp(float a, float b, float c){
    if (c < a)
        return a;
    else if (c > b)
        return b;
    else
        return c;
}

Vector3f getr(Vector3f sray, Vector3f norm){
    Vector3f t2 = sray - 2*(sray.dot(norm)) * norm;
    return t2;
}

//Based on Scratchapixel version
Vector3f getfrac(Vector3f incom, Vector3f norm, float indx){
    float cosi = clamp(-1, 1, incom.dot(norm));
    float et = 1, ea = indx;
    Vector3f n = norm;
    if (cosi < 0)
        cosi = -cosi;
    else{
        swap(et, ea);
        n = -norm;
    }
    float eta = ea/et;
    float k = 1 - eta*eta * (1-cosi * cosi);
    if (k < 0){ //total refrac
        return getr(incom, -norm);
    }
    else{
        return eta * incom + (eta * cosi - sqrt(k)) * n;
    }
}

Camera::Camera(Vector3f la, Vector3f u,
               Vector3f p)
{
    lookat = la;
    up = u;
    pos = p;
    lookat.normalize();
    up.normalize();
    pos.normalize();
    //col major
    Vector3f matn = pos - lookat;
    matn.normalize();
    Vector3f matu = up.cross(matn);
    matu.normalize();
    Vector3f matv = matn.cross(matu);
    this->tra << matu(0), matv(0), matn(0), 0.0f,
                 matu(1), matv(1), matn(1), 0.0f,
                 matu(2), matv(2), matn(2), 0.0f,
                -(pos.dot(matu)), -(pos.dot(matv)),
                -(pos.dot(matn)), 1.0;

}

Vector3f getrtarg(Vector3f illum, float aol, float ldmax){
    Vector3f cs = aol * illum;
    float rr, gr, br;
    rr = cs[0] / (1 + cs[0]);
    gr = cs[1] / (1 + cs[1]);
    br = cs[2] / (1 + cs[2]);

    float rt,gt,bt;
    rt = rr * ldmax;
    gt = gr * ldmax;
    bt = br * ldmax;

    Vector3f targ;
    targ << rt, gt, bt;
    return targ;

}

float getlogavg(vector<float> illlist){
    float lsum = 0;
    float delta = 0.00001;
    for (float f : illlist){
        lsum += log(delta + f);
    }
    return exp(lsum / ((float)HEIGHT * WIDTH));

}

//go from RGB to Illuminance
//CRT formula
float toill(Vector3f rgb){
    return abs(.27*rgb[0] + .67*rgb[1]+.06*rgb[2]);
}

std::vector<Vector3f> torgb(vector<Vector3f> lum, float ldmax, char whichTR){
        vector<float> illum;
        for (Vector3f vf : lum){
            illum.push_back(toill(vf));
        }
        float lwa = getlogavg(illum);
    if (whichTR == 'w'){
        float powa = pow(lwa,.4);
        float ldterm = pow(ldmax/2, 0.4);
        float brackterm = (1.219 + ldterm) / (1.219 + powa);
        float sf = pow(brackterm,2.5);
        vector<Vector3f> trd;
        for (Vector3f rgb : lum){
            trd.push_back(sf * rgb);
        }
        return trd;
    }
    else{
        float aol = .18/lwa;
        vector<Vector3f> targv;
        for (Vector3f vl : lum){
            targv.push_back(getrtarg(vl,aol,ldmax));
        }
        return targv;
    }
}

void writetofile(vector<Vector3f> image, float ldmax)
{
    ofstream filewtt;
    filewtt.open("output.ppm");
    filewtt << "P3\n" << WIDTH << " ";
    filewtt << HEIGHT << "\n";
    filewtt << 255 << "\n";
    for (Vector3f pix : image){
        int r = abs((int) (pix[0] / ldmax) * 255);
        int g = abs((int) (pix[1] / ldmax) * 255);
        int b = abs((int) (pix[2] / ldmax) * 255);
        filewtt << r << " " << g << " ";
        filewtt << b << " \n";
    }
    filewtt.close();

}

Camera::~Camera()
{
    //dtor
}

Vector3f Camera::getcolor(Ray * r, int recdepth){
    vector<Shape *> clones = wld->getshapes();
    vector<Light *> lits = wld->getlits();
    Vector3f colr;
    bool foundcol = false;
    float newt;
    float champt = INF;
    Shape * coll;
    for (Shape * s : clones){
        if (s->checkcollision(r,newt)){
            foundcol = true;
            if (newt < champt){
                champt = newt;
                coll = s;
            }
        }
        }
        if (!foundcol){
            Vector3f dflt;
            dflt << 0.0, 30, 70;
            colr = dflt;
        }
        else
        {
            //colr = coll->getcolor();
            Vector3f colpt = r->getpos() + r->getdir().normalized() * champt;
            Vector3f SS =  lits.at(0)->getpos() - colpt;
            SS.normalize();
            Ray * shadow = new Ray(colpt,SS);
            bool seelight = true;
            float dummyt; //if dist = 0, hitting self
            for (Shape * s : clones){
                if (s != coll){
                    if (s->checkcollision(shadow, dummyt)){
                        if (dummyt != 0){
                            seelight = false;
                            //ambi
                            Vector3f blk;
                            blk << 0.1,0.1,0.1;
                            colr = blk;
                        }
                    }
                }
            }
            if (seelight){
                Vector3f VV = r->getdir();
                VV.normalize();
                Vector3f NN = coll->getnorm(colpt);
                NN.normalize();
                Vector3f RR = getr(VV,NN);
                Intersection * is = new Intersection();
                is->pt = colpt;
                is->incom = SS;
                is->norm = NN;
                is->reflex = RR;
                is->litlist = lits;
                colr = coll->getcolor(is);
                if (recdepth < MAXRECDEPTH){
                    if (coll->kr > 0){
                        Vector3f refr = getr(VV,NN);
                        refr.normalize();
                        Ray * nr = new Ray(colpt, refr);
                        colr += coll->kr * this->getcolor(nr, recdepth + 1);
                    }
                    if (coll->kt > 0){
                        Vector3f frac = getfrac(VV, NN, 1.01);
                        frac.normalize();
                        Ray * fr = new Ray(colpt, frac);
                        colr += coll->kt * this->getcolor(fr, recdepth + 1);
                    }
                }
            }
        }
        return colr;
}

void Camera::captureworld(World * wld){

    //Yo dawg...
    vector<Vector3f>image;
    this->wld = wld;
    float pixh = PHEIGHT / HEIGHT;
    float pixw = PWIDTH / WIDTH;
    float tx = -PWIDTH/2;
    float ty = PHEIGHT/2;

    Vector3f p;
    p << 0.0, 0.0, 0.0;

    //Generate rays and check
    for (int i = 0; i < HEIGHT; i++){
        float ypx = ty - ((.5 + float(i)) * pixh);
        for (int c = 0; c < WIDTH; c++){
                float xpx =  tx + ((.5 + float(c)) * pixw);
                Vector3f raypos;
                raypos << xpx, ypx, FLENGTH;
                raypos.normalize();
                Ray * pxr = new Ray(p,raypos);
                Vector3f colr = this->getcolor(pxr,1);
                image.push_back(colr);

        }
    }
    float ldmax = 1;
    vector<Vector3f> srgb = torgb(image, ldmax, 'w');
    writetofile(srgb,ldmax);
}


#pragma once

#include "Camera.hh"
#include "../util.hh"
#include <Eigen/Geometry>

namespace kt84 {

struct CameraUpright : public Camera {
    double r, theta, phi;

    CameraUpright()
        : r(1)
        , theta(0)
        , phi(0)
    {}

    void init(const Eigen::Vector3d& eye, const Eigen::Vector3d& center_, const Eigen::Vector3d& up) {
        center = center_;
        Eigen::Vector3d v = eye - center;
        r = v.norm();
        double r_xz = std::sqrt(v.x() * v.x() + v.z() * v.z());

        if (r_xz == 0) {
            bool is_y_positive = v.y() > 0;
            theta = std::atan2(up.x(), up.z()) + (is_y_positive ? util::pi() : 0);
            phi   = (is_y_positive ? 0.5 : -0.5) * util::pi();
        } else {
            theta = std::atan2(v.x(), v.z());
            phi   = std::atan(v.y() / r_xz);
        }

        if (up.y() < 0) {
            theta += util::pi();
            phi    = util::pi() - phi;
        }
    }

    Eigen::Vector3d get_up() const {
        return Eigen::Vector3d(
            -sin(theta) * sin(phi),
            cos(phi),
            -cos(theta) * sin(phi));
    }

    Eigen::Vector3d get_eye() const {
        return center + r * Eigen::Vector3d(
            sin(theta) * cos(phi),
            sin(phi),
            cos(theta) * cos(phi));
    }

    void mouse_move(int x, int y) {
        if (auto_flip_y) y = height - y;
        const int viewport_size = (width + height) / 2;

        if (drag_mode == DragMode::NONE)
            return;

        Eigen::Vector2i pos(x, y);
        switch (drag_mode) {
        case DragMode::ROTATE:
            {
                theta -= (2 * util::pi() * (pos.x() - prev_pos.x())) / viewport_size;
                phi   += (2 * util::pi() * (pos.y() - prev_pos.y())) / viewport_size;
            }
            break;
        case DragMode::PAN:
            {
                Eigen::Vector3d right(cos(theta), 0, -sin(theta));
                center -= (r * (pos.x() - prev_pos.x()) / viewport_size) * right;
                center += (r * (pos.y() - prev_pos.y()) / viewport_size) * get_up();
            }
            break;
        case DragMode::ZOOM:
            {
                r *= 1 - (pos.x() - prev_pos.x() + pos.y() - prev_pos.y()) / static_cast<double>(viewport_size);
            }
            break;
        }
        prev_pos = pos;
    }

    void update_center(const Eigen::Vector3d& center_new) {
        // find the point closest to center_new between center and eye. update center and r accordingly.
        // center + t * center_to_eye = center_new
        auto d = center_to_eye();
        double t = d.dot(center_new - center) / d.squaredNorm();
        center += t * d;
        r *= 1 - t;
    }
    void snap_to_canonical() {
        auto snap_to_pi_2 = [] (double& radian) {
            while (radian < 0)
                radian += 2 * util::pi();

            double t = radian / (0.5 * util::pi());
            int n = static_cast<int>(t);
            if (t - n > 0.5)
                ++n;

            radian = n * 0.5 * util::pi();
        };

        snap_to_pi_2(theta);
        snap_to_pi_2(phi  );
    }
};

}


#ifndef STAN_MATH_PRIM_SCAL_PROB_STUDENT_T_LCCDF_HPP
#define STAN_MATH_PRIM_SCAL_PROB_STUDENT_T_LCCDF_HPP

#include <stan/math/prim/scal/meta/is_constant_struct.hpp>
#include <stan/math/prim/scal/meta/partials_return_type.hpp>
#include <stan/math/prim/scal/meta/operands_and_partials.hpp>
#include <stan/math/prim/scal/err/check_consistent_sizes.hpp>
#include <stan/math/prim/scal/err/check_finite.hpp>
#include <stan/math/prim/scal/err/check_not_nan.hpp>
#include <stan/math/prim/scal/err/check_positive_finite.hpp>
#include <stan/math/prim/scal/fun/size_zero.hpp>
#include <stan/math/prim/scal/fun/constants.hpp>
#include <stan/math/prim/scal/fun/square.hpp>
#include <stan/math/prim/scal/fun/value_of.hpp>
#include <stan/math/prim/scal/fun/lbeta.hpp>
#include <stan/math/prim/scal/fun/lgamma.hpp>
#include <stan/math/prim/scal/fun/digamma.hpp>
#include <stan/math/prim/scal/meta/length.hpp>
#include <stan/math/prim/scal/fun/grad_reg_inc_beta.hpp>
#include <stan/math/prim/scal/fun/inc_beta.hpp>
#include <stan/math/prim/scal/meta/include_summand.hpp>
#include <stan/math/prim/scal/meta/scalar_seq_view.hpp>
#include <stan/math/prim/scal/meta/VectorBuilder.hpp>
#include <boost/random/student_t_distribution.hpp>
#include <boost/random/variate_generator.hpp>
#include <limits>

namespace stan {
namespace math {

template <typename T_y, typename T_dof, typename T_loc, typename T_scale>
typename return_type<T_y, T_dof, T_loc, T_scale>::type student_t_lccdf(
    const T_y& y, const T_dof& nu, const T_loc& mu, const T_scale& sigma) {
  typedef typename stan::partials_return_type<T_y, T_dof, T_loc, T_scale>::type
      T_partials_return;

  if (size_zero(y, nu, mu, sigma))
    return 0.0;

  static const char* function = "student_t_lccdf";

  using std::exp;

  T_partials_return P(0.0);

  check_not_nan(function, "Random variable", y);
  check_positive_finite(function, "Degrees of freedom parameter", nu);
  check_finite(function, "Location parameter", mu);
  check_positive_finite(function, "Scale parameter", sigma);

  scalar_seq_view<T_y> y_vec(y);
  scalar_seq_view<T_dof> nu_vec(nu);
  scalar_seq_view<T_loc> mu_vec(mu);
  scalar_seq_view<T_scale> sigma_vec(sigma);
  size_t N = max_size(y, nu, mu, sigma);

  operands_and_partials<T_y, T_dof, T_loc, T_scale> ops_partials(y, nu, mu,
                                                                 sigma);

  // Explicit return for extreme values
  // The gradients are technically ill-defined, but treated as zero
  for (size_t i = 0; i < stan::length(y); i++) {
    if (value_of(y_vec[i]) == -std::numeric_limits<double>::infinity())
      return ops_partials.build(0.0);
  }

  using std::exp;
  using std::log;
  using std::pow;

  T_partials_return digammaHalf = 0;

  VectorBuilder<!is_constant_struct<T_dof>::value, T_partials_return, T_dof>
      digamma_vec(stan::length(nu));
  VectorBuilder<!is_constant_struct<T_dof>::value, T_partials_return, T_dof>
      digammaNu_vec(stan::length(nu));
  VectorBuilder<!is_constant_struct<T_dof>::value, T_partials_return, T_dof>
      digammaNuPlusHalf_vec(stan::length(nu));

  if (!is_constant_struct<T_dof>::value) {
    digammaHalf = digamma(0.5);

    for (size_t i = 0; i < stan::length(nu); i++) {
      const T_partials_return nu_dbl = value_of(nu_vec[i]);

      digammaNu_vec[i] = digamma(0.5 * nu_dbl);
      digammaNuPlusHalf_vec[i] = digamma(0.5 + 0.5 * nu_dbl);
    }
  }

  for (size_t n = 0; n < N; n++) {
    // Explicit results for extreme values
    // The gradients are technically ill-defined, but treated as zero
    if (value_of(y_vec[n]) == std::numeric_limits<double>::infinity()) {
      return ops_partials.build(negative_infinity());
    }

    const T_partials_return sigma_inv = 1.0 / value_of(sigma_vec[n]);
    const T_partials_return t
        = (value_of(y_vec[n]) - value_of(mu_vec[n])) * sigma_inv;
    const T_partials_return nu_dbl = value_of(nu_vec[n]);
    const T_partials_return q = nu_dbl / (t * t);
    const T_partials_return r = 1.0 / (1.0 + q);
    const T_partials_return J = 2 * r * r * q / t;
    const T_partials_return betaNuHalf = exp(lbeta(0.5, 0.5 * nu_dbl));
    T_partials_return zJacobian = t > 0 ? -0.5 : 0.5;

    if (q < 2) {
      T_partials_return z
          = inc_beta(0.5 * nu_dbl, (T_partials_return)0.5, 1.0 - r);
      const T_partials_return Pn = t > 0 ? 0.5 * z : 1.0 - 0.5 * z;
      const T_partials_return d_ibeta
          = pow(r, -0.5) * pow(1.0 - r, 0.5 * nu_dbl - 1) / betaNuHalf;

      P += log(Pn);

      if (!is_constant_struct<T_y>::value)
        ops_partials.edge1_.partials_[n]
            += zJacobian * d_ibeta * J * sigma_inv / Pn;

      if (!is_constant_struct<T_dof>::value) {
        T_partials_return g1 = 0;
        T_partials_return g2 = 0;

        grad_reg_inc_beta(g1, g2, 0.5 * nu_dbl, (T_partials_return)0.5, 1.0 - r,
                          digammaNu_vec[n], digammaHalf,
                          digammaNuPlusHalf_vec[n], betaNuHalf);

        ops_partials.edge2_.partials_[n]
            -= zJacobian * (d_ibeta * (r / t) * (r / t) + 0.5 * g1) / Pn;
      }

      if (!is_constant_struct<T_loc>::value)
        ops_partials.edge3_.partials_[n]
            -= zJacobian * d_ibeta * J * sigma_inv / Pn;
      if (!is_constant_struct<T_scale>::value)
        ops_partials.edge4_.partials_[n]
            -= zJacobian * d_ibeta * J * sigma_inv * t / Pn;

    } else {
      T_partials_return z
          = 1.0 - inc_beta((T_partials_return)0.5, 0.5 * nu_dbl, r);
      zJacobian *= -1;

      const T_partials_return Pn = t > 0 ? 0.5 * z : 1.0 - 0.5 * z;

      T_partials_return d_ibeta
          = pow(1.0 - r, 0.5 * nu_dbl - 1) * pow(r, -0.5) / betaNuHalf;

      P += log(Pn);

      if (!is_constant_struct<T_y>::value)
        ops_partials.edge1_.partials_[n]
            -= zJacobian * d_ibeta * J * sigma_inv / Pn;

      if (!is_constant_struct<T_dof>::value) {
        T_partials_return g1 = 0;
        T_partials_return g2 = 0;

        grad_reg_inc_beta(g1, g2, (T_partials_return)0.5, 0.5 * nu_dbl, r,
                          digammaHalf, digammaNu_vec[n],
                          digammaNuPlusHalf_vec[n], betaNuHalf);

        ops_partials.edge2_.partials_[n]
            -= zJacobian * (-d_ibeta * (r / t) * (r / t) + 0.5 * g2) / Pn;
      }

      if (!is_constant_struct<T_loc>::value)
        ops_partials.edge3_.partials_[n]
            += zJacobian * d_ibeta * J * sigma_inv / Pn;
      if (!is_constant_struct<T_scale>::value)
        ops_partials.edge4_.partials_[n]
            += zJacobian * d_ibeta * J * sigma_inv * t / Pn;
    }
  }
  return ops_partials.build(P);
}

}  // namespace math
}  // namespace stan
#endif


/*-----------------------------------------------------------------------------+
Interval Container Library
Author: Joachim Faulhaber
Copyright (c) 2007-2009: Joachim Faulhaber
Copyright (c) 1999-2006: Cortex Software GmbH, Kantstrasse 57, Berlin
+------------------------------------------------------------------------------+
   Distributed under the Boost Software License, Version 1.0.
      (See accompanying file LICENCE.txt or copy at
           http://www.boost.org/LICENSE_1_0.txt)
+-----------------------------------------------------------------------------*/
/** Example std_copy.cpp \file std_copy.cpp
    \brief Fill interval containers using std::copy.

    Example std_copy shows how algorithm std::copy can be used to
    fill interval containers from other std::containers and how copying
    to interval containers differs from other uses of std::copy.

    \include std_copy_/std_copy.cpp
*/
//[example_std_copy
#include <iostream>
#include <vector>
#include <algorithm>
#include <boost/icl/interval_map.hpp>

using namespace std;
using namespace boost;
using namespace boost::icl;

// 'make_segments' returns a vector of interval value pairs, which
// are not sorted. The values are taken from the minimal example
// in section 'interval combining styles'.
vector<pair<discrete_interval<int>, int> > make_segments()
{
    vector<pair<discrete_interval<int>, int> > segment_vec;
    segment_vec.push_back(make_pair(discrete_interval<int>::right_open(2,4), 1));
    segment_vec.push_back(make_pair(discrete_interval<int>::right_open(4,5), 1));
    segment_vec.push_back(make_pair(discrete_interval<int>::right_open(1,3), 1));
    return segment_vec;
}

// 'show_segments' displays the source segements.
void show_segments(const vector<pair<discrete_interval<int>, int> >& segments)
{
    vector<pair<discrete_interval<int>, int> >::const_iterator iter = segments.begin();
    while(iter != segments.end())
    {
        cout << "(" << iter->first << "," << iter->second << ")";
        ++iter;
    }
}

void std_copy()
{
    // So we have some segments stored in an std container.
    vector<pair<discrete_interval<int>, int> > segments = make_segments();
    // Display the input
    cout << "input sequence: "; show_segments(segments); cout << "\n\n";

    // We are going to 'std::copy' those segments into an interval_map:
    interval_map<int,int> segmap;

    // Use an 'icl::inserter' from <boost/icl/iterator.hpp> to call
    // insertion on the interval container.
    std::copy(segments.begin(), segments.end(),
              icl::inserter(segmap, segmap.end()));
    cout << "icl::inserting: " << segmap << endl;
    segmap.clear();

    // When we are feeding data into interval_maps, most of the time we are
    // intending to compute an aggregation result. So we are not interested
    // the std::insert semantincs but the aggregating icl::addition semantics.
    // To achieve this there is an icl::add_iterator and an icl::adder function
    // provided in <boost/icl/iterator.hpp>.
    std::copy(segments.begin(), segments.end(),
              icl::adder(segmap, segmap.end())); //Aggregating associated values
    cout << "icl::adding   : " << segmap << endl;

    // In this last case, the semantics of 'std::copy' transforms to the
    // generalized addition operation, that is implemented by operator
    // += or + on itl maps and sets.
}

int main()
{
    cout << ">>    Interval Container Library: Example std_copy.cpp    <<\n";
    cout << "-----------------------------------------------------------\n";
    cout << "Using std::copy to fill an interval_map:\n\n";

    std_copy();
    return 0;
}

// Program output:
/*---------------------------------------------------------
>>    Interval Container Library: Example std_copy.cpp    <<
-----------------------------------------------------------
Using std::copy to fill an interval_map:

input sequence: ([2,4),1)([4,5),1)([1,3),1)

icl::inserting: {([1,5)->1)}
icl::adding   : {([1,2)->1)([2,3)->2)([3,5)->1)}
---------------------------------------------------------*/
//]


/* ----------------------------------------------------------------------------

 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
 * Atlanta, Georgia 30332-0415
 * All Rights Reserved
 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)

 * See LICENSE for the license information

 * -------------------------------------------------------------------------- */

/**
 * @file    CreateSFMExampleData.cpp
 * @brief   Create some example data that for inclusion in the data folder
 * @author  Frank Dellaert
 */

#include <gtsam/slam/dataset.h>
#include <gtsam/geometry/CalibratedCamera.h>

#include <boost/assign/std/vector.hpp>

using namespace boost::assign;
using namespace std;
using namespace gtsam;

/* ************************************************************************* */

void createExampleBALFile(const string& filename, const vector<Point3>& P,
    const Pose3& pose1, const Pose3& pose2, const Cal3Bundler& K =
        Cal3Bundler()) {

  // Class that will gather all data
  SfM_data data;

  // Create two cameras
  Rot3 aRb = Rot3::Yaw(M_PI_2);
  Point3 aTb(0.1, 0, 0);
  Pose3 identity, aPb(aRb, aTb);
  data.cameras.push_back(SfM_Camera(pose1, K));
  data.cameras.push_back(SfM_Camera(pose2, K));

  for(const Point3& p: P) {

    // Create the track
    SfM_Track track;
    track.p = p;
    track.r = 1;
    track.g = 1;
    track.b = 1;

    // Project points in both cameras
    for (size_t i = 0; i < 2; i++)
    track.measurements.push_back(make_pair(i, data.cameras[i].project(p)));

    // Add track to data
    data.tracks.push_back(track);
  }

  writeBAL(filename, data);
}

/* ************************************************************************* */

void create5PointExample1() {

  // Create two cameras poses
  Rot3 aRb = Rot3::Yaw(M_PI_2);
  Point3 aTb(0.1, 0, 0);
  Pose3 pose1, pose2(aRb, aTb);

  // Create test data, we need at least 5 points
  vector<Point3> P;
  P += Point3(0, 0, 1), Point3(-0.1, 0, 1), Point3(0.1, 0, 1), //
  Point3(0, 0.5, 0.5), Point3(0, -0.5, 0.5);

  // Assumes example is run in ${GTSAM_TOP}/build/examples
  const string filename = "../../examples/data/5pointExample1.txt";
  createExampleBALFile(filename, P, pose1, pose2);
}

/* ************************************************************************* */

void create5PointExample2() {

  // Create two cameras poses
  Rot3 aRb = Rot3::Yaw(M_PI_2);
  Point3 aTb(10, 0, 0);
  Pose3 pose1, pose2(aRb, aTb);

  // Create test data, we need at least 5 points
  vector<Point3> P;
  P += Point3(0, 0, 100), Point3(-10, 0, 100), Point3(10, 0, 100), //
  Point3(0, 50, 50), Point3(0, -50, 50), Point3(-20, 0, 80), Point3(20, -50, 80);

  // Assumes example is run in ${GTSAM_TOP}/build/examples
  const string filename = "../../examples/data/5pointExample2.txt";
  Cal3Bundler K(500, 0, 0);
  createExampleBALFile(filename, P, pose1, pose2,K);
}

/* ************************************************************************* */

int main(int argc, char* argv[]) {
  create5PointExample1();
  create5PointExample2();
  return 0;
}

/* ************************************************************************* */


//
// Created by jachu on 27.02.18.
//

#ifndef PLANELOC_PLANEESTIMATOR_HPP
#define PLANELOC_PLANEESTIMATOR_HPP

#include <boost/serialization/access.hpp>

#include <Eigen/Dense>

#include "Types.hpp"

class PlaneEstimator {
public:
    PlaneEstimator();

    PlaneEstimator(const Eigen::MatrixXd &pts);

    PlaneEstimator(const Eigen::Vector3d &icentroid,
                   const Eigen::Matrix3d &icovar,
                   int inpts);

    void init(const Eigen::MatrixXd &pts);

    void init(const Eigen::Vector3d &icentroid,
              const Eigen::Matrix3d &icovar,
              int inpts);

    void update(const Eigen::Vector3d &ucentroid,
                const Eigen::Matrix3d &ucovar,
                int unpts);

//    void transform(const Eigen::Matrix4d T);

    double distance(const PlaneEstimator &other) const;

    static void compCentroidAndCovar(const Eigen::MatrixXd &pts,
                                    Eigen::Vector3d &centroid,
                                    Eigen::Matrix3d &covar);

    static void compPlaneParams(const Eigen::Vector3d centroid,
                                const Eigen::Matrix3d &covar,
                                Eigen::Matrix3d &evecs,
                                Eigen::Vector3d &evals,
                                Eigen::Vector4d &planeEq);

    void transform(const Vector7d &transform);

    const Eigen::Vector3d &getCentroid() const {
        return centroid;
    }

    const Eigen::Matrix3d &getCovar() const {
        return covar;
    }

    const Eigen::Matrix3d &getEvecs() const {
        return evecs;
    }

    const Eigen::Vector3d &getEvals() const {
        return evals;
    }

    const Eigen::Vector4d &getPlaneEq() const {
        return planeEq;
    }

    int getNpts() const {
        return npts;
    }

    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
private:

    static void updateCentroidAndCovar(const Eigen::Vector3d &centroid1,
                                      const Eigen::Matrix3d &covar1,
                                      const int &npts1,
                                      const Eigen::Vector3d &centroid2,
                                      const Eigen::Matrix3d &covar2,
                                      const int &npts2,
                                      Eigen::Vector3d &ocentroid,
                                      Eigen::Matrix3d &ocovar,
                                      int &onpts);

    Eigen::Vector3d centroid;

    Eigen::Matrix3d covar;

    Eigen::Matrix3d evecs;

    Eigen::Vector3d evals;

    Eigen::Vector4d planeEq;

    int npts;

    friend class boost::serialization::access;

    template<class Archive>
    void serialize(Archive & ar, const unsigned int version)
    {
        ar & centroid;
        ar & covar;
        ar & evecs;
        ar & evals;
        ar & planeEq;
        ar & npts;
    }
};

#endif //PLANELOC_PLANEESTIMATOR_HPP


#include <iostream>
#include <boost/accumulators/accumulators.hpp>
#include <boost/accumulators/statistics/count.hpp>
#include <boost/accumulators/statistics/sum.hpp>

using namespace boost::accumulators;
int main()
{
	//accumulator_set<int, features<tag::weight_sum> , int> acc;
	accumulator_set<double, features<tag::count, droppable<tag::sum> > >acc;

	acc(3.0);
	acc(2.0);
	acc.drop<tag::sum>();
	acc(1.0);

	//sum will drop acc, now acc is 3.0+2.0
	std::cout<< count(acc) <<' '<< sum(acc) << std::endl;
}


#include "edlib/Basis/Basis1D.hpp"
#include "edlib/Basis/Basis1DZ2.hpp"
#include "edlib/Basis/ToOriginalBasis.hpp"
#include "edlib/EDP/ConstructSparseMat.hpp"
#include "edlib/Hamiltonians/TIXXZ.hpp"

#include "XXZ.hpp"
#include "utils.hpp"

#include <Eigen/Dense>
#include <catch2/catch.hpp>

#include <iostream>
#include <unordered_set>

using namespace Eigen;
using namespace edlib;

constexpr uint32_t max_n = 12;

template<typename Basis> VectorXd translate(Basis&& basis, const VectorXd& r)
{
    VectorXd res(r.size());
    for(uint32_t i = 0; i < r.size(); i++)
    {
        res(basis.rotl(i, 1)) = r(i);
    }
    return res;
}

template<class Basis> uint64_t countDim1D(Basis&& basis, bool useU1)
{
    const auto N = basis.getN();
    const auto k = basis.getK();
    const int expk = (k == 0) ? 1 : -1;

    std::unordered_set<VectorXi, hash_vector> basisVecs;

    for(uint32_t n = 0; n < (1U << N); ++n)
    {
        if(useU1 && (__builtin_popcountll(n) != N / 2))
        {
            continue;
        }
        Eigen::VectorXi v = Eigen::VectorXi::Zero(1UL << N);
        for(uint32_t k = 0; k < N; k++)
        {
            v(basis.rotl(n, k)) += powi(expk, k);
        }
        if(v.cwiseAbs().sum() != 0)
        {
            make_first_positive(v);
            basisVecs.emplace(std::move(v));
        }
    }

    return basisVecs.size();
}

template<class Basis> uint64_t countDim1DZ2(Basis&& basis, bool useU1)
{
    const auto N = basis.getN();
    const auto k = basis.getK();
    const int p = basis.getP();
    const int expk = (k == 0) ? 1 : -1;

    std::unordered_set<VectorXi, hash_vector> basisVecs;

    for(uint32_t n = 0; n < (1U << N); ++n)
    {
        if(useU1 && (__builtin_popcountll(n) != N / 2))
        {
            continue;
        }
        Eigen::VectorXi v = Eigen::VectorXi::Zero(1UL << N);
        for(uint32_t k = 0; k < N; k++)
        {
            v(basis.rotl(n, k)) += powi(expk, k);
            v(basis.rotl(basis.flip(n), k)) += powi(expk, k) * p;
        }
        if(v.cwiseAbs().sum() != 0)
        {
            make_first_positive(v);
            basisVecs.emplace(std::move(v));
        }
    }

    return basisVecs.size();
}

template<class Basis> void CheckBasis1DParity(Basis&& basis, const MatrixXd& r)
{
    const auto k = basis.getK();
    const int p = basis.getP();

    const int expk = (k == 0) ? 1 : -1;

    for(int i = 0; i < r.cols(); i++)
    {
        VectorXd c = r.col(i);
        REQUIRE((c - expk * translate(basis, c)).norm() < 1e-10);
        REQUIRE((c - p * flip(basis, c)).norm() < 1e-10);
    }
}

template<class Basis> void CheckBasis1D(Basis&& basis, const MatrixXd& r)
{
    const auto k = basis.getK();
    const int expk = (k == 0) ? 1 : -1;

    for(int i = 0; i < r.cols(); i++)
    {
        auto c = r.col(i);
        REQUIRE((c - expk * translate(basis, c)).norm() < 1e-10);
    }
}

template<class Basis> void CheckBasisXXZ(Basis&& basis, const MatrixXd& r)
{
    TIXXZ<uint32_t> tiXXZ(basis, 1.0, 0.9);
    const size_t dim = basis.getDim();
    auto hamTI = edp::constructSparseMat<double>(
        dim, [&tiXXZ](uint32_t col) { return tiXXZ.getCol(col); });

    const auto N = basis.getN();

    XXZ xxz(N, 1.0, 0.9);
    auto hamFull = edp::constructSparseMat<double>(1U << N, xxz);
    Eigen::MatrixXd mat = r.transpose() * hamFull * r;

    REQUIRE((Eigen::MatrixXd(hamTI) - mat).norm() < 1e-10);
}

TEST_CASE("Test Basis1D", "[basis1d]")
{
    SECTION("Representatives are correct")
    {
        uint32_t K = 0;
        for(uint32_t N = 4; N <= 20; N += 4)
        {
            Basis1D<uint32_t> basis(N, K, true);

            for(uint32_t i = 0; i < basis.getDim(); ++i)
            {
                uint32_t rep = basis.getNthRep(i);
                const auto [rep2, rot] = basis.findMinRots(rep);
                REQUIRE(rep2 == rep);
            }
        }
    }
    SECTION("Without U1")
    {
        for(uint32_t N = 4; N <= max_n; N += 2)
        {
            for(uint32_t K : {0U, N / 2})
            {
                DYNAMIC_SECTION("Testing N: " << N << ", K: " << K)
                {
                    Basis1D<uint32_t> basis(N, K, false);

                    REQUIRE(basis.getDim() == countDim1D(basis, false));

                    MatrixXd r = basisMatrix(basis);
                    TestBasisMatrix(r);
                    CheckBasis1D(basis, r);
                    CheckBasisXXZ(basis, r);
                }
            }
        }
    }
    SECTION("With U1")
    {
        for(uint32_t N = 4; N <= max_n; N += 2)
        {
            for(uint32_t K : {0U, N / 2})
            {
                DYNAMIC_SECTION("Testing N: " << N << ", K: " << K)
                {
                    Basis1D<uint32_t> basis(N, K, true);

                    REQUIRE(basis.getDim() == countDim1D(basis, true));

                    MatrixXd r = basisMatrix(basis);
                    TestBasisMatrix(r);
                    CheckBasis1D(basis, r);
                    CheckBasisXXZ(basis, r);
                }
            }
        }
    }
}

TEST_CASE("Test Basis1DZ2", "[basis1dz2]")
{
    const std::array<int, 2> ps{-1, 1};

    SECTION("Without U1")
    {
        for(uint32_t N = 4; N <= max_n; N += 2)
        {
            for(uint32_t K : {0U, N / 2})
            {
                for(int p : ps)
                {
                    DYNAMIC_SECTION("Testing N: " << N << ", K: " << K << ", p: " << p)
                    {
                        Basis1DZ2<uint32_t> basis(N, K, p, false);

                        REQUIRE(basis.getDim() == countDim1DZ2(basis, false));

                        MatrixXd r = basisMatrix(basis);
                        TestBasisMatrix(r);
                        CheckBasis1DParity(basis, r);
                        CheckBasisXXZ(basis, r);
                    }
                }
            }
        }
    }
    SECTION("With U1")
    {
        for(uint32_t N = 6; N <= max_n; N += 2)
        {
            for(uint32_t K : {0U, N / 2})
            {
                for(int p : ps)
                {
                    DYNAMIC_SECTION("Testing N: " << N << ", K: " << K << ", p: " << p)
                    {
                        Basis1DZ2<uint32_t> basis(N, K, p, true);

                        REQUIRE(basis.getDim() == countDim1DZ2(basis, true));

                        MatrixXd r = basisMatrix(basis);
                        TestBasisMatrix(r);
                        CheckBasis1D(basis, r);
                        CheckBasisXXZ(basis, r);
                    }
                }
            }
        }
    }
}


#include <boost\filesystem.hpp>
#include <boost\lexical_cast.hpp>
#include "genetic_programming.h"

int Print(fstream &output_file, string &program, int node)
{
	int node1 = 0, node2;

	if (program[node] < FSET_START) {
		if (program[node] <= num_vars)
			output_file << "x" << (int)program[node];
		else
			output_file << args[program[node] - 1];
		return node + 1;
	}
	else {
		switch (program[node]) {
			//case EXP:
			//	output_file << "exp(";
			//	node2 = Print(output_file, program, node + 1);
			//	output_file << ")";
			//	return node2;
			//case SIN:
			//	output_file << "sin(";
			//	node2 = Print(output_file, program, node + 1);
			//	output_file << ")";
			//	return node2;
			//case COS:
			//	output_file << "cos(";
			//	node2 = Print(output_file, program, node + 1);
			//	output_file << ")";
			//	return node2;
			//case LOG:
			//	output_file << "log(abs(";
			//	node2 = Print(output_file, program, node + 1);
			//	output_file << "))";
			//	return node2;
			case ADD:
				output_file << "(";
				node1 = Print(output_file, program, node + 1);
				output_file << " + ";
				break;
			case SUB:
				output_file << "(";
				node1 = Print(output_file, program, node + 1);
				output_file << " - ";
				break;
			case MUL:
				output_file << "(";
				node1 = Print(output_file, program, node + 1);
				output_file << " * ";
				break;
			case DIV:
				output_file << "(";
				node1 = Print(output_file, program, node + 1);
				output_file << " / ";
				break;
			//case POW:
			//	output_file << "(";
			//	node1 = Print(output_file, program, node + 1);
			//	output_file << " ^ ";
			//	break;
		}
	}

	node2 = Print(output_file, program, node1);
	output_file << ")";

	return node2;
}

void PrintInfixProgram(string &program, string &output_filename)
{
	fstream output_file(output_filename, ios::out);
	int node = 0;
	Print(output_file, program, node);
	output_file.close();
}

void SaveProgram(string &program, string filename)
{
	fstream args_file(filename + "_args.txt", ios::out), prog_file(filename + ".txt", ios::out);

	int i = 0;
	for (; i < args.size(); ++i)
		args_file << args[i] << "\n";

	for (i = 0; i < program.size(); ++i)
		prog_file << (int)program[i] << "\n";

	args_file.close();
	prog_file.close();

	PrintInfixProgram(program, filename + "_infix.txt");
}

void TestProgram(string &program, string output_filename)
{
	fstream output_file(output_filename + ".csv", ios::out);

	output_file << "x,y\n";

	for (int fit_case = 0; fit_case < input_data.size(); ++fit_case) {
		for (int var = 0; var < num_vars; ++var) {
			args[var] = input_data[fit_case][var];
			output_file << args[var] << ",";
		}

		current_node = program.begin();
		output_file << Next() << "\n";
	}

	output_file.close();
}

string LoadProgram(string &program_name)
{
	fstream program_file(program_name, ios::in);

	string program, line;

	while (true) {
		getline(program_file, line);
		if (program_file.eof())
			break;
		program += stoi(line);
	}

	program_file.close();

	return program;
}

int main() {
	boost::filesystem::path fullpath(boost::filesystem::current_path());
	string temp = boost::lexical_cast<string>(fullpath);
	string curr_dir = string(temp.begin() + 1, temp.end() - 1);

	SetUp(curr_dir + "\\config.ini");

	cout << "SYMBOLIC REGRESSION w/ GENETIC PROGRAMMING\n" << endl;

	cout << "Pseudo random number generator seed\t= " << seed << endl;
	cout << "Population size\t\t\t\t= " << popsize << endl;
	cout << "Number of generations\t\t\t= " << generations << endl;
	cout << "Max depth at the initialisation\t\t= " << max_depth << endl;
	cout << "Max program length\t\t\t= " << max_length << endl;
	cout << "Tournament size\t\t\t\t= " << tournament_size << endl;
	cout << "Mutation probability per node\t\t= " << pm_per_node << endl;
	cout << "Crossover probability\t\t\t= " << pcrossover << endl;
	cout << "Data filename\t\t\t\t= " << fitness_data_filename << endl;
	cout << "Number of independent variables\t\t= " << num_vars << endl;
	cout << "Number of random constants\t\t= " << num_rand_nums << endl;
	if (num_rand_nums) {
		cout << "Min constant\t\t\t\t= " << min_rand_num << endl;
		cout << "Max constant\t\t\t\t= " << max_rand_num << endl;
	}
	if (seed_w_program)
		cout << "Seed for the initial population\t\t= " << prog_name << endl;

	printf("\nPress any key to continue...\n");
	cin.get();

	vector<Individual> population(popsize);
	Individual best;

	if (seed_w_program) {
		best.program = LoadProgram(prog_name);
		Evaluate(best);
	}

	for (int i = 0; i < popsize; ++i) {
		population[i].program = BuildTree(random(2, max_depth), random(0, 1)); //Ramped half-and-half.
		Evaluate(population[i]);

		if (population[i] < best)
			best = population[i];
	}

	int epochs = generations * popsize, break_flag = 0;

	for (int epoch = 0; epoch < epochs; ++epoch) {
		Individual &offspring = population[Tournament(population, tournament_size, IsGreater<Individual>)]
			= population[Tournament(population, tournament_size, IsLess<Individual>)];

		if (random<float>() < pcrossover)
			SubtreeXO(offspring, population[Tournament(population, tournament_size, IsLess<Individual>)]);
		else
			PtMutation(offspring.program);

		Evaluate(offspring);

		if (offspring < best)
			best = offspring;

		if (((epoch + 1) % popsize) == 0)
			printf("Gen: %d, Program size: %d, Performance: %.4f\n", (1 + epoch / popsize), best.program.size(), best.fitness);

		if (best.fitness < 0.0001)
			break;

		if (break_flag = GetAsyncKeyState(VK_ESCAPE)) {
			char c = 0;

			printf("\n");

			while (c != 'Y' && c != 'y' && c != 'N' && c != 'n') {
				printf("\rDo you want quit the program? (Y/N): ");
				cin >> c;
			}

			if (c == 'Y' || c == 'y') {
				break;
			}
			else {
				break_flag = 0;
				printf("\n");
			}
		}
	}

	if (!break_flag)
		printf("\n");

	char c = 0;
	while (c != 'Y' && c != 'y' && c != 'N' && c != 'n') {
		printf("\rSave the best program? (Y/N): ");
		cin >> c;
	}

	if (c == 'Y' || c == 'y') {
		string filename;
		printf("Save as: ");
		cin >> filename;
		SaveProgram(best.program, filename);
	}

	c = 0;
	while (c != 'Y' && c != 'y' && c != 'N' && c != 'n') {
		printf("\rTest the best program? (Y/N): ");
		cin >> c;
	}

	if (c == 'Y' || c == 'y') {
		string filename;
		printf("Output file: ");
		cin >> filename;
		TestProgram(best.program, filename);
	}

	printf("\nPress any key to quit...\n");
	cin.get();
	cin.get();

	return 0;
}


//  Copyright (c) 2006 Xiaogang Zhang
//  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)

#ifndef BOOST_MATH_BESSEL_IK_HPP
#define BOOST_MATH_BESSEL_IK_HPP

#ifdef _MSC_VER
#pragma once
#endif

#include <boost/math/special_functions/round.hpp>
#include <boost/math/special_functions/gamma.hpp>
#include <boost/math/special_functions/sin_pi.hpp>
#include <boost/math/constants/constants.hpp>
#include <boost/math/policies/error_handling.hpp>
#include <boost/math/tools/config.hpp>

// Modified Bessel functions of the first and second kind of fractional order

namespace boost { namespace math {

namespace detail {

template <class T, class Policy>
struct cyl_bessel_i_small_z
{
   typedef T result_type;

   cyl_bessel_i_small_z(T v_, T z_) : k(0), v(v_), mult(z_*z_/4)
   {
      BOOST_MATH_STD_USING
      term = 1;
   }

   T operator()()
   {
      T result = term;
      ++k;
      term *= mult / k;
      term /= k + v;
      return result;
   }
private:
   unsigned k;
   T v;
   T term;
   T mult;
};

template <class T, class Policy>
inline T bessel_i_small_z_series(T v, T x, const Policy& pol)
{
   BOOST_MATH_STD_USING
   T prefix;
   if(v < max_factorial<T>::value)
   {
      prefix = pow(x / 2, v) / boost::math::tgamma(v + 1, pol);
   }
   else
   {
      prefix = v * log(x / 2) - boost::math::lgamma(v + 1, pol);
      prefix = exp(prefix);
   }
   if(prefix == 0)
      return prefix;

   cyl_bessel_i_small_z<T, Policy> s(v, x);
   boost::uintmax_t max_iter = policies::get_max_series_iterations<Policy>();
#if BOOST_WORKAROUND(__BORLANDC__, BOOST_TESTED_AT(0x582))
   T zero = 0;
   T result = boost::math::tools::sum_series(s, boost::math::policies::get_epsilon<T, Policy>(), max_iter, zero);
#else
   T result = boost::math::tools::sum_series(s, boost::math::policies::get_epsilon<T, Policy>(), max_iter);
#endif
   policies::check_series_iterations<T>("boost::math::bessel_j_small_z_series<%1%>(%1%,%1%)", max_iter, pol);
   return prefix * result;
}

// Calculate K(v, x) and K(v+1, x) by method analogous to
// Temme, Journal of Computational Physics, vol 21, 343 (1976)
template <typename T, typename Policy>
int temme_ik(T v, T x, T* K, T* K1, const Policy& pol)
{
    T f, h, p, q, coef, sum, sum1, tolerance;
    T a, b, c, d, sigma, gamma1, gamma2;
    unsigned long k;

    BOOST_MATH_STD_USING
    using namespace boost::math::tools;
    using namespace boost::math::constants;

    // |x| <= 2, Temme series converge rapidly
    // |x| > 2, the larger the |x|, the slower the convergence
    BOOST_ASSERT(abs(x) <= 2);
    BOOST_ASSERT(abs(v) <= 0.5f);

    T gp = boost::math::tgamma1pm1(v, pol);
    T gm = boost::math::tgamma1pm1(-v, pol);

    a = log(x / 2);
    b = exp(v * a);
    sigma = -a * v;
    c = abs(v) < tools::epsilon<T>() ?
       T(1) : T(boost::math::sin_pi(v) / (v * pi<T>()));
    d = abs(sigma) < tools::epsilon<T>() ?
        T(1) : T(sinh(sigma) / sigma);
    gamma1 = abs(v) < tools::epsilon<T>() ?
        T(-euler<T>()) : T((0.5f / v) * (gp - gm) * c);
    gamma2 = (2 + gp + gm) * c / 2;

    // initial values
    p = (gp + 1) / (2 * b);
    q = (1 + gm) * b / 2;
    f = (cosh(sigma) * gamma1 + d * (-a) * gamma2) / c;
    h = p;
    coef = 1;
    sum = coef * f;
    sum1 = coef * h;

    BOOST_MATH_INSTRUMENT_VARIABLE(p);
    BOOST_MATH_INSTRUMENT_VARIABLE(q);
    BOOST_MATH_INSTRUMENT_VARIABLE(f);
    BOOST_MATH_INSTRUMENT_VARIABLE(sigma);
    BOOST_MATH_INSTRUMENT_CODE(sinh(sigma));
    BOOST_MATH_INSTRUMENT_VARIABLE(gamma1);
    BOOST_MATH_INSTRUMENT_VARIABLE(gamma2);
    BOOST_MATH_INSTRUMENT_VARIABLE(c);
    BOOST_MATH_INSTRUMENT_VARIABLE(d);
    BOOST_MATH_INSTRUMENT_VARIABLE(a);

    // series summation
    tolerance = tools::epsilon<T>();
    for (k = 1; k < policies::get_max_series_iterations<Policy>(); k++)
    {
        f = (k * f + p + q) / (k*k - v*v);
        p /= k - v;
        q /= k + v;
        h = p - k * f;
        coef *= x * x / (4 * k);
        sum += coef * f;
        sum1 += coef * h;
        if (abs(coef * f) < abs(sum) * tolerance)
        {
           break;
        }
    }
    policies::check_series_iterations<T>("boost::math::bessel_ik<%1%>(%1%,%1%) in temme_ik", k, pol);

    *K = sum;
    *K1 = 2 * sum1 / x;

    return 0;
}

// Evaluate continued fraction fv = I_(v+1) / I_v, derived from
// Abramowitz and Stegun, Handbook of Mathematical Functions, 1972, 9.1.73
template <typename T, typename Policy>
int CF1_ik(T v, T x, T* fv, const Policy& pol)
{
    T C, D, f, a, b, delta, tiny, tolerance;
    unsigned long k;

    BOOST_MATH_STD_USING

    // |x| <= |v|, CF1_ik converges rapidly
    // |x| > |v|, CF1_ik needs O(|x|) iterations to converge

    // modified Lentz's method, see
    // Lentz, Applied Optics, vol 15, 668 (1976)
    tolerance = 2 * tools::epsilon<T>();
    BOOST_MATH_INSTRUMENT_VARIABLE(tolerance);
    tiny = sqrt(tools::min_value<T>());
    BOOST_MATH_INSTRUMENT_VARIABLE(tiny);
    C = f = tiny;                           // b0 = 0, replace with tiny
    D = 0;
    for (k = 1; k < policies::get_max_series_iterations<Policy>(); k++)
    {
        a = 1;
        b = 2 * (v + k) / x;
        C = b + a / C;
        D = b + a * D;
        if (C == 0) { C = tiny; }
        if (D == 0) { D = tiny; }
        D = 1 / D;
        delta = C * D;
        f *= delta;
        BOOST_MATH_INSTRUMENT_VARIABLE(delta-1);
        if (abs(delta - 1) <= tolerance)
        {
           break;
        }
    }
    BOOST_MATH_INSTRUMENT_VARIABLE(k);
    policies::check_series_iterations<T>("boost::math::bessel_ik<%1%>(%1%,%1%) in CF1_ik", k, pol);

    *fv = f;

    return 0;
}

// Calculate K(v, x) and K(v+1, x) by evaluating continued fraction
// z1 / z0 = U(v+1.5, 2v+1, 2x) / U(v+0.5, 2v+1, 2x), see
// Thompson and Barnett, Computer Physics Communications, vol 47, 245 (1987)
template <typename T, typename Policy>
int CF2_ik(T v, T x, T* Kv, T* Kv1, const Policy& pol)
{
    BOOST_MATH_STD_USING
    using namespace boost::math::constants;

    T S, C, Q, D, f, a, b, q, delta, tolerance, current, prev;
    unsigned long k;

    // |x| >= |v|, CF2_ik converges rapidly
    // |x| -> 0, CF2_ik fails to converge

    BOOST_ASSERT(abs(x) > 1);

    // Steed's algorithm, see Thompson and Barnett,
    // Journal of Computational Physics, vol 64, 490 (1986)
    tolerance = tools::epsilon<T>();
    a = v * v - 0.25f;
    b = 2 * (x + 1);                              // b1
    D = 1 / b;                                    // D1 = 1 / b1
    f = delta = D;                                // f1 = delta1 = D1, coincidence
    prev = 0;                                     // q0
    current = 1;                                  // q1
    Q = C = -a;                                   // Q1 = C1 because q1 = 1
    S = 1 + Q * delta;                            // S1
    BOOST_MATH_INSTRUMENT_VARIABLE(tolerance);
    BOOST_MATH_INSTRUMENT_VARIABLE(a);
    BOOST_MATH_INSTRUMENT_VARIABLE(b);
    BOOST_MATH_INSTRUMENT_VARIABLE(D);
    BOOST_MATH_INSTRUMENT_VARIABLE(f);

    for (k = 2; k < policies::get_max_series_iterations<Policy>(); k++)     // starting from 2
    {
        // continued fraction f = z1 / z0
        a -= 2 * (k - 1);
        b += 2;
        D = 1 / (b + a * D);
        delta *= b * D - 1;
        f += delta;

        // series summation S = 1 + \sum_{n=1}^{\infty} C_n * z_n / z_0
        q = (prev - (b - 2) * current) / a;
        prev = current;
        current = q;                        // forward recurrence for q
        C *= -a / k;
        Q += C * q;
        S += Q * delta;
        //
        // Under some circumstances q can grow very small and C very
        // large, leading to under/overflow.  This is particularly an
        // issue for types which have many digits precision but a narrow
        // exponent range.  A typical example being a "double double" type.
        // To avoid this situation we can normalise q (and related prev/current)
        // and C.  All other variables remain unchanged in value.  A typical
        // test case occurs when x is close to 2, for example cyl_bessel_k(9.125, 2.125).
        //
        if(q < tools::epsilon<T>())
        {
           C *= q;
           prev /= q;
           current /= q;
           q = 1;
        }

        // S converges slower than f
        BOOST_MATH_INSTRUMENT_VARIABLE(Q * delta);
        BOOST_MATH_INSTRUMENT_VARIABLE(abs(S) * tolerance);
        BOOST_MATH_INSTRUMENT_VARIABLE(S);
        if (abs(Q * delta) < abs(S) * tolerance)
        {
           break;
        }
    }
    policies::check_series_iterations<T>("boost::math::bessel_ik<%1%>(%1%,%1%) in CF2_ik", k, pol);

    if(x >= tools::log_max_value<T>())
       *Kv = exp(0.5f * log(pi<T>() / (2 * x)) - x - log(S));
    else
      *Kv = sqrt(pi<T>() / (2 * x)) * exp(-x) / S;
    *Kv1 = *Kv * (0.5f + v + x + (v * v - 0.25f) * f) / x;
    BOOST_MATH_INSTRUMENT_VARIABLE(*Kv);
    BOOST_MATH_INSTRUMENT_VARIABLE(*Kv1);

    return 0;
}

enum{
   need_i = 1,
   need_k = 2
};

// Compute I(v, x) and K(v, x) simultaneously by Temme's method, see
// Temme, Journal of Computational Physics, vol 19, 324 (1975)
template <typename T, typename Policy>
int bessel_ik(T v, T x, T* I, T* K, int kind, const Policy& pol)
{
    // Kv1 = K_(v+1), fv = I_(v+1) / I_v
    // Ku1 = K_(u+1), fu = I_(u+1) / I_u
    T u, Iv, Kv, Kv1, Ku, Ku1, fv;
    T W, current, prev, next;
    bool reflect = false;
    unsigned n, k;
    int org_kind = kind;
    BOOST_MATH_INSTRUMENT_VARIABLE(v);
    BOOST_MATH_INSTRUMENT_VARIABLE(x);
    BOOST_MATH_INSTRUMENT_VARIABLE(kind);

    BOOST_MATH_STD_USING
    using namespace boost::math::tools;
    using namespace boost::math::constants;

    static const char* function = "boost::math::bessel_ik<%1%>(%1%,%1%)";

    if (v < 0)
    {
        reflect = true;
        v = -v;                             // v is non-negative from here
        kind |= need_k;
    }
    n = iround(v, pol);
    u = v - n;                              // -1/2 <= u < 1/2
    BOOST_MATH_INSTRUMENT_VARIABLE(n);
    BOOST_MATH_INSTRUMENT_VARIABLE(u);

    if (x < 0)
    {
       *I = *K = policies::raise_domain_error<T>(function,
            "Got x = %1% but real argument x must be non-negative, complex number result not supported.", x, pol);
        return 1;
    }
    if (x == 0)
    {
       Iv = (v == 0) ? static_cast<T>(1) : static_cast<T>(0);
       if(kind & need_k)
       {
         Kv = policies::raise_overflow_error<T>(function, 0, pol);
       }
       else
       {
          Kv = std::numeric_limits<T>::quiet_NaN(); // any value will do
       }

       if(reflect && (kind & need_i))
       {
           T z = (u + n % 2);
           Iv = boost::math::sin_pi(z, pol) == 0 ?
               Iv :
               policies::raise_overflow_error<T>(function, 0, pol);   // reflection formula
       }

       *I = Iv;
       *K = Kv;
       return 0;
    }

    // x is positive until reflection
    W = 1 / x;                                 // Wronskian
    if (x <= 2)                                // x in (0, 2]
    {
        temme_ik(u, x, &Ku, &Ku1, pol);             // Temme series
    }
    else                                       // x in (2, \infty)
    {
        CF2_ik(u, x, &Ku, &Ku1, pol);               // continued fraction CF2_ik
    }
    BOOST_MATH_INSTRUMENT_VARIABLE(Ku);
    BOOST_MATH_INSTRUMENT_VARIABLE(Ku1);
    prev = Ku;
    current = Ku1;
    T scale = 1;
    for (k = 1; k <= n; k++)                   // forward recurrence for K
    {
        T fact = 2 * (u + k) / x;
        if((tools::max_value<T>() - fabs(prev)) / fact < fabs(current))
        {
           prev /= current;
           scale /= current;
           current = 1;
        }
        next = fact * current + prev;
        prev = current;
        current = next;
    }
    Kv = prev;
    Kv1 = current;
    BOOST_MATH_INSTRUMENT_VARIABLE(Kv);
    BOOST_MATH_INSTRUMENT_VARIABLE(Kv1);
    if(kind & need_i)
    {
       T lim = (4 * v * v + 10) / (8 * x);
       lim *= lim;
       lim *= lim;
       lim /= 24;
       if((lim < tools::epsilon<T>() * 10) && (x > 100))
       {
          // x is huge compared to v, CF1 may be very slow
          // to converge so use asymptotic expansion for large
          // x case instead.  Note that the asymptotic expansion
          // isn't very accurate - so it's deliberately very hard
          // to get here - probably we're going to overflow:
          Iv = asymptotic_bessel_i_large_x(v, x, pol);
       }
       else if((v > 0) && (x / v < 0.25))
       {
          Iv = bessel_i_small_z_series(v, x, pol);
       }
       else
       {
          CF1_ik(v, x, &fv, pol);                         // continued fraction CF1_ik
          Iv = scale * W / (Kv * fv + Kv1);                  // Wronskian relation
       }
    }
    else
       Iv = std::numeric_limits<T>::quiet_NaN(); // any value will do

    if (reflect)
    {
        T z = (u + n % 2);
        T fact = (2 / pi<T>()) * (boost::math::sin_pi(z) * Kv);
        if(fact == 0)
           *I = Iv;
        else if(tools::max_value<T>() * scale < fact)
           *I = (org_kind & need_i) ? T(sign(fact) * sign(scale) * policies::raise_overflow_error<T>(function, 0, pol)) : T(0);
        else
         *I = Iv + fact / scale;   // reflection formula
    }
    else
    {
        *I = Iv;
    }
    if(tools::max_value<T>() * scale < Kv)
      *K = (org_kind & need_k) ? T(sign(Kv) * sign(scale) * policies::raise_overflow_error<T>(function, 0, pol)) : T(0);
    else
      *K = Kv / scale;
    BOOST_MATH_INSTRUMENT_VARIABLE(*I);
    BOOST_MATH_INSTRUMENT_VARIABLE(*K);
    return 0;
}

}}} // namespaces

#endif // BOOST_MATH_BESSEL_IK_HPP