// The contents of this file are in the public domain. See LICENSE_FOR_EXAMPLE_PROGRAMS.txt
/*

    This is an example illustrating the use the general purpose non-linear
    optimization routines from the dlib C++ Library.

    The library provides implementations of many popular algorithms such as L-BFGS
    and BOBYQA.  These algorithms allow you to find the minimum or maximum of a
    function of many input variables.  This example walks though a few of the ways
    you might put these routines to use.

*/

#include <dlib/optimization.h>
#include <dlib/global_optimization.h>
#include <iostream>

using namespace std;
using namespace dlib;

// ----------------------------------------------------------------------------------------

// In dlib, most of the general purpose solvers optimize functions that take a
// column vector as input and return a double.  So here we make a typedef for a
// variable length column vector of doubles.  This is the type we will use to
// represent the input to our objective functions which we will be minimizing.
typedef matrix<double,0,1> column_vector;

// ----------------------------------------------------------------------------------------
// Below we create a few functions.  When you get down into main() you will see that
// we can use the optimization algorithms to find the minimums of these functions.
// ----------------------------------------------------------------------------------------

double rosen (const column_vector& m)
/*
    This function computes what is known as Rosenbrock's function.  It is
    a function of two input variables and has a global minimum at (1,1).
    So when we use this function to test out the optimization algorithms
    we will see that the minimum found is indeed at the point (1,1).
*/
{
    const double x = m(0);
    const double y = m(1);

    // compute Rosenbrock's function and return the result
    return 100.0*pow(y - x*x,2) + pow(1 - x,2);
}

// This is a helper function used while optimizing the rosen() function.
const column_vector rosen_derivative (const column_vector& m)
/*!
    ensures
        - returns the gradient vector for the rosen function
!*/
{
    const double x = m(0);
    const double y = m(1);

    // make us a column vector of length 2
    column_vector res(2);

    // now compute the gradient vector
    res(0) = -400*x*(y-x*x) - 2*(1-x); // derivative of rosen() with respect to x
    res(1) = 200*(y-x*x);              // derivative of rosen() with respect to y
    return res;
}

// This function computes the Hessian matrix for the rosen() fuction.  This is
// the matrix of second derivatives.
matrix<double> rosen_hessian (const column_vector& m)
{
    const double x = m(0);
    const double y = m(1);

    matrix<double> res(2,2);

    // now compute the second derivatives
    res(0,0) = 1200*x*x - 400*y + 2; // second derivative with respect to x
    res(1,0) = res(0,1) = -400*x;   // derivative with respect to x and y
    res(1,1) = 200;                 // second derivative with respect to y
    return res;
}

// ----------------------------------------------------------------------------------------

class rosen_model
{
    /*!
        This object is a "function model" which can be used with the
        find_min_trust_region() routine.
    !*/

public:
    typedef ::column_vector column_vector;
    typedef matrix<double> general_matrix;

    double operator() (
        const column_vector& x
    ) const { return rosen(x); }

    void get_derivative_and_hessian (
        const column_vector& x,
        column_vector& der,
        general_matrix& hess
    ) const
    {
        der = rosen_derivative(x);
        hess = rosen_hessian(x);
    }
};

// ----------------------------------------------------------------------------------------

#if defined(BUILD_MONOLITHIC)
#define main(cnt, arr)      dlib_optimization_ex_main(cnt, arr)
#endif

