/*=================================================================
* Transform vertices (n*3) with a 3D affine transform
*
* according to David Legland's matlab toolbox: geom3d
*
* Junjie Cao, 2013, jjcao1231@gmail.com
*
=================================================================*/

#include <mex.h>
#include <Eigen/Dense>
#include <vector>

Eigen::MatrixXd transform_point3d(Eigen::MatrixXd& verts, Eigen::Matrix4d& trans)
{
	int NP  = verts.rows();
	Eigen::MatrixXd res(NP,4);
	res.col(0) = verts.col(0);
	res.col(1) = verts.col(1);
	res.col(2) = verts.col(2);
	res.col(3) = Eigen::VectorXd::Ones(NP,1);
	//std::cout << res << std::endl;

	//std::cout << trans * res.row(0).transpose() << std::endl;
	res = res * trans.transpose();
	//std::cout << res << std::endl;
	res.conservativeResize(NP, 3);

	return res;
}
void mexFunction( int nlhs, mxArray *plhs[], int nrhs, const mxArray*prhs[])
{
	////////////////// parse input
	if ( nrhs != 2)
		mexErrMsgTxt("2 arguments needed!");

	double *verts_, *trans_;
	verts_ = mxGetPr(prhs[0]);
	trans_ = mxGetPr(prhs[1]);
	int nverts = mxGetM(prhs[0]);
	if ( mxGetN(prhs[0]) != 3)
		mexErrMsgTxt("center must be n*3 matrix!");
	if ( mxGetN(prhs[1]) != 4 && mxGetM(prhs[1]) != 4)
		mexErrMsgTxt("vector must be 4*4 matrix!");

	Eigen::MatrixXd verts = Eigen::Map<Eigen::MatrixXd>(verts_, nverts, 3);
	Eigen::Matrix4d trans = Eigen::Map<Eigen::Matrix4d>(trans_);

	/////////////////////////////
	Eigen::MatrixXd newverts = transform_point3d(verts,trans);

	//////////////////////// output
	plhs[0] = mxCreateDoubleMatrix(nverts,3,mxREAL);
	double *newverts_ = mxGetPr(plhs[0]);
	Eigen::Map<Eigen::MatrixXd>(newverts_, nverts, 3) = newverts;
}


#ifndef TESTPSEUDOINVERSESVD_HPP_
#define TESTPSEUDOINVERSESVD_HPP_

#include <cxxtest/TestSuite.h>
#include <Eigen/Dense>

#include <cppmath/matrix/PseudoInverseSVD.hpp>

class TestPseudoInverseSVD: public CxxTest::TestSuite
{
public:
    void test_compute()
    {
        const Eigen::MatrixXd::Index rows = 42;
        const Eigen::MatrixXd::Index cols = 7;

        // Generate system of linear equations
        Eigen::MatrixXd A( rows, cols );
        A.setRandom();
        Eigen::VectorXd x( cols );
        x.setRandom();
        Eigen::VectorXd b = A * x;

        cppmath::PseudoInverseSVD< Eigen::MatrixXd > pinv( A );
        const Eigen::MatrixXd& Ainv = pinv.compute();
        Eigen::VectorXd xs = Ainv * b;
        Eigen::VectorXd diff = x - xs;
        TS_ASSERT_LESS_THAN( diff.squaredNorm(), 1e-6 );
    }

    void test_computeArg()
    {
        const Eigen::MatrixXd::Index rows = 23;
        const Eigen::MatrixXd::Index cols = 23;

        // Generate system of linear equations
        Eigen::MatrixXd A( rows, cols );
        A.setRandom();
        Eigen::VectorXd x( cols );
        x.setRandom();
        Eigen::VectorXd b = A * x;

        cppmath::PseudoInverseSVD< Eigen::MatrixXd > pinv( A );
        Eigen::MatrixXd Ainv;
        pinv.compute( &Ainv );
        Eigen::VectorXd xs = Ainv * b;
        Eigen::VectorXd diff = x - xs;
        TS_ASSERT_LESS_THAN( diff.squaredNorm(), 1e-6 );
    }

    void test_multiply()
    {
        const Eigen::MatrixXd::Index rows = 3;
        const Eigen::MatrixXd::Index cols = 2;

        // Generate system of linear equations
        Eigen::MatrixXd A( rows, cols );
        A.setRandom();
        Eigen::VectorXd x( cols );
        x.setRandom();
        Eigen::VectorXd b = A * x;

        cppmath::PseudoInverseSVD< Eigen::MatrixXd > pinv( A );
        Eigen::VectorXd xs = pinv * b;
        Eigen::VectorXd diff = x - xs;
        TS_ASSERT_LESS_THAN( diff.squaredNorm(), 1e-6 );
    }

    void test_identity1()
    {
        const Eigen::MatrixXd::Index rows = 7;
        const Eigen::MatrixXd::Index cols = 7;

        // Generate system of linear equations
        Eigen::MatrixXd A( rows, cols );
        A.setRandom();

        cppmath::PseudoInverseSVD< Eigen::MatrixXd > pinv( A );
        Eigen::MatrixXd I = pinv * A;
        TS_ASSERT( I.isIdentity() );
    }

    void test_identity2()
    {
        const Eigen::MatrixXd::Index rows = 7;
        const Eigen::MatrixXd::Index cols = 7;

        // Generate system of linear equations
        Eigen::MatrixXd A( rows, cols );
        A.setIdentity();

        cppmath::PseudoInverseSVD< Eigen::MatrixXd > pinv( A );
        const Eigen::MatrixXd& I = pinv.compute();
        TS_ASSERT( I.isIdentity() );
    }
};

#endif  // TESTPSEUDOINVERSESVD_HPP_


#include <Eigen/Dense>

#include <iostream>
#include <algorithm>
#include <cmath>
#include <memory>
#include <stdexcept>
#include <string>
#include <utility>
#include <vector>

#include "Functions.hpp"
#include "Types.h"
#include "NeuralNetwork.h"
#include "MatrixUtils.h"

using Eigen::VectorXd;
using Eigen::MatrixXd;

NeuralNetwork::NeuralNetwork(const std::vector<int>& layout,
    const std::string& funcName, const bool randomSeed) : m_layout(layout) {
    createLayers(randomSeed);
    createActivationFunction(funcName);
}

void NeuralNetwork::createLayers(const bool randomSeed) {
    // create layers. Ignore the first as this is the size of the input.
    m_weights.reserve(m_layout.size()-1);

    for (int i=1; i < m_layout.size(); ++i) {
        // size of matrix is defined by the number of nodes (rows) and the
        // size of the output vector from the previous layer (cols)
        Eigen::MatrixXd weights = Eigen::MatrixXd::Zero(m_layout[i-1], m_layout[i]);
        // initialise matrix with random numbers drawn from Gaussian with
        // zero mean and unit variance
        MatrixUtils::initializeRandomWeights(weights, randomSeed);

        m_weights.push_back(weights);
    }
}

void NeuralNetwork::createActivationFunction(const std::string& funcName) {
    auto functions = FunctionFactory::create(funcName);
    m_activation_func = functions.first;
    m_activation_deriv = functions.second;
}

void NeuralNetwork::train(MatrixXd input, MatrixXd expected,
    int maxIter, double alpha) {

    //weight learning rate by number of examples in batch
    alpha *= (1.0/input.rows());

    for (int i=0; i <= maxIter; ++i) {

        MatrixXd output = feedForward(input);
        auto errors = backPropagate(output, expected);
        updateWeights(errors, alpha);

        if (i % 100 == 0) {
            std::cout << "Epoch " << i << std::endl;
        }
    }

}

MatrixXd NeuralNetwork::feedForward(MatrixXd input) {
    // Remove cached vectors from previous run
    m_zVectors.clear();
    m_activations.clear();

    MatrixXd activation = input;
    m_activations.push_back(activation);

    for (int index=0; index < m_layout.size()-1; ++index) {
        // compute weighted input for this layer
        MatrixXd weights = m_weights[index];
        MatrixXd z = activation * weights;
        // cache result of weight-input combination for backprop
        m_zVectors.push_back(z);
        // compute the activation of function from the weighted input
        activation = z.unaryExpr(m_activation_func);
        // cache activation result of the weight-input combination for backprop
        m_activations.push_back(activation);
    }

    m_activations.pop_back();  // remove last activation

    return activation.transpose();
}

std::vector<MatrixXd> NeuralNetwork::backPropagate(MatrixXd output,
    MatrixXd actual) {

    if (checkIfMatriciesSizeMatch(output, actual)) {
        std::string msg = "Output and actual vectors must match in size.";
        throw std::runtime_error(msg);
    }

    if (checkIfFeedForwardPerformed()) {
        std::string msg = "Backpropagation may only be run after feed forward.";
        throw std::runtime_error(msg);
    }

    std::vector<MatrixXd> errors;

    // compute error for last layer of network
    MatrixXd cost = quadratic_cost_derivative(output, actual);

    MatrixXd sigmaPrime = getSigmaPrime();
    MatrixXd a = getActivation();

    MatrixXd delta = cost.transpose().cwiseProduct(sigmaPrime);
    MatrixXd wError = delta.transpose() * a;

    errors.push_back(wError);

    // compute error in previous layers of network
    for (int index = m_weights.size()-2; index >= 0; --index) {
        MatrixXd weights = m_weights[index+1];
        sigmaPrime = getSigmaPrime();
        a = getActivation();

        delta = (weights * delta.transpose()).cwiseProduct(sigmaPrime.transpose());
        wError = delta * a;

        errors.push_back(wError);
    }

    std::reverse(errors.begin(), errors.end());

    return errors;
}

void NeuralNetwork::updateWeights(const std::vector<Eigen::MatrixXd>& errors,
    double alpha) {

    for (int i=0; i < m_weights.size(); ++i) {
        MatrixXd wError = errors[i].transpose();
        m_weights[i] = m_weights[i] - alpha * wError;
    }
}

Eigen::MatrixXd NeuralNetwork::getSigmaPrime() {
    if (m_zVectors.empty()) {
        std::runtime_error("Z value cache is empty.");
    }

    MatrixXd z = m_zVectors.back();
    m_zVectors.pop_back();  // remove z vector from cache
    return z.unaryExpr(m_activation_deriv);
}

Eigen::MatrixXd NeuralNetwork::getActivation() {
    if (m_activations.empty()) {
        std::runtime_error("Activation cache is empty.");
    }

    MatrixXd a = m_activations.back();
    m_activations.pop_back();
    return a;
}

void NeuralNetwork::printWeights() {
    for (int i =0; i < m_weights.size(); ++i) {
        std::cout << "Layer " << i << "------------------------" << std::endl;
        std::cout << m_weights[i] << std::endl;
    }
    std::cout << "-------------------------------" << std::endl;
}


#include <iostream>
#include <armadillo>

arma::sp_mat generateLinkMatrix(std::size_t nNodes, std::size_t maxEdgesPerNode) {

	arma::sp_mat linkMatrix{nNodes, nNodes};
	for(int i=0; i<nNodes ; ++i){
		std::size_t nLinks = rand() % (maxEdgesPerNode+1);

		std::vector<std::size_t> pos;
		for(int j=0; j<nNodes; ++j){
			pos.push_back(j);
		}
		pos.erase(std::begin(pos) + i);

		for(int j=0; j<nLinks; ++j){
			std::size_t index = rand() % pos.size();
			linkMatrix(pos[index],i) = 1/static_cast<double>(nLinks);
			pos.erase(std::begin(pos)+index);
		}
	}

	return linkMatrix;
};

arma::vec rankSites(const arma::sp_mat& linkMatrix, const float& p) {

	arma::vec v = arma::ones<arma::vec>(linkMatrix.n_cols);
	double epsilon = 10e-10;
	while (true) {
		auto previous = v;
		v = (p/linkMatrix.n_cols) * v + (1-p) * (linkMatrix * v);
		if (all((v-previous) < epsilon)){
			break;
		}
	}

	return v;
}

double lerp(const double& value,
			const double& minIn, double maxIn,
			const double& minOut, const double& maxOut) {
	return minOut + (value + minIn) * (maxOut - minOut)/(maxIn - minIn);

}
void makeGraph(const arma::sp_mat& linkMatrix, const arma::vec& rank) {
	using namespace arma;

	std::cout << "digraph {" << std::endl;
	std::cout << "\tnode [style=\"filled\"]" << std::endl;
	for(int i=0; i< linkMatrix.n_cols; ++i) {
		sp_mat::const_iterator it     = linkMatrix.begin_col(i);
		sp_mat::const_iterator it_end = linkMatrix.end_col(i);
		std::cout << "\t" << i << " [";
		std::cout << "fontsize=" << "\""<< lerp(rank[i], rank.min(), rank.max(), 10, 300)<< "pt\"";
		if(rank[i] == rank.max())
			std::cout << " fillcolor=" << "\"green\"";
		std::cout << "]" << std::endl;
		for(; it != it_end; ++it) {
			std::cout << "\t" << i << "->" << it.row() << std::endl;
		}
	}
	std::cout << "}" << std::endl;
}

int main(int argc, char* argv[]) {
	srand (time(NULL));
	arma::sp_mat A = generateLinkMatrix(atoi(argv[1]), atoi(argv[2]));

	arma::vec v = rankSites(A, atof(argv[3]));

	makeGraph(A,v);
}


/* ! Example file that integrates the Lorenz system, showing how the structs defined in make_dace_compatible_with_odeint.hpp work.
*/

