#include <mitsuba/core/quad.h>
#include <boost/bind.hpp>

MTS_NAMESPACE_BEGIN

float legendreP(int l, float x_) {
	SAssert(l >= 0);

	if (l == 0) {
		return (float) 1.0f;
	} else if (l == 1) {
		return x_;
	} else {
		/* Evaluate the recurrence in double precision */
		double x = (double) x_;
		double Lppred = 1.0, Lpred = x, Lcur = 0.0;

		for (int k = 2; k <= l; ++k) {
			Lcur = ((2*k-1) * x * Lpred - (k - 1) * Lppred) / k;
			Lppred = Lpred; Lpred = Lcur;
		}

		return (float) Lcur;
	}
}

double legendreP(int l, double x) {
	SAssert(l >= 0);

	if (l == 0) {
		return (double) 1.0f;
	} else if (l == 1) {
		return x;
	} else {
		double Lppred = 1.0, Lpred = x, Lcur = 0.0;

		for (int k = 2; k <= l; ++k) {
			Lcur = ((2*k-1) * x * Lpred - (k - 1) * Lppred) / k;
			Lppred = Lpred; Lpred = Lcur;
		}

		return Lcur;
	}
}

std::pair<float, float> legendrePD(int l, float x_) {
	SAssert(l >= 0);

	if (l == 0) {
		return std::make_pair((float) 1.0f, (float) 0.0f);
	} else if (l == 1) {
		return std::make_pair(x_, (float) 1.0f);
	} else {
		/* Evaluate the recurrence in double precision */
		double x = (double) x_;
		double Lppred = 1.0, Lpred = x, Lcur = 0.0,
		       Dppred = 0.0, Dpred = 1.0, Dcur = 0.0;

		for (int k = 2; k <= l; ++k) {
			Lcur = ((2*k-1) * x * Lpred - (k - 1) * Lppred) / k;
			Dcur = Dppred + (2*k-1) * Lpred;
			Lppred = Lpred; Lpred = Lcur;
			Dppred = Dpred; Dpred = Dcur;
		}

		return std::make_pair((float) Lcur, (float) Dcur);
	}
}

std::pair<double, double> legendrePD(int l, double x) {
	SAssert(l >= 0);

	if (l == 0) {
		return std::make_pair(1.0, 0.0);
	} else if (l == 1) {
		return std::make_pair(x, 1.0);
	} else {
		double Lppred = 1.0, Lpred = x, Lcur = 0.0,
		       Dppred = 0.0, Dpred = 1.0, Dcur = 0.0;

		for (int k = 2; k <= l; ++k) {
			Lcur = ((2*k-1) * x * Lpred - (k - 1) * Lppred) / k;
			Dcur = Dppred + (2*k-1) * Lpred;
			Lppred = Lpred; Lpred = Lcur;
			Dppred = Dpred; Dpred = Dcur;
		}

		return std::make_pair(Lcur, Dcur);
	}
}

/// Evaluate the function legendrePD(l+1, x) - legendrePD(l-1, x)
static std::pair<double, double> legendreQ(int l, double x) {
	SAssert(l >= 1);

	if (l == 1) {
		return std::make_pair(0.5 * (3*x*x-1) - 1, 3*x);
	} else {
		/* Evaluate the recurrence in double precision */
		double Lppred = 1.0, Lpred = x, Lcur = 0.0,
		       Dppred = 0.0, Dpred = 1.0, Dcur = 0.0;

		for (int k = 2; k <= l; ++k) {
			Lcur = ((2*k-1) * x * Lpred - (k-1) * Lppred) / k;
			Dcur = Dppred + (2*k-1) * Lpred;
			Lppred = Lpred; Lpred = Lcur;
			Dppred = Dpred; Dpred = Dcur;
		}

		double Lnext = ((2*l+1) * x * Lpred - l * Lppred) / (l+1);
		double Dnext = Dppred + (2*l+1) * Lpred;

		return std::make_pair(Lnext - Lppred, Dnext - Dppred);
	}
}

double legendreP(int l, int m, double x) {
	double p_mm = 1;

	if (m > 0) {
		double somx2 = std::sqrt((1 - x) * (1 + x));
		double fact = 1;
		for (int i=1; i<=m; i++) {
			p_mm *= (-fact) * somx2;
			fact += 2;
		}
	}

	if (l == m)
		return p_mm;

	double p_mmp1 = x * (2*m + 1) * p_mm;
	if (l == m+1)
		return p_mmp1;

	double p_ll = 0;
	for (int ll=m+2; ll <= l; ++ll) {
		p_ll = ((2*ll-1)*x*p_mmp1 - (ll+m-1) * p_mm) / (ll-m);
		p_mm = p_mmp1;
		p_mmp1 = p_ll;
	}

	return p_ll;
}

float legendreP(int l, int m, float x) {
	/* Evaluate the recurrence in double precision */
	double p_mm = 1;

	if (m > 0) {
		double somx2 = std::sqrt((1 - x) * (1 + x));
		double fact = 1;
		for (int i=1; i<=m; i++) {
			p_mm *= (-fact) * somx2;
			fact += 2;
		}
	}

	if (l == m)
		return (float) p_mm;

	double p_mmp1 = x * (2*m + 1) * p_mm;
	if (l == m+1)
		return (float) p_mmp1;

	double p_ll = 0;
	for (int ll=m+2; ll <= l; ++ll) {
		p_ll = ((2*ll-1)*x*p_mmp1 - (ll+m-1) * p_mm) / (ll-m);
		p_mm = p_mmp1;
		p_mmp1 = p_ll;
	}

	return (float) p_ll;
}

void gaussLegendre(int n, Float *nodes, Float *weights) {
	if (n-- < 1)
		SLog(EError, "gaussLegendre(): n must be >= 1");

	if (n == 0) {
		nodes[0] = 0;
		weights[0] = 2;
	} else if (n == 1) {
		nodes[0] = (Float) -std::sqrt(1.0/3.0);
		nodes[1] = -nodes[0];
		weights[0] = weights[1] = 1;
	}

	int m = (n+1)/2;
	for (int i=0; i<m; ++i) {
		/* Initial guess for this root using that of a Chebyshev polynomial */

		double x = -std::cos((double) (2*i + 1) / (double) (2*n + 2) * M_PI);
		int it = 0;

		while (true) {
			if (++it > 20)
				SLog(EError, "gaussLegendre(%i): did not converge after 20 iterations!", n);

			/* Search for the interior roots of P_{n+1}(x) using Newton's method. */
			std::pair<double, double> L = legendrePD(n+1, x);
			double step = L.first / L.second;
			x -= step;

			if (std::abs(step) <= 4 * std::abs(x) * std::numeric_limits<double>::epsilon())
				break;
		}

		std::pair<double, double> L = legendrePD(n+1, x);
		weights[i] = weights[n-i] = (Float) (2.0 / ((1-x*x) * (L.second*L.second)));
		nodes[i] = (Float) x; nodes[n-i] = (Float) -x;
		SAssert(i == 0 || x > nodes[i-1]);
	}

	if ((n % 2) == 0) {
		std::pair<double, double> L = legendrePD(n+1, 0.0);
		weights[n/2] = (Float) (2.0 / (L.second*L.second));
		nodes[n/2] = 0;
	}
}

void gaussLobatto(int n, Float *nodes, Float *weights) {
	if (n-- < 2)
		SLog(EError, "gaussLobatto(): n must be >= 2");

	nodes[0] = -1;
	nodes[n] =  1;
	weights[0] = weights[n] = (Float) 2 / (Float) (n * (n+1));

	int m = (n+1)/2;
	for (int i=1; i<m; ++i) {
		/* Initial guess for this root -- see "On the Legendre-Gauss-Lobatto Points
		   and Weights" by Seymor V. Parter, Journal of Sci. Comp., Vol. 14, 4, 1999 */

		double x = -std::cos((i + 0.25) * M_PI / n - 3/(8*n*M_PI * (i + 0.25)));
		int it = 0;

		while (true) {
			if (++it > 20)
				SLog(EError, "gaussLobatto(%i): did not converge after 20 iterations!", n);

			/* Search for the interior roots of P_n'(x) using Newton's method. The same
			   roots are also shared by P_{n+1}-P_{n-1}, which is nicer to evaluate. */

			std::pair<double, double> Q = legendreQ(n, x);
			double step = Q.first / Q.second;
			x -= step;

			if (std::abs(step) <= 4 * std::abs(x) * std::numeric_limits<double>::epsilon())
				break;
		}

		double Ln = legendreP(n, x);
		weights[i] = weights[n-i] = (Float) (2.0 / ((n * (n+1)) * Ln * Ln));
		nodes[i] = (Float) x; nodes[n-i] = (Float) -x;
		SAssert(x > nodes[i-1]);
	}

	if ((n % 2) == 0) {
		double Ln = legendreP(n, 0.0);
		weights[n/2] = (Float) (2.0 / ((n * (n+1)) * Ln * Ln));
		nodes[n/2] = 0.0;
	}
}

/*!
 \brief integral of a one-dimensional function using an adaptive
 Gauss-Lobatto integral

 Copyright (C) 2008 Klaus Spanderen

 This code is based on code in QuantLib, a free-software/open-source library
 for financial quantitative analysts and developers - http://quantlib.org/

 QuantLib is free software: you can redistribute it and/or modify it
 under the terms of the QuantLib license.  You should have received a
 copy of the license along with this program; if not, please email
 <quantlib-dev@lists.sf.net>. The license is also available online at
 <http://quantlib.org/license.shtml>.

 This program is distributed in the hope that it will be useful, but WITHOUT
 ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
 FOR A PARTICULAR PURPOSE. See the license for more details.
*/

const Float GaussLobattoIntegrator::m_alpha = (Float) std::sqrt(2.0/3.0);
const Float GaussLobattoIntegrator::m_beta  = (Float) (1.0/std::sqrt(5.0));
const Float GaussLobattoIntegrator::m_x1	= (Float) 0.94288241569547971906;
const Float GaussLobattoIntegrator::m_x2	= (Float) 0.64185334234578130578;
const Float GaussLobattoIntegrator::m_x3	= (Float) 0.23638319966214988028;

GaussLobattoIntegrator::GaussLobattoIntegrator(size_t maxEvals,
	Float absError, Float relError, bool useConvergenceEstimate, bool warn)
	: m_absError(absError),
	  m_relError(relError),
	  m_maxEvals(maxEvals),
	  m_useConvergenceEstimate(useConvergenceEstimate),
      m_warn(warn) {
	if (m_absError == 0 && m_relError == 0)
		SLog(EError, "GaussLobattoIntegrator:: Absolute and relative "
			"error requirements can't both be zero!");
}

Float GaussLobattoIntegrator::integrate(
		const boost::function<Float (Float)>& f, Float a, Float b, size_t *_evals) const {
	Float factor = 1;
	size_t evals = 0;
	if (a == b) {
		return 0;
	} else if (b < a) {
		std::swap(a, b);
		factor = -1;
	}
	const Float absTolerance = calculateAbsTolerance(f, a, b, evals);
	evals += 2;
	Float result = factor * adaptiveGaussLobattoStep(f, a, b, f(a), f(b), absTolerance, evals);
	if (evals >= m_maxEvals && m_warn)
		SLog(EWarn, "GaussLobattoIntegrator: Maximum number of evaluations reached!");
	if (_evals)
		*_evals = evals;
	return result;
}

Float GaussLobattoIntegrator::calculateAbsTolerance(
		const boost::function<Float (Float)>& f, Float a, Float b, size_t &evals) const {
	const Float m = (a+b)/2;
	const Float h = (b-a)/2;
	const Float y1 = f(a);
	const Float y3 = f(m-m_alpha*h);
	const Float y5 = f(m-m_beta*h);
	const Float y7 = f(m);
	const Float y9 = f(m+m_beta*h);
	const Float y11= f(m+m_alpha*h);
	const Float y13= f(b);

	Float acc = h*((Float) 0.0158271919734801831*(y1+y13)
				 + (Float) 0.0942738402188500455*(f(m-m_x1*h)+f(m+m_x1*h))
				 + (Float) 0.1550719873365853963*(y3+y11)
				 + (Float) 0.1888215739601824544*(f(m-m_x2*h)+ f(m+m_x2*h))
				 + (Float) 0.1997734052268585268*(y5+y9)
				 + (Float) 0.2249264653333395270*(f(m-m_x3*h)+f(m+m_x3*h))
				 + (Float) 0.2426110719014077338*y7);
	evals += 13;

	Float r = 1.0;
	if (m_useConvergenceEstimate) {
		const Float integral2 = (h/6)*(y1+y13+5*(y5+y9));
		const Float integral1 = (h/1470)*
			(77*(y1+y13) + 432*(y3+y11) + 625*(y5+y9) + 672*y7);

		if (std::abs(integral2-acc) != 0.0)
			r = std::abs(integral1-acc)/std::abs(integral2-acc);
		if (r == 0.0 || r > 1.0)
			r = 1.0;
	}
	Float result = std::numeric_limits<Float>::infinity();

	if (m_relError != 0 && acc != 0)
		result = acc * std::max(m_relError,
			std::numeric_limits<Float>::epsilon())
			/ (r*std::numeric_limits<Float>::epsilon());

	if (m_absError != 0)
		result = std::min(result, m_absError
			/ (r*std::numeric_limits<Float>::epsilon()));

	return result;
}

Float GaussLobattoIntegrator::adaptiveGaussLobattoStep(
								 const boost::function<Float (Float)>& f,
								 Float a, Float b, Float fa, Float fb,
								 Float acc, size_t &evals) const {
	const Float h=(b-a)/2;
	const Float m=(a+b)/2;

	const Float mll=m-m_alpha*h;
	const Float ml =m-m_beta*h;
	const Float mr =m+m_beta*h;
	const Float mrr=m+m_alpha*h;

	const Float fmll= f(mll);
	const Float fml = f(ml);
	const Float fm  = f(m);
	const Float fmr = f(mr);
	const Float fmrr= f(mrr);

	const Float integral2=(h/6)*(fa+fb+5*(fml+fmr));
	const Float integral1=(h/1470)*(77*(fa+fb)
		+ 432*(fmll+fmrr) + 625*(fml+fmr) + 672*fm);

	evals += 5;

	if (evals >= m_maxEvals)
		return integral1;

	Float dist = acc + (integral1-integral2);
	if (dist==acc || mll<=a || b<=mrr) {
		return integral1;
	} else {
		return  adaptiveGaussLobattoStep(f,a,mll,fa,fmll,acc,evals)
			  + adaptiveGaussLobattoStep(f,mll,ml,fmll,fml,acc,evals)
			  + adaptiveGaussLobattoStep(f,ml,m,fml,fm,acc,evals)
			  + adaptiveGaussLobattoStep(f,m,mr,fm,fmr,acc,evals)
			  + adaptiveGaussLobattoStep(f,mr,mrr,fmr,fmrr,acc,evals)
			  + adaptiveGaussLobattoStep(f,mrr,b,fmrr,fb,acc,evals);
	}
}