int main(int argc, const char** argv)
try
{
    // Set the starting point to (4,8).  This is the point the optimization algorithm
    // will start out from and it will move it closer and closer to the function's
    // minimum point.   So generally you want to try and compute a good guess that is
    // somewhat near the actual optimum value.
    column_vector starting_point = {4, 8};

    // The first example below finds the minimum of the rosen() function and uses the
    // analytical derivative computed by rosen_derivative().  Since it is very easy to
    // make a mistake while coding a function like rosen_derivative() it is a good idea
    // to compare your derivative function against a numerical approximation and see if
    // the results are similar.  If they are very different then you probably made a
    // mistake.  So the first thing we do is compare the results at a test point:
    cout << "Difference between analytic derivative and numerical approximation of derivative: "
         << length(derivative(rosen)(starting_point) - rosen_derivative(starting_point)) << endl;

    cout << "Find the minimum of the rosen function()" << endl;
    // Now we use the find_min() function to find the minimum point.  The first argument
    // to this routine is the search strategy we want to use.  The second argument is the
    // stopping strategy.  Below I'm using the objective_delta_stop_strategy which just
    // says that the search should stop when the change in the function being optimized
    // is small enough.

    // The other arguments to find_min() are the function to be minimized, its derivative,
    // then the starting point, and the last is an acceptable minimum value of the rosen()
    // function.  That is, if the algorithm finds any inputs to rosen() that gives an output
    // value <= -1 then it will stop immediately.  Usually you supply a number smaller than
    // the actual global minimum.  So since the smallest output of the rosen function is 0
    // we just put -1 here which effectively causes this last argument to be disregarded.

    find_min(bfgs_search_strategy(),  // Use BFGS search algorithm
             objective_delta_stop_strategy(1e-7), // Stop when the change in rosen() is less than 1e-7
             rosen, rosen_derivative, starting_point, -1);
    // Once the function ends the starting_point vector will contain the optimum point
    // of (1,1).
    cout << "rosen solution:\n" << starting_point << endl;

    // Now let's try doing it again with a different starting point and the version
    // of find_min() that doesn't require you to supply a derivative function.
    // This version will compute a numerical approximation of the derivative since
    // we didn't supply one to it.
    starting_point = {-94, 5.2};
    find_min_using_approximate_derivatives(bfgs_search_strategy(),
                                           objective_delta_stop_strategy(1e-7),
                                           rosen, starting_point, -1);
    // Again the correct minimum point is found and stored in starting_point
    cout << "rosen solution:\n" << starting_point << endl;

    // Here we repeat the same thing as above but this time using the L-BFGS
    // algorithm.  L-BFGS is very similar to the BFGS algorithm, however, BFGS
    // uses O(N^2) memory where N is the size of the starting_point vector.
    // The L-BFGS algorithm however uses only O(N) memory.  So if you have a
    // function of a huge number of variables the L-BFGS algorithm is probably
    // a better choice.
    starting_point = {0.8, 1.3};
    find_min(lbfgs_search_strategy(10),  // The 10 here is basically a measure of how much memory L-BFGS will use.
             objective_delta_stop_strategy(1e-7).be_verbose(),  // Adding be_verbose() causes a message to be
                                                                // printed for each iteration of optimization.
             rosen, rosen_derivative, starting_point, -1);

    cout << endl << "rosen solution: \n" << starting_point << endl;

    starting_point = {-94, 5.2};
    find_min_using_approximate_derivatives(lbfgs_search_strategy(10),
                                           objective_delta_stop_strategy(1e-7),
                                           rosen, starting_point, -1);
    cout << "rosen solution: \n"<< starting_point << endl;

    // dlib also supports solving functions subject to bounds constraints on
    // the variables.  So for example, if you wanted to find the minimizer
    // of the rosen function where both input variables were in the range
    // 0.1 to 0.8 you would do it like this:
    starting_point = {0.1, 0.1}; // Start with a valid point inside the constraint box.
    find_min_box_constrained(lbfgs_search_strategy(10),
                             objective_delta_stop_strategy(1e-9),
                             rosen, rosen_derivative, starting_point, 0.1, 0.8);
    // Here we put the same [0.1 0.8] range constraint on each variable, however, you
    // can put different bounds on each variable by passing in column vectors of
    // constraints for the last two arguments rather than scalars.

    cout << endl << "constrained rosen solution: \n" << starting_point << endl;

    // You can also use an approximate derivative like so:
    starting_point = {0.1, 0.1};
    find_min_box_constrained(bfgs_search_strategy(),
                             objective_delta_stop_strategy(1e-9),
                             rosen, derivative(rosen), starting_point, 0.1, 0.8);
    cout << endl << "constrained rosen solution: \n" << starting_point << endl;

    // In many cases, it is useful if we also provide second derivative information
    // to the optimizers.  Two examples of how we can do that are shown below.
    starting_point = {0.8, 1.3};
    find_min(newton_search_strategy(rosen_hessian),
             objective_delta_stop_strategy(1e-7),
             rosen,
             rosen_derivative,
             starting_point,
             -1);
    cout << "rosen solution: \n"<< starting_point << endl;

    // We can also use find_min_trust_region(), which is also a method which uses
    // second derivatives.  For some kinds of non-convex function it may be more
    // reliable than using a newton_search_strategy with find_min().
    starting_point = {0.8, 1.3};
    find_min_trust_region(objective_delta_stop_strategy(1e-7),
                          rosen_model(),
                          starting_point,
                          10 // initial trust region radius
    );
    cout << "rosen solution: \n"<< starting_point << endl;

    // Next, let's try the BOBYQA algorithm.  This is a technique specially
    // designed to minimize a function in the absence of derivative information.
    // Generally speaking, it is the method of choice if derivatives are not available
    // and the function you are optimizing is smooth and has only one local optima.  As
    // an example, consider the be_like_target function defined below:
    column_vector target = {3, 5, 1, 7};
    auto be_like_target = [&](const column_vector& x) {
        return mean(squared(x-target));
    };
    starting_point = {-4,5,99,3};
    find_min_bobyqa(be_like_target,
                    starting_point,
                    9,    // number of interpolation points
                    uniform_matrix<double>(4,1, -1e100),  // lower bound constraint
                    uniform_matrix<double>(4,1, 1e100),   // upper bound constraint
                    10,    // initial trust region radius
                    1e-6,  // stopping trust region radius
                    100    // max number of objective function evaluations
    );
    cout << "be_like_target solution:\n" << starting_point << endl;

    // Finally, let's try the find_min_global() routine.  Like find_min_bobyqa(),
    // this technique is specially designed to minimize a function in the absence
    // of derivative information.  However, it is also designed to handle
    // functions with many local optima.  Where BOBYQA would get stuck at the
    // nearest local optima, find_min_global() won't.  find_min_global() uses a
    // global optimization method based on a combination of non-parametric global
    // function modeling and BOBYQA style quadratic trust region modeling to
    // efficiently find a global minimizer.  It usually does a good job with a
    // relatively small number of calls to the function being optimized.
    //
    // You also don't have to give it a starting point or set any parameters,
    // other than defining bounds constraints.  This makes it the method of
    // choice for derivative free optimization in the presence of multiple local
    // optima.  Its API also allows you to define functions that take a
    // column_vector as shown above or to explicitly use named doubles as
    // arguments, which we do here.
    auto complex_holder_table = [](double x0, double x1)
    {
        // This function is a version of the well known Holder table test
        // function, which is a function containing a bunch of local optima.
        // Here we make it even more difficult by adding more local optima
        // and also a bunch of discontinuities.

        // add discontinuities
        double sign = 1;
        for (double j = -4; j < 9; j += 0.5)
        {
            if (j < x0 && x0 < j+0.5)
                x0 += sign*0.25;
            sign *= -1;
        }
        // Holder table function tilted towards 10,10 and with additional
        // high frequency terms to add more local optima.
        return -( std::abs(sin(x0)*cos(x1)*exp(std::abs(1-std::sqrt(x0*x0+x1*x1)/pi))) -(x0+x1)/10 - sin(x0*10)*cos(x1*10));
    };

    // To optimize this difficult function all we need to do is call
    // find_min_global()
    auto result = find_min_global(complex_holder_table,
                                  {-10,-10}, // lower bounds
                                  {10,10}, // upper bounds
                                  std::chrono::milliseconds(500) // run this long
                                  );

    cout.precision(9);
    // These cout statements will show that find_min_global() found the
    // globally optimal solution to 9 digits of precision:
    cout << "complex holder table function solution y (should be -21.9210397): " << result.y << endl;
    cout << "complex holder table function solution x:\n" << result.x << endl;
}
catch (std::exception& e)
{
    cout << e.what() << endl;
}