#include <dace/dace_s>
#include <boost/numeric/odeint.hpp>
#include <iostream>

using DACE;
using boost::numeric::odeint;

template <typename vectorType, typename timeType>
void lorenz(const AlgebraicVector< vectorType >& x, AlgebraicVector< vectorType >& dxdt, const timeType& t)
{
    const double sigma = 10.0;
    const double R = 28.0;
    const double b = 8.0 / 3.0;

    dxdt.resize( x.size() );
    dxdt[0] = sigma * ( x[1] - x[0] );
    dxdt[1] = R  * x[0] - x[1] - x[0] * x[2];
    dxdt[2] = -b * x[2] + x[0] * x[1];
}

int main()
{
    /* Initialise DACE */
    DACE::DA::init(8, 3); // 8th order, three variables

    /* Initial condition */
    AlgebraicVector<DA> x({10.0, 5.0, 5.0});
    x += 0.1 * DA::identity(3);

    /* Define stepper type */
    typedef runge_kutta_dopri5< AlgebraicVector<DA>, double ,AlgebraicVector<DA>,
                              double, vector_space_algebra > stepper; // Notice vector_space_algebra!
    int steps = integrate_adaptive( make_controlled<stepper>( 1E-10 , 1E-10 ), lorenz, x ,
                                    0.0, 10.0, 0.1 );
    std::cout << x << std::endl;
    std::cout << "steps: " << steps << std::endl;

    return 0;
}


#include <alglib/graph/directed_graph.h>
#include <alglib/graph/shortest_path/dijkstra.h>

using namespace std;
using namespace alglib::graph;

int main() {

    using graph_type = directed_graph<int, int>;

    // vertex_type = int; edge_attribute_type = int = weight
    directed_graph<int, int> G;

    for(int i = 0; i < 10; i++)
        G.add_vertex(i);

    G.add_edge(1, 3, 4);
    G.add_edge(3, 5, 6);
    G.add_edge(1, 5, 7);

    dijkstra<graph_type> dijk (G, 1);

    cout << dijk.shortest_dist_to (5) << endl;

    vector<int> path;

    dijk.shortest_path_to (5, back_inserter (path));

    cout << "Shortest path is: \n";
    for (int v : path)
        cout << v << "\t";
    cout << endl;
}


#pragma clang diagnostic push
#pragma ide diagnostic ignored "cert-err58-cpp"

#include <boost/test/unit_test.hpp>
#include <boost/test/data/monomorphic.hpp>
#include <boost/test/data/test_case.hpp>
#include <optional>
#include "util/geometry/geometry.hpp"
#include "util/geometry/Polygon.hpp"

using aima::util::geometry::LineSegment;
using aima::util::geometry::Point;
using aima::util::geometry::Polygon;
using namespace boost::unit_test::data;
using boost::unit_test_framework::depends_on;

namespace aima::util::geometry {
    std::ostream& boost_test_print_type( std::ostream& out, const std::optional<Point>& point ) {
        if ( point ) {
            out << *point;
        }
        else {
            out << "()";
        }
        return out;
    }
}

BOOST_AUTO_TEST_SUITE( geometry )

    std::tuple<Point, LineSegment, bool> isOnSegmentData[]{
            {
                    Point{ 1, 1 },
                    LineSegment{{ 0, 0 },
                                { 2, 2 }},
                    true
            },
            {
                    Point{ 0, 0 },
                    LineSegment{{ 0, 0 },
                                { 0, 1 }},
                    true
            },
            {
                    Point{ 0, 0 },
                    LineSegment{{ 1, 1 },
                                { 2, 2 }},
                    false
            }
    };

    BOOST_DATA_TEST_CASE( isOnSegment, make( isOnSegmentData ), point, segment, expected ) {
        BOOST_CHECK_EQUAL( aima::util::geometry::isOnSegment( point, segment ), expected );
    }

    using IntersectTestData = std::tuple<LineSegment, LineSegment, std::optional<Point>>[];

    IntersectTestData lineIntersectData{
            {
                    LineSegment{{ 0,  0 },
                                { .5, .5 }},
                    LineSegment{{ 0, 2 },
                                { 2, 0 }},
                    Point{ 1, 1 }
            },
            {
                    LineSegment{{ 1, 0 },
                                { 3, 2 }},
                    LineSegment{{ 1, 2 },
                                { 3, 0 }},
                    Point{ 2, 1 }
            },
            {
                    LineSegment{{ 0, 0 },
                                { 0, 1 }},
                    LineSegment{{ 1, 0 },
                                { 1, 1 }},
                    std::nullopt
            },
            {
                    LineSegment{{ 0, 0 },
                                { 0, 1 }},
                    LineSegment{{ 0, 3 },
                                { 0, 2 }},
                    std::nullopt
            }
    };

    BOOST_DATA_TEST_CASE( lineIntersect, make( lineIntersectData ), p, q, expected ) {
        BOOST_CHECK_EQUAL( aima::util::geometry::lineIntersect( p, q ), expected );
    }

    IntersectTestData segmentIntersectData{
            {
                    LineSegment{{ 0,  0 },
                                { .5, .5 }},
                    LineSegment{{ 0, 2 },
                                { 2, 0 }},
                    std::nullopt
            },
            {
                    LineSegment{{ 0, 1 },
                                { 1, 1 }},
                    LineSegment{{ 0, 2 },
                                { 1, 0 }},
                    Point{ .5, 1 },
            }
    };

    BOOST_TEST_DECORATOR( *depends_on( "geometry/lineIntersect" ))
    BOOST_TEST_DECORATOR( *depends_on( "geometry/isOnSegment" ))

    BOOST_DATA_TEST_CASE( segmentIntersect, make( segmentIntersectData ), p, q, expected ) {
        BOOST_CHECK_EQUAL( aima::util::geometry::segmentIntersect( p, q ), expected );
    }

    std::tuple<Polygon, LineSegment, bool> isCutByData[]{
            {
                    Polygon{{{ 1, 1 }, { 1, 0 }, { 0, 0 }, { 0, 1 }}},
                    LineSegment{{ 1, 0 },
                                { 0, 2 }},
                    true
            }
    };

    BOOST_TEST_DECORATOR( *depends_on( "geometry/segmentIntersect" ))

    BOOST_DATA_TEST_CASE( isCutBy, make( isCutByData ), polygon, lineSegment, expected ) {
        BOOST_CHECK_EQUAL( aima::util::geometry::isCutBy( polygon, lineSegment ), expected );
    }

BOOST_AUTO_TEST_SUITE_END() // NOLINT(cert-err58-cpp)

#pragma clang diagnostic pop


#include <iostream>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/kruskal_min_spanning_tree.hpp>
#include <boost/graph/dijkstra_shortest_paths.hpp>
#include <algorithm>

typedef boost::adjacency_list<
    boost::vecS,
    boost::vecS,
    boost::undirectedS,
    boost::no_property,
    boost::property<boost::edge_weight_t, int>>
    Graph;
typedef boost::graph_traits<Graph>::edge_descriptor Edge;

void testcase()
{
  int n, m;
  std::cin >> n >> m;
  assert(n >= 0 && n <= 100);
  assert(m >= 0 && m <= n * (n - 1) / 2);

  Graph G(n);
  for (int i = 0; i < m; i++)
  {
    int a, b, w;
    std::cin >> a >> b >> w;
    boost::add_edge(a, b, w, G);
  }

  std::vector<Edge> spanning_tree;
  boost::kruskal_minimum_spanning_tree(G, std::back_inserter(spanning_tree));

  auto weights = boost::get(boost::edge_weight, G);
  int sum_of_weights = 0;
  for (auto edge : spanning_tree)
  {
    sum_of_weights += weights[edge];
  }

  std::vector<int> distances(n);
  boost::dijkstra_shortest_paths(G, boost::vertex(0, G), boost::distance_map(boost::make_iterator_property_map(distances.begin(), boost::get(boost::vertex_index, G))));

  std::cout
      << sum_of_weights
      << " "
      << *std::max_element(distances.begin(), distances.end())
      << "\n";
}

int main()
{
  std::ios_base::sync_with_stdio(false);

  int t;
  std::cin >> t;
  for (int i = 0; i < t; i++)
  {
    testcase();
  }

  return 0;
}


#define _USE_MATH_DEFINES
#include "gmmtree.h"
#include <Eigen/Eigenvalues>
#include <cmath>

using namespace probreg;

namespace {
static const Float eps = 1.0e-15;

Float gaussianPdf(const Vector3& x, const Vector3& mu, const Matrix3& cov) {
    const Vector3 d = x - mu;
    const Float det = cov.determinant();
    if (det < eps) return 0.0;
    const Float c = 1.0 / (std::pow(det, 0.5) * std::pow(2.0 * M_PI, x.size() * 0.5));
    const Float ep = -0.5 * d.transpose() * cov.inverse() * d;
    return c * std::exp(ep);
}

Float logLikelihood(const NodeParamArray& nodes, const Matrix3X& points, Integer j0, Integer jn) {
    Float q = 0.0;
    for (Integer i = 0; i < points.cols(); ++i) {
        Float tmp = 0.0;
        for (Integer j = j0; j < jn; ++j) {
            if (std::get<0>(nodes[j]) < eps) continue;
            tmp += std::get<0>(nodes[j]) *
                   gaussianPdf(points.col(i), std::get<1>(nodes[j]), std::get<2>(nodes[j]));
        }
        q += std::log(std::max(tmp, eps));
    }
    return q;
}

Float complexity(const Matrix3& cov) {
    Eigen::SelfAdjointEigenSolver<Matrix3> es(cov);
    auto lmds = es.eigenvalues();
    std::sort(lmds.data(), lmds.data() + lmds.size(), std::greater<Float>());
    return lmds[2] / lmds.sum();
}

Integer child(Integer j) { return (j + 1) * N_NODE; }

Integer level(Integer l) { return N_NODE * (std::pow(N_NODE, l) - 1) / (N_NODE - 1); }

void accumulate(NodeParam& moments, Float gamma, const Vector& z) {
    if (gamma < eps) return;
    std::get<0>(moments) += gamma;
    std::get<1>(moments) += gamma * z;
    std::get<2>(moments) += gamma * z * z.transpose();
}

NodeParam mlEstimator(const NodeParam& moments, Integer n_points, Float lambda_d) {
    NodeParam node;
    std::get<0>(node) = std::get<0>(moments) / n_points;
    if (std::get<0>(moments) < lambda_d) {
        std::get<0>(node) = 0;
        std::get<1>(node).fill(0.0);
        std::get<2>(node) = Matrix3::Identity();
    } else {
        std::get<1>(node) = std::get<1>(moments) / std::get<0>(moments);
        std::get<2>(node) =
            std::get<2>(moments) / std::get<0>(moments) - std::get<1>(node) * std::get<1>(node).transpose();
    }
    return node;
}

}  // namespace

NodeParamArray probreg::buildGmmTree(const Matrix3X& points,
                                     Integer max_tree_level,
                                     Float lambda_s,
                                     Float lambda_d) {
    const Integer n_total = N_NODE * (1 - std::pow(N_NODE, max_tree_level)) / (1 - N_NODE);
    NodeParamArray nodes(n_total);
    auto idxs = (n_total * Vector::Random(n_total)).array().abs().cast<Integer>();
    Float sig2 = 0.0;
    for (Integer i = 0; i < points.cols(); ++i) {
        sig2 += (points.colwise() - points.col(i)).colwise().squaredNorm().sum();
    }
    sig2 /= points.cols() * points.cols() * points.rows() * N_NODE;
    for (Integer j = 0; j < n_total; ++j) {
        std::get<0>(nodes[j]) = 1.0 / N_NODE;
        std::get<1>(nodes[j]) = points.col(idxs[j]);
        std::get<2>(nodes[j]) = Matrix3::Identity() * sig2;
    }
    VectorXi parent_idx = -VectorXi::Ones(points.cols());
    VectorXi current_idx = VectorXi::Zero(points.cols());

    for (Integer l = 0; l < max_tree_level; ++l) {
        Float prev_q = 0.0;
        while (true) {
            const NodeParamArray params =
                gmmTreeEstep(points, nodes, parent_idx, current_idx, max_tree_level);
            gmmTreeMstep(params, l, nodes, points.cols(), lambda_d);
            const Float q = logLikelihood(nodes, points, level(l), level(l + 1));
            if (std::abs(q - prev_q) < lambda_s) {
                break;
            }
            prev_q = q;
        }
        parent_idx = current_idx;
    }
    return nodes;
}

NodeParamArray probreg::gmmTreeEstep(const Matrix3X& points,
                                     const NodeParamArray& nodes,
                                     const VectorXi& parent_idx,
                                     VectorXi& current_idx,
                                     Integer max_tree_level) {
    const Integer n_total = N_NODE * (1 - std::pow(N_NODE, max_tree_level)) / (1 - N_NODE);
    NodeParamArray moments(n_total);
    for (Integer j = 0; j < n_total; ++j) {
        std::get<0>(moments[j]) = 0.0;
        std::get<1>(moments[j]).fill(0.0);
        std::get<2>(moments[j]).fill(0.0);
    }

    for (Integer i = 0; i < points.cols(); ++i) {
        const Integer j0 = child(parent_idx[i]);
        Vector gamma = Vector::Zero(N_NODE);
        for (Integer j = j0; j < j0 + N_NODE; ++j) {
            gamma[j - j0] = std::get<0>(nodes[j]) *
                            gaussianPdf(points.col(i), std::get<1>(nodes[j]), std::get<2>(nodes[j]));
        }
        const Float den = gamma.sum();
        if (den > eps) {
            gamma /= den;
        }
        else {
            gamma.fill(0.0);
        }
        for (Integer j = j0; j < j0 + N_NODE; ++j) {
            accumulate(moments[j], gamma[j - j0], points.col(i));
        }
        Integer max_j;
        gamma.maxCoeff(&max_j);
        current_idx[i] = j0 + max_j;
    }
    return moments;
}