/* Adaptive multidimensional integration of a vector of const Integrand &s.
 *
 * Copyright (c) 2005-2010 Steven G. Johnson
 *
 * Portions (see comments) based on HIntLib (also distributed under
 * the GNU GPL, v2 or later), copyright (c) 2002-2005 Rudolf Schuerer.
 *     (http://www.cosy.sbg.ac.at/~rschuer/hintlib/)
 *
 * Portions (see comments) based on GNU GSL (also distributed under
 * the GNU GPL, v2 or later), copyright (c) 1996-2000 Brian Gough.
 *     (http://www.gnu.org/software/gsl/)
 *
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 2 of the License, or
 * (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program; if not, write to the Free Software
 * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
 *
 */

/* Adaptive multidimensional integration on hypercubes (or, really,
   hyper-rectangles) using cubature rules.

   A cubature rule takes a function and a hypercube and evaluates
   the function at a small number of points, returning an estimate
   of the integral as well as an estimate of the error, and also
   a suggested dimension of the hypercube to subdivide.

   Given such a rule, the adaptive integration is simple:

   1) Evaluate the cubature rule on the hypercube(s).
      Stop if converged.

   2) Pick the hypercube with the largest estimated error,
      and divide it in two along the suggested dimension.

   3) Goto (1).

 The basic algorithm is based on the adaptive cubature described in

     A. C. Genz and A. A. Malik, "An adaptive algorithm for numeric
     integration over an N-dimensional rectangular region,"
     J. Comput. Appl. Math. 6 (4), 295-302 (1980).

 and subsequently extended to integrating a vector of const Integrand &s in

     J. Berntsen, T. O. Espelid, and A. Genz, "An adaptive algorithm
     for the approximate calculation of multiple integrals,"
     ACM Trans. Math. Soft. 17 (4), 437-451 (1991).

 Note, however, that we do not use any of code from the above authors
 (in part because their code is Fortran 77, but mostly because it is
 under the restrictive ACM copyright license).  I did make use of some
 GPL code from Rudolf Schuerer's HIntLib and from the GNU Scientific
 Library as listed in the copyright notice above, on the other hand.

 I am also grateful to Dmitry Turbiner <dturbiner@alum.mit.edu>, who
 implemented an initial prototype of the "vectorized" functionality
 for evaluating multiple points in a single call (as opposed to
 multiple functions in a single call).  (Although Dmitry implemented
 a working version, I ended up re-implementing this feature from
 scratch as part of a larger code-cleanup, and in order to have
 a single code path for the vectorized and non-vectorized APIs.  I
 subsequently implemented the algorithm by Gladwell to extract
 even more parallelism by evalutating many hypercubes at once.)
*/

/***************************************************************************/
/* Basic datatypes */

typedef NDIntegrator::VectorizedIntegrand VectorizedIntegrand;

typedef struct {
	Float val, err;
} esterr;

static Float relError(esterr ee) {
	return (ee.val == 0 ? std::numeric_limits<Float>::infinity() :
		std::abs(ee.err / ee.val));
}

static Float errMax(unsigned int fdim, const esterr *ee) {
	Float errmax = 0;
	unsigned int k;
	for (k = 0; k < fdim; ++k)
		if (ee[k].err > errmax) errmax = ee[k].err;
	return errmax;
}

typedef struct {
	unsigned int dim;
	Float *data;	/* length 2*dim = center followed by half-widths */
	Float vol;	/* cache volume = product of widths */
} hypercube;

static Float compute_vol(const hypercube *h) {
	unsigned int i;
	Float vol = 1;
	for (i = 0; i < h->dim; ++i)
		vol *= 2 * h->data[i + h->dim];
	return vol;
}

static hypercube make_hypercube(unsigned int dim, const Float *center, const Float *halfwidth) {
	unsigned int i;
	hypercube h;
	h.dim = dim;
	h.data = (Float *) malloc(sizeof(Float) * dim * 2);
	h.vol = 0;
	if (h.data) {
		for (i = 0; i < dim; ++i) {
			h.data[i] = center[i];
			h.data[i + dim] = halfwidth[i];
		}
		h.vol = compute_vol(&h);
	}
	return h;
}

static hypercube make_hypercube_range(unsigned int dim, const Float *xmin, const Float *xmax) {
	hypercube h = make_hypercube(dim, xmin, xmax);
	unsigned int i;
	if (h.data) {
		for (i = 0; i < dim; ++i) {
			h.data[i] = 0.5f * (xmin[i] + xmax[i]);
			h.data[i + dim] = 0.5f * (xmax[i] - xmin[i]);
		}
		h.vol = compute_vol(&h);
	}
	return h;
}

static void destroy_hypercube(hypercube *h) {
	free(h->data);
	h->dim = 0;
}

typedef struct {
	hypercube h;
	unsigned int splitDim;
	unsigned int fdim; /* dimensionality of vector const Integrand & */
	esterr *ee; /* array of length fdim */
	Float errmax; /* max ee[k].err */
} region;

static region make_region(const hypercube *h, unsigned int fdim) {
	region R;
	R.h = make_hypercube(h->dim, h->data, h->data + h->dim);
	R.splitDim = 0;
	R.fdim = fdim;
	R.ee = R.h.data ? (esterr *) malloc(sizeof(esterr) * fdim) : NULL;
	return R;
}

static void destroy_region(region *R) {
	destroy_hypercube(&R->h);
	free(R->ee);
	R->ee = 0;
}

static bool cut_region(region *R, region *R2) {
	unsigned int d = R->splitDim, dim = R->h.dim;
	*R2 = *R;
	R->h.data[d + dim] *= 0.5f;
	R->h.vol *= 0.5f;
	R2->h = make_hypercube(dim, R->h.data, R->h.data + dim);
	if (!R2->h.data)
		return NDIntegrator::EFailure;
	R->h.data[d] -= R->h.data[d + dim];
	R2->h.data[d] += R->h.data[d + dim];
	R2->ee = (esterr *) malloc(sizeof(esterr) * R2->fdim);
	return R2->ee == NULL;
}

struct rule_s; /* forward declaration */

typedef NDIntegrator::EResult (*evalError_func)(struct rule_s *r,
			      unsigned int fdim, const VectorizedIntegrand &f,
			      unsigned int nR, region *R);
typedef void (*destroy_func)(struct rule_s *r);

typedef struct rule_s {
	unsigned int dim, fdim;         /* the dimensionality & number of functions */
	unsigned int num_points;       /* number of evaluation points */
	unsigned int num_regions; /* max number of regions evaluated at once */
	Float *pts; /* points to eval: num_regions * num_points * dim */
	Float *vals; /* num_regions * num_points * fdim */
	evalError_func evalError;
	destroy_func destroy;
} rule;

static void destroy_rule(rule *r) {
	if (r) {
		if (r->destroy)
			r->destroy(r);
		free(r->pts);
		free(r);
	}
}

static NDIntegrator::EResult alloc_rule_pts(rule *r, unsigned int num_regions) {
	if (num_regions > r->num_regions) {
		free(r->pts);
		r->pts = r->vals = NULL;
		r->num_regions = 0;
		/* allocate extra so that repeatedly calling alloc_rule_pts with
		   growing num_regions only needs a logarithmic number of allocations */
		num_regions *= 2;
		r->pts = (Float *) malloc(sizeof(Float) *
			     (num_regions * r->num_points * (r->dim + r->fdim)));
		if (r->fdim + r->dim > 0 && !r->pts)
			return NDIntegrator::EFailure;
		r->vals = r->pts + num_regions * r->num_points * r->dim;
		r->num_regions = num_regions;
	}
	return NDIntegrator::ESuccess;
}

static rule *make_rule(size_t sz, /* >= sizeof(rule) */
		       unsigned int dim, unsigned int fdim, unsigned int num_points,
		       evalError_func evalError, destroy_func destroy) {
	rule *r;

	if (sz < sizeof(rule))
		return NULL;
	r = (rule *) malloc(sz);
	if (!r)
		return NULL;
	r->pts = r->vals = NULL;
	r->num_regions = 0;
	r->dim = dim; r->fdim = fdim;
	r->num_points = num_points;
	r->evalError = evalError;
	r->destroy = destroy;
	return r;
}

/* note: all regions must have same fdim */
static int eval_regions(unsigned int nR, region *R,
			const VectorizedIntegrand & f, rule *r)
{
	unsigned int iR;
	if (nR == 0)
		return NDIntegrator::ESuccess; /* nothing to evaluate */
	if (r->evalError(r, R->fdim, f, nR, R))
		return NDIntegrator::EFailure;
	for (iR = 0; iR < nR; ++iR)
		R[iR].errmax = errMax(R->fdim, R[iR].ee);
	return NDIntegrator::ESuccess;
}

/***************************************************************************/
/* Functions to loop over points in a hypercube. */

/* Based on orbitrule.cpp in HIntLib-0.0.10 */

/* ls0 returns the least-significant 0 bit of n (e.g. it returns
   0 if the LSB is 0, it returns 1 if the 2 LSBs are 01, etcetera). */
static unsigned int ls0(unsigned int n)
{
#if defined(__GNUC__) && \
	((__GNUC__ == 3 && __GNUC_MINOR__ >= 4) || __GNUC__ > 3)
	return __builtin_ctz(~n); /* gcc builtin for version >= 3.4 */
#else
	const unsigned int bits[256] = {
		0, 1, 0, 2, 0, 1, 0, 3, 0, 1, 0, 2, 0, 1, 0, 4,
		0, 1, 0, 2, 0, 1, 0, 3, 0, 1, 0, 2, 0, 1, 0, 5,
		0, 1, 0, 2, 0, 1, 0, 3, 0, 1, 0, 2, 0, 1, 0, 4,
		0, 1, 0, 2, 0, 1, 0, 3, 0, 1, 0, 2, 0, 1, 0, 6,
		0, 1, 0, 2, 0, 1, 0, 3, 0, 1, 0, 2, 0, 1, 0, 4,
		0, 1, 0, 2, 0, 1, 0, 3, 0, 1, 0, 2, 0, 1, 0, 5,
		0, 1, 0, 2, 0, 1, 0, 3, 0, 1, 0, 2, 0, 1, 0, 4,
		0, 1, 0, 2, 0, 1, 0, 3, 0, 1, 0, 2, 0, 1, 0, 7,
		0, 1, 0, 2, 0, 1, 0, 3, 0, 1, 0, 2, 0, 1, 0, 4,
		0, 1, 0, 2, 0, 1, 0, 3, 0, 1, 0, 2, 0, 1, 0, 5,
		0, 1, 0, 2, 0, 1, 0, 3, 0, 1, 0, 2, 0, 1, 0, 4,
		0, 1, 0, 2, 0, 1, 0, 3, 0, 1, 0, 2, 0, 1, 0, 6,
		0, 1, 0, 2, 0, 1, 0, 3, 0, 1, 0, 2, 0, 1, 0, 4,
		0, 1, 0, 2, 0, 1, 0, 3, 0, 1, 0, 2, 0, 1, 0, 5,
		0, 1, 0, 2, 0, 1, 0, 3, 0, 1, 0, 2, 0, 1, 0, 4,
		0, 1, 0, 2, 0, 1, 0, 3, 0, 1, 0, 2, 0, 1, 0, 8,
	};
	unsigned int bit = 0;
	while ((n & 0xff) == 0xff) {
		n >>= 8;
		bit += 8;
	}
	return bit + bits[n & 0xff];
#endif
}

/**
 *  Evaluate the integration points for all 2^n points (+/-r,...+/-r)
 *
 *  A Gray-code ordering is used to minimize the number of coordinate updates
 *  in p, although this doesn't matter as much now that we are saving all pts.
 */
static void evalR_Rfs(Float *pts, unsigned int dim, Float *p, const Float *c, const Float *r) {
	unsigned int signs = 0; /* 0/1 bit = +/- for corresponding element of r[] */

	/* We start with the point where r is ADDed in every coordinate
	   (this implies signs=0). */
	for (unsigned int i = 0; i < dim; ++i)
		p[i] = c[i] + r[i];

	/* Loop through the points in Gray-code ordering */
	for (unsigned i = 0;; ++i) {
		unsigned int mask, d;
		memcpy(pts, p, sizeof(Float) * dim); pts += dim;
		d = ls0(i);	/* which coordinate to flip */
		if (d >= dim)
			break;

		/* flip the d-th bit and add/subtract r[d] */
		mask = 1U << d;
		signs ^= mask;
		p[d] = (signs & mask) ? c[d] - r[d] : c[d] + r[d];
	}
}

static void evalRR0_0fs(Float *pts, unsigned int dim, Float *p, const Float *c, const Float *r) {
	for (unsigned i = 0; i < dim - 1; ++i) {
		p[i] = c[i] - r[i];
		for (unsigned j = i + 1; j < dim; ++j) {
			p[j] = c[j] - r[j];
			memcpy(pts, p, sizeof(Float) * dim); pts += dim;
			p[i] = c[i] + r[i];
			memcpy(pts, p, sizeof(Float) * dim); pts += dim;
			p[j] = c[j] + r[j];
			memcpy(pts, p, sizeof(Float) * dim); pts += dim;
			p[i] = c[i] - r[i];
			memcpy(pts, p, sizeof(Float) * dim); pts += dim;
			p[j] = c[j];	/* Done with j -> Restore p[j] */
		}
		p[i] = c[i];		/* Done with i -> Restore p[i] */
	}
}

static void evalR0_0fs4d(Float *pts, unsigned int dim, Float *p, const Float *c,
			 const Float *r1, const Float *r2) {
	memcpy(pts, p, sizeof(Float) * dim); pts += dim;
	for (unsigned i = 0; i < dim; i++) {
		p[i] = c[i] - r1[i];
		memcpy(pts, p, sizeof(Float) * dim); pts += dim;
		p[i] = c[i] + r1[i];
		memcpy(pts, p, sizeof(Float) * dim); pts += dim;
		p[i] = c[i] - r2[i];
		memcpy(pts, p, sizeof(Float) * dim); pts += dim;
		p[i] = c[i] + r2[i];
		memcpy(pts, p, sizeof(Float) * dim); pts += dim;
		p[i] = c[i];
	}
}