/***************************************************************************
/* Javier Juan Albarracin - jajuaal1@ibime.upv.es                         */
/* Universidad Politecnica de Valencia, Spain                             */
/*                                                                        */
/* Copyright (C) 2014 Javier Juan Albarracin                              */
/*                                                                        */
/***************************************************************************
* Principal Component Analysis                                             *
***************************************************************************/

#ifndef PRINCIPALCOMPONENTANALYSIS_HPP
#define PRINCIPALCOMPONENTANALYSIS_HPP

#include <Eigen/Dense>
#include <Eigen/Eigenvalues>
#include <omp.h>

using namespace Eigen;

class PrincipalComponentAnalysis
{
public:
    PrincipalComponentAnalysis();

    template <class Derived>
    void compute(const MatrixBase<Derived> &dataset, const bool zscores = false);
    template <class Derived>
    MatrixXd dimensionalityReductionComponents(const MatrixBase<Derived> &dataset, const unsigned int components);
    template <class Derived>
    MatrixXd dimensionalityReductionVarianceExplained(const MatrixBase<Derived> &dataset, const double varianceExplained, const unsigned int minComponents = 0, const unsigned int maxComponents = 0);
    template <class Derived>
    Matrix<typename Derived::Scalar, Dynamic, Dynamic> filteringComponents(const MatrixBase<Derived> &dataset, const unsigned int components);
    template <class Derived>
    Matrix<typename Derived::Scalar, Dynamic, Dynamic> filteringVarianceExplained(const MatrixBase<Derived> &dataset, const double varianceExplained, const unsigned int minComponents = 0, const unsigned int maxComponents = 0);