void probreg::gmmTreeMstep(
    const NodeParamArray& params, Integer l, NodeParamArray& nodes, Integer n_points, Float lambda_d) {
    const Integer lb = level(l);
    const Integer le = level(l + 1);

    for (Integer j = lb; j < le; ++j) {
        nodes[j] = mlEstimator(params[j], n_points, lambda_d);
    }
}

NodeParamArray probreg::gmmTreeRegEstep(const Matrix3X& points,
                                        const NodeParamArray& nodes,
                                        Integer max_tree_level,
                                        Float lambda_c) {
    const Integer n_total = N_NODE * (1 - std::pow(N_NODE, max_tree_level)) / (1 - N_NODE);
    NodeParamArray moments(n_total);
    for (Integer j = 0; j < n_total; ++j) {
        std::get<0>(moments[j]) = 0.0;
        std::get<1>(moments[j]).fill(0.0);
        std::get<2>(moments[j]).fill(0.0);
    }

    for (Integer i = 0; i < points.cols(); ++i) {
        Integer search_id = -1;
        Vector gamma = Vector::Zero(N_NODE);
        for (Integer l = 0; l < max_tree_level; ++l) {
            const Integer j0 = child(search_id);
            for (Integer j = j0; j < j0 + N_NODE; ++j) {
                gamma[j - j0] = std::get<0>(nodes[j]) *
                                gaussianPdf(points.col(i), std::get<1>(nodes[j]), std::get<2>(nodes[j]));
            }
            const Float den = gamma.sum();
            if (den > eps) {
                gamma /= den;
            } else {
                gamma.fill(0.0);
            }
            gamma.maxCoeff(&search_id);
            search_id += j0;
            if (complexity(std::get<2>(nodes[search_id])) <= lambda_c) break;
            accumulate(moments[search_id], gamma[search_id - j0], points.col(i));
        }
    }
    return moments;
}


/**
 * Exercise 5 : Read in a gph-file, computes the longest (with respect to the edge weights) shortest path from any vertex to the vertex with index 1.
 *
 * @author FirstSanny
 */

#include <iostream>
#include <iomanip>
#include <fstream>
#include <utility>
#include <vector>
#include <climits>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/dijkstra_shortest_paths.hpp>
#include <boost/graph/named_function_params.hpp>
#include <boost/program_options.hpp>
#include <boost/timer/timer.hpp>
#include "Dijkstra.h"

// Constants
namespace {

const char* FILEEND = ".gph";

}

// declaring print
using std::cout;
using std::endl;
using std::cerr;
using std::string;

// declaring types
namespace po = boost::program_options;
using Graph = boost::adjacency_list<boost::listS, boost::vecS,
  boost::undirectedS, boost::no_property,
  boost::property<boost::edge_weight_t, double>> ;
using VertexDescriptor = boost::graph_traits < Graph >::vertex_descriptor;
using Directions = std::vector<VertexDescriptor >;
using Edge = std::pair<int, int >;
using Edges = std::vector<Edge >;
using Weights = std::vector<double >;

po::variables_map parseCommandLine(po::options_description desc, int argn,
		char* argv[]) {
	desc.add_options()("help", "produce help message")("algo,m",
			po::value<int>(),
			"algorithm to solve shortest Path. \n 1 - libary (boost)\n 2 - own Algo")(
			"input-file", po::value<string>(), "input file");
	po::positional_options_description p;
	p.add("input-file", -1);
	po::variables_map vm;
	po::store(
			po::command_line_parser(argn, argv).options(desc).positional(p).run(),
			vm);
	po::notify(vm);
	return vm;
}

char parseAlgoOption(const po::variables_map& vm, char useOwnAlgo) {
	if (vm.count("algo")) {
		useOwnAlgo = vm["algo"].as<int>();
		useOwnAlgo--;
	}
	if (useOwnAlgo != 1 && useOwnAlgo != 0) {
		cout << "No valid Option was given for Algorithm! Fallback: algo from libary" << endl;
		useOwnAlgo = 0;
	}
	if (useOwnAlgo) {
		cout << "Using own algorithm..." << endl;
	} else {
		cout << "Using algorithm from boost-libary..." << endl;
	}
	return useOwnAlgo;
}

/** Reading in a Graphfile, computes the longest shortest path */
int main(int argn, char *argv[]) {
	boost::timer::auto_cpu_timer t;
	if (argn <= 1) {
		cerr << "ERROR : There was no filename" << endl;
		return 1;
	}

	po::options_description desc("Allowed options");
	po::variables_map vm = parseCommandLine(desc, argn, argv);

	if (vm.count("help")) {
	    cout << desc << "\n";
	    return 1;
	}
	char useOwnAlgo = 0;
	useOwnAlgo = parseAlgoOption(vm, useOwnAlgo);

	std::ifstream fileStream;
	if(vm.count("input-file") == 0){
		cerr << "No input-file was given!" << endl;
		return 1;
	}

	string filename = vm["input-file"].as<string >();
	filename += FILEEND;
	cout << "Going to parse the file " << filename << endl;
	fileStream.open(filename.c_str(), std::ios::in);

	if ( (fileStream.rdstate()) != 0 ){
		std::perror("ERROR : Encoutered Problem opening file");
		return 1;
	}

	string line;

	unsigned int edgeCount;
	unsigned int vertexCount;

	if(std::getline(fileStream, line)){
		sscanf(line.c_str(), "%d %d", &vertexCount, &edgeCount);
		cout << "Vertexcount: " << vertexCount << endl;
		cout << "Edgecount: " << edgeCount << endl;
		line.clear();
		vertexCount++;
	} else {
		cerr << "ERROR : File was empty" << endl;
		return 1;
	}

	Edges* edges = new Edges(edgeCount);
	Weights* weights = new Weights(edgeCount);

	cout << "Reading edges..." << endl;
	while (getline(fileStream, line)) {
		int start;
		int end;
		double weight;
		int count = sscanf(line.c_str(), "%d %d %lf", &start, &end, &weight);
		if (count != 3) {
			line.clear();
			continue;
		}
		edges->push_back(std::make_pair(start, end));
		weights->push_back(weight);
		line.clear();
	}

	Weights weightsForShortestpath;

	if(useOwnAlgo){
		cout << "Compute shortest paths via Dijkstra(own)..." << endl;
		Dijkstra* d = new Dijkstra();
		weightsForShortestpath = d->dijkstra(vertexCount, *edges, *weights, 1);
		delete d;
	} else {

		cout << "Creating graph..." << endl;
		Graph g{edges->begin(), edges->end(), weights->begin(), vertexCount};

		Directions* directions = new std::vector<VertexDescriptor>(vertexCount);
		cout << "Compute shortest paths via Dijkstra(boost)..." << endl;
		Weights* weightMapPointer = new Weights(vertexCount);
		boost::dijkstra_shortest_paths(g, 1,
			boost::predecessor_map(//
					boost::make_iterator_property_map(directions->begin(), boost::get(boost::vertex_index, g))) //
				.distance_map(//
					boost::make_iterator_property_map(weightMapPointer->begin(), boost::get(boost::vertex_index, g))));
		weightsForShortestpath = *weightMapPointer;
		delete weightMapPointer;
		delete directions;
	}

	cout << "Search longest shortest path..." << endl;
	int vertex = 1;
	double distance = 0.;

	for(unsigned int i=2; i<vertexCount; i++){
		if(weightsForShortestpath[i]>distance){
			distance = weightsForShortestpath[i];
			vertex = i;
		}
	}

	cout << "RESULT VERTEX " << vertex << endl;
	cout << "RESULT DIST " << std::fixed << std::setprecision(2) << distance << endl;

	delete edges;
	delete weights;

	return 0;
}


/* Boost test/pow.cpp
 * test the pow function
 *
 * Copyright 2002-2003 Guillaume Melquiond
 *
 * Distributed under the Boost Software License, Version 1.0.
 * (See accompanying file LICENSE_1_0.txt or
 * copy at http://www.boost.org/LICENSE_1_0.txt)
 */

#include <boost/numeric/interval.hpp>
#include <boost/core/lightweight_test.hpp>
#include "bugs.hpp"

bool test_pow(double al, double au, double bl, double bu, int p) {
  typedef boost::numeric::interval<double> I;
  I b = pow(I(al, au), p);
  return b.lower() == bl && b.upper() == bu;
}

int main() {
  BOOST_TEST(test_pow(2, 3, 8, 27, 3));
  BOOST_TEST(test_pow(2, 3, 16, 81, 4));
  BOOST_TEST(test_pow(-3, 2, -27, 8, 3));
  BOOST_TEST(test_pow(-3, 2, 0, 81, 4));
  BOOST_TEST(test_pow(-3, -2, -27, -8, 3));
  BOOST_TEST(test_pow(-3, -2, 16, 81, 4));

  BOOST_TEST(test_pow(2, 4, 1./64, 1./8, -3));
  BOOST_TEST(test_pow(2, 4, 1./256, 1./16, -4));
  BOOST_TEST(test_pow(-4, -2, -1./8, -1./64, -3));
  BOOST_TEST(test_pow(-4, -2, 1./256, 1./16, -4));

  BOOST_TEST(test_pow(2, 3, 1, 1, 0));
  BOOST_TEST(test_pow(-3, 2, 1, 1, 0));
  BOOST_TEST(test_pow(-3, -2, 1, 1, 0));

# ifdef BOOST_BORLANDC
  ::detail::ignore_warnings();
# endif
  return boost::report_errors();
}


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

TEST(AgradRev, atan2_var_var) {
  AVAR a = 1.2;
  AVAR b = 3.9;
  AVAR f = atan2(a, b);
  EXPECT_FLOAT_EQ(atan2(1.2, 3.9), f.val());

  AVEC x = createAVEC(a, b);
  VEC g;
  f.grad(x, g);
  EXPECT_FLOAT_EQ(3.9 / (1.2 * 1.2 + 3.9 * 3.9), g[0]);
  EXPECT_FLOAT_EQ(-1.2 / (1.2 * 1.2 + 3.9 * 3.9), g[1]);
}

TEST(AgradRev, atan2_dvd) {
  AVAR sigma = 1;
  AVEC x = createAVEC(sigma);
  AVAR f = atan2(1.0, sigma) / 3.14;
  VEC g;
  f.grad(x, g);

  AVAR sigma1 = 1;
  AVEC x1 = createAVEC(sigma1);
  AVAR f1 = atan2(1.0, sigma1);
  VEC g1;
  f1.grad(x1, g1);

  EXPECT_FLOAT_EQ(3.14 * g[0], g1[0]);
}
TEST(AgradRev, atan2_var_var__integration) {
  double c = 5.0;
  AVAR a = 1.2;
  AVAR b = 3.9;
  AVAR f = atan2(a, b) * c;
  EXPECT_FLOAT_EQ(atan2(1.2, 3.9) * c, f.val());

  AVEC x = createAVEC(a, b);
  VEC g;
  f.grad(x, g);
  EXPECT_FLOAT_EQ(3.9 / (1.2 * 1.2 + 3.9 * 3.9) * c, g[0]);
  EXPECT_FLOAT_EQ(-1.2 / (1.2 * 1.2 + 3.9 * 3.9) * c, g[1]);
}

TEST(AgradRev, atan2_var_double) {
  AVAR a = 1.2;

  double b = 3.9;
  AVAR f = atan2(a, b);
  EXPECT_FLOAT_EQ(atan2(1.2, 3.9), f.val());

  AVEC x = createAVEC(a);
  VEC g;
  f.grad(x, g);
  EXPECT_FLOAT_EQ(3.9 / (1.2 * 1.2 + 3.9 * 3.9), g[0]);
}

TEST(AgradRev, atan2_double_var) {
  double a = 1.2;
  AVAR b = 3.9;
  AVAR f = atan2(a, b);
  EXPECT_FLOAT_EQ(atan2(1.2, 3.9), f.val());

  AVEC x = createAVEC(b);
  VEC g;
  f.grad(x, g);
  EXPECT_FLOAT_EQ(-1.2 / (1.2 * 1.2 + 3.9 * 3.9), g[0]);
}

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

TEST(AgradRev, atan2_nan) {
  atan2_fun atan2_;
  test_nan(atan2_, 3.0, 5.0, false, true);
}

TEST(AgradRev, check_varis_on_stack) {
  AVAR a = 1.2;
  AVAR b = 3.9;
  test::check_varis_on_stack(stan::math::atan2(a, b));
  test::check_varis_on_stack(stan::math::atan2(a, 3.9));
  test::check_varis_on_stack(stan::math::atan2(1.2, b));
}


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

/* Authors: Hye-Jong KIM */

#ifndef OMMATH_HPP_
#define OMMATH_HPP_

#include "../../include/open_manipulator/OMDebug.hpp"

#include <Eigen.h>        // Calls main Eigen matrix class library
#include <Eigen/LU>       // Calls inverse, determinant, LU decomp., etc.