#define num0_0(dim) (1U)
#define numR0_0fs(dim) (2 * (dim))
#define numRR0_0fs(dim) (2 * (dim) * (dim-1))
#define numR_Rfs(dim) (1U << (dim))

/***************************************************************************/
/* Based on rule75genzmalik.cpp in HIntLib-0.0.10: An embedded
   cubature rule of degree 7 (embedded rule degree 5) due to A. C. Genz
   and A. A. Malik.  See:

         A. C. Genz and A. A. Malik, "An imbedded [sic] family of fully
         symmetric numerical integration rules," SIAM
         J. Numer. Anal. 20 (3), 580-588 (1983).
*/

typedef struct {
     rule parent;

     /* temporary arrays of length dim */
     Float *widthLambda, *widthLambda2, *p;

     /* dimension-dependent constants */
     Float weight1, weight3, weight5;
     Float weightE1, weightE3;
} rule75genzmalik;

#define real(x) ((Float)(x))
#define to_int(n) ((int)(n))

static int isqr(int x)
{
     return x * x;
}

static void destroy_rule75genzmalik(rule *r_)
{
     rule75genzmalik *r = (rule75genzmalik *) r_;
     free(r->p);
}

static NDIntegrator::EResult rule75genzmalik_evalError(rule *r_, unsigned int fdim, const VectorizedIntegrand &f, unsigned int nR, region *R) {
	/* lambda2 = sqrt(9/70), lambda4 = sqrt(9/10), lambda5 = sqrt(9/19) */
	const Float lambda2 = (Float) 0.3585685828003180919906451539079374954541;
	const Float lambda4 = (Float) 0.9486832980505137995996680633298155601160;
	const Float lambda5 = (Float) 0.6882472016116852977216287342936235251269;
	const Float weight2 = (Float) (980.0 / 6561.0);
	const Float weight4 = (Float) (200.0 / 19683.0);
	const Float weightE2 = (Float) (245.0 / 486.0);
	const Float weightE4 = (Float) (25.0 / 729.0);
	const Float ratio = (lambda2 * lambda2) / (lambda4 * lambda4);

	rule75genzmalik *r = (rule75genzmalik *) r_;
	unsigned int i, j, dim = r_->dim, npts = 0;
	Float *diff, *pts, *vals;

	if (alloc_rule_pts(r_, nR))
		return NDIntegrator::EFailure;
	pts = r_->pts; vals = r_->vals;

	for (unsigned int iR = 0; iR < nR; ++iR) {
		const Float *center = R[iR].h.data;
		const Float *halfwidth = R[iR].h.data + dim;

		for (i = 0; i < dim; ++i)
			r->p[i] = center[i];

		for (i = 0; i < dim; ++i)
			r->widthLambda2[i] = halfwidth[i] * lambda2;
		for (i = 0; i < dim; ++i)
			r->widthLambda[i] = halfwidth[i] * lambda4;

		/* Evaluate points in the center, in (lambda2,0,...,0) and
			(lambda3=lambda4, 0,...,0).  */
		evalR0_0fs4d(pts + npts*dim, dim, r->p, center,
			r->widthLambda2, r->widthLambda);
		npts += num0_0(dim) + 2 * numR0_0fs(dim);

		/* Calculate points for (lambda4, lambda4, 0, ...,0) */
		evalRR0_0fs(pts + npts*dim, dim, r->p, center, r->widthLambda);
		npts += numRR0_0fs(dim);

		/* Calculate points for (lambda5, lambda5, ..., lambda5) */
		for (i = 0; i < dim; ++i)
			r->widthLambda[i] = halfwidth[i] * lambda5;
		evalR_Rfs(pts + npts*dim, dim, r->p, center, r->widthLambda);
		npts += numR_Rfs(dim);
	}

	/* Evaluate the const Integrand & function(s) at all the points */
	f((size_t) npts, pts, vals);

	/* we are done with the points, and so we can re-use the pts
	   array to store the maximum difference diff[i] in each dimension
	   for each hypercube */
	diff = pts;
	for (i = 0; i < dim * nR; ++i)
		diff[i] = 0;

	for (j = 0; j < fdim; ++j) {
		for (unsigned int iR = 0; iR < nR; ++iR) {
			Float result, res5th;
			Float val0, sum2=0, sum3=0, sum4=0, sum5=0;
			unsigned int k, k0 = 0;

			/* accumulate j-th function values into j-th integrals
			   NOTE: this relies on the ordering of the eval functions
			   above, as well as on the internal structure of
			   the evalR0_0fs4d function */

			val0 = vals[0]; /* central point */
			k0 += 1;

			for (k = 0; k < dim; ++k) {
				Float v0 = vals[k0 + 4*k];
				Float v1 = vals[(k0 + 4*k) + 1];
				Float v2 = vals[(k0 + 4*k) + 2];
				Float v3 = vals[(k0 + 4*k) + 3];

				sum2 += v0 + v1;
				sum3 += v2 + v3;

				diff[iR * dim + k] +=
					std::abs(v0 + v1 - 2*val0 - ratio * (v2 + v3 - 2*val0));
			}
			k0 += 4*k;

			for (k = 0; k < numRR0_0fs(dim); ++k)
				sum4 += vals[k0 + k];
			k0 += k;

			for (k = 0; k < numR_Rfs(dim); ++k)
				sum5 += vals[k0 + k];

			/* Calculate fifth and seventh order results */
			result = R[iR].h.vol * (r->weight1 * val0 + weight2 * sum2 + r->weight3 * sum3 + weight4 * sum4 + r->weight5 * sum5);
			res5th = R[iR].h.vol * (r->weightE1 * val0 + weightE2 * sum2 + r->weightE3 * sum3 + weightE4 * sum4);

			R[iR].ee[j].val = result;
			R[iR].ee[j].err = std::abs(res5th - result);

			vals += r_->num_points;
		}
	}

	/* figure out dimension to split: */
	for (unsigned int iR = 0; iR < nR; ++iR) {
		Float maxdiff = 0;
		unsigned int dimDiffMax = 0;

		for (i = 0; i < dim; ++i) {
			if (diff[iR*dim + i] > maxdiff) {
				maxdiff = diff[iR*dim + i];
				dimDiffMax = i;
	    	}
		}
		R[iR].splitDim = dimDiffMax;
	}
	return NDIntegrator::ESuccess;
}

static rule *make_rule75genzmalik(unsigned int dim, unsigned int fdim) {
	rule75genzmalik *r;

	if (dim < 2) return NULL; /* this rule does not support 1d integrals */

	/* Because of the use of a bit-field in evalR_Rfs, we are limited
	   to be < 32 dimensions (or however many bits are in unsigned).
	   This is not a practical limitation...long before you reach
	   32 dimensions, the Genz-Malik cubature becomes excruciatingly
	   slow and is superseded by other methods (e.g. Monte-Carlo). */
	if (dim >= sizeof(unsigned) * 8)
		return NULL;

	r = (rule75genzmalik *) make_rule(sizeof(rule75genzmalik),
			dim, fdim, num0_0(dim) + 2 * numR0_0fs(dim)
			+ numRR0_0fs(dim) + numR_Rfs(dim),
			rule75genzmalik_evalError,
			destroy_rule75genzmalik);
     if (!r)
		 return NULL;

	r->weight1 = (real(12824 - 9120 * to_int(dim) + 400 * isqr(to_int(dim))) / real(19683));
	r->weight3 = real(1820 - 400 * to_int(dim)) / real(19683);
	r->weight5 = real(6859) / real(19683) / real(1U << dim);
	r->weightE1 = (real(729 - 950 * to_int(dim) + 50 * isqr(to_int(dim))) / real(729));
	r->weightE3 = real(265 - 100 * to_int(dim)) / real(1458);
	r->p = (Float *) malloc(sizeof(Float) * dim * 3);
	if (!r->p) {
		destroy_rule((rule *) r);
		return NULL;
	}
	r->widthLambda = r->p + dim;
	r->widthLambda2 = r->p + 2 * dim;
	return (rule *) r;
}

/***************************************************************************/
/* 1d 15-point Gaussian quadrature rule, based on qk15.c and qk.c in
   GNU GSL (which in turn is based on QUADPACK). */

static NDIntegrator::EResult rule15gauss_evalError(rule *r,
				 unsigned int fdim, const VectorizedIntegrand & f,
				 unsigned int nR, region *R) {
     /* Gauss quadrature weights and kronrod quadrature abscissae and
	    weights as evaluated with 80 decimal digit arithmetic by
	    L. W. Fullerton, Bell Labs, Nov. 1981. */
	const unsigned int n = 8;
	const Float xgk[8] = {  /* abscissae of the 15-point kronrod rule */
		(Float) 0.991455371120812639206854697526329,
		(Float) 0.949107912342758524526189684047851,
		(Float) 0.864864423359769072789712788640926,
		(Float) 0.741531185599394439863864773280788,
		(Float) 0.586087235467691130294144838258730,
		(Float) 0.405845151377397166906606412076961,
		(Float) 0.207784955007898467600689403773245,
		(Float) 0.000000000000000000000000000000000
		/* xgk[1], xgk[3], ... abscissae of the 7-point gauss rule.
		   xgk[0], xgk[2], ... to optimally extend the 7-point gauss rule */
	};
	static const Float wg[4] = {  /* weights of the 7-point gauss rule */
		(Float) 0.129484966168869693270611432679082,
		(Float) 0.279705391489276667901467771423780,
		(Float) 0.381830050505118944950369775488975,
		(Float) 0.417959183673469387755102040816327
	};
	static const Float wgk[8] = { /* weights of the 15-point kronrod rule */
		(Float) 0.022935322010529224963732008058970,
		(Float) 0.063092092629978553290700663189204,
		(Float) 0.104790010322250183839876322541518,
		(Float) 0.140653259715525918745189590510238,
		(Float) 0.169004726639267902826583426598550,
		(Float) 0.190350578064785409913256402421014,
		(Float) 0.204432940075298892414161999234649,
		(Float) 0.209482141084727828012999174891714
	};
	unsigned int j, npts = 0;
	Float *pts, *vals;

	if (alloc_rule_pts(r, nR))
		return NDIntegrator::EFailure;

	pts = r->pts; vals = r->vals;

	for (unsigned int iR = 0; iR < nR; ++iR) {
		const Float center = R[iR].h.data[0];
		const Float halfwidth = R[iR].h.data[1];

		pts[npts++] = center;

		for (j = 0; j < (n - 1) / 2; ++j) {
			int j2 = 2*j + 1;
			Float w = halfwidth * xgk[j2];
			pts[npts++] = center - w;
			pts[npts++] = center + w;
		}
		for (j = 0; j < n/2; ++j) {
			int j2 = 2*j;
			Float w = halfwidth * xgk[j2];
			pts[npts++] = center - w;
			pts[npts++] = center + w;
		}

		R[iR].splitDim = 0; /* no choice but to divide 0th dimension */
	}

	f((size_t) npts, pts, vals);

	for (unsigned int k = 0; k < fdim; ++k) {
		for (unsigned int iR = 0; iR < nR; ++iR) {
			const Float halfwidth = R[iR].h.data[1];
			Float result_gauss = vals[0] * wg[n/2 - 1];
			Float result_kronrod = vals[0] * wgk[n - 1];
			Float result_abs = std::abs(result_kronrod);
			Float result_asc, mean, err;

			/* accumulate integrals */
			npts = 1;
			for (j = 0; j < (n - 1) / 2; ++j) {
				int j2 = 2*j + 1;
				Float v = vals[npts] + vals[npts+1];
				result_gauss += wg[j] * v;
				result_kronrod += wgk[j2] * v;
				result_abs += wgk[j2] * (std::abs(vals[npts]) + std::abs(vals[npts+1]));
				npts += 2;
			}
			for (j = 0; j < n/2; ++j) {
				int j2 = 2*j;
				result_kronrod += wgk[j2] * (vals[npts] + vals[npts+1]);
				result_abs += wgk[j2] * (std::abs(vals[npts]) + std::abs(vals[npts+1]));
				npts += 2;
			}

			/* integration result */
			R[iR].ee[k].val = result_kronrod * halfwidth;

			/* error estimate (from GSL, probably dates back to QUADPACK
			... not completely clear to me why we don't just use
			std::abs(result_kronrod - result_gauss) * halfwidth */
			mean = result_kronrod * 0.5f;
			result_asc = wgk[n - 1] * std::abs(vals[0] - mean);
			npts = 1;
			for (j = 0; j < (n - 1) / 2; ++j) {
				int j2 = 2*j + 1;
				result_asc += wgk[j2] * (std::abs(vals[npts]-mean)
					     + std::abs(vals[npts+1]-mean));
				npts += 2;
			}
			for (j = 0; j < n/2; ++j) {
				int j2 = 2*j;
				result_asc += wgk[j2] * (std::abs(vals[npts]-mean)
					     + std::abs(vals[npts+1]-mean));
				npts += 2;
			}
			err = std::abs(result_kronrod - result_gauss) * halfwidth;
			result_abs *= halfwidth;
			result_asc *= halfwidth;
			if (result_asc != 0 && err != 0) {
				/* Recommended error estimate for the 7-15 G-K rule */
				Float scale = std::pow((200 * err / result_asc), (Float) 1.5);
				err = (scale < 1) ? result_asc * scale : result_asc;
			}
			#if 0
				/* This seems a bit excessive (and creates problems for single
				   precision code) */
				if (result_abs > std::numeric_limits<Float>::min() / (50 * std::numeric_limits<Float>::epsilon())) {
					Float min_err = 50 * std::numeric_limits<Float>::epsilon() * result_abs;
					if (min_err > err)
						err = min_err;
				}
			#endif
			R[iR].ee[k].err = err;

			/* increment vals to point to next batch of results */
			vals += 15;
		}
	}
	return NDIntegrator::ESuccess;
}

static rule *make_rule15gauss(unsigned int dim, unsigned int fdim) {
     if (dim != 1) return NULL; /* this rule is only for 1d integrals */

     return make_rule(sizeof(rule), dim, fdim, 15, rule15gauss_evalError, 0);
}

/***************************************************************************/
/* binary heap implementation (ala _Introduction to Algorithms_ by
   Cormen, Leiserson, and Rivest), for use as a priority queue of
   regions to integrate. */