    MatrixXd scores() const { return m_scores; }
    MatrixXd coefficients() const { return m_coefficients; }
    VectorXd latent() const { return m_latent; }
    VectorXd explained() const { return m_explained; }
    RowVectorXd mu() const { return m_mu; }
    unsigned int components() const { return m_components; }

private:
    MatrixXd m_scores;
    MatrixXd m_coefficients;
    VectorXd m_latent;
    VectorXd m_explained;
    RowVectorXd m_mu;
    unsigned int m_components;
    SelfAdjointEigenSolver<MatrixXd> m_solver;
};

/***************************** Implementation *****************************/

PrincipalComponentAnalysis::PrincipalComponentAnalysis()
: m_scores(MatrixXd()), m_coefficients(MatrixXd()), m_latent(VectorXd()), m_explained(VectorXd()), m_mu(RowVectorXd()), m_components(0)
{
}

template<class Derived>
void PrincipalComponentAnalysis::compute(const MatrixBase<Derived> &dataset, const bool zscores)
{
    // Check extreme case
    if (dataset.rows() == 1)
    {
        m_latent = VectorXd::Ones(1);
        m_explained = m_latent / m_latent.sum();
        m_coefficients = MatrixXd::Ones(dataset.cols(), 1);
        m_scores = dataset.template cast<double>() * m_coefficients;
        return;
    }

    // Subtract mean of each variable
    m_mu = dataset.template cast<double>().colwise().mean();
    MatrixXd centered = dataset.template cast<double>().rowwise() - m_mu;

    // Standarize std if required
    if (zscores)
    {
        RowVectorXd stdScaling = (centered.cwiseProduct(centered).colwise().sum() / (double) (dataset.rows() - 1)).cwiseSqrt();
        #pragma omp parallel for
        for (int i = 0; i < centered.rows(); ++i)
            centered.row(i) = centered.row(i).cwiseQuotient(stdScaling);
    }

    // Compute the covariance matrix.
    MatrixXd covarianceMatrix = (centered.transpose() * centered) / (double)(dataset.rows() - 1);

    // Compute Singular Value Decomposition
    m_solver.compute(covarianceMatrix);

    // Store results
    m_latent = m_solver.eigenvalues();
    m_explained = m_latent / m_latent.sum();
    m_coefficients = m_solver.eigenvectors();
    m_scores = centered * m_coefficients;
}