#include <math.h>

#define DEG2RAD (M_PI / 180.0)
#define RAD2DEG (180.0 / M_PI)

class OMMath
{
 public:
  OMMath(){}
  ~OMMath(){}

  float sign(float number)
  {
    if (num >= 0.0)
    {
      return 1.0;
    }
    else
    {
      return -1.0;
    }
  }

  Eigen::Vector3f makeEigenVector3(float v1, float v2, float v3)
  {
    Eigen::Vector3f temp;
    temp << v1, v2, v3;
    return temp;
  }

  Eigen::Matrix3f makeEigenMatrix3(float m11, float m12, float m13, float m21, float m22, float m23, float m31, float m32, float m33)
  {
    Eigen::Matrix3f temp;
    temp << m11, m12, m13, m21, m22, m23, m31, m32, m33;
    return temp;
  }

  Eigen::Matrix3f makeRotationMatrix(float roll, float pitch, float yaw)
  {
    Eigen::Matrix3f rotation_matrix;
    Eigen::Matrix3f roll_matrix;
    Eigen::Matrix3f pitch_matrix;
    Eigen::Matrix3f yaw_matrix;

    roll_matrix  << 1.000,  0.000,     0.000,
                    0.000,  cos(roll), sin(roll),
                    0.000, -sin(roll), cos(roll);

    pitch_matrix << cos(pitch),  0.000, sin(pitch),
                    0.000,       1.000, 0.000,
                    -sin(pitch), 0.000, cos(pitch);

    yaw_matrix   << cos(yaw), -sin(yaw), 0.000,
                    sin(yaw), cos(yaw),  0.000,
                    0.000,    0.000,     1.000;

    rotation_matrix = roll_matrix * pitch_matrix * yaw_matrix;

    return rotation_matrix;
  }

  Eigen::Matrix3f makeRotationMatrix(Eigen::Vector3f rotation_vector)
  {
    Eigen::Matrix3f rotation_matrix;
    rotation_matrix = makeRotationMatrix(rotation_vector(0), rotation_vector(1), rotation_vector(2));
    return rotation_matrix;
  }

  Eigen::Vector3f makeRotationVector(Eigen::Matrix3f rotation_matrix)
  {
    Eigen::Matrix3f R = rotation_matrix;
    Eigen::Vector3f l = Eigen::Vector3f::Zero();
    Eigen::Vector3f rotation_vector = Eigen::Vector3f::Zero();

    float theta = 0.0;
    float diag  = 0.0;
    bool diagonal_matrix = true;

    l << R(2,1) - R(1,2),
         R(0,2) - R(2,0),
         R(1,0) - R(0,1);
    theta = atan2(l.norm(), R(0,0) + R(1,1) + R(2,2) - 1);
    diag  = R(0,0) + R(1,1) + R(2,2);

    for (int8_t i = 0; i < 3; i++)
    {
      for (int8_t j = 0; j < 3; j++)
      {
        if (i != j)
        {
          if (R(i, j) != 0)
          {
            diagonal_matrix = false;
          }
        }
      }
    }
    if (R == Eigen::Matrix3f::Identity())
    {
      rotation_vector = Eigen::Vector3f::Zero();
    }
    else if (diagonal_matrix == true)
    {
      rotation_vector << R(0,0) + 1, R(1,1) + 1, R(2,2) + 1;
      rotation_vector = rotation_vector * M_PI_2;
    }
    else
    {
      rotation_vector = theta * (l / l.norm());
    }
    return rotation_vector;
  }

  Eigen::Matrix3f skewSymmetricMatrix(Eigen::Vector3f v)
  {
    Eigen::Matrix3f skew_symmetric_matrix = Eigen::Matrix3f::Zero();

    skew_symmetric_matrix <<    0,  -v(2),  v(1),
                            v(2),      0, -v(0),
                            -v(1),   v(0),     0;

    return skew_symmetric_matrix;
  }

  Eigen::Matrix3f rodriguesRotationMatrix(Eigen::Vector3f axis, float angle)
  {
    Eigen::Matrix3f skew_symmetric_matrix = Eigen::Matrix3f::Zero();
    Eigen::Matrix3f rotation_matrix = Eigen::Matrix3f::Zero();
    Eigen::Matrix3f Identity_matrix = Eigen::Matrix3f::Identity();

    skew_symmetric_matrix = skewSymmetricMatrix(axis);
    rotation_matrix = Identity_matrix +
                      skew_symmetric_matrix * sin(angle) +
                      skew_symmetric_matrix * skew_symmetric_matrix * (1 - cos(angle));

    return rotation_matrix;
  }

  Eigen::Vector3f differentialPosition(Eigen::Vector3f desired_position, Eigen::Vector3f present_position)
  {
    Eigen::Vector3f differential_position;
    differential_position = desired_position - present_position;

    return differential_position;
  }

  Eigen::Vector3f differentialOrientation(Eigen::Matrix3f desired_orientation, Eigen::Matrix3f present_orientation)
  {
    Eigen::Vector3f differential_orientation;
    differential_orientation = present_orientation * makeRotationVector(present_orientation.transpose() * desired_orientation);

    return differential_orientation;
  }

  Eigen::VectorXf differentialPose(Eigen::Vector3f desired_position, Eigen::Vector3f present_position, Eigen::Matrix3f desired_orientation, Eigen::Matrix3f present_orientation)
  {
    Eigen::Vector3f differential_position;
    Eigen::Vector3f differential_orientation;
    Eigen::VectorXf  differential_pose(6);

    differential_position = differentialPosition(desired_position, present_position);
    differential_orientation = differentialOrientation(desired_orientation, present_orientation)
    differential_pose << differential_position(0),
                         differential_position(1),
                         differential_position(2),
                         differential_orientation(0),
                         differential_orientation(1),
                         differential_orientation(2);
    return differential_pose;
  }
};

#endif // OMMATH_HPP_


/*
 * This is part of the fl library, a C++ Bayesian filtering library
 * (https://github.com/filtering-library)
 *
 * Copyright (c) 2015 Max Planck Society,
 * 				 Autonomous Motion Department,
 * 			     Institute for Intelligent Systems
 *
 * This Source Code Form is subject to the terms of the MIT License (MIT).
 * A copy of the license can be found in the LICENSE file distributed with this
 * source code.
 */

/**
 * \file linear_algebra.hpp
 * \date January 2015
 * \author Jan Issac (jan.issac@gmail.com)
 * \author Manuel Wuthrich (manuel.wuthrich@gmail.com)
 */

#pragma once

#include <Eigen/Dense>

#include <cmath>
#include <vector>