typedef region heap_item;
#define KEY(hi) ((hi).errmax)

typedef struct {
	unsigned int n, nalloc;
	heap_item *items;
	unsigned int fdim;
	esterr *ee; /* array of length fdim of the total const Integrand & & error */
} heap;

static void heap_resize(heap *h, unsigned int nalloc) {
	h->nalloc = nalloc;
	h->items = (heap_item *) realloc(h->items, sizeof(heap_item) * nalloc);
}

static heap heap_alloc(unsigned int nalloc, unsigned int fdim) {
	heap h;
	unsigned int i;
	h.n = 0;
	h.nalloc = 0;
	h.items = 0;
	h.fdim = fdim;
	h.ee = (esterr *) malloc(sizeof(esterr) * fdim);
	if (h.ee) {
		for (i = 0; i < fdim; ++i)
			h.ee[i].val = h.ee[i].err = 0;
		heap_resize(&h, nalloc);
	}
	return h;
}

/* note that heap_free does not deallocate anything referenced by the items */
static void heap_free(heap *h) {
	h->n = 0;
	heap_resize(h, 0);
	h->fdim = 0;
	free(h->ee);
}

static NDIntegrator::EResult heap_push(heap *h, heap_item hi) {
	int insert;
	unsigned int fdim = h->fdim;

	for (unsigned int i = 0; i < fdim; ++i) {
		h->ee[i].val += hi.ee[i].val;
		h->ee[i].err += hi.ee[i].err;
	}
	insert = h->n;
	if (++(h->n) > h->nalloc) {
		heap_resize(h, h->n * 2);
		if (!h->items)
			return NDIntegrator::EFailure;
	}
	while (insert) {
		int parent = (insert - 1) / 2;
		if (KEY(hi) <= KEY(h->items[parent]))
			break;
		h->items[insert] = h->items[parent];
		insert = parent;
	}
	h->items[insert] = hi;
	return NDIntegrator::ESuccess;
}

static NDIntegrator::EResult heap_push_many(heap *h, unsigned int ni, heap_item *hi) {
     unsigned int i;
     for (i = 0; i < ni; ++i)
	  if (heap_push(h, hi[i])) return NDIntegrator::EFailure;
     return NDIntegrator::ESuccess;
}

static heap_item heap_pop(heap *h) {
	heap_item ret;
	int i, n, child;
	if (!(h->n))
		SLog(EError, "attempted to pop an empty heap\n");

	ret = h->items[0];
	h->items[i = 0] = h->items[n = --(h->n)];
	while ((child = i * 2 + 1) < n) {
		int largest;
		heap_item swap;

		if (KEY(h->items[child]) <= KEY(h->items[i]))
			largest = i;
		else
			largest = child;
		if (++child < n && KEY(h->items[largest]) < KEY(h->items[child]))
			largest = child;
		if (largest == i)
			break;
		swap = h->items[i];
		h->items[i] = h->items[largest];
		h->items[i = largest] = swap;
	}
	unsigned int fdim = h->fdim;
	for (unsigned int j = 0; j < fdim; ++j) {
		h->ee[j].val -= ret.ee[j].val;
		h->ee[j].err -= ret.ee[j].err;
	}
	return ret;
}

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

/* adaptive integration, analogous to adaptintegrator.cpp in HIntLib */

static NDIntegrator::EResult ruleadapt_integrate(rule *r, unsigned int fdim,
		const VectorizedIntegrand & f, const hypercube *h, size_t maxEval,
		Float reqAbsError, Float reqRelError, Float *val, Float *err, size_t &numEval, int parallel) {
	heap regions;
	unsigned int i, j;
	region *R = NULL; /* array of regions to evaluate */
	unsigned int nR_alloc = 0;
	esterr *ee = NULL;

	regions = heap_alloc(1, fdim);
	if (!regions.ee || !regions.items)
		goto bad;

	ee = (esterr *) malloc(sizeof(esterr) * fdim);
	if (!ee)
		goto bad;

	nR_alloc = 2;
	R = (region *) malloc(sizeof(region) * nR_alloc);
	if (!R)
		goto bad;
	R[0] = make_region(h, fdim);
	if (!R[0].ee || eval_regions(1, R, f, r) || heap_push(&regions, R[0]))
		goto bad;
	numEval += r->num_points;

	while (numEval < maxEval || !maxEval) {
		for (j = 0; j < fdim && (regions.ee[j].err <= reqAbsError ||
			relError(regions.ee[j]) <= reqRelError); ++j)
			;
		if (j == fdim)
			break; /* convergence */

		if (parallel) {
			/* Maximize potential parallelism

			   adapted from I. Gladwell, "Vectorization of one dimensional
			   quadrature codes," pp. 230--238 in _Numerical Integration. Recent
			   Developments, Software and Applications_, G. Fairweather and
			   P. M. Keast, eds., NATO ASI Series C203, Dordrecht (1987), as
			   described in J. M. Bull and T. L. Freeman, "Parallel Globally
			   Adaptive Algorithms for Multi-dimensional Integration,"
			   http://citeseerx.ist.psu.edu/viewdoc/summary?doi=10.1.1.42.6638

			   Basically, this evaluates in one shot all regions
			   that *must* be evaluated in order to reduce the
			   error to the requested bound: the minimum set of
			   largest-error regions whose errors push the total
			   error over the bound.

			   [Note: Bull and Freeman claim that the Gladwell
			   approach is intrinsically inefficent because it
			   "requires sorting", and propose an alternative
			   algorithm that "only" requires three passes over the
			   entire set of regions.  Apparently, they didn't
			   realize that one could use a heap data structure, in
			   which case the time to pop K biggest-error regions
			   out of N is only O(K log N), much better than the
			   O(N) cost of the Bull and Freeman algorithm if
			   K << N, and it is also much simpler.] */
			unsigned int nR = 0;
			for (j = 0; j < fdim; ++j)
				ee[j] = regions.ee[j];
			do {
				if (nR + 2 > nR_alloc) {
					nR_alloc = (nR + 2) * 2;
					R = (region *) realloc(R, nR_alloc * sizeof(region));
					if (!R)
						goto bad;
				}
				R[nR] = heap_pop(&regions);
				for (j = 0; j < fdim; ++j)
					ee[j].err -= R[nR].ee[j].err;
				if (cut_region(R+nR, R+nR+1))
					goto bad;
				numEval += r->num_points * 2;
				nR += 2;
				for (j = 0; j < fdim && (ee[j].err <= reqAbsError
					|| relError(ee[j]) <= reqRelError); ++j)
					;
				if (j == fdim)
					break; /* other regions have small errs */
			} while (regions.n > 0 && (numEval < maxEval || !maxEval));
			if (eval_regions(nR, R, f, r) || heap_push_many(&regions, nR, R))
				goto bad;
		} else { /* minimize number of function evaluations */
			R[0] = heap_pop(&regions); /* get worst region */
			if (cut_region(R, R+1) || eval_regions(2, R, f, r)
				|| heap_push_many(&regions, 2, R))
				goto bad;
			numEval += r->num_points * 2;
		}
	}

     /* re-sum integral and errors */
	for (j = 0; j < fdim; ++j)
		val[j] = err[j] = 0;
	for (i = 0; i < regions.n; ++i) {
		for (j = 0; j < fdim; ++j) {
			val[j] += regions.items[i].ee[j].val;
			err[j] += regions.items[i].ee[j].err;
		}
		destroy_region(&regions.items[i]);
	}

	/* printf("regions.nalloc = %d\n", regions.nalloc); */
	free(ee);
	heap_free(&regions);
	free(R);
	return NDIntegrator::ESuccess;

bad:
	free(ee);
	heap_free(&regions);
	free(R);
	return NDIntegrator::EFailure;
}

static NDIntegrator::EResult integrate(unsigned fdim, const VectorizedIntegrand & f,
		     unsigned dim, const Float *xmin, const Float *xmax,
		     size_t maxEval, Float reqAbsError, Float reqRelError,
		     Float *val, Float *err, size_t &numEval, int parallel) {
	NDIntegrator::EResult status;

	numEval = 0;
	if (fdim == 0) /* nothing to do */
		return NDIntegrator::ESuccess;
	if (dim == 0) { /* trivial integration */
		f(1, xmin, val);
		for (unsigned int i = 0; i < fdim; ++i)
			err[i] = 0;
		return NDIntegrator::ESuccess;
	}
	rule *r = dim == 1 ? make_rule15gauss(dim, fdim)
		: make_rule75genzmalik(dim, fdim);
	if (!r) {
		for (unsigned int i = 0; i < fdim; ++i) {
			val[i] = 0;
			err[i] = std::numeric_limits<Float>::infinity();
		}
		return NDIntegrator::EFailure;
	}
	hypercube h = make_hypercube_range(dim, xmin, xmax);
	status = !h.data ? NDIntegrator::EFailure
		: ruleadapt_integrate(r, fdim, f, &h,
			maxEval, reqAbsError, reqRelError,
			val, err, numEval, parallel);
	destroy_hypercube(&h);
	destroy_rule(r);
	return status;
}

class VectorizationAdapter {
public:
	VectorizationAdapter(const NDIntegrator::Integrand &integrand, size_t fdim,
			size_t dim) : m_integrand(integrand), m_fdim(fdim), m_dim(dim) {
		m_temp = new Float[m_fdim];
	}

	~VectorizationAdapter() {
		delete[] m_temp;
	}

	void f(size_t nPt, const Float *in, Float *out) {
		for (size_t i = 0; i < nPt; ++i) {
			m_integrand(in + i*m_dim, m_temp);
	  		for (size_t k = 0; k < m_fdim; ++k)
				out[k*nPt + i] = m_temp[k];
		}
	}
private:
	const NDIntegrator::Integrand &m_integrand;
	size_t m_fdim, m_dim;
	Float *m_temp;
};

NDIntegrator::NDIntegrator(size_t fDim, size_t dim,
			size_t maxEvals, Float absError, Float relError)
 : m_fdim(fDim), m_dim(dim), m_maxEvals(maxEvals), m_absError(absError),
  m_relError(relError) { }

NDIntegrator::EResult NDIntegrator::integrate(const Integrand &f, const Float *min,
		const Float *max, Float *result, Float *error, size_t *_evals) const {
	VectorizationAdapter adapter(f, m_fdim, m_dim);
	size_t evals = 0;
	EResult retval = mitsuba::integrate((unsigned int) m_fdim, boost::bind(
		&VectorizationAdapter::f, &adapter, _1, _2, _3), (unsigned int) m_dim,
		min, max, m_maxEvals, m_absError, m_relError, result, error, evals, false);
	if (_evals)
		*_evals = evals;
	return retval;
}

NDIntegrator::EResult NDIntegrator::integrateVectorized(const VectorizedIntegrand &f, const Float *min,
		const Float *max, Float *result, Float *error, size_t *_evals) const {
	size_t evals = 0;
	EResult retval = mitsuba::integrate((unsigned int) m_fdim, f, (unsigned int) m_dim,
		min, max, m_maxEvals, m_absError, m_relError, result, error, evals, true);
	if (_evals)
		*_evals = evals;
	return retval;
}

MTS_NAMESPACE_END


#include <fstream>
#include <cstring>
#include <cmath>
#include <stdlib.h>
#include <assert.h>
#include <iomanip>

#include <armadillo>

#include "math_utility.hh"
#include "matrix/utility.hh"

#include "cad_utility.hh"
#include "global_constants.hh"

/**
 * @return great circle distance between two point on a spherical Earth
 * @brief Refrence: Bate et al. "Fundamentals of Astrodynamics", Dover 1971, p. 310
 *
 * @param[in] lon1 longitude of first  point - rad
 * @param[in] lat1 latitude  of first  point - rad
 * @param[in] lon2 longitude of second point - rad
 * @param[in] lat2 latitude  of second point - rad
 *
 * @author 030414 Created from FORTRAN by Peter H Zipfel
 */
double cad::distance(const double &lon1,
                const double &lat1,
                const double &lon2,
                const double &lat2) {
    double dum =
        sin(lat2) * sin(lat1) + cos(lat2) * cos(lat1) * cos(lon2 - lon1);
    if (fabs(dum) > 1.)
        dum = 1. * sign(dum);
    double distancex = REARTH * acos(dum) * 1.e-3;

    return distancex;
}

/**
 * @brief Calculates geodetic longitude, latitude, and altitude from inertial
 *        displacement vector
 *        using the WGS 84 reference ellipsoid
 *        Reference: Britting,K.R."Inertial Navigation Systems Analysis", Wiley. 1971
 *
 * @return std::tuple(lon, lat, alt)
 *                    lon geodetic longitude - rad
 *                    lat geodetic latitude - rad
 *                    alt altitude above ellipsoid - m
 *
 * @param[in] SBII(3x1) = Inertial position - m
 * @param[in] TEI = T.M from Inertia coordinate to ECEF coordinate
 *
 * @author 030414 Created from FORTRAN by Peter H Zipfel
 * @author 170121 Create Armadillo Version by soncyang
 */
std::tuple<double, double, double> cad::geo84_in(arma::vec3 SBII,
                                                 arma::mat33 TEI) {
    int count(0);
    double lat0(0);
    double alamda(0);

    /* tuple */
    double lon(0);
    double lat(0);
    double alt(0);

    arma::vec3 SBEE;
    SBEE = TEI * SBII;
    /** initializing geodetic latitude using geocentric latitude */
    double dbi = norm(SBEE);
    double latg = asin(SBEE(2, 0) / dbi);
    // double latg = atan2(SBEE(2), sqrt(SBEE(0) * SBEE(0) + SBEE(1) * SBEE(1)));
    lat = latg;

    /** iterating to calculate geodetic latitude and altitude */
    do {
        lat0 = lat;
        double r0 =
            SMAJOR_AXIS *
            (1. - FLATTENING * (1. - cos(2. * lat0)) / 2. +
             5. * pow(FLATTENING, 2) * (1. - cos(4. * lat0)) / 16.);  /** eq 4-21 */
        alt = dbi - r0;
        double dd = FLATTENING * sin(2. * lat0) *
                    (1. - FLATTENING / 2. - alt / r0);  /** eq 4-15 */
        lat = latg + dd;
        count++;
        assert(count <= 100 &&
               " *** Stop: Geodetic latitude does not "
               "converge,'cad_geo84_in()' *** ");
    }while (fabs(lat - lat0) > SMALL);

    /** longitude */
    double sbee1 = SBEE(0, 0);
    double sbee2 = SBEE(1, 0);
    double dum4 = asin(sbee2 / sqrt(sbee1 * sbee1 + sbee2 * sbee2));
    // double dum4 = atan2(sbee2, sbee1);
    /** Resolving the multi-valued arcsin function */
    if ((sbee1 >= 0.0) && (sbee2 >= 0.0))
        alamda = dum4;  /** quadrant I */
    if ((sbee1 < 0.0) && (sbee2 >= 0.0))
        alamda = (180. * RAD) - dum4;  /** quadrant II */
    if ((sbee1 < 0.0) && (sbee2 < 0.0))
        alamda = (180. * RAD) - dum4;  /** quadrant III */
    if ((sbee1 > 0.0) && (sbee2 < 0.0))
        alamda = (360. * RAD) + dum4;  /** quadrant IV */
    lon = alamda;  /** - WEII3 * time - GW_CLONG; */
    if ((lon) > (180. * RAD))
        lon = -((360. * RAD) - lon);  /** east positive, west negative */

    return std::make_tuple(lon, lat, alt);
}