template<class Derived>
MatrixXd PrincipalComponentAnalysis::dimensionalityReductionComponents(const MatrixBase<Derived> &dataset, const unsigned int components)
{
    // Compute PCA
    compute(dataset);

    // Get the dataset in the new components dimensional space
    m_components = components;
    return m_scores.rightCols(components);
}

template<typename  Derived>
MatrixXd PrincipalComponentAnalysis::dimensionalityReductionVarianceExplained(const MatrixBase<Derived> &dataset, const double varianceExplained, const unsigned int minComponents, const unsigned int maxComponents)
{
    // Compute PCA
    compute(dataset);

    // Get components to explain at least <varianceExplained>
    m_components = 0;
    double cumsum = 0;
    for (int i = m_explained.size() - 1; i >= 0; --i)
    {
        m_components++;
        cumsum += m_explained(i);
        if (cumsum > varianceExplained)
            break;
    }
    if (minComponents != 0)
        m_components = m_components < minComponents ? minComponents : m_components;
    if (maxComponents != 0)
        m_components = m_components > maxComponents ? maxComponents : m_components;

    // Get the dataset in the new components dimensional space
    return m_scores.rightCols(m_components);
}

template <typename  Derived>
Matrix<typename Derived::Scalar, Dynamic, Dynamic> PrincipalComponentAnalysis::filteringComponents(const MatrixBase<Derived> &dataset, const unsigned int components)
{
    // Compute PCA
    compute(dataset);

    // Rebuild dataset with components
    m_components = components;
    MatrixXd filteredDataset = (m_scores.rightCols(m_components) * m_coefficients.rightCols(m_components).transpose());
    filteredDataset = filteredDataset.rowwise() + m_mu;

    return filteredDataset.cast<Derived::Scalar>();
}

template <typename  Derived>
Matrix<typename Derived::Scalar, Dynamic, Dynamic> PrincipalComponentAnalysis::filteringVarianceExplained(const MatrixBase<Derived> &dataset, const double varianceExplained, const unsigned int minComponents, const unsigned int maxComponents)
{
    // Compute PCA
    compute(dataset);

    // Get components to explain at least <varianceExplained>
    m_components = 0;
    double cumsum = 0;
    for (int i = m_explained.size() - 1; i >= 0; i--)
    {
        m_components++;
        cumsum += m_explained(i);
        if (cumsum > varianceExplained)
            break;
    }
    if (minComponents != 0)
        m_components = m_components < minComponents ? minComponents : m_components;
    if (maxComponents != 0)
        m_components = m_components > maxComponents ? maxComponents : m_components;

    // Rebuild dataset with components
    MatrixXd filteredDataset = m_scores.rightCols(m_components) * m_coefficients.rightCols(m_components).transpose();
    filteredDataset = filteredDataset.rowwise() + m_mu;

    return filteredDataset.cast<typename Derived::Scalar>();
}

#endif


#include "LinearRegression.h"
#include "TrainingType.h"
#include <armadillo>
#include <assert.h>
#include <iostream>

using namespace arma;

LinearRegression::LinearRegression(mat &x, vec &y, double regPara)
    : x{x}, y{y}, trained{false}, regPara{regPara} {
  assert(x.n_rows == y.n_rows);

  // Create bias column and append at the end of  x
  mat bias = ones<mat>(this->ExampleNumber(), 1);
  this->x.insert_cols(0, bias);
}

LinearRegression::~LinearRegression() {}

void LinearRegression::AddData(mat &extraX, vec &extraY) {
  assert(extraX.n_rows == extraY.n_rows);
  // Add 1 because x has a bias column
  assert((extraX.n_cols + 1) == this->x.n_cols);

  this->trained = false;
  // Add Bias column to latest added input
  mat bias = ones<mat>(extraX.n_rows, 1);
  mat inputX = extraX;
  inputX.insert_cols(0, bias);
  this->x.insert_rows(this->x.n_rows, inputX);
  this->y.insert_rows(this->y.n_rows, extraY);
}