namespace fl
{
/**
 * \ingroup linear_algebra
 *
 * \brief Blockweise matrix inversion using the Sherman-Morrision-Woodbury
 * indentity given that \f$\Sigma^{-1}_{aa}\f$ of
 *
 * \f$
 *  \begin{pmatrix} \Sigma_{aa} & \Sigma_{ab} \\
 *                  \Sigma_{ba} & \Sigma_{bb} \end{pmatrix}^{-1}
 * \f$
 *
 * is already available.
 *
 * \f$
 *  \begin{pmatrix} \Sigma_{aa} & \Sigma_{ab} \\
 *                  \Sigma_{ba} & \Sigma_{bb} \end{pmatrix}^{-1} =
 * \begin{pmatrix} \Lambda_{aa} & \Lambda_{ab} \\
 *                 \Lambda_{ba} & \Lambda_{bb} \end{pmatrix} = \Lambda
 * \f$
 *
 * \param [in]  A_inv   \f$ \Sigma^{-1}_{aa} \f$
 * \param [in]  B       \f$ \Sigma_{ab} \f$
 * \param [in]  C       \f$ \Sigma_{ba} \f$
 * \param [in]  D       \f$ \Sigma_{bb} \f$
 * \param [out] L_A     \f$ \Lambda_{aa} \f$
 * \param [out] L_B     \f$ \Lambda_{ab} \f$
 * \param [out] L_C     \f$ \Lambda_{ba} \f$
 * \param [out] L_D     \f$ \Lambda_{bb} \f$
 */
template <typename MatrixAInv,
          typename MatrixB,
          typename MatrixC,
          typename MatrixD,
          typename MatrixLA,
          typename MatrixLB,
          typename MatrixLC,
          typename MatrixLD>
inline void smw_inverse(const MatrixAInv& A_inv,
                        const MatrixB& B,
                        const MatrixC& C,
                        const MatrixD& D,
                        MatrixLA& L_A,
                        MatrixLB& L_B,
                        MatrixLC& L_C,
                        MatrixLD& L_D)
{
    auto&& AinvB = (A_inv * B).eval();

    L_D = (D - C * AinvB).inverse();
    L_C = -(L_D * C * A_inv).eval();
    L_A = A_inv - AinvB * L_C;
    L_B = L_C.transpose();
}

/**
 * \ingroup linear_algebra
 *
 * \brief Blockweise matrix inversion using the Sherman-Morrision-Woodbury
 * indentity given that \f$\Sigma^{-1}_{aa}\f$ of
 *
 * \f$
 *  \begin{pmatrix} \Sigma_{aa} & \Sigma_{ab} \\
 *                  \Sigma_{ba} & \Sigma_{bb} \end{pmatrix}^{-1}
 * \f$
 *
 * is already available.
 *
 * \f$
 *  \begin{pmatrix} \Sigma_{aa} & \Sigma_{ab} \\
 *                  \Sigma_{ba} & \Sigma_{bb} \end{pmatrix}^{-1} =
 * \begin{pmatrix} \Lambda_{aa} & \Lambda_{ab} \\
 *                 \Lambda_{ba} & \Lambda_{bb} \end{pmatrix} = \Lambda
 * \f$
 *
 * \param [in]  A_inv   \f$ \Sigma^{-1}_{aa} \f$
 * \param [in]  B       \f$ \Sigma_{ab} \f$
 * \param [in]  C       \f$ \Sigma_{ba} \f$
 * \param [in]  D       \f$ \Sigma_{bb} \f$
 * \param [out] L_A     \f$ \Lambda_{aa} \f$
 * \param [out] L_B     \f$ \Lambda_{ab} \f$
 * \param [out] L_C     \f$ \Lambda_{ba} \f$
 * \param [out] L_D     \f$ \Lambda_{bb} \f$
 * \param [out] L       \f$ \Lambda \f$
 */
template <typename MatrixAInv,
          typename MatrixB,
          typename MatrixC,
          typename MatrixD,
          typename MatrixLA,
          typename MatrixLB,
          typename MatrixLC,
          typename MatrixLD,
          typename ResultMatrix>
inline void smw_inverse(const MatrixAInv& A_inv,
                        const MatrixB& B,
                        const MatrixC& C,
                        const MatrixD& D,
                        MatrixLA& L_A,
                        MatrixLB& L_B,
                        MatrixLC& L_C,
                        MatrixLD& L_D,
                        ResultMatrix& L)
{
    smw_inverse(A_inv, B, C, D, L_A, L_B, L_C, L_D);

    L.resize(L_A.rows() + L_C.rows(), L_A.cols() + L_B.cols());

    L.block(0, 0, L_A.rows(), L_A.cols()) = L_A;
    L.block(0, L_A.cols(), L_B.rows(), L_B.cols()) = L_B;
    L.block(L_A.rows(), 0, L_C.rows(), L_C.cols()) = L_C;
    L.block(L_A.rows(), L_A.cols(), L_D.rows(), L_D.cols()) = L_D;
}

/**
 * \ingroup linear_algebra
 *
 * \brief Blockweise matrix inversion using the Sherman-Morrision-Woodbury
 * indentity given that \f$\Sigma^{-1}_{aa}\f$ of
 *
 * \f$
 *  \begin{pmatrix} \Sigma_{aa} & \Sigma_{ab} \\
 *                  \Sigma_{ba} & \Sigma_{bb} \end{pmatrix}^{-1}
 * \f$
 *
 * is already available.
 *
 * \f$
 *  \begin{pmatrix} \Sigma_{aa} & \Sigma_{ab} \\
 *                  \Sigma_{ba} & \Sigma_{bb} \end{pmatrix}^{-1} =
 * \begin{pmatrix} \Lambda_{aa} & \Lambda_{ab} \\
 *                 \Lambda_{ba} & \Lambda_{bb} \end{pmatrix} = \Lambda
 * \f$
 *
 * \param [in]  A_inv   \f$ \Sigma^{-1}_{aa} \f$
 * \param [in]  B       \f$ \Sigma_{ab} \f$
 * \param [in]  C       \f$ \Sigma_{ba} \f$
 * \param [in]  D       \f$ \Sigma_{bb} \f$
 * \param [out] L       \f$ \Lambda \f$
 */
template <typename MatrixAInv,
          typename MatrixB,
          typename MatrixC,
          typename MatrixD,
          typename ResultMatrix>
inline void smw_inverse(const MatrixAInv& A_inv,
                        const MatrixB& B,
                        const MatrixC& C,
                        const MatrixD& D,
                        ResultMatrix& L)
{
    MatrixAInv L_A;
    MatrixB L_B;
    MatrixC L_C;
    MatrixD L_D;

    smw_inverse(A_inv, B, C, D, L_A, L_B, L_C, L_D, L);
}

///**
// * \ingroup linear_algebra
// *
// * \brief Normalizes the values of input vector such that their sum is equal
// to
// * the specified \c sum. For instance, any convex combination requires that
// the
// * weights of the weighted sum sums up to 1.
// */
// template <typename T>
// inline std::vector<T> normalize(const std::vector<T>& input, T sum)
//{
//    T old_sum = 0;
//    for(size_t i = 0; i < input.size(); i++)
//    {
//        old_sum += input[i];
//    }
//    T factor = sum/old_sum;

//    std::vector<T> output(input.size());
//    for(size_t i = 0; i < input.size(); i++)
//    {
//        output[i] = factor*input[i];
//    }

//    return output;
//}

/**
 * \ingroup linear_algebra
 * \brief Constructs the quaternion matrix for the specified quaternion vetcor
 *
 * \param q_xyzw  Quaternion vector
 *
 * \return Matrix representation of the quaternion vector
 */
inline Eigen::Matrix<double, 4, 3> quaternion_matrix(
    const Eigen::Matrix<double, 4, 1>& q_xyzw)
{
    Eigen::Matrix<double, 4, 3> Q;
    Q << q_xyzw(3), q_xyzw(2), -q_xyzw(1), -q_xyzw(2), q_xyzw(3), q_xyzw(0),
        q_xyzw(1), -q_xyzw(0), q_xyzw(3), -q_xyzw(0), -q_xyzw(1), -q_xyzw(2);

    return 0.5 * Q;
}

/**
 * \ingroup linear_algebra
 * \todo DOC
 * \todo Implement numerically robust version and test it
 */
template <typename RegularMatrix, typename SquareRootMatrix>
void square_root(const RegularMatrix& regular_matrix,
                 SquareRootMatrix& square_root)
{
    square_root = regular_matrix.llt().matrixL();

//    typedef Eigen::Matrix<typename RegularMatrix::Scalar,
//                          RegularMatrix::SizeAtCompileTime,
//                          1> Vector;

//    Eigen::LDLT<RegularMatrix> ldlt;
//    ldlt.compute(regular_matrix);
//    Vector D_sqrt = ldlt.vectorD();
//    for (int i = 0; i < D_sqrt.rows(); ++i)
//    {
//        D_sqrt(i) = std::sqrt(std::fabs(D_sqrt(i)));
//    }

//    return ldlt.transpositionsP().transpose() * (Matrix)ldlt.matrixL() *
//           D_sqrt.asDiagonal();
}

/**
 * \ingroup linear_algebra
 * \todo DOC
 */
template <typename RegularMatrix>
void sqrt_diagonal(const RegularMatrix& regular_matrix,
                   RegularMatrix& square_root)
{
    square_root = regular_matrix;
    for (size_t i = 0; i < square_root.rows(); ++i)
    {
        square_root(i, i) = std::sqrt(square_root(i, i));
    }
}

/**
 * \ingroup linear_algebra
 * \brief Computes the square root of each element of the regular_matrix
 */
template <typename RegularMatrix, typename SquareRootVector>
void sqrt_diagonal_vector(const RegularMatrix& diagonal_vector,
                          SquareRootVector& square_root)
{
    square_root = diagonal_vector;
    for (size_t i = 0; i < square_root.rows(); ++i)
    {
        square_root(i, 0) = std::sqrt(square_root(i, 0));
    }
}

/**
 * \ingroup linear_algebra
 * \brief Inverts a diagonal matrix represented by a vector. The result is again
 *        a vector of the inverted diagonal elements.
 */
template <typename SrcDiagonalMatrix, typename DestDiagonalMatrix>
void invert_diagonal_vector(const SrcDiagonalMatrix& diagonal,
                            DestDiagonalMatrix& diagonal_inverse)
{
    diagonal_inverse = diagonal.array().cwiseInverse();

    //    diagonal_inverse.resize(diagonal.size());
    //    for (int i = 0; i < diagonal.size(); ++i)
    //    {
    //        diagonal_inverse(i) = 1./diagonal(i);
    //    }
}

/**
 * \ingroup linear_algebra
 *
 * \brief Computes the Frobenius norm of a given real matrix.
 *
 * The Frobenius norm is given by
 *
 * \f$
 *   \| A \|_F := \sqrt{\sum\limits_{i=1}^m \sum\limits_{j=1}^n |a_{ij} |^2 }
 * \f$
 */
template <typename Derived>
double frobenius_norm(const Eigen::MatrixBase<Derived>& a)
{
    return std::sqrt(a.array().abs().square().sum());
}

/**
 * \ingroup linear_algebra
 * \brief Checks whether two matrices of the same size are similiar w.r.t. some
 * \f$\epsilon\f$.
 *
 * The similarity check is performed based on the Frobenius Norm
 * (\ref frobenius_norm):
 *
 * For any real matrix set \f$M \subseteq \mathbb{R}^{m\times n} \f$
 *
 * \f$ \text{are_similar}: (M \times M) \mapsto \{0, 1\}\f$
 *
 * \f$ \text{are_similar}(a, b)
 *     := \mathbf{1}_{[0, \epsilon)}\left(\| a - b \|_F\right)\f$
 * with \f$ a, b \in M\f$ and the indicator function
 *
 * \f$ \mathbf{1}_{[0, \epsilon)}(x) :=
 *     \begin{cases} 1 & x \in [0, \epsilon)
 *                  \\ 0, & \text{otherwise} \end{cases}\f$
 */
// template <typename DerivedA, typename DerivedB>
// bool are_similar(const Eigen::MatrixBase<DerivedA>& a,
//                 const Eigen::MatrixBase<DerivedB>& b,
//                 const double epsilon = 1.e-6)
//{
//    return frobenius_norm(a - b) < epsilon;
//}

template <typename DerivedA, typename DerivedB>
bool are_similar(const Eigen::MatrixBase<DerivedA>& a,
                 const Eigen::MatrixBase<DerivedB>& b,
                 const double epsilon = 1.e-6)
{
    // return (a - b).cwiseAbs().maxCoeff() < epsilon;
    return frobenius_norm(a - b) < epsilon;
}

/**
 * \ingroup linear_algebra
 * \brief Robust decomposition of M = L*L^T for positive semidefinite matrices
 */
template <typename Scalar, int Size>
Eigen::Matrix<Scalar, Size, Size> matrix_sqrt(
    Eigen::Matrix<Scalar, Size, Size> M)
{
    typedef Eigen::Matrix<Scalar, Size, Size> Matrix;
    typedef Eigen::Matrix<Scalar, Size, 1> Vector;

    Eigen::LDLT<Matrix> ldlt;
    ldlt.compute(M);
    Vector D_sqrt = ldlt.vectorD();
    for (int i = 0; i < D_sqrt.rows(); ++i)
    {
        D_sqrt(i) = std::sqrt(std::fabs(D_sqrt(i)));
    }

    return ldlt.transpositionsP().transpose() * (Matrix)ldlt.matrixL() *
           D_sqrt.asDiagonal();
}

/**
 * \ingroup linear_algebra
 * \brief Checks wheather the specified dense matrix has a diagonal form
 *
 * \return true if diagonal, false otherwise
 */
template <typename Derived>
bool is_diagonal(const Eigen::MatrixBase<Derived>& m)
{
    Derived z = m;
    z -= z.diagonal().asDiagonal();

    return z.isZero();
}

/**
 * \ingroup linear_algebra
 * \brief Robust AX=B solver where X is a vector or a matrix
 */
template <typename MatrixA, typename VectorsB>
VectorsB solve(
    const Eigen::MatrixBase<MatrixA>& A,
    const VectorsB& B,
    typename std::enable_if<VectorsB::RowsAtCompileTime == 1>::type* = 0)
{
    // householderQr() was not providing always a solution
    // VectorsB x = A.householderQr().solve(B).eval();

    assert(A.cols() == B.rows());
    assert(A.cols() == 1);
    assert(A.rows() == 1);

    VectorsB x = B / A(0);
    return x;  // RVO
}

/**
 * \ingroup linear_algebra
 * \brief Robust AX=B solver where X is a vector or a matrix
 */
template <typename MatrixA, typename VectorsB>
VectorsB solve(
    const Eigen::MatrixBase<MatrixA>& A,
    const VectorsB& B,
    typename std::enable_if<(VectorsB::RowsAtCompileTime != 1)>::type* = 0)
{
    // householderQr() was not providing always a solution
    // VectorsB x = A.householderQr().solve(B).eval();

    assert(A.cols() == B.rows());

    VectorsB x = A.colPivHouseholderQr().solve(B).eval();
    return x;  // RVO
}
}


// Copyright Nick Thompson, 2017
// Use, modification and distribution are subject to the
// Boost Software License, Version 1.0.
// (See accompanying file LICENSE_1_0.txt
// or copy at http://www.boost.org/LICENSE_1_0.txt)

#define BOOST_TEST_MODULE tanh_sinh_quadrature_test

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

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

#include <boost/math/concepts/real_concept.hpp>
#include <boost/test/included/unit_test.hpp>
#include <boost/test/tools/floating_point_comparison.hpp>
#include <boost/math/quadrature/tanh_sinh.hpp>
#include <boost/math/special_functions/sinc.hpp>
#include <boost/multiprecision/cpp_bin_float.hpp>
#include <boost/multiprecision/cpp_dec_float.hpp>
#include <boost/math/special_functions/next.hpp>
#include <boost/math/special_functions/gamma.hpp>
#include <boost/math/special_functions/beta.hpp>
#include <boost/math/special_functions/ellint_rc.hpp>
#include <boost/math/special_functions/ellint_rj.hpp>

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

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

#if !defined(TEST1) && !defined(TEST2) && !defined(TEST3) && !defined(TEST4) && !defined(TEST5) && !defined(TEST6) && !defined(TEST7) && !defined(TEST8)\
    && !defined(TEST1A) && !defined(TEST1B) && !defined(TEST2A) && !defined(TEST3A) && !defined(TEST6A) && !defined(TEST9)
#  define TEST1
#  define TEST2
#  define TEST3
#  define TEST4
#  define TEST5
#  define TEST6
#  define TEST7
#  define TEST8
#  define TEST1A
#  define TEST1B
#  define TEST2A
#  define TEST3A
#  define TEST6A
#  define TEST9
#endif

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

template <class T>
void print_levels(const T& v, const char* suffix)
{
   std::cout << "{\n";
   for (unsigned i = 0; i < v.size(); ++i)
   {
      std::cout << "      { ";
      for (unsigned j = 0; j < v[i].size(); ++j)
      {
         std::cout << v[i][j] << suffix << ", ";
      }
      std::cout << "},\n";
   }
   std::cout << "   };\n";
}

template <class T>
void print_complement_indexes(const T& v)
{
   std::cout << "\n   {";
   for (unsigned i = 0; i < v.size(); ++i)
   {
      unsigned index = 0;
      while (v[i][index] >= 0)
         ++index;
      std::cout << index << ", ";
   }
   std::cout << "};\n";
}

template <class T>
void print_levels(const std::pair<T, T>& p, const char* suffix = "")
{
   std::cout << "   static const std::vector<std::vector<Real> > abscissa = ";
   print_levels(p.first, suffix);
   std::cout << "   static const std::vector<std::vector<Real> > weights = ";
   print_levels(p.second, suffix);
   std::cout << "   static const std::vector<unsigned > indexes = ";
   print_complement_indexes(p.first);
}

template <class Real>
std::pair<std::vector<std::vector<Real>>, std::vector<std::vector<Real>> > generate_constants(unsigned max_index, unsigned max_rows)
{
   using boost::math::constants::half_pi;
   using boost::math::constants::two_div_pi;
   using boost::math::constants::pi;
   auto g = [](Real t) { return tanh(half_pi<Real>()*sinh(t)); };
   auto w = [](Real t) { Real cs = cosh(half_pi<Real>() * sinh(t));  return half_pi<Real>() * cosh(t) / (cs * cs); };
   auto gc = [](Real t) { Real u2 = half_pi<Real>() * sinh(t);  return 1 / (exp(u2) *cosh(u2)); };
   auto g_inv = [](float x)->float { return asinh(two_div_pi<float>()*atanh(x)); };
   auto gc_inv = [](float x)
   {
      float l = log(sqrt((2 - x) / x));
      return log((sqrt(4 * l * l + pi<float>() * pi<float>()) + 2 * l) / pi<float>());
   };

   std::vector<std::vector<Real>> abscissa, weights;

   std::vector<Real> temp;

   float t_crossover = gc_inv(0.5f);

   Real h = 1;
   for (unsigned i = 0; i < max_index; ++i)
   {
      temp.push_back((i < t_crossover) ? g(i * h) : -gc(i * h));
   }
   abscissa.push_back(temp);
   temp.clear();

   for (unsigned i = 0; i < max_index; ++i)
   {
      temp.push_back(w(i * h));
   }
   weights.push_back(temp);
   temp.clear();

   for (unsigned row = 1; row < max_rows; ++row)
   {
      h /= 2;
      for (Real t = h; t < max_index - 1; t += 2 * h)
         temp.push_back((t < t_crossover) ? g(t) : -gc(t));
      abscissa.push_back(temp);
      temp.clear();
   }
   h = 1;
   for (unsigned row = 1; row < max_rows; ++row)
   {
      h /= 2;
      for (Real t = h; t < max_index - 1; t += 2 * h)
         temp.push_back(w(t));
      weights.push_back(temp);
      temp.clear();
   }

   return std::make_pair(abscissa, weights);
}

template <class Real>
const tanh_sinh<Real>& get_integrator()
{
   static const tanh_sinh<Real> integrator(15);
   return integrator;
}