/**
 * @return geodetic velocity vector information from inertial postion and
 * velocity
 * using the WGS 84 reference ellipsoid
 *
 * @brief Calls utilities
 *    geo84_in(...), tdi84(...)
 *
 * @param[out] dvbe geodetic velocity - m/s
 * @param[out] psivdx geodetic heading angle - deg
 * @param[out] thtvdx geodetic flight path angle - deg
 *
 * @param[in] SBII(3x1) Inertial position - m
 * @param[in] VBII(3x1) Inertial velocity - m
 *
 * @author 040710 created by Peter H Zipfel
 */
std::tuple<double, double, double> cad::geo84vel_in(arma::vec3 SBII,
                                                    arma::vec3 VBII,
                                                    arma::mat33 TEI) {
    double lon(0);
    double lat(0);
    double alt(0);

    /* tuple */
    double dvbe(0);
    double psivdx(0);
    double thtvdx(0);

    /** geodetic longitude, latitude and altitude */
    std::tie(lon, lat, alt) = geo84_in(SBII, TEI);
    arma::mat33 TDI = tdi84(lon, lat, alt, TEI);

    /** Earth's angular velocity skew-symmetric matrix (3x3) */
    arma::mat33 WEII(arma::fill::zeros);
    WEII(0, 1) = -WEII3;
    WEII(1, 0) = WEII3;

    /** geographic velocity in geodetic axes VBED(3x1) and flight path angles */
    arma::vec3 VBED = TDI * (VBII - WEII * SBII);
    arma::vec3 POLAR = pol_from_cart(VBED);

    dvbe   = POLAR[0];
    psivdx = DEG * POLAR[1];
    thtvdx = DEG * POLAR[2];

    return std::make_tuple(dvbe, psivdx, thtvdx);
}

/**
 * @return geocentric lon, lat, alt from inertial displacement vector
 * for spherical Earth
 *
 * @param[out] lonc = geocentric longitude - rad
 * @param[out] latc = geocentric latitude - rad
 * @param[out] altc = geocentric latitude - m
 *
 * @param[in] SBII = Inertial position - m
 * @param[in] time = simulation time - sec
 *
 * @author 010628 Created by Peter H Zipfel
 * @author 030416 Modified for SBII (not SBIE) input, PZi
 */

std::tuple<double, double, double> cad::geoc_in(arma::vec3 SBII,
                                                const double &time) {
    double lon_cel(0);
    double sbii1 = SBII(0);
    double sbii2 = SBII(1);
    double sbii3 = SBII(2);
    arma::vec3 RESULT;

    /* tuple returns */
    double lonc(0);
    double latc(0);
    double altc(0);

    /** latitude */
    double dbi = sqrt(sbii1 * sbii1 + sbii2 * sbii2 + sbii3 * sbii3);
    latc = asin((sbii3) / dbi);

    /** altitude */
    altc = dbi - REARTH;

    /** longitude */
    double dum4 = asin(sbii2 / sqrt(sbii1 * sbii1 + sbii2 * sbii2));
    /** Resolving the multi-valued arcsin function */
    if ((sbii1 >= 0.0) && (sbii2 >= 0.0))
        lon_cel = dum4;  /** quadrant I */
    if ((sbii1 < 0.0) && (sbii2 >= 0.0))
        lon_cel = (180. * RAD) - dum4;  /** quadrant II */
    if ((sbii1 < 0.0) && (sbii2 < 0.0))
        lon_cel = (180. * RAD) - dum4;  /** quadrant III */
    if ((sbii1 > 0.0) && (sbii2 < 0.0))
        lon_cel = (360. * RAD) + dum4;  /** quadrant IV */
    lonc = lon_cel - WEII3 * time - GW_CLONG;
    if ((lonc) > (180. * RAD))
        lonc = -((360. * RAD) - lonc);  /** east positive, west negative */

    return std::make_tuple(lonc, latc, altc);
}

/**
 * @return lon, lat, alt from displacement vector in Earth coord for spherical
 * earth
 * RETURN[0]=lon
 * RETURN[1]=lat
 * RETURN[2]=alt
 *
 * @param[in] SBIE = displacement of vehicle wrt Earth center in Earth coordinates
 *
 * @author 010628 Created by Peter H Zipfel
 */
std::tuple<double, double, double> cad::geoc_ine(arma::vec3 SBIE) {
    double dum4(0);
    double alamda(0);
    double x(0), y(0), z(0);
    double dbi(0);
    double alt(0);
    double lat(0);
    double lon(0);

    /** downloading inertial components */
    x = SBIE(0);
    y = SBIE(1);
    z = SBIE(2);
    /** Latitude */
    // dbi = sqrt(x * x + y * y + z * z);
    dbi = norm(SBIE);
    lat = asin((z) / dbi);

    /** Altitude */
    alt = dbi - REARTH;

    /** Longitude */
    dum4 = asin(y / sqrt(x * x + y * y));

    // Resolving the multi-valued arcsin function
    if ((x >= 0.0) && (y >= 0.0)) {
        alamda = dum4;  // quadrant I
    }
    if ((x < 0.0) && (y >= 0.0)) {
        alamda = (180.0 * RAD) - dum4;  // quadrant II
    }
    if ((x < 0.0) && (y < 0.0)) {
        alamda = (180.0 * RAD) - dum4;  // quadrant III
    }
    if ((x >= 0.0) && (y < 0.0)) {
        alamda = (360.0 * RAD) + dum4;  // quadrant IV
    }

    lon = alamda;
    if ((lon) > (180.0 * RAD)) {
        lon = -((360.0 * RAD) - lon);  // east positive, west negative
    }

    return std::make_tuple(lon, lat, alt);
}
/**
 * @brief Earth gravitational acceleration, using the WGS 84 ellipsoid
 *      Ref: Chatfield, A.B.,"Fundamentals of High Accuracy Inertial
 *      Navigation",p.10, Prog.Astro and Aeronautics, Vol 174, AIAA, 1997.
 *
 * @return GRAVG(3x1) = gravitational acceleration in geocentric coord - m/s^2
 *
 * @param[in] SBII = inertial displacement vector - m
 * @param[in] time = simulation time - sec
 *
 * @author 030417 Created from FORTRAN by Peter H Zipfel
 */
arma::vec3 cad::grav84(arma::vec3 SBII, const double &time) {
    double lonc(0), latc(0), altc(0);

    std::tie(lonc, latc, altc) = geoc_in(SBII, time);
    double dbi = norm(SBII);
    double dum1 = GM / (dbi * dbi);
    double dum2 = 3 * sqrt(5.);
    double dum3 = pow((SMAJOR_AXIS / dbi), 2);
    double gravg1 = -dum1 * dum2 * C20 * dum3 * sin(latc) * cos(latc);
    double gravg2 = 0;
    double gravg3 =
        dum1 * (1. + dum2 / 2. * C20 * dum3 * (3. * pow(sin(latc), 2) - 1.));
    return arma::vec3({gravg1, gravg2, gravg3});
}

/**
 * @brief Returns the inertial displacement vector from longitude, latitude and
 *      altitude
 *      using the WGS 84 reference ellipsoid
 *      Reference:
 *      1. Britting,K.R."Inertial Navigation Systems Analysis"
 *         pp.45-49, Wiley, 1971
 *      2. Geodetic_Coordinate_Conversion.pdf James R. Clynch February 2006 p.3
 *
 * @return SBII(3x1) = Inertial vehicle position - m
 *
 * @param[in] lon = geodetic longitude - rad
 * @param[in] lat = geodetic latitude - rad
 * @param[in] alt = altitude above ellipsoid - m
 * @param[in] TEI = T.M from Inertia coordinate to ECEF coordinate
 *
 * @author 030411 Created from FORTRAN by Peter H Zipfel
 * @author 170121 Create Armadillo Version by soncyanga
 */
arma::vec3 cad::in_geo84(const double lon,
                         const double lat,
                         const double alt,
                         arma::mat33 TEI) {
    arma::vec3 SBIE;
    arma::vec3 SBII;

    // deflection of the normal, dd, and length of earth's radius to ellipse
    // surface, R0

    double r0 = SMAJOR_AXIS/sqrt(1 - FLATTENING * (2 - FLATTENING) * sin(lat) * sin(lat));

    double e = sqrt(2 * FLATTENING - FLATTENING * FLATTENING);

    // vehicle's displacement vector from earth's center SBID(3x1) in geodetic
    // coord.
    double dbi = r0 + alt;
    SBIE(0, 0) = dbi * cos(lat)* cos(lon);
    SBIE(1, 0) = dbi * cos(lat) * sin(lon);
    SBIE(2, 0) = ((1. - e * e) * r0 + alt) * sin(lat);

    SBII = trans(TEI) * SBIE;
    // double dum = (1.0 - FLATTENING) * (1.0 - FLATTENING);
    // double lamdas = atan(dum * tan(lat));
    // double rs = sqrt(SMAJOR_AXIS * SMAJOR_AXIS / (1.0 + ((1.0 / dum) - 1.0) * sin(lamdas) * sin(lamdas)));

    // SBIE(0, 0) = rs * cos(lamdas) * cos(lon) + alt * cos(lat) * cos(lon);
    // SBIE(1, 0) = rs * cos(lamdas) * sin(lon) + alt * cos(lat) * sin(lon);
    // SBIE(2, 0) = rs * sin(lamdas) + alt * sin(lat);

    // SBII = trans(TEI) * SBIE;
    return SBII;
}

/**
 * @brief Returns the inertial displacement vector from geocentric longitude, latitude
 *      and altitude
 *      for spherical Earth
 *
 * @return SBII = position of vehicle wrt center of Earth, in inertial coord
 *
 * @param[in] lon = geographic longitude - rad
 * @param[in] lat = geocentric latitude - rad
 * @param[in] alt = altitude above spherical Earth = m
 *
 * @author 010405 Created by Peter H Zipfel
 */
arma::vec3 cad::in_geoc(const double &lon,
                   const double &lat,
                   const double &alt,
                   const double &time) {
    arma::vec3 VEC;

    double dbi = alt + REARTH;
    double cel_lon = lon + WEII3 * time + GW_CLONG;
    double clat = cos(lat);
    double slat = sin(lat);
    double clon = cos(cel_lon);
    double slon = sin(cel_lon);

    VEC(0) = dbi * clat * clon;
    VEC(1) = dbi * clat * slon;
    VEC(2) = dbi * slat;

    return VEC;
}

/**
 * @brief Calculates inertial displacement and velocity vectors from orbital elements
 *      Reference: Bate et al. "Fundamentals of Astrodynamics", Dover 1971, p.71
 *
 * @return parabola_flag = 0 ok
 *                       = 1 not suitable (divide by zero), because parabolic
 *                           trajectory
 *
 * @param[out] SBII = Inertial position - m
 * @param[out] SBII = Inertial velocity - m/s
 *
 * @param[in] semi = semi-major axis of orbital ellipsoid - m
 * @param[in] ecc = eccentricity of elliptical orbit - ND
 * @param[in] inclx = inclination of orbital wrt equatorial plane - deg
 * @param[in] lon_anodex = celestial longitude of the ascending node - deg
 * @param[in] arg_perix = argument of periapsis (ascending node to periapsis) - deg
 * @param[in] true_anomx = true anomaly (periapsis to satellite) - deg
 *
 * @author 040510 Created by Peter H Zipfel
 */
int cad::in_orb(arma::vec3 &SBII,
           arma::vec3 &VBII,
           const double &semi,
           const double &ecc,
           const double &inclx,
           const double &lon_anodex,
           const double &arg_perix,
           const double &true_anomx) {
    // local variable
    int parabola_flag(0);
    arma::vec3 SBIP;
    arma::vec3 VBIP;
    arma::mat33 TIP;

    // semi-latus rectum from semi-major axis and eccentricity
    double pp = semi * (1 - ecc * ecc);

    // angles
    double c_true_anom = cos(true_anomx * RAD);
    double s_true_anom = sin(true_anomx * RAD);

    // inertial distance in perifocal coordinates
    double dbi = pp / (1 + ecc * c_true_anom);

    // inertial position vector
    SBIP[0] = dbi * c_true_anom;
    SBIP[1] = dbi * s_true_anom;
    SBIP[2] = 0;

    // pypass calculation if parabola
    if (pp == 0) {
        parabola_flag = 1;
    } else {
        // inertial velocity
        double dum = sqrt(GM / pp);
        VBIP[0] = -dum * s_true_anom;
        VBIP[1] = dum * (ecc + c_true_anom);
        VBIP[2] = 0;
    }

    // transforming to inertial coordinates
    TIP = tip(inclx * RAD, lon_anodex * RAD, arg_perix * RAD);
    SBII = TIP * SBIP;
    VBII = TIP * VBIP;

    return parabola_flag;
}

/**
 * @brief Projects initial state through 'tgo' to final state along a Keplerian
 *      trajectory
 *      Based on Ray Morth, unpublished utility
 *
 * @return kepler_flan = 0: good Kepler projection;
 *                     = 1: bad (# of iterations>20, or neg. sqrt), no new proj cal,
 *                          use prev value;  - ND
 * @param[out] SPII = projected inertial position after tgo - m
 * @param[out] VPII = projected inertial velocity after tgo - m/s
 *
 * @param[in] SBII = current inertial position - m
 * @param[in] VBII = current inertial velocity - m/s
 * @param[in] tgo = time-to-go to projected point - sec
 *
 * @author 040319 Created from FORTRAN by Peter H Zipfel
 */