void LinearRegression::Train(TrainingType Type, double alpha,
                             unsigned int iters) {
  if (Type == normalEquation) {
    this->NormalEquation();
  } else if (Type == gradientDescent) {
    this->GradientDescent(alpha, iters);
  } else {
    std::cerr << "Invalid training type" << std::endl;
  }
}

void LinearRegression::NormalEquation() {
  mat xtx = (this->x.t() * this->x);
  mat L = eye<mat>(xtx.n_rows, xtx.n_cols);
  L[0] = 0;
  // Check if xtx is full-rank matrix
  if (rank(xtx) == xtx.n_rows || this->regPara > 0) {
    this->theta = pinv(xtx - (this->regPara * L)) * this->x.t() * this->y;
    this->trained = true;
  } else {
    std::cerr << "you have to regularize your data set" << std::endl;
  }
}

uword LinearRegression::ExampleNumber() { return this->x.n_rows; }

double LinearRegression::Predict(vec &x) {
  if (!this->trained) {
    std::cerr << "This model hasn't been trained" << std::endl;
    return 0.0;
  }
  vec bias = vec("1");
  vec input = x;
  input.insert_rows(0, bias);
  return (input.t() * this->theta).eval()(0, 0);
}

vec LinearRegression::CostDerivative() {
  vec deriv = (((this->x * this->theta) - this->y).t() * this->x).t();
  vec thetaWithoutFirst = this->theta;
  thetaWithoutFirst[0] = 0;
  return 1 / (double)this->ExampleNumber() * deriv +
         this->regPara / (double)this->ExampleNumber() * thetaWithoutFirst;
}

double LinearRegression::SelfCost() { return this->Cost(this->x); }

double LinearRegression::Cost(mat &inputX) {
  this->InitializeTheta();
  //--J(Theta) = 1/2m * (X Theta - y)^T (X Theta - y) + lambda theta^2--//
  assert(inputX.n_cols == this->theta.n_rows);
  vec ve = (inputX * this->theta) - this->y;
  vec thetaWithoutFirst = this->theta;
  thetaWithoutFirst[0] = 0;
  return (((float)1 / 2) * this->ExampleNumber() * ve.t() * ve +
          this->regPara * thetaWithoutFirst.t() * thetaWithoutFirst)
      .eval()(0, 0);
}

void LinearRegression::GradientDescent(double alpha, unsigned int iters) {
  this->InitializeTheta();
  for (unsigned int i = 0; i < iters; i++) {
    this->theta = this->theta - (alpha * this->CostDerivative());
  }

  this->trained = true;
}

void LinearRegression::InitializeTheta() {
  if (this->trained != true || this->theta.n_rows != this->x.n_cols) {
    // Initialize Theta
    this->theta = zeros<vec>(this->x.n_cols);
  }
}


#include <Eigen/Core>
#include <iostream>
#include <LBFGS.h>

using Eigen::VectorXd;
using Eigen::MatrixXd;
using namespace LBFGSpp;

double foo(const VectorXd& x, VectorXd& grad)
{
    const int n = x.size();
    VectorXd d(n);
    for(int i = 0; i < n; i++)
        d[i] = i;

    double f = (x - d).squaredNorm();
    grad.noalias() = 2.0 * (x - d);
    return f;
}

int main()
{
    const int n = 10;
    LBFGSParam<double> param;
    LBFGSSolver<double> solver(param);

    VectorXd x = VectorXd::Zero(n);
    double fx;
    int niter = solver.minimize(foo, x, fx);

    std::cout << niter << " iterations" << std::endl;
    std::cout << "x = \n" << x.transpose() << std::endl;
    std::cout << "f(x) = " << fx << std::endl;

    return 0;
}


#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;
}