template <class Real>
Real get_convergence_tolerance()
{
   return boost::math::tools::root_epsilon<Real>();
}

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

template<class Real>
void test_quadratic()
{
    std::cout << "Testing quadratic functions are integrated properly by tanh_sinh on type " << boost::typeindex::type_id<Real>().pretty_name() << "\n";
    Real tol = 10*boost::math::tools::epsilon<Real>();
    auto integrator = get_integrator<Real>();
    auto f = [](const Real& x) { return 5*x*x + 7*x + 12; };
    Real error;
    Real L1;
    Real Q = integrator.integrate(f, (Real) 0, (Real) 1, get_convergence_tolerance<Real>(), &error, &L1);
    BOOST_CHECK_CLOSE_FRACTION(Q, (Real) 17 + half<Real>()*third<Real>(), tol);
    BOOST_CHECK_CLOSE_FRACTION(L1, (Real) 17 + half<Real>()*third<Real>(), tol);
}

template<class Real>
void test_singular()
{
    std::cout << "Testing integration of endpoint singularities on type " << boost::typeindex::type_id<Real>().pretty_name() << "\n";
    Real tol = 10*boost::math::tools::epsilon<Real>();
    Real error;
    Real L1;
    auto integrator = get_integrator<Real>();
    auto f = [](const Real& x) { return log(x)*log(1-x); };
    Real Q = integrator.integrate(f, (Real) 0, (Real) 1, get_convergence_tolerance<Real>(), &error, &L1);
    Real Q_expected = 2 - pi<Real>()*pi<Real>()*half<Real>()*third<Real>();

    BOOST_CHECK_CLOSE_FRACTION(Q, Q_expected, tol);
    BOOST_CHECK_CLOSE_FRACTION(L1, Q_expected, tol);
}

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

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

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

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

    // Oh it suffers on this one.
    auto f6 = [](Real t)->Real { Real x = log(t); return x*x; };
    Q = integrator.integrate(f6, (Real) 0 , (Real) 1, get_convergence_tolerance<Real>(), &error, &L1);
    Q_expected = 2;

    BOOST_CHECK_CLOSE_FRACTION(Q, Q_expected, 50*tol);
    BOOST_CHECK_CLOSE_FRACTION(L1, Q_expected, 50*tol);

    // Although it doesn't get to the requested tolerance on this integral, the error bounds can be queried and are reasonable:
    tol = sqrt(boost::math::tools::epsilon<Real>());
    auto f7 = [](const Real& t) { return sqrt(tan(t)); };
    Q = integrator.integrate(f7, (Real) 0 , (Real) half_pi<Real>(), get_convergence_tolerance<Real>(), &error, &L1);
    Q_expected = pi<Real>()/root_two<Real>();
    BOOST_CHECK_CLOSE_FRACTION(Q, Q_expected, tol);
    BOOST_CHECK_CLOSE_FRACTION(L1, Q_expected, tol);

    auto f8 = [](const Real& t) { return log(cos(t)); };
    Q = integrator.integrate(f8, (Real) 0 , half_pi<Real>(), get_convergence_tolerance<Real>(), &error, &L1);
    Q_expected = -pi<Real>()*ln_two<Real>()*half<Real>();
    BOOST_CHECK_CLOSE_FRACTION(Q, Q_expected, tol);
    BOOST_CHECK_CLOSE_FRACTION(L1, -Q_expected, tol);
}

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

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

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

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

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

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

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

template<class Real>
void test_integration_over_real_line()
{
    std::cout << "Testing integrals over entire real line in 'A Comparison of Three High Precision Quadrature Schemes' on type " << boost::typeindex::type_id<Real>().pretty_name() << "\n";
    Real tol = 10 * boost::math::tools::epsilon<Real>();
    Real Q;
    Real Q_expected;
    Real error;
    Real L1;
    auto integrator = get_integrator<Real>();

    auto f1 = [](const Real& t) { return 1/(1+t*t);};
    Q = integrator.integrate(f1, std::numeric_limits<Real>::has_infinity ? -std::numeric_limits<Real>::infinity() : -boost::math::tools::max_value<Real>(), std::numeric_limits<Real>::has_infinity ? std::numeric_limits<Real>::infinity() : boost::math::tools::max_value<Real>(), get_convergence_tolerance<Real>(), &error, &L1);
    Q_expected = pi<Real>();
    BOOST_CHECK_CLOSE_FRACTION(Q, Q_expected, tol);
    BOOST_CHECK_CLOSE_FRACTION(L1, Q_expected, tol);

    auto f2 = [](const Real& t) { return exp(-t*t*half<Real>()); };
    Q = integrator.integrate(f2, std::numeric_limits<Real>::has_infinity ? -std::numeric_limits<Real>::infinity() : -boost::math::tools::max_value<Real>(), std::numeric_limits<Real>::has_infinity ? std::numeric_limits<Real>::infinity() : boost::math::tools::max_value<Real>(), get_convergence_tolerance<Real>(), &error, &L1);
    Q_expected = root_two_pi<Real>();
    BOOST_CHECK_CLOSE_FRACTION(Q, Q_expected, tol * 2);
    BOOST_CHECK_CLOSE_FRACTION(L1, Q_expected, tol * 2);

    // This test shows how oscillatory integrals are approximated very poorly by this method:
    //std::cout << "Testing sinc integral: \n";
    //Q = integrator.integrate(boost::math::sinc_pi<Real>, -std::numeric_limits<Real>::infinity(), std::numeric_limits<Real>::infinity(), &error, &L1);
    //std::cout << "Error estimate of sinc integral: " << error << std::endl;
    //std::cout << "L1 norm of sinc integral " << L1 << std::endl;
    //Q_expected = pi<Real>();
    //BOOST_CHECK_CLOSE(Q, Q_expected, 100*tol);

    auto f4 = [](const Real& t) { return 1/cosh(t);};
    Q = integrator.integrate(f4, std::numeric_limits<Real>::has_infinity ? -std::numeric_limits<Real>::infinity() : -boost::math::tools::max_value<Real>(), std::numeric_limits<Real>::has_infinity ? std::numeric_limits<Real>::infinity() : boost::math::tools::max_value<Real>(), get_convergence_tolerance<Real>(), &error, &L1);
    Q_expected = pi<Real>();
    BOOST_CHECK_CLOSE_FRACTION(Q, Q_expected, tol);
    BOOST_CHECK_CLOSE_FRACTION(L1, Q_expected, tol);

}

template<class Real>
void test_right_limit_infinite()
{
    std::cout << "Testing right limit infinite for tanh_sinh in 'A Comparison of Three High Precision Quadrature Schemes' on type " << boost::typeindex::type_id<Real>().pretty_name() << "\n";
    Real tol = 10 * boost::math::tools::epsilon<Real>();
    Real Q;
    Real Q_expected;
    Real error;
    Real L1;
    auto integrator = get_integrator<Real>();

    // Example 11:
    auto f1 = [](const Real& t) { return 1/(1+t*t);};
    Q = integrator.integrate(f1, 0, std::numeric_limits<Real>::has_infinity ? std::numeric_limits<Real>::infinity() : boost::math::tools::max_value<Real>(), get_convergence_tolerance<Real>(), &error, &L1);
    Q_expected = half_pi<Real>();
    BOOST_CHECK_CLOSE(Q, Q_expected, 100*tol);

    // Example 12
    auto f2 = [](const Real& t) { return exp(-t)/sqrt(t); };
    Q = integrator.integrate(f2, 0, std::numeric_limits<Real>::has_infinity ? std::numeric_limits<Real>::infinity() : boost::math::tools::max_value<Real>(), get_convergence_tolerance<Real>(), &error, &L1);
    Q_expected = root_pi<Real>();
    BOOST_CHECK_CLOSE(Q, Q_expected, 1000*tol);

    auto f3 = [](const Real& t) { return exp(-t)*cos(t); };
    Q = integrator.integrate(f3, 0, std::numeric_limits<Real>::has_infinity ? std::numeric_limits<Real>::infinity() : boost::math::tools::max_value<Real>(), get_convergence_tolerance<Real>(), &error, &L1);
    Q_expected = half<Real>();
    BOOST_CHECK_CLOSE(Q, Q_expected, 100*tol);

    auto f4 = [](const Real& t) { return 1/(1+t*t); };
    Q = integrator.integrate(f4, 1, std::numeric_limits<Real>::has_infinity ? std::numeric_limits<Real>::infinity() : boost::math::tools::max_value<Real>(), get_convergence_tolerance<Real>(), &error, &L1);
    Q_expected = pi<Real>()/4;
    BOOST_CHECK_CLOSE(Q, Q_expected, 100*tol);
}