int cad::kepler(arma::vec3 &SPII,
           arma::vec3 &VPII,
           arma::vec3 SBII,
           arma::vec3 VBII,
           const double &tgo) {
    // local variables
    double sde(0);
    double cde(0);
    int kepler_flag(0);

    double sqrt_GM = sqrt(GM);
    double ro = norm(SBII);
    double vo = norm(VBII);
    double rvo = dot(SBII, VBII);
    double a1 = vo * vo / GM;
    double sa = ro / (2 - ro * a1);
    if (sa < 0) {
        // return without re-calculating SPII, VPII
        kepler_flag = 1;
        return kepler_flag;
    }
    double smua = sqrt_GM * sqrt(sa);
    double mdot = smua / (sa * sa);

    // calculating 'de'iteratively
    double dm = mdot * tgo;
    double de = dm;  // initialize eccentricity
    double a11 = rvo / smua;
    double a21 = (sa - ro) / sa;
    int count20 = 0;
    double adm(0);
    do {
        cde = 1 - cos(de);
        sde = sin(de);
        double dmn = de + a11 * cde - a21 * sde;
        double dmerr = dm - dmn;

        adm = fabs(dmerr) / mdot;
        double dmde = 1 + a11 * sde - a21 * (1 - cde);
        de = de + dmerr / dmde;
        count20++;
        if (count20 > 20) {
            // return without re-calculating SPII, VPII
            kepler_flag = 1;
            return kepler_flag;
        }
    }while (adm > SMALL);

    // projected position
    double fk = (ro - sa * cde) / ro;
    double gk = (dm + sde - de) / mdot;
    SPII = SBII * fk + VBII * gk;

    // projected velocity
    double rp = norm(SPII);
    double fdk = -smua * sde / ro;
    double gdk = rp - sa * cde;
    VPII = SBII * (fdk / rp) + VBII * (gdk / rp);

    return kepler_flag;
}

/**
 * @brief Projects initial state through 'tgo' to final state along a Keplerian
 *      trajectory
 *      Based on: Bate, Mueller, White, "Fundamentals of Astrodynamics", Dover 1971
 *
 * @return iter_flan = 0: # of iterations < 20;
 *                   = 1: # of iterations > 20;  - ND
 *
 * @param[out] SPII = projected inertial position after tgo - m
 * @param[out] VPII = projected inertial velocity after tgo - m/s
 *
 * @param[in] SBII = current inertial position - m
 * @param[in] VBII = current inertial velocity - m/s
 * @param[in] tgo = time-to-go to projected point - sec
 *
 * @author 040318 Created from ASTRO_KEP by Peter H Zipfel
 */
int cad::kepler1(arma::vec3 &SPII,
            arma::vec3 &VPII,
            arma::vec3 SBII,
            arma::vec3 VBII,
            const double &tgo) {
    double c(0);
    double s(0);
    double z(0);
    double dt(0);
    int iter_flag(0);

    double ro = norm(SBII);
    double vo = norm(VBII);
    double al = (2 * GM / ro - vo * vo) / GM;
    /*
     * double en = -GM * al / 2;  // specific mechanical energy - J/kg
     * Unused variable.
     */
    arma::vec3 AM = skew_sym(SBII) * VBII;
    /*
     * double h = norm(AM);  // angular momentum - m^2/s
     * Unused variable.
     */
    double dum = dot(SBII, VBII);
    double sqrt_GM = sqrt(GM);

    // initial guaess of x
    double x = 0;
    int count20 = 0;

    // calculating x using newton iteration
    do {
        count20++;
        z = x * x * al;
        std::tie(c, s) = kepler1_ucs(z);
        dt =
            (x * x * x * s + dum * x * x * c / sqrt_GM + ro * x * (1 - z * s)) /
            sqrt_GM;
        double dtx =
            (x * x * c + dum * x * (1 - z * s) / sqrt_GM + ro * (1 - z * c)) /
            sqrt_GM;
        x = x + (tgo - dt) / dtx;
    }while (fabs((tgo - dt) / tgo) > SMALL);

    // projected inertial position
    double f = 1 - x * x * c / ro;
    double g = tgo - x * x * s / sqrt_GM;
    SPII = SBII * f + VBII * g;

    // projecting inertial velocity
    double rx = norm(SPII);
    double fd = sqrt_GM * x * (z * s - 1) / (ro * rx);
    double gd = 1 - x * x * c / rx;
    VPII = SBII * fd + VBII * gd;

    // diagnostic: bad iteration if count20 > 20
    if (count20 > 20)
        iter_flag = 1;
    return iter_flag;
}

/**
 * @brief Calculates utility functions c(z) and s(z) for kepler(...)
 *      Reference: Bate, Mueller, White, "Fundamentals of Astrodynamics", Dover 1971,
 *      p.196
 *
 * @param[out] c = c(z) utility function
 * @param[out] s = s(z) utility function
 *
 * @param[in] z = z-variable
 *
 * @author 040318 Created from ASTRO_UCS by Peter H Zipfel
 */
std::tuple<double, double> cad::kepler1_ucs(const double &z) {
    double sd(0);
    double cd(0);

    /* tuple */
    double c(0);
    double s(0);

    if (z > 0.1) {
        c = (1 - cos(sqrt(z))) / z;
        s = (sqrt(z) - sin(sqrt(z))) / sqrt(z * z * z);
    }
    if (z < -0.1) {
        c = (1 - cosh(sqrt(-z))) / z;
        s = (sinh(sqrt(-z)) - sqrt(-z)) / sqrt(-z * z * z);
    }
    if (fabs(z) <= 0.1) {
        double dc = 2;
        c = 1 / dc;
        double dcd = -24;
        cd = 1 / dcd;
        double ds = 6;
        s = 1 / ds;
        double dsd = -120;
        sd = 1 / dsd;

        for (int k = 1; k < 7; k++) {
            double z_pow_k = pow(-z, k);
            int n = 2 * k + 1;
            dc = dc * n * (n + 1);
            c = c + z_pow_k / dc;
            dcd = dcd * (n + 2) * (n + 3);
            cd = cd - (k + 1) * z_pow_k / dcd;
            ds = ds * (n + 1) * (n + 2);
            s = s + z_pow_k / ds;
            dsd = dsd * (n + 3) * (n + 4);
            sd = sd - (k + 1) * z_pow_k / dsd;
        }
    }

    return std::make_tuple(c, s);
}

/**
 * @brief Calculates the orbital elements from inertial displacement and velocity
 *      Reference: Bate et al. "Fundamentals of Astrodynamics", Dover 1971, p.58
 *
 * @return cadorbin_flag = 0 ok
 *                         1 'true_anomx' not calculated, because of circular orbit
 *                         2 'semi' not calculated, because parabolic orbit
 *                         3 'lon_anodex' not calculated, because equatorial orbit
 *                         13 'arg_perix' not calculated, because equatorialand/or
 *                            circular orbit
 * @param[out] semi = semi-major axis of orbital ellipsoid - m
 * @param[out] ecc = eccentricity of elliptical orbit - ND
 * @param[out] inclx = inclination of orbital wrt equatorial plane - deg
 * @param[out] lon_anodex = celestial longitude of the ascending node - deg
 * @param[out] arg_perix = argument of periapsis (ascending node to periapsis) - deg
 * @param[out] true_anomx = true anomaly (periapsis to satellite) - deg
 *
 * @param[in] SBII = Inertial position - m
 * @param[in] VBII = Inertial velocity - m/s
 *
 * @author 040510 Created by Peter H Zipfel
 * @author 170121 Create Armadillo Version by soncyang
 */
int cad::orb_in(double &semi,
                   double &ecc,
                   double &inclx,
                   double &lon_anodex,
                   double &arg_perix,
                   double &true_anomx,
                   arma::vec3 &SBII,
                   arma::vec3 &VBII) {
    // local variable
    int cadorbin_flag(0);
    arma::vec3 NODE_I;
    double lon_anode(0);
    double arg_peri(0);
    double true_anom(0);

    // angular momentum vector of orbit
    arma::mat ANGL_MOM_I = skew_sym(SBII) * VBII;
    double angl_mom = norm(ANGL_MOM_I);

    // vector of the ascending node (undefined if inclx=0)
    NODE_I(0) = -ANGL_MOM_I(1);
    NODE_I(1) = ANGL_MOM_I(0);
    double node = norm(NODE_I);

    // orbit eccentricity vector and magnitude
    double dbi = norm(SBII);
    double dvbi = norm(VBII);
    arma::vec3 EI;
    EI = (SBII * (dvbi * dvbi - GM / dbi) - VBII * dot(SBII, VBII)) * (1 / GM);
    ecc = norm(EI);

    // semi latus rectum
    double pp = angl_mom * angl_mom / GM;

    // semi-major axis of orbit
    if (pp == 1)
        cadorbin_flag = 2;
    else
        semi = pp / (1 - ecc * ecc);

    // orbit inclination
    double arg = ANGL_MOM_I(2) * (1 / angl_mom);
    if (fabs(arg) > 1)
        arg = 1;
    inclx = acos(arg) * DEG;

    // bypass calculations if equatorial orbit
    if (node < SMALL) {
        cadorbin_flag = 3;
    } else {
        // longitude of the ascending node
        arg = NODE_I(0) / node;
        if (fabs(arg) > 1)
            arg = 1;
        lon_anode = acos(arg);
    }

    // bypass calculations if circular and/or equatorial orbit
    if (ecc < SMALL || node < SMALL) {
        cadorbin_flag = 13;
    } else {
        // argument of periapsis
        arg = dot(NODE_I, EI) * (1 / (node * ecc));
        if (fabs(arg) > 1)
            arg = 1;
        arg_peri = acos(arg);
    }

    // bypass calculations if circular orbit
    if (ecc < SMALL) {
        cadorbin_flag = 1;
    } else {
        // true anomaly
        arg = dot(SBII, EI) * (1 / (dbi * ecc));
        if (fabs(arg) > 1)
            arg = 1;
        true_anom = acos(arg);
    }

    // quadrant resolution
    double quadrant = dot(SBII, VBII);
    if (quadrant >= 0)
        true_anomx = true_anom * DEG;
    else
        true_anomx = (2 * PI - true_anom) * DEG;

    if (EI(2) >= 0)
        arg_perix = arg_peri * DEG;
    else
        arg_perix = (2 * PI - arg_peri) * DEG;

    if (NODE_I(1) > 0)
        lon_anodex = lon_anode * DEG;
    else
        lon_anodex = (2 * PI - lon_anode) * DEG;

    return cadorbin_flag;
}

/**
 * @brief Returns the T.M. of geodetic wrt inertial coordinates
 *        using the WGS 84 reference ellipsoid
 *
 * @return TDI(3x3) = T.M.of geosetic wrt inertial coord - ND
 *
 * @param[in] lon = geodetic longitude - rad
 * @param[in] lat = geodetic latitude - rad
 * @param[in] alt = altitude above ellipsoid - m
 * @param[in] TEI = T.M from Inertia coordinate to ECEF coordinate
 *
 * @author 030424 Created by Peter H Zipfel
 * @author 170121 Create Armadillo Version by sonicyang
 */
arma::mat33 cad::tdi84(const double &lon,
                           const double &lat,
                           const double &alt,
                           arma::mat33 TEI) {
    arma::mat33 TDE;

    // celestial longitude of vehicle at simulation 'time'
    // double lon_cel = GW_CLONG + WEII3 * time + lon;

    // T.M. of geodetic coord wrt ECEF coord., TDE(3x3)
    double tdi13 = cos(lat);
    double tdi33 = -sin(lat);
    double tdi22 = cos(lon);
    double tdi21 = -sin(lon);
    TDE(0, 0) = tdi33 * tdi22;
    TDE(0, 1) = -tdi33 * tdi21;
    TDE(0, 2) = tdi13;
    TDE(1, 0) = tdi21;
    TDE(1, 1) = tdi22;
    TDE(1, 2) = 0;
    TDE(2, 0) = -tdi13 * tdi22;
    TDE(2, 1) = tdi13 * tdi21;
    TDE(2, 2) = tdi33;

    return TDE * TEI;
}

arma::mat33 cad::tde84(const double &lon,
                           const double &lat,
                           const double &alt) {
    arma::mat33 TGE;
    arma::mat33 TGD;

    double r0 = SMAJOR_AXIS * (1. - FLATTENING * (1. - cos(2. * lat)) / 2. +
                               5. * pow(FLATTENING, 2) * (1. - cos(4. * lat)) /
                                   16.);  // eq 4-21
    double dd = FLATTENING * sin(2. * lat) *
                (1. - FLATTENING / 2. - alt / r0);  // eq 4-15

    TGE = tge(lon, lat - dd);

    // T.M. of geographic (geocentric) wrt geodetic coord., TGD(3x3)
    TGD(0, 0) = cos(dd);
    TGD(2, 2) = cos(dd);
    TGD(1, 1) = 1;
    TGD(2, 0) = sin(dd);
    TGD(0, 2) = -sin(dd);

    // T.M. of geodetic coord wrt inertial coord., TDI(3x3)
    // double tdi13 = cos(lat);
    // double tdi33 = -sin(lat);
    // double tdi22 = cos(lon);
    // double tdi21 = -sin(lon);
    // TDE(0, 0) = tdi33 * tdi22;
    // TDE(0, 1) = -tdi33 * tdi21;
    // TDE(0, 2) = tdi13;
    // TDE(1, 0) = tdi21;
    // TDE(1, 1) = tdi22;
    // TDE(1, 2) = 0;
    // TDE(2, 0) = -tdi13 * tdi22;
    // TDE(2, 1) = tdi13 * tdi21;
    // TDE(2, 2) = tdi33;

    return trans(TGD) * TGE;
}

/**
 * @brief Returns the T.M. of earth wrt inertial coordinates
 *
 * @return TEI = T.M. of Earthy wrt inertial coordinates
 *
 * @param[in] = time since start of simulation - s
 *
 * @author 010628 Created by Peter H Zipfel
 */
arma::mat33 cad::tei(const double &time) {
    arma::mat33 TEI(arma::fill::eye);

    double xi = WEII3 * time + GW_CLONG;
    double sxi = sin(xi);
    double cxi = cos(xi);

    TEI(0, 0) = cxi;
    TEI(0, 1) = sxi;
    TEI(1, 0) = -sxi;
    TEI(1, 1) = cxi;

    return TEI;
}

/**
 * @return TGE = the T.M. of geographic wrt earth coordinates, TGE
 *               spherical Earth only
 *
 * @param[in] lon = geographic longitude - rad
 * @param[in] lat = geographic latitude - rad
 *
 * @author 010628 Created by Peter H Zipfel
 */
arma::mat33 cad::tge(const double &lon, const double &lat) {
    arma::mat33 TGE(arma::fill::zeros);

    double clon = cos(lon);
    double slon = sin(lon);
    double clat = cos(lat);
    double slat = sin(lat);

    TGE(0, 0) = (-slat * clon);
    TGE(0, 1) = (-slat * slon);
    TGE(0, 2) = clat;
    TGE(1, 0) = -slon;
    TGE(1, 1) = clon;
    TGE(1, 2) = 0.0;
    TGE(2, 0) = (-clat * clon);
    TGE(2, 1) = (-clat * slon);
    TGE(2, 2) = -slat;

    return TGE;
}

/**
 * @brief Returns the T.M. of geographic (geocentric) wrt inertial
 *        using the WGS 84 reference ellipsoid
 *        Reference: Britting,K.R."Inertial Navigation Systems Analysis",
 *        pp.45-49, Wiley, 1971
 *
 * @return TGI(3x3) = T.M.of geographic wrt inertial coord - ND
 *
 * @param[in] lon = geodetic longitude - rad
 * @param[in] lat = geodetic latitude - rad
 * @param[in] alt = altitude above ellipsoid - m
 *
 * @author 030414 Created from FORTRAN by Peter H Zipfel
 * @author 170121 Create Armadillo Version by soncyang
 */
arma::mat33 cad::tgi84(const double &lon,
                  const double &lat,
                  const double &alt,
                  arma::mat33 TEI) {
    arma::mat33 TDI = tdi84(lon, lat, alt, TEI);
    arma::mat33 TGD(arma::fill::zeros);

    // deflection of the normal, dd, and length of earth's radius to ellipse
    // surface, R0
    double r0 = SMAJOR_AXIS * (1. - FLATTENING * (1. - cos(2. * lat)) / 2. +
                               5. * pow(FLATTENING, 2) * (1. - cos(4. * lat)) /
                                   16.);  // eq 4-21
    double dd = FLATTENING * sin(2. * lat) *
                (1. - FLATTENING / 2. - alt / r0);  // eq 4-15

    // T.M. of geographic (geocentric) wrt geodetic coord., TGD(3x3)
    TGD(0, 0) = cos(dd);
    TGD(2, 2) = cos(dd);
    TGD(1, 1) = 1;
    TGD(2, 0) = sin(dd);
    TGD(0, 2) = -sin(dd);

    // T.M. of geographic (geocentric) wrt inertial coord., TGI(3x3)
    arma::mat33 TGI = TGD * TDI;

    return TGI;
}
/**
 * @brief Returns the transformation matrix of inertial wrt perifocal coordinates
 *
 * @return TIP = TM of inertial wrt perifocal
 *
 * @param[in] incl = inclination of orbital wrt equatorial plane - rad
 * @param[in] lon_anode = celestial longitude of the ascending node - rad
 * @param[in] arg_peri = argument of periapsis (ascending node to periapsis) - rad
 *
 * @author 040510 Created by Peter H Zipfel
 */
arma::mat33 cad::tip(const double &incl,
                const double &lon_anode,
                const double &arg_peri) {
    // local variable
    arma::mat33 TIP(arma::fill::zeros);

    double clon_anode = cos(lon_anode);
    double carg_peri = cos(arg_peri);
    double cincl = cos(incl);
    double slon_anode = sin(lon_anode);
    double sarg_peri = sin(arg_peri);
    double sincl = sin(incl);

    TIP(0, 0) = clon_anode * carg_peri - slon_anode * sarg_peri * cincl;
    TIP(0, 1) = -clon_anode * sarg_peri - slon_anode * carg_peri * cincl;
    TIP(0, 2) = slon_anode * sincl;
    TIP(1, 0) = slon_anode * carg_peri + clon_anode * sarg_peri * cincl;
    TIP(1, 1) = -slon_anode * sarg_peri + clon_anode * carg_peri * cincl;
    TIP(1, 2) = -clon_anode * sincl;
    TIP(2, 0) = sarg_peri * sincl;
    TIP(2, 1) = carg_peri * sincl;
    TIP(2, 2) = cincl;

    return TIP;
}


static bool eigen_did_assert = false;
#define eigen_assert(X) if(!eigen_did_assert && !(X)){ std::cout << "### Assertion raised in " << __FILE__ << ":" << __LINE__ << ":\n" #X << "\n### The following would happen without assertions:\n"; eigen_did_assert = true;}

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

#ifndef M_PI
#define M_PI 3.1415926535897932384626433832795
#endif

using namespace Eigen;
using namespace std;

int main(int, char**)
{
  cout.precision(3);
  Matrix3f A(3,3);
A << 1,2,3,  4,5,6,  7,8,10;
Matrix<float,3,2> B;
B << 3,1, 3,1, 4,1;
Matrix<float,3,2> X;
X = A.fullPivLu().solve(B);
cout << "The solution with right-hand side (3,3,4) is:" << endl;
cout << X.col(0) << endl;
cout << "The solution with right-hand side (1,1,1) is:" << endl;
cout << X.col(1) << endl;

  return 0;
}


#include <Eigen/Dense>
#include "../include/numerical_gradient.h"

namespace MyDL
{

    using namespace Eigen;

    MatrixXd numerical_gradient(double (*f)(MatrixXd&), MatrixXd& x)
    {
        double h = 1e-4;
        int size_x = x.rows() * x.cols();
        MatrixXd grad(x.rows(), x.cols());

        for (int i = 0; i < size_x; i++)
        {
            double tmp_val = x(i);
            double f_plus_h;
            double f_minus_h;

            // f(x + h)
            x(i) = tmp_val + h;
            f_plus_h = f(x);

            // f(x - h)
            x(i) = tmp_val - h;
            f_minus_h = f(x);

            grad(i) = (f_plus_h - f_minus_h) / (2 * h);

            x(i) = tmp_val;
        }

        return grad;
    }

    // TwoLayerNetなどで用いるもの(ラムダ式使用・DNNのインスタンスに含まれるパラメータを参照して使用する場合)
    MatrixXd numerical_gradient(const std::function<double(MatrixXd)> &f, MatrixXd &x)
    {
        double h = 1e-4;
        int size_x = x.rows() * x.cols();
        MatrixXd grad(x.rows(), x.cols());

        for (int i = 0; i < size_x; i++)
        {
            double tmp_val = x(i);
            double f_plus_h;
            double f_minus_h;

            // f(x + h)
            x(i) = tmp_val + h;
            f_plus_h = f(x);

            // f(x - h)
            x(i) = tmp_val - h;
            f_minus_h = f(x);

            grad(i) = (f_plus_h - f_minus_h) / (2 * h);

            x(i) = tmp_val;
        }

        return grad;
    }

    MatrixXd numerical_gradient(const std::function<vector<MatrixXd>(MatrixXd)> &f, MatrixXd &X)
    {
        double h = 1e-4;
        int size_X = X.rows() * X.cols();
        MatrixXd grad(X.rows(), X.cols());
        double f_plus_h, f_minus_h;

        for (int i = 0; i < size_X; i++)
        {
            double tmp_val = X(i);

            // f(x + h)
            X(i) = tmp_val + h;
            f_plus_h = f(X)[0].sum();

            // f(x - h)
            X(i) = tmp_val - h;
            f_minus_h = f(X)[0].sum();

            grad(i) = (f_plus_h - f_minus_h) / (2 * h);

            X(i) = tmp_val;
        }

        return grad;
    }

    MatrixXd numerical_gradient(const std::function<vector<MatrixXd>(VectorXd)> &f, VectorXd &X)
    {
        double h = 1e-4;
        int size_X = X.rows();
        VectorXd grad(X.rows());
        double f_plus_h, f_minus_h;

        for (int i = 0; i < size_X; i++)
        {
            double tmp_val = X(i);

            // f(x + h)
            X(i) = tmp_val + h;
            f_plus_h = f(X)[0].sum();

            // f(x - h)
            X(i) = tmp_val - h;
            f_minus_h = f(X)[0].sum();

            grad(i) = (f_plus_h - f_minus_h) / (2 * h);

            X(i) = tmp_val;
        }

        return grad;
    }
}


/*
   Oscar Efrain RAMOS PONCE, LAAS-CNRS
   Date: 28/10/2014
   Object to estimate a polynomial that fits some data.
*/

#ifndef _POLY_ESTIMATOR_HH_
#define _POLY_ESTIMATOR_HH_

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

/**
 * Compute the pseudo-inverse using Eigen
 * @param [in] matrix_in is the input matrix.
 * @param [out] pseudo_inv is the pseudo-inverse of the input matrix.
 * @param [in] pinvtoler is the tolerance in the SVD decomposition
 */
void pinv(const Eigen::MatrixXd& matrix_in,
          Eigen::MatrixXd& pseudo_inv,
          const double& pinvtoler = 1.0e-6);

/**
 * Object to fit a polynomial of a given order. It provides a generic fitting
 * polynomial of any order. However, it cannot be used by itself since the
 * proper coefficient of the polynomial (which represents the estimation) needs
 * to be specified. Moreover, the derived classes implement a faster computation
 * based on the specific case.
 *
 */
class PolyEstimator
{

public:

  /**
   * Create a polynomial estimator on a window of length N
   * @param order is the order of the polynomial estimator.
   * @param N is the window length.
   * @param dt is the control (sampling) time
   */
  PolyEstimator(const unsigned int& order,
                const unsigned int& N,
                const double& dt);

  /**
   * Estimate the generic polynomial given a new element. The order of the
   * polynomial is specified in the constructor. Note that this function can be
   * slow if no specialization of fit() has been done, since the generic
   * algorithm would be used.
   * @param [out] estimee is the calculated estimation
   * @param [in] data_element is the new data vector.
   * @param [in] time is the time stamp corresponding to the new data.
   */
  void estimate(std::vector<double>& estimee,
                const std::vector<double>& data_element,
                const double& time);

  /**
   * Estimate the polynomial given a new element assuming a constant time
   * difference. This constant time difference between consecutive samples is
   * given by dt (specified in the constructor). <br>Note: This function will
   * only work if dt is different to zero.
   * @param [out] estimee is the calculated estimation.
   * @param [in] data_element is the new data vector.
   */
  virtual void estimate(std::vector<double>& estimee,
                        const std::vector<double>& data_element) = 0;

  /**
   * Estimate the polynomial given a new element using a recursive
   * algorithm. This method is faster. However, it takes the time as it is (it
   * does not set the lowest time to zero), which can create "ill-conditions".
   * @param [out] estimee is the calculated estimation
   * @param [in] data_element is the new data.
   * @param [in] time is the time stamp corresponding to the new data.
   */
  virtual void estimateRecursive(std::vector<double>& estimee,
                                 const std::vector<double>& data_element,
                                 const double& time) = 0;

  /**
   * Get the time derivative of the estimated polynomial.
   * @param [out] estimeeDerivative is the calculated time derivative.
   * @param [in] order The order of the derivative (e.g. 1 means the first derivative).
   */
  virtual void getEstimateDerivative(std::vector<double>& estimeeDerivative,
                                     const unsigned int order) = 0;

  /**
   * Set the size of the filter window.
   * @param [in] N size
   */
  void setWindowLength(const unsigned int& N);

  /**
   * Get the size of the filter window.
   * @return Size
   */
  unsigned int getWindowLength();

protected:

  /**
   * Find the regressor which best fits in least square sense the last N data
   * sample couples. The order of the regressor is given in the constructor.
   */
  virtual void fit();

  /**
   * Get the estimation when using the generic fit function (in poly-estimator)
   */
  virtual double getEsteeme() = 0;

  /// Order of the polynomial estimator
  unsigned int order_;

  /// Window length
  unsigned int N_;

  /// Sampling (control) time
  double dt_;

  /// Indicate that dt is zero (dt is invalid)
  bool dt_zero_;

  /// Indicate that there are not enough elements to compute. The reason is that
  /// it is one of the first runs, and the estimate will be zero.
  bool first_run_;

  /// All the data (N elements of size dim)
  std::vector< std::vector<double> > elem_list_;

  /// Time vector corresponding to each element in elem_list_
  std::vector< double > time_list_;

  /// Circular index to each data and time element
  unsigned int pt_;

  /// Coefficients for the least squares solution
  Eigen::VectorXd coeff_;

  /// Time vector setting the lowest time to zero (for numerical stability).
  std::vector<double> t_;

  /// Data vector for a single dimension (a single dof). It is only one 'column'
  /// of the elem_list_ 'matrix'
  std::vector<double> x_;

  /// Matrix containing time components. It is only used for the generic fit
  /// computation, such that the estimation is \f$c = R^{\#} x\f$, where \f$x\f$
  /// is the data vector.
  Eigen::MatrixXd R_;

};

#endif


/********************************************************************************
 * Copyright 2009 The Robotics Group, The Maersk Mc-Kinney Moller Institute,
 * Faculty of Engineering, University of Southern Denmark
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 ********************************************************************************/

#include "LinearAlgebra.hpp"

#include <rw/core/macros.hpp>

#include <Eigen/SVD>

using namespace rw::math;

Eigen::MatrixXd LinearAlgebra::pseudoInverse (const Eigen::MatrixXd& am, double precision)
{
    if (am.rows () < am.cols ()) {
        // RW_THROW("pseudoInverse require rows >= to cols!");
        Eigen::MatrixXd a = am.transpose ();
        Eigen::JacobiSVD< Eigen::MatrixXd > svd =
            a.jacobiSvd (Eigen::ComputeThinU | Eigen::ComputeThinV);
        double tolerance = precision * std::max (a.cols (), a.rows ()) *
                           svd.singularValues ().array ().abs ().maxCoeff ();
        return (svd.matrixV () *
                Eigen::MatrixXd ((svd.singularValues ().array ().abs () > tolerance)
                                     .select (svd.singularValues ().array ().inverse (), 0))
                    .asDiagonal () *
                svd.matrixU ().adjoint ())
            .transpose ();
    }
    else {
        Eigen::JacobiSVD< Eigen::MatrixXd > svd =
            am.jacobiSvd (Eigen::ComputeThinU | Eigen::ComputeThinV);
        double tolerance = precision * std::max (am.cols (), am.rows ()) *
                           svd.singularValues ().array ().abs ().maxCoeff ();
        return svd.matrixV () *
               Eigen::MatrixXd ((svd.singularValues ().array ().abs () > tolerance)
                                    .select (svd.singularValues ().array ().inverse (), 0))
                   .asDiagonal () *
               svd.matrixU ().adjoint ();
    }
}