template<class Real>
void test_left_limit_infinite()
{
    std::cout << "Testing left limit infinite for tanh_sinh in 'A Comparison of Three High Precision Quadrature Schemes' on type " << boost::typeindex::type_id<Real>().pretty_name() << "\n";
    Real tol = 10 * boost::math::tools::epsilon<Real>();
    Real Q;
    Real Q_expected;
    auto integrator = get_integrator<Real>();

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

// A horrible function taken from
// http://www.chebfun.org/examples/quad/GaussClenCurt.html
template<class Real>
void test_horrible()
{
    std::cout << "Testing Trefenthen's horrible integral on type " << boost::typeindex::type_id<Real>().pretty_name() << "\n";
    // We only know the integral to double precision, so requesting a higher tolerance doesn't make sense.
    Real tol = 10 * std::numeric_limits<float>::epsilon();
    Real Q;
    Real Q_expected;
    Real error;
    Real L1;
    auto integrator = get_integrator<Real>();

    auto f = [](Real x)->Real { return x*sin(2*exp(2*sin(2*exp(2*x) ) ) ); };
    Q = integrator.integrate(f, (Real) -1, (Real) 1, get_convergence_tolerance<Real>(), &error, &L1);
    // NIntegrate[x*Sin[2*Exp[2*Sin[2*Exp[2*x]]]], {x, -1, 1}, WorkingPrecision -> 130, MaxRecursion -> 100]
    Q_expected = boost::lexical_cast<Real>("0.33673283478172753598559003181355241139806404130031017259552729882281");
    BOOST_CHECK_CLOSE_FRACTION(Q, Q_expected, tol);
    // Over again without specifying the bounds:
    Q = integrator.integrate(f, get_convergence_tolerance<Real>(), &error, &L1);
    BOOST_CHECK_CLOSE_FRACTION(Q, Q_expected, tol);
}

// Some examples of tough integrals from NR, section 4.5.4:
template<class Real>
void test_nr_examples()
{
    std::cout << "Testing singular integrals from NR 4.5.4 on type " << boost::typeindex::type_id<Real>().pretty_name() << "\n";
    Real tol = 10 * boost::math::tools::epsilon<Real>();
    Real Q;
    Real Q_expected;
    Real error;
    Real L1;
    auto integrator = get_integrator<Real>();

    auto f1 = [](Real x)->Real
    {
       return (sin(x * half<Real>()) * exp(-x) / x) / sqrt(x);
    };
    Q = integrator.integrate(f1, 0, std::numeric_limits<Real>::has_infinity ? std::numeric_limits<Real>::infinity() : boost::math::tools::max_value<Real>(), get_convergence_tolerance<Real>(), &error, &L1);
    Q_expected = sqrt(pi<Real>()*(sqrt((Real) 5) - 2));
    BOOST_CHECK_CLOSE_FRACTION(Q, Q_expected, 25*tol);

    auto f2 = [](Real x)->Real { return pow(x, -(Real) 2/(Real) 7)*exp(-x*x); };
    Q = integrator.integrate(f2, 0, std::numeric_limits<Real>::has_infinity ? std::numeric_limits<Real>::infinity() : boost::math::tools::max_value<Real>());
    Q_expected = half<Real>()*boost::math::tgamma((Real) 5/ (Real) 14);
    BOOST_CHECK_CLOSE_FRACTION(Q, Q_expected, tol * 6);

}

// Test integrand known to fool some termination schemes:
template<class Real>
void test_early_termination()
{
    std::cout << "Testing Clenshaw & Curtis's example of integrand which fools termination schemes on type " << boost::typeindex::type_id<Real>().pretty_name() << "\n";
    Real tol = 10 * boost::math::tools::epsilon<Real>();
    Real Q;
    Real Q_expected;
    Real error;
    Real L1;
    auto integrator = get_integrator<Real>();

    auto f1 = [](Real x)->Real { return 23*cosh(x)/ (Real) 25 - cos(x) ; };
    Q = integrator.integrate(f1, (Real) -1, (Real) 1, get_convergence_tolerance<Real>(), &error, &L1);
    Q_expected = 46*sinh((Real) 1)/(Real) 25 - 2*sin((Real) 1);
    BOOST_CHECK_CLOSE_FRACTION(Q, Q_expected, tol);
    // Over again with no bounds:
    Q = integrator.integrate(f1);
    BOOST_CHECK_CLOSE_FRACTION(Q, Q_expected, tol);
}

// Test some definite integrals from the CRC handbook, 32nd edition:
template<class Real>
void test_crc()
{
    std::cout << "Testing CRC formulas on type " << boost::typeindex::type_id<Real>().pretty_name() << "\n";
    Real tol = 10 * boost::math::tools::epsilon<Real>();
    Real Q;
    Real Q_expected;
    Real error;
    Real L1;
    auto integrator = get_integrator<Real>();

    // CRC Definite integral 585
    auto f1 = [](Real x)->Real { Real t = log(1/x); return x*x*t*t*t; };
    Q = integrator.integrate(f1, (Real) 0, (Real) 1, get_convergence_tolerance<Real>(), &error, &L1);
    Q_expected = (Real) 2/ (Real) 27;
    BOOST_CHECK_CLOSE_FRACTION(Q, Q_expected, tol);

    // CRC 636:
    auto f2 = [](Real x)->Real { return sqrt(cos(x)); };
    Q = integrator.integrate(f2, (Real) 0, (Real) half_pi<Real>(), get_convergence_tolerance<Real>(), &error, &L1);
    //Q_expected = pow(two_pi<Real>(), 3*half<Real>())/pow(boost::math::tgamma((Real) 1/ (Real) 4), 2);
    Q_expected = boost::lexical_cast<Real>("1.198140234735592207439922492280323878227212663215651558263674952946405214143915670835885556489793389375907225");
    BOOST_CHECK_CLOSE_FRACTION(Q, Q_expected, tol);

    // CRC Section 5.5, integral 585:
    for (int n = 0; n < 3; ++n) {
        for (int m = 0; m < 3; ++m) {
            auto f = [&](Real x)->Real { return pow(x, Real(m))*pow(log(1/x), Real(n)); };
            Q = integrator.integrate(f, (Real) 0, (Real) 1, get_convergence_tolerance<Real>(), &error, &L1);
            // Calculation of the tgamma function is not exact, giving spurious failures.
            // Casting to cpp_bin_float_100 beforehand fixes most of them.
            cpp_bin_float_100 np1 = n + 1;
            cpp_bin_float_100 mp1 = m + 1;
            Q_expected = boost::lexical_cast<Real>((tgamma(np1)/pow(mp1, np1)).str());
            BOOST_CHECK_CLOSE_FRACTION(Q, Q_expected, tol);
        }
    }

    // CRC Section 5.5, integral 591
    // The parameter p allows us to control the strength of the singularity.
    // Rapid convergence is not guaranteed for this function, as the branch cut makes it non-analytic on a disk.
    // This converges only when our test type has an extended exponent range as all the area of the integral
    // occurs so close to 0 (or 1) that we need abscissa values exceptionally small to find it.
    // "There's a lot of room at the bottom".
    // We also use a 2 argument functor so that 1-x is evaluated accurately:
    if (std::numeric_limits<Real>::max_exponent > std::numeric_limits<double>::max_exponent)
    {
       for (Real p = Real (-0.99); p < 1; p += Real(0.1)) {
          auto f = [&](Real x, Real cx)->Real
          {
             //return pow(x, p) / pow(1 - x, p);
             return cx < 0 ? exp(p * (log(x) - boost::math::log1p(-x))) : pow(x, p) / pow(cx, p);
          };
          Q = integrator.integrate(f, (Real)0, (Real)1, get_convergence_tolerance<Real>(), &error, &L1);
          Q_expected = 1 / sinc_pi(p*pi<Real>());
          BOOST_CHECK_CLOSE_FRACTION(Q, Q_expected, 10 * tol);
       }
    }
    // There is an alternative way to evaluate the above integral: by noticing that all the area of the integral
    // is near zero for p < 0 and near 1 for p > 0 we can substitute exp(-x) for x and remap the integral to the
    // domain (0, INF).  Internally we need to expand out the terms and evaluate using logs to avoid spurious overflow,
    // this gives us
    // for p > 0:
    for (Real p = Real(0.99); p > 0; p -= Real(0.1)) {
       auto f = [&](Real x)->Real
       {
          return exp(-x * (1 - p) + p * log(-boost::math::expm1(-x)));
       };
       Q = integrator.integrate(f, 0, boost::math::tools::max_value<Real>(), get_convergence_tolerance<Real>(), &error, &L1);
       Q_expected = 1 / sinc_pi(p*pi<Real>());
       BOOST_CHECK_CLOSE_FRACTION(Q, Q_expected, 10 * tol);
    }
    // and for p < 1:
    for (Real p = Real (-0.99); p < 0; p += Real(0.1)) {
       auto f = [&](Real x)->Real
       {
          return exp(-p * log(-boost::math::expm1(-x)) - (1 + p) * x);
       };
       Q = integrator.integrate(f, 0, boost::math::tools::max_value<Real>(), get_convergence_tolerance<Real>(), &error, &L1);
       Q_expected = 1 / sinc_pi(p*pi<Real>());
       BOOST_CHECK_CLOSE_FRACTION(Q, Q_expected, 10 * tol);
    }

    // CRC Section 5.5, integral 635
    for (int m = 0; m < 10; ++m) {
        auto f = [&](Real x)->Real { return 1/(1 + pow(tan(x), m)); };
        Q = integrator.integrate(f, (Real) 0, half_pi<Real>(), get_convergence_tolerance<Real>(), &error, &L1);
        Q_expected = half_pi<Real>()/2;
        BOOST_CHECK_CLOSE_FRACTION(Q, Q_expected, tol);
    }

    // CRC Section 5.5, integral 637:
    //
    // When h gets very close to 1, the strength of the singularity gradually increases until we
    // no longer have enough exponent range to evaluate the integral....
    // We also have to use the 2-argument functor version of the integrator to avoid
    // cancellation error, since the singularity is near PI/2.
    //
    Real limit = std::numeric_limits<Real>::max_exponent > std::numeric_limits<double>::max_exponent
       ? .95f : .85f;
    for (Real h = Real(0.01); h < limit; h += Real(0.1)) {
        auto f = [&](Real x, Real xc)->Real { return xc > 0 ? pow(1/tan(xc), h) : pow(tan(x), h); };
        Q = integrator.integrate(f, (Real) 0, half_pi<Real>(), get_convergence_tolerance<Real>(), &error, &L1);
        Q_expected = half_pi<Real>()/cos(h*half_pi<Real>());
        BOOST_CHECK_CLOSE_FRACTION(Q, Q_expected, tol);
    }
    // CRC Section 5.5, integral 637:
    //
    // Over again, but with argument transformation, we want:
    //
    // Integral of tan(x)^h over (0, PI/2)
    //
    // Note that the bulk of the area is next to the singularity at PI/2,
    // so we'll start by replacing x by PI/2 - x, and that tan(PI/2 - x) == 1/tan(x)
    // so we now have:
    //
    // Integral of 1/tan(x)^h over (0, PI/2)
    //
    // Which is almost the ideal form, except that when h is very close to 1
    // we run out of exponent range in evaluating the integral arbitrarily close to 0.
    // So now we substitute exp(-x) for x: this stretches out the range of the
    // integral to (-log(PI/2), +INF) with the singularity at +INF giving:
    //
    // Integral of exp(-x)/tan(exp(-x))^h over (-log(PI/2), +INF)
    //
    // We just need a way to evaluate the function without spurious under/overflow
    // in the exp terms.  Note that for small x: tan(x) ~= x, so making this
    // substitution and evaluating by logs we have:
    //
    // exp(-x)/tan(exp(-x))^h ~= exp((h - 1) * x)  for x > -log(epsilon);
    //
    // Here's how that looks in code:
    //
    for (Real i = 80; i < 100; ++i) {
       Real h = i / 100;
       auto f = [&](Real x)->Real
       {
          if (x > -log(boost::math::tools::epsilon<Real>()))
             return exp((h - 1) * x);
          else
          {
             Real et = exp(-x);
             // Need to deal with numeric instability causing et to be greater than PI/2:
             return et >= boost::math::constants::half_pi<Real>() ? 0 : et * pow(1 / tan(et), h);
          }
       };
       Q = integrator.integrate(f, -log(half_pi<Real>()), boost::math::tools::max_value<Real>(), get_convergence_tolerance<Real>(), &error, &L1);
       Q_expected = half_pi<Real>() / cos(h*half_pi<Real>());
       BOOST_CHECK_CLOSE_FRACTION(Q, Q_expected, 5 * tol);
    }

    // CRC Section 5.5, integral 670:

    auto f3 = [](Real x)->Real { return sqrt(log(1/x)); };
    Q = integrator.integrate(f3, (Real) 0, (Real) 1, get_convergence_tolerance<Real>(), &error, &L1);
    Q_expected = root_pi<Real>()/2;
    BOOST_CHECK_CLOSE_FRACTION(Q, Q_expected, tol);

}

template <class Real>
void test_sf()
{
   using std::sqrt;
   // Test some special functions that we already know how to evaluate:
   std::cout << "Testing special functions on type " << boost::typeindex::type_id<Real>().pretty_name() << "\n";
   Real tol = 10 * boost::math::tools::epsilon<Real>();
   auto integrator = get_integrator<Real>();

   // incomplete beta:
   if (std::numeric_limits<Real>::digits10 < 37) // Otherwise too slow
   {
      Real a(100), b(15);
      auto f = [&](Real x)->Real { return boost::math::ibeta_derivative(a, b, x); };
      BOOST_CHECK_CLOSE_FRACTION(integrator.integrate(f, 0, Real(0.25)), boost::math::ibeta(100, 15, Real(0.25)), tol * 10);
      // Check some really extreme versions:
      a = 1000;
      b = 500;
      BOOST_CHECK_CLOSE_FRACTION(integrator.integrate(f, 0, 1), Real(1), tol * 15);
      //
      // This is as extreme as we can get in this domain: otherwise the function has all it's
      // area so close to zero we never get in there no matter how many levels we go down:
      //
      a = Real(1) / 15;
      b = 30;
      BOOST_CHECK_CLOSE_FRACTION(integrator.integrate(f, 0, 1), Real(1), tol * 25);
   }

   Real x = 2, y = 3, z = 0.5, p = 0.25;
   // Elliptic integral RC:
   Real Q = integrator.integrate([&](const Real& t)->Real { return 1 / (sqrt(t + x) * (t + y)); }, 0, std::numeric_limits<Real>::has_infinity ? std::numeric_limits<Real>::infinity() : boost::math::tools::max_value<Real>()) / 2;
   BOOST_CHECK_CLOSE_FRACTION(Q, boost::math::ellint_rc(x, y), tol);
   // Elliptic Integral RJ:
   BOOST_CHECK_CLOSE_FRACTION(Real((Real(3) / 2) * integrator.integrate([&](Real t)->Real { return 1 / (sqrt((t + x) * (t + y) * (t + z)) * (t + p)); }, 0, std::numeric_limits<Real>::has_infinity ? std::numeric_limits<Real>::infinity() : boost::math::tools::max_value<Real>())), boost::math::ellint_rj(x, y, z, p), tol);

   z = 5.5;
   if (std::numeric_limits<Real>::digits10 > 40)
      tol *= 200;
   else if (!std::numeric_limits<Real>::is_specialized)
      tol *= 3;
   // tgamma expressed as an integral:
   BOOST_CHECK_CLOSE_FRACTION(integrator.integrate([&](Real t)->Real { using std::pow; using std::exp; return t > 10000 ? Real(0) : Real(pow(t, z - 1) * exp(-t)); }, 0, std::numeric_limits<Real>::has_infinity ? std::numeric_limits<Real>::infinity() : boost::math::tools::max_value<Real>()),
      boost::math::tgamma(z), tol);
   BOOST_CHECK_CLOSE_FRACTION(integrator.integrate([](const Real& t)->Real {  using std::exp; return exp(-t*t); }, std::numeric_limits<Real>::has_infinity ? -std::numeric_limits<Real>::infinity() : -boost::math::tools::max_value<Real>(), std::numeric_limits<Real>::has_infinity ? std::numeric_limits<Real>::infinity() : boost::math::tools::max_value<Real>()),
      boost::math::constants::root_pi<Real>(), tol);
}

template <class Real>
void test_2_arg()
{
   BOOST_MATH_STD_USING
   std::cout << "Testing 2 argument functors on type " << boost::typeindex::type_id<Real>().pretty_name() << "\n";
   Real tol = 10 * boost::math::tools::epsilon<Real>();

   auto integrator = get_integrator<Real>();

   //
   // There are a whole family of integrals of the general form
   // x(1-x)^-N ; N < 1
   // which have all the interesting behaviour near the 2 singularities
   // and all converge, see: http://www.wolframalpha.com/input/?i=integrate+(x+*+(1-x))%5E-1%2FN+from+0+to+1
   //
   Real Q = integrator.integrate([&](const Real& t, const Real & tc)->Real
   {
      return tc < 0 ? 1 / sqrt(t * (1-t)) : 1 / sqrt(t * tc);
   }, 0, 1);
   BOOST_CHECK_CLOSE_FRACTION(Q, boost::math::constants::pi<Real>(), tol);
   Q = integrator.integrate([&](const Real& t, const Real & tc)->Real
   {
      return tc < 0 ? 1 / boost::math::cbrt(t * (1-t)) : 1 / boost::math::cbrt(t * tc);
   }, 0, 1);
   BOOST_CHECK_CLOSE_FRACTION(Q, boost::math::pow<2>(boost::math::tgamma(Real(2) / 3)) / boost::math::tgamma(Real(4) / 3), tol * 4);
   //
   // We can do the same thing with ((1+x)(1-x))^-N ; N < 1
   //
   Q = integrator.integrate([&](const Real& t, const Real & tc)->Real
   {
      return t < 0 ? 1 / sqrt(-tc * (1-t)) : 1 / sqrt((t + 1) * tc);
   }, -1, 1);
   BOOST_CHECK_CLOSE_FRACTION(Q, boost::math::constants::pi<Real>(), tol);
   Q = integrator.integrate([&](const Real& t, const Real & tc)->Real
   {
      return t < 0 ? 1 / sqrt(-tc * (1-t)) : 1 / sqrt((t + 1) * tc);
   });
   BOOST_CHECK_CLOSE_FRACTION(Q, boost::math::constants::pi<Real>(), tol);
   Q = integrator.integrate([&](const Real& t, const Real & tc)->Real
   {
      return t < 0 ? 1 / boost::math::cbrt(-tc * (1-t)) : 1 / boost::math::cbrt((t + 1) * tc);
   }, -1, 1);
   BOOST_CHECK_CLOSE_FRACTION(Q, sqrt(boost::math::constants::pi<Real>()) * boost::math::tgamma(Real(2) / 3) / boost::math::tgamma(Real(7) / 6), tol * 10);
   Q = integrator.integrate([&](const Real& t, const Real & tc)->Real
   {
      return t < 0 ? 1 / boost::math::cbrt(-tc * (1-t)) : 1 / boost::math::cbrt((t + 1) * tc);
   });
   BOOST_CHECK_CLOSE_FRACTION(Q, sqrt(boost::math::constants::pi<Real>()) * boost::math::tgamma(Real(2) / 3) / boost::math::tgamma(Real(7) / 6), tol * 10);
   //
   // These are taken from above, and do not get to full precision via the single arg functor:
   //
   auto f7 = [](const Real& t, const Real& tc) { return t < 1 ? sqrt(tan(t)) : sqrt(1/tan(tc)); };
   Real error, L1;
   Q = integrator.integrate(f7, (Real)0, (Real)half_pi<Real>(), get_convergence_tolerance<Real>(), &error, &L1);
   Real Q_expected = pi<Real>() / root_two<Real>();
   BOOST_CHECK_CLOSE_FRACTION(Q, Q_expected, tol);
   BOOST_CHECK_CLOSE_FRACTION(L1, Q_expected, tol);

   auto f8 = [](const Real& t, const Real& tc) { return t < 1 ? log(cos(t)) : log(sin(tc)); };
   Q = integrator.integrate(f8, (Real)0, half_pi<Real>(), get_convergence_tolerance<Real>(), &error, &L1);
   Q_expected = -pi<Real>()*ln_two<Real>()*half<Real>();
   BOOST_CHECK_CLOSE_FRACTION(Q, Q_expected, tol);
   BOOST_CHECK_CLOSE_FRACTION(L1, -Q_expected, tol);
}

template <class Complex>
void test_complex()
{
   typedef typename Complex::value_type value_type;
   //
   // Integral version of the confluent hypergeometric function:
   // https://dlmf.nist.gov/13.4#E1
   //
   value_type tol = 10 * boost::math::tools::epsilon<value_type>();
   Complex a(2, 3), b(3, 4), z(0.5, -2);

   auto f = [&](value_type t)
   {
      return exp(z * t) * pow(t, a - value_type(1)) * pow(value_type(1) - t, b - a - value_type(1));
   };

   auto integrator = get_integrator<value_type>();
   auto Q = integrator.integrate(f, value_type(0), value_type(1), get_convergence_tolerance<value_type>());
   //
   // Expected result computed from http://www.wolframalpha.com/input/?i=1F1%5B(2%2B3i),+(3%2B4i);+(0.5-2i)%5D+*+gamma(2%2B3i)+*+gamma(1%2Bi)+%2F+gamma(3%2B4i)
   //
   Complex expected(boost::lexical_cast<value_type>("-0.2911081612888249710582867318081776512805281815037891183828405999609246645054069649838607112484426042883371996"),
      boost::lexical_cast<value_type>("0.4507983563969959578849120188097153649211346293694903758252662015991543519595834937475296809912196906074655385"));

   value_type error = abs(expected - Q);
   BOOST_CHECK_LE(error, tol);

   //
   // Sin Integral https://dlmf.nist.gov/6.2#E9
   //
   auto f2 = [z](value_type t)
   {
      return -exp(-z * cos(t)) * cos(z * sin(t));
   };
   Q = integrator.integrate(f2, value_type(0), boost::math::constants::half_pi<value_type>(), get_convergence_tolerance<value_type>());

   expected = Complex(boost::lexical_cast<value_type>("0.8893822921008980697856313681734926564752476188106405688951257340480164694708337246829840859633322683740376134733"),
      -boost::lexical_cast<value_type>("2.381380802906111364088958767973164614925936185337231718483495612539455538280372745733208000514737758457795502168"));
   expected -= boost::math::constants::half_pi<value_type>();

   error = abs(expected - Q);
   BOOST_CHECK_LE(error, tol);
}

BOOST_AUTO_TEST_CASE(tanh_sinh_quadrature_test)
{
#ifdef GENERATE_CONSTANTS
   //
   // Generate pre-computed coefficients:
   std::cout << std::setprecision(35);
   print_levels(generate_constants<cpp_bin_float_100>(10, 8), "L");

#else

#ifdef TEST1

    test_right_limit_infinite<float>();
    test_left_limit_infinite<float>();
    test_linear<float>();
    test_quadratic<float>();
    test_singular<float>();
    test_ca<float>();
    test_three_quadrature_schemes_examples<float>();
    test_horrible<float>();
    test_integration_over_real_line<float>();
    test_nr_examples<float>();
#endif
#ifdef TEST1A
    test_early_termination<float>();
    test_2_arg<float>();
#endif
#ifdef TEST1B
    test_crc<float>();
#endif
#ifdef TEST2
    test_right_limit_infinite<double>();
    test_left_limit_infinite<double>();
    test_linear<double>();
    test_quadratic<double>();
    test_singular<double>();
    test_ca<double>();
    test_three_quadrature_schemes_examples<double>();
    test_horrible<double>();
    test_integration_over_real_line<double>();
    test_nr_examples<double>();
    test_early_termination<double>();
    test_sf<double>();
    test_2_arg<double>();
#endif
#ifdef TEST2A
    test_crc<double>();
#endif

#ifndef BOOST_MATH_NO_LONG_DOUBLE_MATH_FUNCTIONS

#ifdef TEST3
    test_right_limit_infinite<long double>();
    test_left_limit_infinite<long double>();
    test_linear<long double>();
    test_quadratic<long double>();
    test_singular<long double>();
    test_ca<long double>();
    test_three_quadrature_schemes_examples<long double>();
    test_horrible<long double>();
    test_integration_over_real_line<long double>();
    test_nr_examples<long double>();
    test_early_termination<long double>();
    test_sf<long double>();
    test_2_arg<long double>();
#endif
#ifdef TEST3A
    test_crc<long double>();

#endif
#endif

#ifdef TEST4
    test_right_limit_infinite<cpp_bin_float_quad>();
    test_left_limit_infinite<cpp_bin_float_quad>();
    test_linear<cpp_bin_float_quad>();
    test_quadratic<cpp_bin_float_quad>();
    test_singular<cpp_bin_float_quad>();
    test_ca<cpp_bin_float_quad>();
    test_three_quadrature_schemes_examples<cpp_bin_float_quad>();
    test_horrible<cpp_bin_float_quad>();
    test_nr_examples<cpp_bin_float_quad>();
    test_early_termination<cpp_bin_float_quad>();
    test_crc<cpp_bin_float_quad>();
    test_sf<cpp_bin_float_quad>();
    test_2_arg<cpp_bin_float_quad>();

#endif
#ifdef TEST5

    test_sf<cpp_bin_float_50>();
    test_sf<cpp_bin_float_100>();
    test_sf<boost::multiprecision::number<boost::multiprecision::cpp_bin_float<150> > >();

#endif
#ifndef BOOST_MATH_NO_LONG_DOUBLE_MATH_FUNCTIONS
#ifdef TEST6

    test_right_limit_infinite<boost::math::concepts::real_concept>();
    test_left_limit_infinite<boost::math::concepts::real_concept>();
    test_linear<boost::math::concepts::real_concept>();
    test_quadratic<boost::math::concepts::real_concept>();
    test_singular<boost::math::concepts::real_concept>();
    test_ca<boost::math::concepts::real_concept>();
    test_three_quadrature_schemes_examples<boost::math::concepts::real_concept>();
    test_horrible<boost::math::concepts::real_concept>();
    test_integration_over_real_line<boost::math::concepts::real_concept>();
    test_nr_examples<boost::math::concepts::real_concept>();
    test_early_termination<boost::math::concepts::real_concept>();
    test_sf<boost::math::concepts::real_concept>();
    test_2_arg<boost::math::concepts::real_concept>();
#endif
#ifdef TEST6A
    test_crc<boost::math::concepts::real_concept>();

#endif
#endif
#ifdef TEST7
    test_sf<cpp_dec_float_50>();
#endif
#if defined(TEST8) && defined(BOOST_HAS_FLOAT128)

    test_right_limit_infinite<boost::multiprecision::float128>();
    test_left_limit_infinite<boost::multiprecision::float128>();
    test_linear<boost::multiprecision::float128>();
    test_quadratic<boost::multiprecision::float128>();
    test_singular<boost::multiprecision::float128>();
    test_ca<boost::multiprecision::float128>();
    test_three_quadrature_schemes_examples<boost::multiprecision::float128>();
    test_horrible<boost::multiprecision::float128>();
    test_integration_over_real_line<boost::multiprecision::float128>();
    test_nr_examples<boost::multiprecision::float128>();
    test_early_termination<boost::multiprecision::float128>();
    test_crc<boost::multiprecision::float128>();
    test_sf<boost::multiprecision::float128>();
    test_2_arg<boost::multiprecision::float128>();

#endif
#ifdef TEST9
    test_complex<std::complex<double> >();
    test_complex<std::complex<float> >();
#endif

#endif
}

#else

int main() { return 0; }

#endif


// Copyright Yamaha 2021
// MIT License
// https://github.com/yamaha-bps/cbr_control/blob/master/LICENSE

#ifndef NL_PROBLEM_PENDULUM_HPP_
#define NL_PROBLEM_PENDULUM_HPP_

#include <cbr_math/math.hpp>

#include <Eigen/Dense>
#include <cstdint>
#include <functional>
#include <limits>

template<typename T1, typename T2>
auto pendulum_dynamics(const Eigen::MatrixBase<T1> & x, const Eigen::MatrixBase<T2> & u)
{
  // Define nx, nu
  constexpr auto nx = T1::RowsAtCompileTime;
  constexpr auto nu = T2::RowsAtCompileTime;

  // Initialize Parameters
  constexpr double l = 1.;
  constexpr double m = 1.;
  constexpr double b = 0.2;
  constexpr double gr = 9.81;

  using T = typename decltype(x * u.transpose())::EvalReturnType::Scalar;

  // xDot = f(x) + g(x)*u
  Eigen::Matrix<typename T1::Scalar, nx, 1> f;
  Eigen::Matrix<typename T1::Scalar, nx, nu> g;

  // Define States
  const auto & theta = x[0];
  const auto & thetaDot = x[1];

  // Dynamics
  f[0] = thetaDot;
  f[1] = -(gr / l) * sin(theta) - b / (m * l * l) * thetaDot;
  g[0] = 0.;
  g[1] = 1 / (m * l * l);

  return (f + g * u).eval();
}

struct NlOcpPendulum
{
  constexpr static std::size_t nx = 2;
  constexpr static std::size_t nu = 1;
  constexpr static bool xl_always_feasible = false;

  using state_t = Eigen::Matrix<double, nx, 1>;
  using deriv_t = Eigen::Matrix<double, nx, 1>;
  using input_t = Eigen::Matrix<double, nu, 1>;
  using Q_t = Eigen::Matrix<double, nx, nx>;
  using R_t = Eigen::Matrix<double, nu, nu>;

  const Q_t Q = (state_t() << 20.0, 1.0).finished().asDiagonal();
  const Q_t QT = (state_t() << 20.0, 1.0).finished().asDiagonal();
  const R_t R = (R_t() << 0.1).finished();

  template<typename T1, typename T2>
  auto get_f(const Eigen::MatrixBase<T1> & x, const Eigen::MatrixBase<T2> & u) const
  {
    static_assert(
      T1::RowsAtCompileTime == nx &&
      T1::ColsAtCompileTime == 1,
      "First argument must be an nx*1 Eigen Matrix");

    static_assert(
      T2::RowsAtCompileTime == nu &&
      T2::ColsAtCompileTime == 1,
      "Second argument must be an nu*1 Eigen Matrix");

    return pendulum_dynamics<T1, T2>(x, u);
  }

  void get_state_lb(double, Eigen::Ref<state_t> state_lb) const
  {
    const double kInfinity = std::numeric_limits<double>::infinity();
    state_lb <<
      -kInfinity,
      -cbr::deg2rad(200.0);
  }

  void get_state_ub(double, Eigen::Ref<state_t> state_ub) const
  {
    const double kInfinity = std::numeric_limits<double>::infinity();
    state_ub <<
      kInfinity,
      cbr::deg2rad(200.0);
  }

  void get_input_lb(double, Eigen::Ref<input_t> input_lb) const
  {
    input_lb << -0.1;
  }

  void get_input_ub(double, Eigen::Ref<input_t> input_ub) const
  {
    input_ub << 0.1;
  }

  const Q_t & get_Q(double) const {return Q;}
  const R_t & get_R(double) const {return R;}
  const Q_t & get_QT() const {return QT;}
};

#endif  // NL_PROBLEM2_HPP_