void LinearAlgebra::svd (const Eigen::MatrixXd& M, Eigen::MatrixXd& U, Eigen::VectorXd& sigma,
                         Eigen::MatrixXd& V)
{
    const Eigen::JacobiSVD< Eigen::MatrixXd > svd =
        M.jacobiSvd (Eigen::ComputeFullU | Eigen::ComputeFullV);
    U     = svd.matrixU ();
    sigma = svd.singularValues ();
    V     = svd.matrixV ();
}

bool LinearAlgebra::checkPenroseConditions (const Eigen::MatrixXd& A, const Eigen::MatrixXd& X,
                                            double prec)
{
    const Eigen::MatrixXd AX = A * X;
    const Eigen::MatrixXd XA = X * A;

    if (((AX * A) - A).lpNorm< Eigen::Infinity > () > prec)
        return false;
    if (((XA * X) - X).lpNorm< Eigen::Infinity > () > prec)
        return false;
    if ((AX.transpose () - AX).lpNorm< Eigen::Infinity > () > prec)
        return false;
    if ((XA.transpose () - XA).lpNorm< Eigen::Infinity > () > prec)
        return false;
    return true;
}

template<>
std::pair< LinearAlgebra::EigenMatrix< double >::type, LinearAlgebra::EigenVector< double >::type >
LinearAlgebra::eigenDecompositionSymmetric< double > (const Eigen::MatrixXd& Am1)
{
    Eigen::SelfAdjointEigenSolver< Eigen::MatrixXd > eigenSolver;
    eigenSolver.compute (Am1);
    return std::make_pair (eigenSolver.eigenvectors (), eigenSolver.eigenvalues ());
}

template<>
std::pair< LinearAlgebra::EigenMatrix< std::complex< double > >::type,
           LinearAlgebra::EigenVector< std::complex< double > >::type >
LinearAlgebra::eigenDecomposition< double > (const Eigen::MatrixXd& Am1)
{
    Eigen::EigenSolver< Eigen::MatrixXd > eigenSolver;
    eigenSolver.compute (Am1);
    return std::make_pair (eigenSolver.eigenvectors (), eigenSolver.eigenvalues ());
}


#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MODULE MatrixTests
#include <boost/test/unit_test.hpp>
#include <stdlib.h>
#include <algorithm>
#include <array>
#include <stdlib.h>

#include "graph/PBQPGraph.hpp"
#include "graph/Matrix.hpp"
#include "graph/PBQPNode.hpp"
#include "graph/PBQPEdge.hpp"

namespace pbqppapa {

/**
 * Generates a n x n matrix filled with only 1
 */
template<typename T>
Matrix<T> genMatrix(int n, int value) {
	T* data = new T[n * n];
	Matrix<T> mat = Matrix<T>(n, n, data);
	delete data;
	for (int i = 0; i < n * n; i++) {
		mat.getRaw(i) = value;
	}
	return mat;
}

template<typename T>
Matrix<T> genMatrixAscending(int n, int value) {
	T* data = new T[n * n];
	Matrix<T> matrix = Matrix<T>(n, n, data);
	delete data;
	for (int i = 0; i < n * n; i++) {
		matrix.getRaw(i) = value++;
	}
	return matrix;
}

Matrix<int> genMatrixRandom(int maxLength) {
	srand(time(NULL));
	int rows = rand() % maxLength + 1;
	int columns = rand() % maxLength + 1;
	Matrix<int> matrix = Matrix<int>(rows, columns);
	for (int row = 0; row < rows; row++) {
		for (int column = 0; column < columns; column++) {
			matrix.get(row, column) = rand();
		}
	}
	return matrix;
}

//Not used anywhere at the moment, but intentionally left here, because it massively eases debugging
void printMatrix(Matrix<int>& matrix) {
	BOOST_TEST_MESSAGE("---------");
	BOOST_TEST_MESSAGE("Rows" << matrix.getRowCount());
	BOOST_TEST_MESSAGE("Columns" << matrix.getColumnCount());
	for (int row = 0; row < matrix.getRowCount(); row++) {
		for (int column = 0; column < matrix.getColumnCount(); column++) {
			BOOST_TEST_MESSAGE(
					"Column: " << column << " ; " << "Row: " << row << "  " << matrix.get(row, column));
		}
		BOOST_TEST_MESSAGE("---");
	}
}

BOOST_AUTO_TEST_CASE(basicMatrixGeneration) {
	for (int i = 0; i <= 15; i++) {
		Matrix<int> matrix = genMatrix<int>(i, i);
		BOOST_CHECK_EQUAL(matrix.getColumnCount(), i);
		BOOST_CHECK_EQUAL(matrix.getRowCount(), i);
		BOOST_CHECK_EQUAL(matrix.getElementCount(), i * i);
		int count = 0;
		for (int row = 0; row < i; row++) {
			for (int column = 0; column < i; column++) {
				BOOST_CHECK_EQUAL(matrix.get(row, column), i);
				BOOST_CHECK_EQUAL(matrix.getRaw(count++), i);
			}
		}
	}
}

BOOST_AUTO_TEST_CASE(matrixPlus) {
	int size = 15;
	int firstValue = 2358;
	int secondValue = 2734;
	Matrix<int> matrix = genMatrixAscending<int>(size, firstValue);
	Matrix<int> matrix2 = genMatrixAscending<int>(size, secondValue);
	matrix += matrix2;
	int counter = 0;
	for (int row = 0; row < size; row++) {
		for (int column = 0; column < size; column++) {
			BOOST_CHECK_EQUAL(matrix.get(row, column),
					firstValue + secondValue + (counter++ * 2));
		}
	}
}

BOOST_AUTO_TEST_CASE(matrixMinus) {
	int size = 15;
	int firstValue = 473235;
	int secondValue = 9284736;
	Matrix<int> matrix = genMatrixAscending<int>(size, firstValue);
	Matrix<int> matrix2 = genMatrixAscending<int>(size, secondValue);
	matrix -= matrix2;
	for (int row = 0; row < size; row++) {
		for (int column = 0; column < size; column++) {
			BOOST_CHECK_EQUAL(matrix.get(row, column),
					firstValue - secondValue);
		}
	}
}

BOOST_AUTO_TEST_CASE(matrixMultiply) {
	int size = 15;
	int value = 425327;
	int factor = 18;
	Matrix<int> matrix = genMatrixAscending<int>(size, value);
	matrix *= factor;
	int counter = 0;
	for (int row = 0; row < size; row++) {
		for (int column = 0; column < size; column++) {
			BOOST_CHECK_EQUAL(matrix.get(row, column),
					(value + counter++) * factor);
		}
	}
}

BOOST_AUTO_TEST_CASE(matrixDivide) {
	int size = 15;
	int value = 38482;
	int divisor = 3;
	Matrix<int> matrix = genMatrixAscending<int>(size, value);
	matrix /= divisor;
	int counter = 0;
	for (int row = 0; row < size; row++) {
		for (int column = 0; column < size; column++) {
			BOOST_CHECK_EQUAL(matrix.get(row, column),
					(value + counter++) / divisor);
		}
	}
}

template<typename T>
void checkMatrixTranspose(Matrix<T>& m1, Matrix<T>& m2) {
	BOOST_CHECK_EQUAL(m1.getColumnCount(), m2.getRowCount());
	BOOST_CHECK_EQUAL(m1.getRowCount(), m2.getColumnCount());
	for (int row = 0; row < m1.getRowCount(); row++) {
		for (int column = 0; column < m1.getColumnCount(); column++) {
			BOOST_CHECK_EQUAL(m1.get(row, column), m2.get(column, row));
		}
	}
}

BOOST_AUTO_TEST_CASE(matrixTranspose) {
	for (int i = 0; i < 20; i++) {
		Matrix<int> matrix = genMatrixRandom(20);
		Matrix<int> transposed = matrix.transpose();
		checkMatrixTranspose<int>(matrix, transposed);
	}
}

}


// Barrier functions that grow to infinity as x -> 0+. Includes gradient and
// hessian functions, too. These barrier functions can be used to impose
// inequlity constraints on a function.

#pragma once

#include <Eigen/Core>

namespace ipc {

/// @brief Function that grows to infinity as x approaches 0 from the right.
///
/// \f$b(d) = -(d-\hat{d})^2\ln\left(\frac{d}{\hat{d}}\right)\f$
///
/// @param d The distance.
/// @param dhat Activation distance of the barrier.
/// @return The value of the barrier function at d.
template <typename T> T barrier(const T& d, double dhat);

/// @brief Derivative of the barrier function.
///
/// \f$b'(d) = (\hat{d}-d) \left( 2\ln\left( \frac{d}{\hat{d}} \right) -
/// \frac{\hat{d}}{d} + 1\right)\f$
///
/// @param d The distance.
/// @param dhat Activation distance of the barrier.
/// @return The derivative of the barrier wrt d.
double barrier_gradient(double d, double dhat);

/// @brief Second derivative of the barrier function.
///
/// \f$b''(d) = \left( \frac{\hat{d}}{d} + 2 \right) \frac{\hat{d}}{d} -
/// 2\ln\left( \frac{d}{\hat{d}} \right) - 3\f$
///
/// @param d The distance.
/// @param dhat Activation distance of the barrier.
/// @return The second derivative of the barrier wrt d.
double barrier_hessian(double d, double dhat);

} // namespace ipc

#include "barrier.tpp"


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

#include <fstream>
#include <boost/algorithm/string.hpp>

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

ENREGISTRER_PROBLEME(82, "Path sum: three ways") {
    // The minimal path sum in the 5 by 5 matrix below, by starting in any cell in the left column and finishing in any
    // cell in the right column, and only moving up, down, and right, is indicated in red and bold; the sum is equal to
    // 994.
    //
    // Find the minimal path sum, in matrix.txt (right click and "Save Link/Target As..."), a 31K text file containing
    // a 80 by 80 matrix, from the left column to the right column.
    matrice graphe;
    std::ifstream ifs("data/p082_matrix.txt");
    std::string ligne;
    while (ifs >> ligne) {
        std::vector<std::string> v;
        boost::split(v, ligne, boost::is_any_of(","));
        vecteur l;
        for (const auto &s: v) {
            l.push_back(std::stoull(s));
        }
        graphe.push_back(std::move(l));
    }

    const nombre taille = graphe.size();
    matrice chemin(taille, vecteur(taille, 0));
    for (size_t i = 0; i < taille; ++i) {
        vecteur chemin_bas(taille, 0);
        for (size_t j = 0; j < taille; ++j) {
            if (i == 0)
                chemin_bas[j] = graphe[j][i];
            else if (j == 0)
                chemin_bas[j] = chemin[j][i - 1] + graphe[j][i];
            else
                chemin_bas[j] = std::min(chemin[j][i - 1], chemin_bas[j - 1]) + graphe[j][i];
        }

        vecteur chemin_haut(taille, 0);
        for (size_t jj = 0; jj < taille; ++jj) {
            const size_t j = taille - jj - 1;
            if (i == 0)
                chemin_haut[j] = graphe[j][i];
            else if (j == taille - 1)
                chemin_haut[j] = chemin[j][i - 1] + graphe[j][i];
            else
                chemin_haut[j] = std::min(chemin[j][i - 1], chemin_haut[j + 1]) + graphe[j][i];
        }

        for (size_t j = 0; j < taille; ++j)
            chemin[j][i] = std::min(chemin_haut[j], chemin_bas[j]);
    }

    nombre resultat = std::numeric_limits<nombre>::max();
    for (const auto &c: chemin) {
        resultat = std::min(resultat, c.back());
    }

    return std::to_string(resultat);
}


#include <iostream>
#include <Eigen/Geometry>
#include <Eigen/Dense>
#include <math.h>
using Eigen::MatrixXd;
using Eigen::Matrix3d;
using Eigen::Matrix4d;
using namespace std;

struct Coordinate
{
	double x, y, z;

	Coordinate(double x, double y, double z)
	{
		this->x = x;
		this->y = y;
		this->z = z;
	}
};

struct Rotation
{
	double r, p, y;

	Rotation(double roll, double pitch, double yall)
	{
		r = roll;
		p = pitch;
		y = yall;
	}
};

#define D 3

Matrix3d RotationMatrix(float, float, float);
Matrix4d HomogeneousTransformationMatrix(Coordinate, Rotation);
Matrix4d TranslationMatrix(Coordinate);

int main()
{
	Matrix3d R;
	Matrix4d T;

	T = HomogeneousTransformationMatrix(Coordinate(4,5,6), Rotation(0, 0, 0));

	cout << T << endl;
}

Matrix3d RotationX(float thetax)
{
	Matrix3d Rx;

	Rx << 1, 0, 0,
		  0, cos(thetax), -sin(thetax),
		  0, sin(thetax), sin(thetax);

	return Rx;
}

Matrix3d RotationY(float thetay)
{
	Matrix3d Ry;

	Ry << cos(thetay), 0, sin(thetay),
		  0, 1, 0,
		  -sin(thetay), 0, cos(thetay);

	return Ry;
}

Matrix3d RotationZ(float thetaz)
{
	Matrix3d Rz;

	Rz << cos(thetaz), -sin(thetaz), 0,
		  sin(thetaz), cos(thetaz), 0,
		  0, 0, 1;

	return Rz;
}

Matrix3d RotationMatrix(float thetaz1, float thetay, float thetaz2) // Angle in degrees
{
	return RotationZ(thetaz1)*RotationY(thetay)*RotationZ(thetaz2);
}

Matrix4d HomogeneousTransformationMatrix(Coordinate origin, Rotation rot)
{
	Matrix4d T;

	T.block(0, 0, 3, 3) << RotationMatrix(rot.r, rot.p, rot.y);
	T.block(0, 3, 3, 1) << origin.x, origin.y, origin.z;
	T.block(3, 0, 1, 3) << MatrixXd::Zero(1,3);
	T.block(3, 3, 1, 1) << MatrixXd::Identity(1,1);

	return T;
}

Matrix4d TranslationMatrix(Coordinate origin)
{
	return HomogeneousTransformationMatrix(origin, Rotation(0,0,0));
}