#include <Eigen/Dense>
#include <Eigen/LU>

#include <iostream>
#include <iomanip>

#include "timer.h"

// Models a resistence between node i,j with i < j (could, in principle, handle different resistences)
typedef std::pair< int, int>            resistor;
// Vector containing all resistences in the circuit (excluded the variable one, which is modelled afterwards)
typedef std::vector< resistor >         resistor_topology;
// Models a ground or source of voltage, indexes store the node connected to the ground/source through a resistence
typedef std::tuple<int, int, double>    voltage;
// Vector containing a voltage object for each node connected to sink or source.
typedef std::vector< voltage >          voltage_topology;

//! \brief Class implementing the topology of the circuit (cf. Figure)
//! Computes impedance of the entire circuit (between node 16 and 17) exploiting
//! the SMW formula for the inversion of low rank perturbations of a matrix A0,
//! whose factorization in known in advance
template <class Matrix>
class ImpedanceMap {
    static const std::size_t nnodes = 15; //< Compile time hard-coded size of circuit
public:
    //! \brief Constructor: Builds system matrix and rhs and performs lu decomposition
    //! The lu decomposition is stored in lu and can be reused in the SMW formula
    //! to avoid expensive matrix solves for repeated usages of the operator()
    //! \param R_ resistence R
    //! \param W_ source voltage W at node 16, gound is 0 at node 17
    ImpedanceMap(double R_, double W_) : R(R_), W(W_) {
        // In the following, instead of specifying directly the entries of A\_0 and of rhs
        // we define some auxiliary structure and automatically generate the right entries for
        // the structures we specified.
        // This way: we avoid silly mistakes, avoid a very long list of matrix entreis specifications,
        // have a flexible way (we may easily change the topology) and automatically take care of shifting the indices
        // Of course, if you want, you can manually put each entry in the matrix and rhs. The result (should) be the same)

        // We implement the topology of the resistances by pushing each
        // Resistance between node i < j in a std::vector of pair (i,j) \in \mathbb{N}^2
        // This way we can automatically build a symmetric matrix, take care of indexing
        // and avoid dumb mistakes during the matrix filling
        resistor_topology T;
        T.reserve(23);
        T.push_back(resistor(1,2));
        T.push_back(resistor(1,5));
        T.push_back(resistor(2,5));
        T.push_back(resistor(2,3));
        T.push_back(resistor(2,14));
        T.push_back(resistor(3,4));
        T.push_back(resistor(3,15));
        T.push_back(resistor(4,6));
        T.push_back(resistor(4,15));
        T.push_back(resistor(5,7));
        T.push_back(resistor(5,14));
        T.push_back(resistor(6,9));
        T.push_back(resistor(6,15));
        T.push_back(resistor(7,10));
        T.push_back(resistor(7,11));
        T.push_back(resistor(8,9));
        T.push_back(resistor(8,12));
        T.push_back(resistor(8,13));
        T.push_back(resistor(8,15));
        T.push_back(resistor(9,13));
        T.push_back(resistor(10,11));
        T.push_back(resistor(11,12));
        T.push_back(resistor(12,13));
        // Default matrix entries sert to one (varistor)
        T.push_back(resistor(14,15));

        // We implement voltage gound and source topology by specifiying which node is connected to ground/source
        // trough a resistance. This will be also part of rhs
        voltage_topology S;
        S.reserve(4);
        S.push_back(voltage(6, 16, W));
        S.push_back(voltage(7, 17, 0));
        S.push_back(voltage(11,17, 0));
        S.push_back(voltage(14,17, 0));

        // Automatically build A0 filling from topology
        Matrix A0 = Matrix::Zero(nnodes, nnodes);
        for(auto it: T) {
            // Shift indeces down by 1
            auto i = it.first - 1;
            auto j = it.second - 1;
            // Fill diagonal: each resistance contributes += R into the diagonal of both nodes
            A0(i,i) += 1;
            A0(j,j) += 1;
            // Fill the off diagonal (negative part in \Delta W_{i,j}, the matrix is kept symmetric
            A0(i,j) -= 1;
            A0(j,i) -= 1;
        }

        // Fill in the rest (source and ground), i.e. components with $\Delta W_{i,j}$ with j > 15
        // Each node i connected to ground or source contributes to the rhs with R * W (R resistence between node i and ground/source
        // node, W is voltage at sink or source) and to its own diagonal with R
        rhs = Matrix::Zero(nnodes,1);
        for(auto it: S) {
            // Shift index down by 1 and get voltage in W2
            int i = std::get<0>(it) - 1;
            auto W2 = std::get<2>(it);
            // Add voltage in rhs (resistance assumed to be R)
            rhs(i) += W2;
            // Add resistance to matrix diagonal: contribution of source current
            A0(i,i) += 1;
        }

        // Precompute lu factorizaion of A0
        lu = A0.lu();
    };

    //! \brief Compute the impedance given the resistence variable R_x
    //! Use SMW formula for low rank perturbations to avoid expensive inversion
    //! if factorization of the base system matrix is already known
    //! \param Rx resistence R_x > 0 of the varistor between node 14 and 15
    //! \return impedance = W  * I of the system A(R_x)
    double operator()(double Rx) {
        // Store the scaled factor for convenience
        double fac = R/Rx;

        // There are many ways to create the same matrix u*v
        // Create U, the 15x2 matrix in (A+UV)
        Matrix U = Matrix::Zero(nnodes, 1);
        U(13) = -1;
        U(14) = 1;
        // V will be U tranps

        //// Use SMW formula to compute (A + UU')^{-1} rhs
        // Formula is A^{-1} rhs - A^{-1} * u * (I + V*A^{-1}*U)^{-1} V * A^{-1}

        // Start by precomputing A^{-1} rhs (needed twice), column vector of length 15
        auto Ainvrhs = lu.solve(rhs);
        // The precompute A^{-1} U (15x2 matrix), needed twice
        auto Ainvu = lu.solve(U);
        // The compute alpha, 2x2 matrix whose inverse is cheap
        auto alpha = (Matrix::Identity(2,2) + U.transpose() / -1 * (1-fac) * Ainvu).inverse();
        // Put the formula toghether, x is a 15x1 column vector containing voltages at each node (except 16,17, prescribed)
        auto x = Ainvrhs - Ainvu * alpha * U.transpose() / -1 * (1-fac) * Ainvrhs;

        // Compute the current I = \Delta W_{16,5} / R and then impedance = W / I
        // \Delta W_{16,5} = (W - x_5)
        return W * R / (W - x(5));
    };
private:
    Eigen::PartialPivLU< Matrix > lu; //< Store lu decomposition of a for efficiency
    double R, W; //< Resistance R and source voltage W
    Matrix rhs; //< Store rhs vector prescribing sink and source voltages
};

int main(void) {
    ImpedanceMap<Eigen::MatrixXd> IM = ImpedanceMap<Eigen::MatrixXd>(1, 1);
    std::cout << "Impedance [R = 1]: " << IM(1) << std::endl;

    std::cout << std::setw(30) << "Impedance [Ohm]" << std::setw(30) << "R_x [Ohm]" << std::endl;
    std::cout << std::setw(30) << IM(0)             << std::setw(30) << " " << 0 << std::endl;
    std::cout << std::setw(30) << IM(0.1)           << std::setw(30) << " " << 0.1 << std::endl;
    for(auto Rx = 1; Rx <= 1024; Rx *= 2) {
        std::cout << std::setw(30) << IM(Rx)        << std::setw(30) << " " << Rx << std::endl;
    }

    timer<> tmr;
    tmr.start();
    ImpedanceMap<Eigen::MatrixXd> IM2 = ImpedanceMap<Eigen::MatrixXd>(1, 1);
    IM2(1);
    tmr.stop();
    std::cout << "Took:             " << tmr.min().count() / 1000000. << " ms" << std::endl;

}

Nummerical Methods for CSE/PS3/solutions_ps3/impedancemap.cpp


#include <iostream>
#include <string>
#include <vector>
#include <Eigen/Core>
#include <QApplication>
#include <visoptslider/visoptslider.hpp>
#include <optimization-test-functions.hpp>

int main(int argc, char *argv[])
{
    QApplication app(argc, argv);

    // Define the target function and bound
    constexpr int num_dimensions = 3;
    constexpr auto target_function = [](const Eigen::VectorXd& x)
    {
        return otf::GetValue(x, otf::FunctionType::Rosenbrock);
    };
    const Eigen::Vector3d upper_bound(+ 2.0, + 2.0, + 2.0);
    const Eigen::Vector3d lower_bound(- 2.0, - 2.0, - 2.0);
    constexpr double maximum_value = 200.0;
    constexpr double minimum_value = 0.0;

    // Optional settings
    const std::vector<std::string> labels = { "x1", "x2", "x3" };
    constexpr bool show_values = true;
    constexpr int resolution = 200;

    // Instantiate and initialize the widget
    visopt::SlidersWidget sliders_widget;
    sliders_widget.initialize(num_dimensions,
                              target_function,
                              upper_bound,
                              lower_bound,
                              maximum_value,
                              minimum_value,
                              labels,
                              show_values,
                              resolution);

    // Set a callback function
    sliders_widget.setCallback([&sliders_widget]()
    {
        std::cout << sliders_widget.getArgument().transpose() << std::endl;
    });

    // Show the widget
    sliders_widget.show();
    return app.exec();
}


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

#ifndef BOOST_MATH_INTERPOLATORS_PCHIP_HPP
#define BOOST_MATH_INTERPOLATORS_PCHIP_HPP
#include <memory>
#include <boost/math/interpolators/detail/cubic_hermite_detail.hpp>

namespace boost::math::interpolators {

template<class RandomAccessContainer>
class pchip {
public:
    using Real = typename RandomAccessContainer::value_type;

    pchip(RandomAccessContainer && x, RandomAccessContainer && y,
          Real left_endpoint_derivative = std::numeric_limits<Real>::quiet_NaN(),
          Real right_endpoint_derivative = std::numeric_limits<Real>::quiet_NaN())
    {
        if (x.size() < 4)
        {
            throw std::domain_error("Must be at least four data points.");
        }
        RandomAccessContainer s(x.size(), std::numeric_limits<Real>::quiet_NaN());
        if (isnan(left_endpoint_derivative))
        {
            // O(h) finite difference derivative:
            // This, I believe, is the only derivative guaranteed to be monotonic:
            s[0] = (y[1]-y[0])/(x[1]-x[0]);
        }
        else
        {
            s[0] = left_endpoint_derivative;
        }

        for (decltype(s.size()) k = 1; k < s.size()-1; ++k) {
            Real hkm1 = x[k] - x[k-1];
            Real dkm1 = (y[k] - y[k-1])/hkm1;

            Real hk = x[k+1] - x[k];
            Real dk = (y[k+1] - y[k])/hk;
            Real w1 = 2*hk + hkm1;
            Real w2 = hk + 2*hkm1;
            if ( (dk > 0 && dkm1 < 0) || (dk < 0 && dkm1 > 0) || dk == 0 || dkm1 == 0)
            {
                s[k] = 0;
            }
            else
            {
                s[k] = (w1+w2)/(w1/dkm1 + w2/dk);
            }

        }
        // Quadratic extrapolation at the other end:
        auto n = s.size();
        if (isnan(right_endpoint_derivative))
        {
            s[n-1] = (y[n-1]-y[n-2])/(x[n-1] - x[n-2]);
        }
        else
        {
            s[n-1] = right_endpoint_derivative;
        }
        impl_ = std::make_shared<detail::cubic_hermite_detail<RandomAccessContainer>>(std::move(x), std::move(y), std::move(s));
    }

    Real operator()(Real x) const {
        return impl_->operator()(x);
    }

    Real prime(Real x) const {
        return impl_->prime(x);
    }

    friend std::ostream& operator<<(std::ostream & os, const pchip & m)
    {
        os << *m.impl_;
        return os;
    }

    void push_back(Real x, Real y) {
        using std::abs;
        using std::isnan;
        if (x <= impl_->x_.back()) {
             throw std::domain_error("Calling push_back must preserve the monotonicity of the x's");
        }
        impl_->x_.push_back(x);
        impl_->y_.push_back(y);
        impl_->dydx_.push_back(std::numeric_limits<Real>::quiet_NaN());
        auto n = impl_->size();
        impl_->dydx_[n-1] = (impl_->y_[n-1]-impl_->y_[n-2])/(impl_->x_[n-1] - impl_->x_[n-2]);
        // Now fix s_[n-2]:
        auto k = n-2;
        Real hkm1 = impl_->x_[k] - impl_->x_[k-1];
        Real dkm1 = (impl_->y_[k] - impl_->y_[k-1])/hkm1;

        Real hk = impl_->x_[k+1] - impl_->x_[k];
        Real dk = (impl_->y_[k+1] - impl_->y_[k])/hk;
        Real w1 = 2*hk + hkm1;
        Real w2 = hk + 2*hkm1;
        if ( (dk > 0 && dkm1 < 0) || (dk < 0 && dkm1 > 0) || dk == 0 || dkm1 == 0)
        {
            impl_->dydx_[k] = 0;
        }
        else
        {
            impl_->dydx_[k] = (w1+w2)/(w1/dkm1 + w2/dk);
        }
    }

private:
    std::shared_ptr<detail::cubic_hermite_detail<RandomAccessContainer>> impl_;
};

}
#endif


#ifndef MLT_MODELS_REGRESSORS_RIDGE_REGRESSION_HPP
#define MLT_MODELS_REGRESSORS_RIDGE_REGRESSION_HPP

#include <type_traits>

#include <Eigen/Core>

#include "linear_regressor.hpp"
#include "../../utils/linear_solvers.hpp"

namespace mlt {
namespace models {
namespace regressors {
	using namespace utils::linear_solvers;

	template <class Solver = SVDSolver>
	class RidgeRegression : public LinearRegressor<RidgeRegression<Solver>> {
	public:
		explicit RidgeRegression(double regularization, bool fit_intercept = true) : LinearRegressor(fit_intercept),
			_regularization(regularization), _solver(Solver()) {}

		template <class S, class = enable_if<is_same<decay_t<S>, Solver>::value>>
		explicit RidgeRegression(double regularization, S&& solver, bool fit_intercept = true) : LinearRegressor(fit_intercept),
			_regularization(regularization), _solver(forward<S>(solver)) {}

		Self& fit(Features input, Target target, bool = true) {
			MatrixXd input_prime(input.rows() + (_fit_intercept ? 1 : 0), input.cols());
			input_prime.topRows(input.rows()) << input;
			MatrixXd reg = MatrixXd::Identity(input_prime.rows(), input_prime.rows()) * _regularization;

			if (_fit_intercept) {
				input_prime.bottomRows<1>() = VectorXd::Ones(input.cols());
				reg(reg.rows() - 1, reg.cols() - 1) = 0;
			}

			_set_coefficients(_solver.compute((input_prime * input_prime.transpose() + reg)).solve(input_prime * target.transpose()).transpose());

			return _self();
		}

	protected:
		double _regularization;
		Solver _solver;
	};
}
}
}
#endif


#include <stdlib.h>
#include <stdio.h>
#include <vector>
#include <string>
#include <sstream>
#include <iostream>
#include <stdexcept>
#include <functional>
#include <mutex>
#include <Eigen/Geometry>

#ifndef SIMPLE_DTW_HPP
#define SIMPLE_DTW_HPP

namespace simple_dtw
{
    template<typename FirstDatatype, typename SecondDatatype, typename DistanceFn,
             typename FirstAllocator = std::allocator<FirstDatatype>,
             typename SecondAllocator = std::allocator<SecondDatatype>>
    class SimpleDTW
    {
    private:

        void InitializeMatrix(const size_t first_sequence_size, const size_t second_sequence_size)
        {
            const ssize_t rows = (ssize_t)first_sequence_size + 1;
            const ssize_t cols = (ssize_t)second_sequence_size + 1;
            if (dtw_matrix_.rows() < rows || dtw_matrix_.cols() < cols)
            {
                dtw_matrix_ = Eigen::MatrixXd::Zero(rows, cols);
                if (rows > 1 && cols > 1)
                {
                    for (ssize_t row = 1; row < rows; row++)
                    {
                        dtw_matrix_(row, 0) = std::numeric_limits<double>::infinity();
                    }
                    for (ssize_t col = 1; col < cols; col++)
                    {
                        dtw_matrix_(0, col) = std::numeric_limits<double>::infinity();
                    }
                }
            }

        }

        Eigen::MatrixXd dtw_matrix_;

    public:

        SimpleDTW()
        {
            InitializeMatrix(0, 0);
        }

        SimpleDTW(const size_t first_sequence_size, const size_t second_sequence_size)
        {
            InitializeMatrix(first_sequence_size, second_sequence_size);
        }

        double EvaluateWarpingCost(
                const std::vector<FirstDatatype, FirstAllocator>& first_sequence,
                const std::vector<SecondDatatype, SecondAllocator>& second_sequence,
                const DistanceFn& distance_fn)
        {
            InitializeMatrix(first_sequence.size(), second_sequence.size());
            //Compute DTW cost for the two sequences
            for (ssize_t i = 1; i <= (ssize_t)first_sequence.size(); i++)
            {
                const FirstDatatype& first_item = first_sequence[(size_t)i - 1];
                for (ssize_t j = 1; j <= (ssize_t)second_sequence.size(); j++)
                {
                    const SecondDatatype& second_item = second_sequence[(size_t)j - 1];
                    const double index_cost = distance_fn(first_item, second_item);
                    double prev_cost = 0.0;
                    // Get the three neighboring values from the matrix to use for the update
                    double im1j = dtw_matrix_(i - 1, j);
                    double im1jm1 = dtw_matrix_(i - 1, j - 1);
                    double ijm1 = dtw_matrix_(i, j - 1);
                    // Start the update step
                    if (im1j < im1jm1 && im1j < ijm1)
                    {
                        prev_cost = im1j;
                    }
                    else if (ijm1 < im1j && ijm1 < im1jm1)
                    {
                        prev_cost = ijm1;
                    }
                    else
                    {
                        prev_cost = im1jm1;
                    }
                    // Update the value in the matrix
                    const double new_cost = index_cost + prev_cost;
                    dtw_matrix_(i, j) = new_cost;
                }
            }
            //Return total path cost
            const double warping_cost = dtw_matrix_((ssize_t)first_sequence.size(), (ssize_t)second_sequence.size());
            return warping_cost;
        }
    };

    // DistanceFn must match the prototype std::function<double(const FirstDataype&, const SecondDatatype&)>
    template<typename FirstDatatype, typename SecondDatatype, typename DistanceFn,
             typename FirstAllocator = std::allocator<FirstDatatype>,
             typename SecondAllocator = std::allocator<SecondDatatype>>
    inline double ComputeDTWDistance(
            const std::vector<FirstDatatype, FirstAllocator>& first_sequence,
            const std::vector<SecondDatatype, SecondAllocator>& second_sequence,
            const DistanceFn& distance_fn)
    {
        SimpleDTW<FirstDatatype, SecondDatatype, DistanceFn, FirstAllocator, SecondAllocator> dtw_evaluator;
        return dtw_evaluator.EvaluateWarpingCost(first_sequence, second_sequence, distance_fn);
    }
}

#endif // SIMPLE_DTW_HPP


//
// Copyright (c) 2020 CNRS INRIA
//

#include "pinocchio/autodiff/cppad.hpp"
#include "pinocchio/multibody/model.hpp"
#include "pinocchio/algorithm/joint-configuration.hpp"

#include "utils/model-generator.hpp"

#include <boost/test/unit_test.hpp>
#include <boost/utility/binary.hpp>

using namespace pinocchio;

BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE)

BOOST_AUTO_TEST_CASE(test_joint_configuration)
{
  typedef double Scalar;
  using CppAD::AD;
  using CppAD::NearEqual;

  typedef AD<Scalar> ADScalar;

  typedef pinocchio::ModelTpl<Scalar> Model;

  Model model; buildAllJointsModel(model);
  Eigen::VectorXd q2 = Eigen::VectorXd::Random(model.nq);
  Eigen::VectorXd q1 = Eigen::VectorXd::Random(model.nq);
  normalize(model,q1);
  normalize(model,q2);

  Eigen::VectorXd v = Eigen::VectorXd::Random(model.nv);
  std::vector<Eigen::VectorXd> results_q(2,Eigen::VectorXd::Zero(model.nq));
  std::vector<Eigen::VectorXd> results_v(2,Eigen::VectorXd::Zero(model.nv));

  typedef pinocchio::ModelTpl<ADScalar> ADModel;
  typedef ADModel::ConfigVectorType ADConfigVectorType;
  ADModel ad_model = model.cast<ADScalar>();
  ADConfigVectorType ad_q1(model.nq);
  ADConfigVectorType ad_q2(model.nq);
  ADConfigVectorType ad_v(model.nv);

  typedef Eigen::Matrix<Scalar,Eigen::Dynamic,1> VectorX;
  typedef Eigen::Matrix<ADScalar,Eigen::Dynamic,1> VectorXAD;
  typedef Eigen::Matrix<Scalar,Eigen::Dynamic,Eigen::Dynamic> MatrixX;
  typedef Eigen::Matrix<ADScalar,Eigen::Dynamic,Eigen::Dynamic> MatrixXAD;

  //Integrate
  {
    VectorXAD ad_x(model.nq+model.nv);
    ad_x << q1.cast<ADScalar>(), v.cast<ADScalar>();
    CppAD::Independent(ad_x);
    ad_q1 = ad_x.head(model.nq);
    ad_v = ad_x.tail(model.nv);

    VectorXAD ad_y(model.nq);
    pinocchio::integrate(ad_model,ad_q1,ad_v,ad_y);
    CppAD::ADFun<Scalar> ad_fun(ad_x,ad_y);

    CPPAD_TESTVECTOR(Scalar) x_eval((size_t)(ad_x.size()));
    Eigen::Map<VectorX>(x_eval.data(),ad_x.size()) << q1,v;
    CPPAD_TESTVECTOR(Scalar) y_eval((size_t)(ad_y.size()));
    y_eval = ad_fun.Forward(0,x_eval);
    results_q[0] = Eigen::Map<VectorX>(y_eval.data(),ad_y.size());

    pinocchio::integrate(model,q1,v,results_q[1]);
    BOOST_CHECK(results_q[0].isApprox(results_q[1]));

    Eigen::Map<VectorX>(x_eval.data(),ad_x.size()) << q1,VectorX::Zero(model.nv);
    y_eval = ad_fun.Forward(0,x_eval);
    results_q[0] = Eigen::Map<VectorX>(y_eval.data(),ad_y.size());
    BOOST_CHECK(results_q[0].isApprox(q1));
  }

  //Difference
  {
    VectorXAD ad_x(model.nq+model.nq);
    ad_x << q1.cast<ADScalar>(), q2.cast<ADScalar>();
    CppAD::Independent(ad_x);
    ad_q1 = ad_x.head(model.nq);
    ad_q2 = ad_x.tail(model.nq);

    VectorXAD ad_y(model.nv);
    pinocchio::difference(ad_model,ad_q1,ad_q2,ad_y);
    CppAD::ADFun<Scalar> ad_fun(ad_x,ad_y);

    CPPAD_TESTVECTOR(Scalar) x_eval((size_t)(ad_x.size()));
    Eigen::Map<VectorX>(x_eval.data(),ad_x.size()) << q1,q2;
    CPPAD_TESTVECTOR(Scalar) y_eval((size_t)(ad_y.size()));
    y_eval = ad_fun.Forward(0,x_eval);
    results_v[0] = Eigen::Map<VectorX>(y_eval.data(),ad_y.size());

    pinocchio::difference(model,q1,q2,results_v[1]);
    BOOST_CHECK(results_v[0].isApprox(results_v[1]));

    Eigen::Map<VectorX>(x_eval.data(),ad_x.size()) << q1,q1;
    y_eval = ad_fun.Forward(0,x_eval);
    results_v[0] = Eigen::Map<VectorX>(y_eval.data(),ad_y.size());
    BOOST_CHECK(results_v[0].isZero());
  }

  //dDifference
  std::vector<MatrixX> results_J0(2,MatrixX::Zero(model.nv,model.nv));
  std::vector<MatrixX> results_J1(2,MatrixX::Zero(model.nv,model.nv));
  {
    VectorXAD ad_x(model.nq+model.nq);
    ad_x << q1.cast<ADScalar>(), q2.cast<ADScalar>();
    CppAD::Independent(ad_x);
    ad_q1 = ad_x.head(model.nq);
    ad_q2 = ad_x.tail(model.nq);

    MatrixXAD ad_y(2*model.nv,model.nv);
    pinocchio::dDifference(ad_model,ad_q1,ad_q2,ad_y.topRows(model.nv),pinocchio::ARG0);
    pinocchio::dDifference(ad_model,ad_q1,ad_q2,ad_y.bottomRows(model.nv),pinocchio::ARG1);
    VectorXAD ad_y_flatten = Eigen::Map<VectorXAD>(ad_y.data(),ad_y.size());
    CppAD::ADFun<Scalar> ad_fun(ad_x,ad_y_flatten);

    CPPAD_TESTVECTOR(Scalar) x_eval((size_t)(ad_x.size()));
    Eigen::Map<VectorX>(x_eval.data(),ad_x.size()) << q1, q2;
    CPPAD_TESTVECTOR(Scalar) y_eval((size_t)(ad_y.size()));
    y_eval = ad_fun.Forward(0,x_eval);
    results_J0[0] = Eigen::Map<MatrixX>(y_eval.data(),ad_y.rows(),ad_y.cols()).topRows(model.nv);
    results_J1[0] = Eigen::Map<MatrixX>(y_eval.data(),ad_y.rows(),ad_y.cols()).bottomRows(model.nv);

    // w.r.t q1
    pinocchio::dDifference(model,q1,q2,results_J0[1],pinocchio::ARG0);
    BOOST_CHECK(results_J0[0].isApprox(results_J0[1]));

    // w.r.t q2
    pinocchio::dDifference(model,q1,q2,results_J1[1],pinocchio::ARG1);
    BOOST_CHECK(results_J1[0].isApprox(results_J1[1]));

    Eigen::Map<VectorX>(x_eval.data(),ad_x.size()) << q1, q1;
    y_eval = ad_fun.Forward(0,x_eval);
    results_J0[0] = Eigen::Map<MatrixX>(y_eval.data(),ad_y.rows(),ad_y.cols()).topRows(model.nv);
    results_J1[0] = Eigen::Map<MatrixX>(y_eval.data(),ad_y.rows(),ad_y.cols()).bottomRows(model.nv);

    BOOST_CHECK((-results_J0[0]).isIdentity());
    BOOST_CHECK(results_J1[0].isIdentity());
  }

}

BOOST_AUTO_TEST_SUITE_END()


//
// OpenTissue, A toolbox for physical based simulation and animation.
// Copyright (C) 2007 Department of Computer Science, University of Copenhagen
//
#include <OpenTissue/configuration.h>

#include <OpenTissue/core/math/math_random.h>
#include <OpenTissue/core/math/big/big_types.h>
#include <OpenTissue/core/math/big/big_generate_random.h>
#include <OpenTissue/core/math/big/big_is_orthonormal.h>
#include <OpenTissue/core/math/big/big_gram_schmidt.h>

#include <OpenTissue/core/math/big/big_generate_PD.h>
#include <OpenTissue/core/math/big/big_generate_PSD.h>
#include <OpenTissue/core/math/big/big_is_symmetric.h>

#include <OpenTissue/core/math/big/io/big_matlab_write.h>

#define BOOST_AUTO_TEST_MAIN
#include <OpenTissue/utility/utility_push_boost_filter.h>
#include <boost/test/auto_unit_test.hpp>
#include <boost/test/unit_test_suite.hpp>
#include <boost/test/floating_point_comparison.hpp>
#include <boost/test/test_tools.hpp>
#include <OpenTissue/utility/utility_pop_boost_filter.h>

BOOST_AUTO_TEST_SUITE(opentissue_math_big_modified_gram_schmidt);

BOOST_AUTO_TEST_CASE(random_test_case)
{
  typedef ublas::compressed_matrix<double> matrix_type;

  matrix_type A;

  for(size_t tst=0;tst<5;++tst)
  {
    OpenTissue::math::big::generate_random(10, 10, A);

    bool not_ortho = !OpenTissue::math::big::is_orthonormal( A );
    BOOST_CHECK(not_ortho);

    OpenTissue::math::big::gram_schmidt(A);

    bool did_it = OpenTissue::math::big::is_orthonormal( A );
    BOOST_CHECK(did_it);
  }

  {
    using namespace OpenTissue::math::big;
    OpenTissue::math::big::fast_generate_PD( 10, A );

    std::cout << "A = " << A << ";" <<std::endl;

    bool is_ok = OpenTissue::math::big::is_symmetric( A );
    BOOST_CHECK(is_ok);

    OpenTissue::math::big::generate_PSD( 10, A, 0.5 );

    std::cout << "B = " << A << ";" <<std::endl;

    bool is_also_ok = OpenTissue::math::big::is_symmetric( A );
    BOOST_CHECK(is_also_ok);
  }
}

BOOST_AUTO_TEST_SUITE_END();


#include <stan/math/prim/scal.hpp>
#include <gtest/gtest.h>
#include <boost/random/mersenne_twister.hpp>
#include <boost/math/distributions.hpp>

TEST(ProbDistributionsPoisson, error_check) {
  using std::log;

  boost::random::mt19937 rng;
  EXPECT_NO_THROW(stan::math::poisson_rng(6, rng));

  EXPECT_THROW(stan::math::poisson_rng(-6, rng), std::domain_error);

  EXPECT_NO_THROW(stan::math::poisson_rng(1e9, rng));

  EXPECT_THROW(stan::math::poisson_rng(pow(2.0, 31), rng), std::domain_error);

  EXPECT_NO_THROW(stan::math::poisson_log_rng(6, rng));

  EXPECT_NO_THROW(stan::math::poisson_log_rng(-6, rng));

  EXPECT_NO_THROW(stan::math::poisson_log_rng(log(1e9), rng));

  EXPECT_THROW(stan::math::poisson_log_rng(log(pow(2.0, 31)), rng),
               std::domain_error);
}

TEST(ProbDistributionsPoisson, chiSquareGoodnessFitTest) {
  boost::random::mt19937 rng;
  int N = 1000;
  int K = boost::math::round(2 * std::pow(N, 0.4));
  boost::math::poisson_distribution<> dist(5);
  boost::math::chi_squared mydist(K - 1);

  int loc[K - 1];
  for (int i = 1; i < K; i++)
    loc[i - 1] = i - 1;

  int count = 0;
  double bin[K];
  double expect[K];
  for (int i = 0; i < K; i++) {
    bin[i] = 0;
    expect[i] = N * pdf(dist, i);
  }
  expect[K - 1] = N * (1 - cdf(dist, K - 1));

  while (count < N) {
    int a = stan::math::poisson_rng(5, rng);
    int i = 0;
    while (i < K - 1 && a > loc[i])
      ++i;
    ++bin[i];
    count++;
  }

  double chi = 0;

  for (int j = 0; j < K; j++)
    chi += ((bin[j] - expect[j]) * (bin[j] - expect[j]) / expect[j]);

  EXPECT_TRUE(chi < quantile(complement(mydist, 1e-6)));
}

TEST(ProbDistributionsPoisson, chiSquareGoodnessFitTest2) {
  using std::log;

  boost::random::mt19937 rng;
  int N = 1000;
  int K = boost::math::round(2 * std::pow(N, 0.4));
  boost::math::poisson_distribution<> dist(5);
  boost::math::chi_squared mydist(K - 1);

  int loc[K - 1];
  for (int i = 1; i < K; i++)
    loc[i - 1] = i - 1;

  int count = 0;
  double bin[K];
  double expect[K];
  for (int i = 0; i < K; i++) {
    bin[i] = 0;
    expect[i] = N * pdf(dist, i);
  }
  expect[K - 1] = N * (1 - cdf(dist, K - 1));

  while (count < N) {
    int a = stan::math::poisson_log_rng(log(5), rng);
    int i = 0;
    while (i < K - 1 && a > loc[i])
      ++i;
    ++bin[i];
    count++;
  }

  double chi = 0;

  for (int j = 0; j < K; j++)
    chi += ((bin[j] - expect[j]) * (bin[j] - expect[j]) / expect[j]);

  EXPECT_TRUE(chi < quantile(complement(mydist, 1e-6)));
}


#include <stan/math/prim/scal.hpp>
#include <boost/math/special_functions/fpclassify.hpp>
#include <gtest/gtest.h>

TEST(MathFunctions, log_inv_logit) {
  using stan::math::log_inv_logit;
  using std::log;
  using stan::math::inv_logit;

  EXPECT_FLOAT_EQ(log(inv_logit(-7.2)), log_inv_logit(-7.2));
  EXPECT_FLOAT_EQ(log(inv_logit(0.0)), log_inv_logit(0.0));
  EXPECT_FLOAT_EQ(log(inv_logit(1.9)), log_inv_logit(1.9));
}

TEST(MathFunctions, log_inv_logit_nan) {
  double nan = std::numeric_limits<double>::quiet_NaN();

  EXPECT_PRED1(boost::math::isnan<double>,
               stan::math::log_inv_logit(nan));
}


#include <boost/numeric/ublas/matrix_sparse.hpp>
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/io.hpp>
#include <boost/numeric/bindings/traits/ublas_vector2.hpp>
#include <boost/numeric/bindings/traits/ublas_sparse.hpp>
#include <boost/numeric/bindings/mumps/mumps_driver.hpp>
#include <iostream>
#include <fstream>
#include <complex>

template <typename T>
int test() {
  namespace ublas = ::boost::numeric::ublas ;
  namespace mumps = ::boost::numeric::bindings::mumps ;

  int const n = 10 ;

  typedef ublas::coordinate_matrix<T, ublas::column_major, 1, ublas::unbounded_array<int> > coo_type ;

  coo_type coo( n, n, n + 6 ) ;

  for (int i=0; i<n; ++i) coo(i,i) = i+1.0 ;
  coo(2,3) = T(1.0) ;
  coo(2,4) = T(1.0) ;
  coo(5,6) = T(-1.0) ;
  coo(2,6) = T(1.0) ;
  coo(9,0) = T(1.0) ;
  coo(2,7) = T(-1.0) ;

  coo.sort() ;
  std::cout << "matrix " << coo << std::endl ;

  ublas::vector<T> v( 10 ) ;
  ublas::vector<T> w( 10 ) ;

  std::fill( w.begin(), w.end(), 1.0 ) ;

  for (int i=1; i<n; ++i) {
    w(i) += w(i-1) ;
  }

  for (int i=0; i<n; ++i) {
    v[i] = T(coo(i,i)) * w[i] ;
  }
  v[2] += T(coo(2,3)) * w[3] ;
  v[2] += T(coo(2,4)) * w[4] ;
  v[5] += T(coo(5,6)) * w[6] ;
  v[2] += T(coo(2,6)) * w[6] ;
  v[9] += T(coo(9,0)) * w[0] ;
  v[2] += T(coo(2,7)) * w[7] ;
  std::cout << "rhs : " << v << std::endl ;

  mumps::mumps< coo_type > mumps_coo ;

  mumps_coo.icntl[2]=mumps_coo.icntl[3] = 0 ;

  // Analysis
  mumps_coo.job = 1 ;
  matrix_integer_data( mumps_coo, coo ) ;
  driver( mumps_coo ) ;

  // Factorization
  mumps_coo.job = 2 ;
  matrix_value_data( mumps_coo, coo ) ;
  driver( mumps_coo ) ;

  // Solve
  mumps_coo.job = 3 ;
  rhs_sol_value_data( mumps_coo, v ) ;
  driver( mumps_coo ) ;

  std::cout << "w : " << w << std::endl ;
  std::cout << "v : " << v << std::endl ;

  if ( norm_2( v - w ) > 1.e-10 * norm_2( v ) ) return 1 ;

  return 0 ;
}

int main() {
  if ( test<float>() ) return 1 ;
  if ( test<double>() ) return 2 ;
  if ( test< std::complex<float> >() ) return 3 ;
  if ( test< std::complex<double> >() ) return 4 ;
  return 0 ;
}


// Boost.Geometry (aka GGL, Generic Geometry Library)
// QuickBook Example

// Copyright (c) 2020 Digvijay Janartha, Hamirpur, India.

// Use, modification and distribution is 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)

//[perimeter
//` Calculate the perimeter of a polygon

#include <iostream>

#include <boost/geometry.hpp>
#include <boost/geometry/geometries/point_xy.hpp>
#include <boost/geometry/geometries/polygon.hpp>

namespace bg = boost::geometry; /*< Convenient namespace alias >*/

int main()
{
    // Calculate the perimeter of a cartesian polygon
    bg::model::polygon<bg::model::d2::point_xy<double> > poly;
    bg::read_wkt("POLYGON((0 0,3 4,5 -5,-2 -4, 0 0))", poly);
    double perimeter = bg::perimeter(poly);
    std::cout << "Perimeter: " << perimeter << std::endl;

    return 0;
}

//]

//[perimeter_output
/*`
Output:
[pre
Perimeter: 25.7627
]
*/
//]


/*****************************************************************************
*
* Rokko: Integrated Interface for libraries of eigenvalue decomposition
*
* Copyright (C) 2015 Rokko Developers https://github.com/t-sakashita/rokko
*
* 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 <rokko/rokko.hpp>
#include <rokko/lapack.h>
#include <boost/lexical_cast.hpp>

typedef rokko::localized_vector<double> vector_t;
typedef rokko::localized_matrix<double, rokko::matrix_col_major> matrix_t;

int main(int argc, char *argv[]) {
  int info;
  int n = 6;
  if (argc > 1) n = boost::lexical_cast<int>(argv[1]);
  std::cout << "n = " << n << std::endl;

  // generate symmmetric random martix
  matrix_t mat = matrix_t::Random(n, n);
  mat += mat.transpose().eval(); // eval() is required to avoid aliasing issue
  std::cout << "Input random matrix A:\n" << mat << std::endl;

  // diagonalization
  vector_t w(n);
  matrix_t v = mat; // v will be overwritten by eigenvectors
  info = LAPACKE_dsyev(LAPACK_COL_MAJOR, 'V', 'U', n, &v(0, 0), n, &w(0));
  std::cout << "Eigenvalues:\n" << w << std::endl;
  std::cout << "Eigenvectors:\n" << v << std::endl;

  // check correctness of diagonalization
  matrix_t wmat = matrix_t::Zero(n, n);
  for (int i = 0; i < n; ++i) wmat(i, i) = w(i);
  matrix_t check = v.transpose() * mat * v;
  std::cout << "Vt * A * V:\n" << check << std::endl;
  std::cout << "| W - Vt * A * V | = " << (wmat - check).norm() << std::endl;
}


// ------ language="C++" file="src/test-rk4.cc"
#include "methods.hh"
#include "types.hh"

#include <argagg/argagg.hpp>

#include <vector>
#include <cstdlib>
#include <cmath>
#include <iostream>
#include <Eigen/Dense>

using namespace pint;

// ------ begin <<harmonic-oscillator>>[0]
template <typename real_t, typename vector_t>
ODE<real_t, vector_t> harmonic_oscillator
    ( real_t omega_0
    , real_t zeta )
{
    return [=] (real_t t, vector_t const &y) {
        return vector_t
            ( y[1]
            , -2 * zeta * omega_0 * y[1] - omega_0*omega_0 * y[0] );
    };
}
// ------ end
// ------ begin <<backward-euler-harmonic-oscillator>>[0]
template <typename real_t, typename vector_t>
Integral<real_t, vector_t> backward_euler_harmonic_oscillator
    ( real_t omega_0
    , real_t zeta )
{
    return [=]
        ( vector_t const &y
        , real_t t_init
        , real_t t_end ) -> vector_t
    {
        real_t h = t_end - t_init;
        real_t d = 1 + 2*h*omega_0*zeta;
        real_t q = y[0] / (1 + h * (1 - y[1] / d));
        real_t p = (y[1] - h*omega_0*omega_0*q) / d;
        return vector_t(q, p);
    };
}
// ------ end

template <typename real_t, typename vector_t>
std::vector<vector_t> solve_iterative
    ( IterationStep<real_t, vector_t> step
    , std::vector<vector_t> const &y_0
    , std::vector<real_t> const &t
    , real_t abs_err
    , unsigned max_iter )
{
    std::vector<vector_t> y = y_0;

    for (unsigned i = 0; i < t.size(); ++i) {
        std::cout << t[i] << " " << y[i][0] << " " << y[i][1] << std::endl;
    }
    std::cout << "\n\n";

    for (unsigned i = 0; i < max_iter; ++i) {
        auto y_next = step(y, t);
        real_t max_err = 0.0;
        for (unsigned i = 0; i < t.size(); ++i) {
            real_t err = (y_next[i] - y[i]).norm();
            max_err = (err > max_err ? err : max_err);
        }
        y = y_next;

        std::cout << "# iteration=" << i + 1
                  << " max_abs_err=" << max_err << "\n";
        for (unsigned i = 0; i < t.size(); ++i) {
            std::cout << t[i] << " " << y[i][0] << " " << y[i][1] << std::endl;
        }
        std::cout << "\n\n";
    }
    return y;
}

int main(int argc, char **argv)
{
    argagg::parser argparser
        {{ { "help",   {"-h", "--help"}
           , "shows this help message", 0 }
         , { "omega0", {"--omega0"}
           , "undamped angular frequency (default 1.0)", 1 }
         , { "zeta",   {"--zeta"}
           , "damping ratio (default 0.5)", 1 }
         , { "n",      {"--n"}
           , "number of time slices (default 9)", 1 }
         , { "h",      {"--h"}
           , "size of time step in fine integrator (default 0.01)", 1 }
        }};

    argagg::parser_results args;
    try {
        args = argparser.parse(argc, argv);
    } catch (const std::exception& e) {
        std::cerr << e.what() << std::endl;
        return EXIT_FAILURE;
    }

    if (args["help"]) {
        std::cerr << "Parareal test case: harmonic oscillator\n";
        std::cerr << argparser;
        return EXIT_SUCCESS;
    }

    using real_t = double;
    using vector_t = Eigen::Vector2d;

    unsigned n    = args["n"].as<unsigned>(9);
    real_t h      = args["h"].as<real_t>(0.01);
    real_t omega0 = args["omega0"].as<real_t>(1.0);
    real_t zeta   = args["zeta"].as<real_t>(0.5);

    auto ts = linspace<real_t>(0, 15.0, n);
    auto ode = harmonic_oscillator<real_t, vector_t>(omega0, zeta);
    auto rk4 = runge_kutta_4<real_t, vector_t>(ode);
    auto coarse = backward_euler_harmonic_oscillator<real_t, vector_t>(omega0, zeta);
    // auto coarse = backward_euler<real_t, vector_t>(ode, 1e-6);
    auto fine = iterate_step<real_t, vector_t>(rk4, h);
    auto y_0 = solve(coarse, vector_t(1.0, 0.0), ts);

    auto t_ref = linspace<real_t>(0, 15.0, 100);
    auto y_ref = solve(fine, vector_t(1.0, 0.0), t_ref);
    for (unsigned i = 0; i < t_ref.size(); ++i) {
        std::cout << t_ref[i] << " " << y_ref[i][0] << " " << y_ref[i][1] << std::endl;
    }
    std::cout << "\n\n";

    auto y = solve_iterative
        ( parareal(coarse, fine)
        , y_0
        , ts
        , 1e-6
        , n );

    return EXIT_SUCCESS;
}
// ------ end


#include <Eigen/Dense>
#include <fstream>
#include <iostream>
#include <vector>
#include "Configuration.hpp"
#include "MatrixSolver.hpp"
#include "matrixIO.hpp"

using namespace Eigen;

int main(int argc, char *argv[])
{
  std::cout << "SIDEMADE – Simple Dense Matrix Decomposition\n"
            << std::endl;

  if (argc != 2) {
    std::cerr << "SIDEMADE needs to be called with exactly one argument – the "
              << "configuration file, e.g. 'sidemade config.yml'." << std::endl;
    return -1;
  }

  // read configuration
  Configuration configuration{std::string{argv[1]}};

  // create solver
  MatrixSolver solver{configuration.decompositionType};

  // set a different arbitrary seed for the random generator to avoid a match
  // between random matrices (generated with a seed of 0) and random vectors
  srand(1000);

  // fill matrix and vector structures
  // A is read from file, b is set randomly, x is the solution
  const MatrixXd A = matrixIO::openData(configuration.matrixFileName,
                                        configuration.matrixSize);
  const VectorXd b = VectorXd::Random(configuration.matrixSize);
  VectorXd       x = VectorXd(configuration.matrixSize);

  // solve A * x = b
  std::cout << "Solving ...\n"
            << std::endl;
  solver.solve(A, b, x);

  const double resAbs = (A * x - b).norm(); // norm() computes l2 norm
  const double relRes = resAbs / x.norm();
  // std::cout << "Solution is:\n" << x << std::endl;
  std::cout << "Absolute l2 residual is: " << resAbs << std::endl;
  std::cout << "Relative l2 residual is: " << relRes << std::endl;

  return 0;
}


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

using namespace Eigen;
using namespace std;

int main(int, char**)
{
  cout.precision(3);
  MatrixXd ones = MatrixXd::Ones(3,3);
cout << "The operator norm of the 3x3 matrix of ones is "
     << ones.selfadjointView<Lower>().operatorNorm() << endl;

  return 0;
}


// An example showing TEASER++ registration with the Stanford bunny model
#include <chrono>
#include <iostream>
#include <random>

#include <Eigen/Core>

#include <teaser/log.h>
#include <teaser/ply_io.h>
#include <teaser/registration.h>

// Macro constants for generating noise and outliers
#define NOISE_BOUND 0.05
#define N_OUTLIERS 1700
#define OUTLIER_TRANSLATION_LB 5
#define OUTLIER_TRANSLATION_UB 10

inline double getAngularError(Eigen::Matrix3d R_exp, Eigen::Matrix3d R_est) {
  return std::abs(std::acos(fmin(fmax(((R_exp.transpose() * R_est).trace() - 1) / 2, -1.0), 1.0)));
}

void addNoiseAndOutliers(Eigen::Matrix<double, 3, Eigen::Dynamic>& tgt) {
  //int N_OUTLIERS = tgt.size() * 90 / 100;
  // Add uniform noise
  Eigen::Matrix<double, 3, Eigen::Dynamic> noise =
      Eigen::Matrix<double, 3, Eigen::Dynamic>::Random(3, tgt.cols()) * NOISE_BOUND;
  NOISE_BOUND / 2;
  tgt = tgt + noise;

  // Add outliers
  std::random_device rd;
  std::mt19937 gen(rd());
  std::uniform_int_distribution<> dis2(0, tgt.cols() - 1); // pos of outliers
  std::uniform_int_distribution<> dis3(OUTLIER_TRANSLATION_LB,
                                       OUTLIER_TRANSLATION_UB); // random translation
  std::vector<bool> expected_outlier_mask(tgt.cols(), false);
  for (int i = 0; i < N_OUTLIERS; ++i) {
    int c_outlier_idx = dis2(gen);
    assert(c_outlier_idx < expected_outlier_mask.size());
    expected_outlier_mask[c_outlier_idx] = true;
    tgt.col(c_outlier_idx).array() += dis3(gen); // random translation
  }
}

int main() {
  // Load the .ply file
  teaser::PLYReader reader;
  teaser::PointCloud src_cloud;
  auto status = reader.read("./example_data/bun_zipper_res3.ply", src_cloud);
  int N = src_cloud.size();

  // Convert the point cloud to Eigen
  Eigen::Matrix<double, 3, Eigen::Dynamic> src(3, N);
  for (size_t i = 0; i < N; ++i) {
    src.col(i) << src_cloud[i].x, src_cloud[i].y, src_cloud[i].z;
  }

  // Homogeneous coordinates
  Eigen::Matrix<double, 4, Eigen::Dynamic> src_h;
  src_h.resize(4, src.cols());
  src_h.topRows(3) = src;
  src_h.bottomRows(1) = Eigen::Matrix<double, 1, Eigen::Dynamic>::Ones(N);

  // Apply an arbitrary SE(3) transformation
  Eigen::Matrix4d T;
  // clang-format off
  T << 9.96926560e-01,  6.68735757e-02, -4.06664421e-02, -1.15576939e-01,
      -6.61289946e-02, 9.97617877e-01,  1.94008687e-02, -3.87705398e-02,
      4.18675510e-02, -1.66517807e-02,  9.98977765e-01, 1.14874890e-01,
      0,              0,                0,              1;
  // clang-format on

  // Apply transformation
  Eigen::Matrix<double, 4, Eigen::Dynamic> tgt_h = T * src_h;
  Eigen::Matrix<double, 3, Eigen::Dynamic> tgt = tgt_h.topRows(3);

  // Add some noise & outliers
  addNoiseAndOutliers(tgt);

  // Run TEASER++ registration
  // Prepare solver parameters
  teaser::RobustRegistrationSolver::Params params;
  params.noise_bound = NOISE_BOUND;
  params.cbar2 = 1;
  params.estimate_scaling = false;
  params.rotation_max_iterations = 100;
  params.rotation_gnc_factor = 1.4;
  params.rotation_estimation_algorithm =
      teaser::RobustRegistrationSolver::ROTATION_ESTIMATION_ALGORITHM::GNC_TLS;
  params.rotation_cost_threshold = 0.005;

  // Solve with TEASER++
  teaser::RobustRegistrationSolver solver(params);
  std::chrono::steady_clock::time_point begin = std::chrono::steady_clock::now();
  solver.solve(src, tgt);
  std::chrono::steady_clock::time_point end = std::chrono::steady_clock::now();

  auto solution = solver.getSolution();

  // Compare results
  std::cout << "=====================================" << std::endl;
  std::cout << "          TEASER++ Results           " << std::endl;
  std::cout << "=====================================" << std::endl;
  std::cout << "Expected rotation: " << std::endl;
  std::cout << T.topLeftCorner(3, 3) << std::endl;
  std::cout << "Estimated rotation: " << std::endl;
  std::cout << solution.rotation << std::endl;
  std::cout << "Error (deg): " << getAngularError(T.topLeftCorner(3, 3), solution.rotation)
            << std::endl;
  std::cout << std::endl;
  std::cout << "Expected translation: " << std::endl;
  std::cout << T.topRightCorner(3, 1) << std::endl;
  std::cout << "Estimated translation: " << std::endl;
  std::cout << solution.translation << std::endl;
  std::cout << "Error (m): " << (T.topRightCorner(3, 1) - solution.translation).norm() << std::endl;
  std::cout << std::endl;
  std::cout << "Number of correspondences: " << N << std::endl;
  std::cout << "Number of outliers: " << N_OUTLIERS << std::endl;
  std::cout << "Time taken (s): "
            << std::chrono::duration_cast<std::chrono::microseconds>(end - begin).count() /
                   1000000.0
            << std::endl;
}


// (C) Copyright Andrew Sutton 2007
//
// Use, modification and distribution are subject to the
// Boost Software License, Version 1.0 (See accompanying file
// LICENSE_1_0.txt or http://www.boost.org/LICENSE_1_0.txt)

//[inclusive_mean_geodesic_example
#include <iostream>
#include <iomanip>

#include <boost/graph/directed_graph.hpp>
#include <boost/graph/exterior_property.hpp>
#include <boost/graph/floyd_warshall_shortest.hpp>
#include <boost/graph/geodesic_distance.hpp>
#include "helper.hpp"

using namespace std;
using namespace boost;

// This template structure defines the function that we will apply
// to compute both the per-vertex mean geodesic distances and the
// graph's mean geodesic distance.
template <typename Graph,
          typename DistanceType,
          typename ResultType,
          typename Divides = divides<ResultType> >
struct inclusive_average
{
    typedef DistanceType distance_type;
    typedef ResultType result_type;

    result_type operator ()(distance_type d, const Graph& g)
    {
        if(d == numeric_values<distance_type>::infinity()) {
            return numeric_values<result_type>::infinity();
        }
        else {
            return div(result_type(d), result_type(num_vertices(g)));
        }
    }
    Divides div;
};

// The Page type stores the name of each vertex in the graph and
// represents web pages that can be navigated to.
struct WebPage
{
    string name;
};

// The Link type stores an associated probability of traveling
// from one page to another.
struct Link
{
    float probability;
};

// Declare the graph type and its vertex and edge types.
typedef directed_graph<WebPage, Link> Graph;
typedef graph_traits<Graph>::vertex_descriptor Vertex;
typedef graph_traits<Graph>::edge_descriptor Edge;

// The name map provides an abstract accessor for the names of
// each vertex. This is used during graph creation.
typedef property_map<Graph, string WebPage::*>::type NameMap;

// Declare a matrix type and its corresponding property map that
// will contain the distances between each pair of vertices.
typedef exterior_vertex_property<Graph, float> DistanceProperty;
typedef DistanceProperty::matrix_type DistanceMatrix;
typedef DistanceProperty::matrix_map_type DistanceMatrixMap;

// Declare the weight map as an accessor into the bundled
// edge property.
typedef property_map<Graph, float Link::*>::type WeightMap;

// Declare a container and its corresponding property map that
// will contain the resulting mean geodesic distances of each
// vertex in the graph.
typedef exterior_vertex_property<Graph, float> GeodesicProperty;
typedef GeodesicProperty::container_type GeodesicContainer;
typedef GeodesicProperty::map_type GeodesicMap;

static float exclusive_geodesics(const Graph&, DistanceMatrixMap, GeodesicMap);
static float inclusive_geodesics(const Graph&, DistanceMatrixMap, GeodesicMap);

int
main(int argc, char *argv[])
{
    // Create the graph, a name map that providse abstract access
    // to the web page names, and the weight map as an accessor to
    // the edge weights (or probabilities).
    Graph g;
    NameMap nm(get(&WebPage::name, g));
    WeightMap wm(get(&Link::probability, g));

    // Read the weighted graph from standard input.
    read_weighted_graph(g, nm, wm, cin);

    // Compute the distances between all pairs of vertices using
    // the Floyd-Warshall algorithm. The weight map was created
    // above so it could be populated when the graph was read in.
    DistanceMatrix distances(num_vertices(g));
    DistanceMatrixMap dm(distances, g);
    floyd_warshall_all_pairs_shortest_paths(g, dm, weight_map(wm));

    // Create the containers and the respective property maps that
    // will contain the mean geodesics averaged both including
    // self-loop distances and excluding them.
    GeodesicContainer exclude(num_vertices(g));
    GeodesicContainer include(num_vertices(g));
    GeodesicMap exmap(exclude, g);
    GeodesicMap inmap(include, g);

    float ex = exclusive_geodesics(g, dm, exmap);
    float in = inclusive_geodesics(g, dm, inmap);

    // Print the mean geodesic distance of each vertex and finally,
    // the graph itself.
    cout << setw(12) << setiosflags(ios::left) << "vertex";
    cout << setw(12) << setiosflags(ios::left) << "excluding";
    cout << setw(12) << setiosflags(ios::left) << "including" << endl;
    graph_traits<Graph>::vertex_iterator i, end;
    for(tie(i, end) = vertices(g); i != end; ++i) {
        cout << setw(12) << setiosflags(ios::left)
             << g[*i].name
             << setw(12) << get(exmap, *i)
             << setw(12) << get(inmap, *i) << endl;
    }
    cout << "small world (excluding self-loops): " << ex << endl;
    cout << "small world (including self-loops): " << in << endl;

    return 0;
}

float
exclusive_geodesics(const Graph& g, DistanceMatrixMap dm, GeodesicMap gm)
{
    // Compute the mean geodesic distances, which excludes distances
    // of self-loops by default. Return the measure for the entire graph.
    return all_mean_geodesics(g, dm, gm);
}

float
inclusive_geodesics(const Graph &g, DistanceMatrixMap dm, GeodesicMap gm)
{
    // Create a new measure object for computing the mean geodesic
    // distance of all vertices. This measure will actually be used
    // for both averages.
    inclusive_average<Graph, float, float> m;

    // Compute the mean geodesic distance using the inclusive average
    // to account for self-loop distances. Return the measure for the
    // entire graph.
    return all_mean_geodesics(g, dm, gm, m);
}
//]


/*
 * resizing_lattice.cpp
 *
 * Demonstrates the usage of resizing of the state type during integration.
 * Examplary system is a strongly nonlinear, disordered Hamiltonian lattice
 * where the spreading of energy is invastigated
 *
 * Copyright 2009-2012 Karsten Ahnert and Mario Mulansky.
 * 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 <iostream>
#include <utility>

#include <boost/numeric/odeint.hpp>

#include <boost/ref.hpp>
#include <boost/random.hpp>

using namespace std;
using namespace boost::numeric::odeint;

//[ resizing_lattice_system_class
typedef vector< double > coord_type;
typedef pair< coord_type , coord_type > state_type;

struct compacton_lattice
{
    const int m_max_N;
    const double m_beta;
    int m_pot_start_index;
    vector< double > m_pot;

    compacton_lattice( int max_N , double beta , int pot_start_index )
        : m_max_N( max_N ) , m_beta( beta ) , m_pot_start_index( pot_start_index ) , m_pot( max_N )
    {
        srand( time( NULL ) );
        // fill random potential with iid values from [0,1]
        boost::mt19937 rng;
        boost::uniform_real<> unif( 0.0 , 1.0 );
        boost::variate_generator< boost::mt19937&, boost::uniform_real<> > gen( rng , unif );
        generate( m_pot.begin() , m_pot.end() , gen );
    }

    void operator()( const coord_type &q , coord_type &dpdt )
    {
        // calculate dpdt = -dH/dq of this hamiltonian system
        // dp_i/dt = - V_i * q_i^3 - beta*(q_i - q_{i-1})^3 + beta*(q_{i+1} - q_i)^3
        const int N = q.size();
        double diff = q[0] - q[N-1];
        for( int i=0 ; i<N ; ++i )
        {
            dpdt[i] = - m_pot[m_pot_start_index+i] * q[i]*q[i]*q[i] -
                    m_beta * diff*diff*diff;
            diff = q[(i+1) % N] - q[i];
            dpdt[i] += m_beta * diff*diff*diff;
        }
    }

    void energy_distribution( const coord_type &q , const coord_type &p , coord_type &energies )
    {
        // computes the energy per lattice site normalized by total energy
        const size_t N = q.size();
        double en = 0.0;
        for( size_t i=0 ; i<N ; i++ )
        {
            const double diff = q[(i+1) % N] - q[i];
            energies[i] = p[i]*p[i]/2.0
                + m_pot[m_pot_start_index+i]*q[i]*q[i]*q[i]*q[i]/4.0
                + m_beta/4.0 * diff*diff*diff*diff;
            en += energies[i];
        }
        en = 1.0/en;
        for( size_t i=0 ; i<N ; i++ )
        {
            energies[i] *= en;
        }
    }

    double energy( const coord_type &q , const coord_type &p )
    {
        // calculates the total energy of the excitation
        const size_t N = q.size();
        double en = 0.0;
        for( size_t i=0 ; i<N ; i++ )
        {
            const double diff = q[(i+1) % N] - q[i];
            en += p[i]*p[i]/2.0
                + m_pot[m_pot_start_index+i]*q[i]*q[i]*q[i]*q[i] / 4.0
                + m_beta/4.0 * diff*diff*diff*diff;
        }
        return en;
    }

    void change_pot_start( const int delta )
    {
        m_pot_start_index += delta;
    }
};
//]

//[ resizing_lattice_resize_function
void do_resize( coord_type &q , coord_type &p , coord_type &distr , const int N )
{
    q.resize( N );
    p.resize( N );
    distr.resize( N );
}
//]

const int max_N = 1024;
const double beta = 1.0;

int main()
{
    //[ resizing_lattice_initialize
    //start with 60 sites
    const int N_start = 60;
    coord_type q( N_start , 0.0 );
    q.reserve( max_N );
    coord_type p( N_start , 0.0 );
    p.reserve( max_N );
    // start with uniform momentum distribution over 20 sites
    fill( p.begin()+20 , p.end()-20 , 1.0/sqrt(20.0) );

    coord_type distr( N_start , 0.0 );
    distr.reserve( max_N );

    // create the system
    compacton_lattice lattice( max_N , beta , (max_N-N_start)/2 );

    //create the stepper, note that we use an always_resizer because state size might change during steps
    typedef symplectic_rkn_sb3a_mclachlan< coord_type , coord_type , double , coord_type , coord_type , double ,
            range_algebra , default_operations , always_resizer > hamiltonian_stepper;
    hamiltonian_stepper stepper;
    hamiltonian_stepper::state_type state = make_pair( q , p );
    //]

    //[ resizing_lattice_steps_loop
    double t = 0.0;
    const double dt = 0.1;
    const int steps = 10000;
    for( int step = 0 ; step < steps ; ++step )
    {
        stepper.do_step( boost::ref(lattice) , state , t , dt );
        lattice.energy_distribution( state.first , state.second , distr );
        if( distr[10] > 1E-150 )
        {
            do_resize( state.first , state.second , distr , state.first.size()+20 );
            rotate( state.first.begin() , state.first.end()-20 , state.first.end() );
            rotate( state.second.begin() , state.second.end()-20 , state.second.end() );
            lattice.change_pot_start( -20 );
            cout << t << ": resized left to " << distr.size() << ", energy = " << lattice.energy( state.first , state.second ) << endl;
        }
        if( distr[distr.size()-10] > 1E-150 )
        {
            do_resize( state.first , state.second , distr , state.first.size()+20 );
            cout << t << ": resized right to " << distr.size() << ", energy = " << lattice.energy( state.first , state.second ) << endl;
        }
        t += dt;
    }
    //]

    cout << "final lattice size: " << distr.size() << ", final energy: " << lattice.energy( state.first , state.second ) << endl;
}


#include <iostream>
#include <fstream>
#include <string>
#include <tuple>
#include <vector>
#include <queue>
#include <boost/algorithm/string.hpp>
#include <math.h>
#include <chrono>
#include <stdlib.h>

using namespace std;

typedef std::chrono::high_resolution_clock Clock;
typedef std::vector<vector<int>> AdjList;

double distance(const tuple<int, int> &p1, const tuple<int, int> &p2) {
  return sqrt( (std::get<0>(p1) - std::get<0>(p2)) * (std::get<0>(p1) - std::get<0>(p2)) + (std::get<1>(p1) - std::get<1>(p2)) * (std::get<1>(p1) - std::get<1>(p2)) );
}

std::vector<std::vector<double>> createAdjMatrix (const std::vector<std::tuple<int, int>> &nodes){
  std::vector<std::vector<double>> adjMatrix(nodes.size(), vector<double>(nodes.size()));
  for (int i = 0; i < nodes.size(); i++) {
    for (int j = 0; j < nodes.size(); j++){
      adjMatrix[i][j] = distance(nodes[i], nodes[j]);
    }
  }
  return adjMatrix;
}

double pathWeightVsReverse(std::vector<int> &path, int i, int j, const std::vector<std::vector<double>> &adjMatrix){
  if (j-1 < 0)
    return ( adjMatrix[path[i]][path[(i+1)%path.size()]] + adjMatrix[path[path.size()-1]][path[j]] ) - ( adjMatrix[path[i]][path[path.size()-1]] + adjMatrix[path[(i+1)%path.size()]][path[j]] );
  else
    return ( adjMatrix[path[i]][path[(i+1)%path.size()]] + adjMatrix[path[j-1]][path[j]] ) - ( adjMatrix[path[i]][path[j-1]] + adjMatrix[path[(i+1)%path.size()]][path[j]] );
}

void printTour(std::vector<int> &tour, const std::vector<tuple<int,int>> &nodes, std::string name){
  //cout << name << "  TOUR: ";
  for ( const auto &i : tour) {
     cout << "(" << get<0>(nodes[i]) << ", " << get<1>(nodes[i]) << ")" << endl;
  }
}

void printTour(std::vector<int> &tour){
  cout << "TOUR: ";
  for ( const auto &i : tour) {
    cout << i << " ";
  }
  cout << endl;
}

void reverseSubtour (std::vector<int> &tour, int &i, int &j) {
  if (j <= i) {
    /**/
    vector <int> subtour = vector<int>(tour.begin() + i + 1, tour.end());
    subtour.insert(subtour.end(),  tour.begin(), tour.begin() + j);

    int index = i+1;
    for (std::vector<int>::reverse_iterator it = subtour.rbegin(); it != subtour.rend(); it++) {
      tour[index%tour.size()] = *it;
      index += 1;
    }

    //cout << "REVERSING SUBOTUR: " << i << ", " << j << endl;
    /*
    int tourSize = tour.size();
    int a = i + 1;
    int b = (j-1) < 0 ? tourSize - 1 : j -1;
    int tmp;
    for (int c = 0; c < (tourSize - i + j - 2)/2; c++) {
        //printTour(tour);
        //cout << tourSize - i + j - 1 - c << "\tswapping a: " << a << ", b: " << b << endl;
        //cout << "\tswapping tour[a]: " << tour[a] << ", tour[b]: " << tour[b] << endl;
        tmp = tour[a];
        tour[a] = tour[b];
        tour[b] = tmp;
        a = (a + 1) % tourSize;
        b = (b - 1) < 0 ? tourSize - 1 : b - 1;
        //cout <<"\t\t" << a << ", " << b << endl;
    }
   */
  } else {
    std::reverse(tour.begin() + i + 1, tour.begin() + j);
  }
}

double tourWeight(std::vector<int> tour, std::vector<std::vector<double>> adjMatrix) {
  double tourWeight = 0;
  for (int i = 0; i < tour.size(); i++)
    tourWeight += adjMatrix[tour[i]][tour[(i+1)%tour.size()]];
  return tourWeight;
}

std::vector<int> reverseTours (std::vector<int> &tour, const std::vector<std::tuple<int, int>> &nodes, const std::vector<std::vector<double>> &adjMatrix){
  int counter = 0;
  int tourSize = tour.size();
  int tourSizeSq = tourSize*tourSize;
  int subtourSize;

  while (counter < tourSizeSq) {
    for (int i = 0; i < tourSize; i++) {
      for (int j = 0; j < tourSize; j++){

        subtourSize = ((j <= i) ? ((tourSize - i) + j) : (j - i));

        if (subtourSize > 3) {
           if (pathWeightVsReverse(tour, i, j, adjMatrix) > 0) {
             reverseSubtour(tour, i, j);
             counter = 0;
           }
        }
        counter += 1;
        if (counter > tourSizeSq) { goto label2; }
      }
    }
  }
label2:

  return tour;
}

std::vector<std::tuple<int,int>> readPipedInput() {
  std::string line;
  std::vector<std::string> strs;
  std::vector<std::tuple<int, int>> nodes;
  int i = 0; int j = 0;
  std::string black_dot = "0";
  while (std::getline(std::cin, line))  {
    boost::split(strs, line, boost::is_any_of(" \n"));
    for (std::vector<std::string>::const_iterator it = strs.begin(); it != strs.end(); ++it) {
        if (*it == black_dot){nodes.push_back(std::make_tuple(i, j));}
        j += 1;
     }
    i += 1;
    j = 0;
  }
  return nodes;
}
std::vector<std::tuple<int, int>> readInput(string const &str1) {
  string line;
  std::vector<std::string> strs;
  std::vector<std::tuple<int, int>> nodes;
  ifstream myfile (str1);
  int i = 0; int j = 0;
  std::basic_string<char> black_dot = "0";
  if (myfile.is_open())
  {
    while ( getline (myfile,line) )
    {
      boost::split(strs, line, boost::is_any_of(" \n"));
      for (std::vector<std::string>::const_iterator it = strs.begin(); it != strs.end(); ++it) {
          if (*it == black_dot){nodes.push_back(std::make_tuple(i, j));}
          j += 1;
       }
      i += 1;
      j = 0;
    }
    myfile.close();
  }
  else cout << "Unable to open file";
  return nodes;
}

struct compareNodeWeight {
  bool operator()(const std::tuple<double, int, int> &lhs, std::tuple<double,int,int> &rhs) {
    return std::get<0>(lhs) >  std::get<0>(rhs);
  }
};

int searchHeap(std::vector<std::tuple<double,int,int>> &heap, int vertex) {
  for (int i = 0; i < heap.size(); i++) {
    if (std::get<1>(heap[i]) == vertex) {
      return i;
    }
  }
  return -1;
}

void siftUp(std::vector<std::tuple<double,int,int>> &heap, int index) {
  int parent = (index - 1) / 2;
  double indexValue = std::get<0>(heap[parent]);
  double parentValue = std::get<0>(heap[index]);
  std::tuple<double,int,int> tmp;

  while (index != 0 && std::get<0>(heap[(index-1)/2]) > std::get<0>(heap[index]) ) {
    parent = (index - 1) / 2;
    indexValue = std::get<0>(heap[index]);
    parentValue = std::get<0>(heap[parent]);
    tmp = heap[index];
    heap[index] = heap[parent];
    heap[parent] = tmp;
    index = parent;
  }
}

std::vector<std::tuple<int,int>> prims (std::vector<std::vector<double>> adjMatrix) {
  std::vector<int> knownNodes(adjMatrix.size(), 0); // knownNodes[i] = 0 iff i is not known, else 1

  std::vector<std::tuple<double,int,int>> heap;
  for (int i = 1; i < adjMatrix.size(); i++ ){
    heap.push_back(std::make_tuple(INT_MAX, i, -1));
  }

  heap.push_back(std::make_tuple(0, 0, -1));
  std::make_heap(heap.begin(), heap.end(), compareNodeWeight());

  std::tuple<double, int, int> minVal;
  double weight; int u; int u_prev;
  std::vector<std::tuple<int,int>> edges;
  int index;

  while (heap.size() > 0) {
    std::pop_heap(heap.begin(), heap.end(), compareNodeWeight());
    minVal = heap.back();
    heap.pop_back();
    weight = std::get<0>(minVal);
    u = std::get<1>(minVal);
    u_prev = std::get<2>(minVal);

    if (knownNodes[u] == 0) {
      knownNodes[u] = 1;
      if (u_prev != -1) {
        edges.push_back(std::make_tuple(u, u_prev));
      }
    }
    for (int v = 0; v < adjMatrix.size(); v++ ) {
      if (knownNodes[v] == 0) {
        index = searchHeap(heap, v);
        if (index != -1) {
          if (weight + adjMatrix[u][v] < std::get<0>(heap[index])) {
            heap[index] = std::make_tuple(weight + adjMatrix[u][v], v, u);
            siftUp(heap, index);
          }
        }
      }
    }
  }
  return edges;
}

std::vector<vector<int>> constructAdjList(std::vector<std::tuple<int,int>> &MST, const std::vector<std::tuple<int,int>> &nodes) {
  std::vector<std::vector<int>> adjList(nodes.size(), std::vector<int>());

  for (int i = 0; i < MST.size(); i++) {
    std::tuple<int,int> edge = MST[i];
    adjList[std::get<0>(edge)].push_back(std::get<1>(edge));
    adjList[std::get<1>(edge)].push_back(std::get<0>(edge));
  }
  return adjList;
}

std::vector<int> DFSUtil(int v, bool visited[], const std::vector<std::vector<int>> &adjList, std::vector<int> &tour) {
    visited[v] = true;
    tour.push_back(v);

    for (std::vector<int>::const_iterator i = adjList[v].begin(); i != adjList[v].end(); ++i)
        if (!visited[*i])
            DFSUtil(*i, visited, adjList, tour);
    return tour;
}

std::vector<int> DFS(int v, const std::vector<std::vector<int>> adjList) {
    bool *visited = new bool[adjList.size()];
    for (int i = 0; i < adjList.size(); i++)
        visited[i] = false;
    std::vector<int> dfsTour;
    std::vector<int> tour = DFSUtil(v, visited, adjList, dfsTour);
    return tour;
}

std::vector<int> nearestNeighbor(std::vector<int> &tour, const std::vector<std::vector<double>> &adjMatrix) {
  int visitedVertices[adjMatrix.size()] = {0};
  int visitedCount = 0;

  int currentVertex = 0;
  int nextVertex;
  double minEdgeWeight;

  std::vector<int> nnTour;

  while(visitedCount < adjMatrix.size()) {
    nnTour.push_back(currentVertex);
    minEdgeWeight = INT_MAX;
    for (int v = 0; v < adjMatrix.size(); v++){
      if (!visitedVertices[v] && currentVertex != v && adjMatrix[currentVertex][v] < minEdgeWeight) {
        minEdgeWeight = adjMatrix[currentVertex][v];
        nextVertex = v;
      }
    }
    visitedVertices[currentVertex] = 1;
    currentVertex = nextVertex;
    visitedCount+=1;
  }
  return nnTour;

}

int main (int argc, char *argv[]) {
  /*
  std::string input_file = argv[1];
  cout << "INPUT FILE: " << input_file << endl;
  const std::basic_string<char> T = input_file;
  */
  const std::vector<std::tuple<int, int>> nodes = readPipedInput();

  std::vector<int> tour;
  for (int i=0; i<nodes.size(); ++i) tour.push_back(i);
  //printTour(tour, nodes, "ORIGINAL");

  //cout << "Number of cities: " << tour.size() << endl;
  auto t1 = Clock::now();
  const std::vector<std::vector<double>> adjMatrix = createAdjMatrix(nodes);
  auto t2 = Clock::now();
  /*std::cout << "Create AdjMatrix Delta t2-t1: "
            << std::chrono::duration_cast<std::chrono::milliseconds>(t2 - t1).count()
            << " milliseconds" << std::endl;
  */
//  tour = nearestNeighbor(tour, adjMatrix);
//  printTour(tour, nodes, "NEAREST NEIGHBOR");

  t1 = Clock::now();
  std::vector<tuple<int,int>> MST = prims(adjMatrix);
  t2 = Clock::now();
  /*std::cout << "PRIMS Delta t2-t1: "
            << std::chrono::duration_cast<std::chrono::milliseconds>(t2 - t1).count()
            << " milliseconds" << std::endl;
  */

  t1 = Clock::now();
  const std::vector<vector<int>> adjList = constructAdjList(MST, nodes);
  t2 = Clock::now();
  /*std::cout << "Construct AdjList  Delta t2-t1: "
            << std::chrono::duration_cast<std::chrono::milliseconds>(t2 - t1).count()
            << " milliseconds" << std::endl;
  */
  t1 = Clock::now();
  tour = DFS(0, adjList);
  t2 = Clock::now();
  //printTour(tour, nodes, "DFS");
  /*std::cout << "DFS  Delta t2-t1: "
            << std::chrono::duration_cast<std::chrono::milliseconds>(t2 - t1).count()
            << " seconds" << std::endl;
  */

  t1 = Clock::now();
  std::vector<int> reversedTour = reverseTours(tour, nodes, adjMatrix);
  t2 = Clock::now();
  printTour(tour, nodes, "REVERSED");
  /*cout << "Number of cities: " << tour.size() << endl;
  std::cout << "Reverse Tour Delta t2-t1: "
            << std::chrono::duration_cast<std::chrono::milliseconds>(t2 - t1).count()
            << " milliseconds" << std::endl;
  */
  return 0;
}


#include <CGAL/Exact_predicates_inexact_constructions_kernel.h>
#include <CGAL/Polyhedron_3.h>
#include <CGAL/Simple_cartesian.h>
#include <CGAL/Surface_mesh.h>

#include <CGAL/optimal_bounding_box.h>
#include <CGAL/Polygon_mesh_processing/measure.h>
#include <CGAL/IO/read_off_points.h>

#include <fstream>
#include <iostream>
#include <utility>
#include <limits>

#ifdef BUILD_PY

#include <Python.h>
#include <boost/python.hpp>
#include <boost/python/list.hpp>

#endif

typedef CGAL::Exact_predicates_inexact_constructions_kernel Kernel;
typedef Kernel::Point_3                                     Point;
typedef Kernel::Aff_transformation_3                        Aff_transformation;
typedef CGAL::Surface_mesh<Point>                           Surface_mesh;

/**
 * returns the transformation matrix and the
 */
std::pair<std::array<double, 16>, std::array<double,3>> createOptimalBoundingBox(std::string pathToSTL)
{
  std::ifstream stream(pathToSTL);
  std::list<Point> p;
  if (!stream || !CGAL::read_off_points(stream, std::back_inserter(p)))
  {
      std::cerr << "Error: cannot read file data" << std::endl;
      throw std::exception();
  }
  // compute rotated optimal bounding box
  std::array<Point, 8> obb_points;
  CGAL::oriented_bounding_box(p, obb_points, CGAL::parameters::use_convex_hull(true));

  /* untested */
  Aff_transformation tf;
  CGAL::oriented_bounding_box(p, tf, CGAL::parameters::use_convex_hull(true));

  std::array<double, 6> bb_min_max;
  for(int i = 0; i < 3; i++)
  {
    bb_min_max[i*2+0] = DBL_MIN;
    bb_min_max[i*2+1] = DBL_MAX;

  }
  // rotate points such that the bounding is axis aligned
  // and get the minimum and maximum values for each axis
  std::array<Point, 8> obb_points_rotated;
  for(int i = 0; i < obb_points.size(); i++)
  {
    obb_points_rotated[i] = tf.transform(obb_points[i]);
    bb_min_max[0] = std::max(bb_min_max[0], obb_points_rotated[i].x());
    bb_min_max[1] = std::min(bb_min_max[1], obb_points_rotated[i].x());
    bb_min_max[2] = std::max(bb_min_max[2], obb_points_rotated[i].y());
    bb_min_max[3] = std::min(bb_min_max[3], obb_points_rotated[i].y());
    bb_min_max[4] = std::max(bb_min_max[4], obb_points_rotated[i].z());
    bb_min_max[5] = std::min(bb_min_max[5], obb_points_rotated[i].z());
  }

  std::array<double,3> bb_size;
  bb_size[0] =  bb_min_max[0] - bb_min_max[1];
  bb_size[1] =  bb_min_max[2] - bb_min_max[3];
  bb_size[2] =  bb_min_max[4] - bb_min_max[5];
  //std::cout << "size xyz: " << bb_size[0] <<  " " << bb_size[1] << " " << bb_size[1] << std::endl;

  double offset_x = bb_size[0]/2 - bb_min_max[0];
  double offset_y = bb_size[1]/2 - bb_min_max[2];
  double offset_z = bb_size[2]/2 - bb_min_max[4];
  //std::cout << "x_off: " << offset_x << " y_off: "  << offset_y << " z_off: " << offset_z << " " << std::endl;

  // apply offset to each point of the bounding box for when it is written to file
  Kernel::Vector_3 off(offset_x, offset_y, offset_z);
  for(int i = 0; i<8; i++)
  {
    obb_points_rotated[i] += off;
  }

  // Create mesh, both rotated and not rotated and write each to a file
  Surface_mesh obb_sm;
  CGAL::make_hexahedron(obb_points[0], obb_points[1], obb_points[2], obb_points[3],
                        obb_points[4], obb_points[5], obb_points[6], obb_points[7], obb_sm);
  std::ofstream("box.off") << obb_sm;
  Surface_mesh obb_sm_rotated;
  CGAL::make_hexahedron(obb_points_rotated[0], obb_points_rotated[1], obb_points_rotated[2], obb_points_rotated[3],
                        obb_points_rotated[4], obb_points_rotated[5], obb_points_rotated[6], obb_points_rotated[7], obb_sm_rotated);
  std::ofstream("box_rotated.off") << obb_sm_rotated;

  // put the values from the affine transformation matrix into a standard cpp array
  std::array<double,16> tf_array;
  for(int i = 0; i < 4; i++)
  {
    for(int j = 0; j < 4; j++)
    {
      tf_array[i*4+j] = tf.m(i,j);
    }
  }
  // translation of the bounding box is not included in the affine transformation matrix
  // therefore we add it here
  Kernel::Vector_3 off_rotated = tf.inverse().transform(off);
  tf_array[3] = -off_rotated.x();
  tf_array[7] = -off_rotated.y();
  tf_array[11] = -off_rotated.z();

  return std::make_pair(tf_array, bb_size);
}

#ifdef BUILD_PY
boost::python::list optimalBoundingBoxWrapper(std::string filename)
{
  std::pair<std::array<double, 16>, std::array<double,3>> bb_tf_and_size = createOptimalBoundingBox(filename);
  boost::python::list return_value;

  boost::python::list py_tf;
  for (double d : bb_tf_and_size.first)
  {
    py_tf.append(d);
  }
  boost::python::list py_bb_size;
  for (double limit : bb_tf_and_size.second)
  {
    py_bb_size.append(limit);
  }
  return_value.append(py_tf);
  return_value.append(py_bb_size);

  return return_value;
}
BOOST_PYTHON_MODULE(py_optimal_bounding_box)
{
    using namespace boost::python;

    def("create_optimal_bounding_box", &optimalBoundingBoxWrapper);
}
#endif

// main function if it is called by itself, takes filename as cli input
// prints resulting bounding box and saves it as .off file
int main(int argc, char** argv)
{
  if(argc != 2)
  {
      std::cerr << "Usage: " << argv[0] << " <input file.off>" << std::endl;
      return EXIT_FAILURE;
  }
  auto a = createOptimalBoundingBox(argv[1]);
  // TODO do something with result
}


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

  @copyright 2016 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_FUNCTION_ATANH_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_ATANH_HPP_INCLUDED

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{

 /*!

    @ingroup group-hyperbolic
    Function object implementing atanh capabilities

    Returns the hyperbolic tangent argument \f$\frac12\log\frac{1+x}{1-x}\f$

    @par Semantic:

    For every parameter of floating type @c T

    @code
    T r = atanh(x);
    @endcode

    @par Decorators

    - raw_  is faster but inaccurate near Zero

    @see log, Half, oneminus, inc

  **/
  Value atanh(Value const & v0);
} }
#endif

#include <boost/simd/function/scalar/atanh.hpp>
#include <boost/simd/function/simd/atanh.hpp>

#endif


#define BOOST_TEST_NO_LIB
#include <boost/test/auto_unit_test.hpp>

#include "coconut/pulp/math/Angle.hpp"
#include "coconut/pulp/math/Rotation.hpp"

using namespace coconut;
using namespace coconut::pulp;
using namespace coconut::pulp::math;

namespace /* anonymous */ {

BOOST_AUTO_TEST_SUITE(PulpTestSuite);
BOOST_AUTO_TEST_SUITE(PulpMathTestSuite);
BOOST_AUTO_TEST_SUITE(PulpMathRotationTestSuite);

BOOST_AUTO_TEST_CASE(AppliesRotationToVector) {
	const auto rotation = Rotation({ 2.0f, 1.0f, 3.0f }, -23.0_deg);

	const auto start = Vec3(1.0f, 3.0f, 5.0f);
	const auto end = Vec3(1.565343f, 3.60607f, 4.421081f);
	const auto rotated = rotation.apply(start);
	BOOST_CHECK_EQUAL(rotated, end);
}

BOOST_AUTO_TEST_CASE(AppendsRotations) {
	const auto first = Rotation({ 2.0f, 1.0f, 3.0f }, -23.0_deg);
	const auto then = Rotation({ 0.0f, 1.0f, 0.0f }, 180.0_deg);
	const auto rotation = first << then;

	const auto start = Vec3(1.0f, 3.0f, 5.0f);
	const auto end = Vec3(-1.565343f, 3.60607f, -4.421081f);
	const auto rotated = rotation.apply(start);
	BOOST_CHECK_EQUAL(rotated, end);
}

BOOST_AUTO_TEST_CASE(LinearlyInterpolatesRotations) {
	const auto r1 = Rotation({ 0.0f, 1.0f, 0.0f }, 0.0_deg);
	const auto r2 = Rotation({ 0.0f, 1.0f, 0.0f }, 10.0_deg);

	const auto interpolated = lerp(r1, r2, 0.25f);
	const auto expected = Rotation({ 0.0f, 1.0f, 0.0f }, 2.5_deg);

	BOOST_CHECK_EQUAL(interpolated.rotationQuaternion(), expected.rotationQuaternion());
}

BOOST_AUTO_TEST_CASE(SphericallyLinearlyInterpolatesRotations) {
	const auto r1 = Rotation({ 0.0f, 1.0f, 0.0f }, 0.0_deg);
	const auto r2 = Rotation({ 0.0f, 1.0f, 0.0f }, 90.0_deg);

	const auto interpolated = slerp(r1, r2, 0.25f);
	const auto expected = Rotation({ 0.0f, 1.0f, 0.0f }, degrees(90.0f * 0.25f));

	BOOST_CHECK_EQUAL(interpolated.rotationQuaternion(), expected.rotationQuaternion());
}

BOOST_AUTO_TEST_SUITE_END(/* PulpMathRotationTestSuite */);
BOOST_AUTO_TEST_SUITE_END(/* PulpMathTestSuite */);
BOOST_AUTO_TEST_SUITE_END(/* PulpTestSuite */);

} // anonymous namespace


// Software License for MTL
//
// Copyright (c) 2007 The Trustees of Indiana University.
//               2008 Dresden University of Technology and the Trustees of Indiana University.
//               2010 SimuNova UG (haftungsbeschränkt), www.simunova.com.
// All rights reserved.
// Authors: Peter Gottschling and Andrew Lumsdaine
//
// This file is part of the Matrix Template Library
//
// See also license.mtl.txt in the distribution.

#include <boost/numeric/meta_math/power_of_2.hpp>
#include <boost/numeric/mtl/utility/exception.hpp>

void test_power_of_2()
{
    using namespace meta_math;

    MTL_THROW_IF(power_of_2<0>::value != 1, mtl::runtime_error("wrong value"));
    MTL_THROW_IF(power_of_2<1>::value != 2, mtl::runtime_error("wrong value"));
    MTL_THROW_IF(power_of_2<2>::value != 4, mtl::runtime_error("wrong value"));
    MTL_THROW_IF(power_of_2<3>::value != 8, mtl::runtime_error("wrong value"));
}

int main()
{
    test_power_of_2();
    return 0;
}


/* test_cauchy.cpp
 *
 * Copyright Steven Watanabe 2011
 * 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)
 *
 * $Id$
 *
 */

#include <boost/random/cauchy_distribution.hpp>
#include <boost/random/uniform_real.hpp>
#include <boost/math/distributions/cauchy.hpp>

#define BOOST_RANDOM_DISTRIBUTION boost::random::cauchy_distribution<>
#define BOOST_RANDOM_DISTRIBUTION_NAME cauchy
#define BOOST_MATH_DISTRIBUTION boost::math::cauchy
#define BOOST_RANDOM_ARG1_TYPE double
#define BOOST_RANDOM_ARG1_NAME median
#define BOOST_RANDOM_ARG1_DEFAULT 1000.0
#define BOOST_RANDOM_ARG1_DISTRIBUTION(n) boost::uniform_real<>(-n, n)
#define BOOST_RANDOM_ARG2_TYPE double
#define BOOST_RANDOM_ARG2_NAME sigma
#define BOOST_RANDOM_ARG2_DEFAULT 1000.0
#define BOOST_RANDOM_ARG2_DISTRIBUTION(n) boost::uniform_real<>(0.001, n)

#include "test_real_distribution.ipp"


// Copyright Nick Thompson, 2019
// 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 vector_barycentric_rational

#include <cmath>
#include <random>
#include <array>
#include <Eigen/Dense>
#include <boost/numeric/ublas/vector.hpp>
#include <boost/random/uniform_real_distribution.hpp>
#include <boost/type_index.hpp>
#include <boost/test/included/unit_test.hpp>
#include <boost/test/tools/floating_point_comparison.hpp>
#include <boost/math/interpolators/barycentric_rational.hpp>
#include <boost/math/interpolators/vector_barycentric_rational.hpp>

using std::sqrt;
using std::abs;
using std::numeric_limits;

template<class Real>
void test_agreement_with_1d()
{
    std::cout << "Testing with 1D interpolation on type "
              << boost::typeindex::type_id<Real>().pretty_name()  << "\n";
    std::mt19937 gen(4723);
    boost::random::uniform_real_distribution<Real> dis(0.1f, 1);
    std::vector<Real> t(100);
    std::vector<Eigen::Vector2d> y(100);
    t[0] = dis(gen);
    y[0][0] = dis(gen);
    y[0][1] = dis(gen);
    for (size_t i = 1; i < t.size(); ++i)
    {
        t[i] = t[i-1] + dis(gen);
        y[i][0] = dis(gen);
        y[i][1] = dis(gen);
    }

    std::vector<Eigen::Vector2d> y_copy = y;
    std::vector<Real> t_copy = t;
    std::vector<Real> t_copy0 = t;
    std::vector<Real> t_copy1 = t;

    std::vector<Real> y_copy0(y.size());
    std::vector<Real> y_copy1(y.size());
    for (size_t i = 0; i < y.size(); ++i) {
        y_copy0[i] = y[i][0];
        y_copy1[i] = y[i][1];
    }

    boost::random::uniform_real_distribution<Real> dis2(t[0], t[t.size()-1]);
    boost::math::interpolators::vector_barycentric_rational<decltype(t), decltype(y)> interpolator(std::move(t), std::move(y));
    boost::math::interpolators::barycentric_rational<Real> scalar_interpolator0(std::move(t_copy0), std::move(y_copy0));
    boost::math::interpolators::barycentric_rational<Real> scalar_interpolator1(std::move(t_copy1), std::move(y_copy1));

    Eigen::Vector2d z;

    size_t samples = 0;
    while (samples++ < 1000)
    {
        Real t = dis2(gen);
        interpolator(z, t);
        BOOST_CHECK_CLOSE(z[0], scalar_interpolator0(t), 10000*numeric_limits<Real>::epsilon());
        BOOST_CHECK_CLOSE(z[1], scalar_interpolator1(t), 10000*numeric_limits<Real>::epsilon());
    }
}

template<class Real>
void test_interpolation_condition_eigen()
{
    std::cout << "Testing interpolation condition for barycentric interpolation on Eigen vectors of type "
              << boost::typeindex::type_id<Real>().pretty_name()  << "\n";
    std::mt19937 gen(4723);
    boost::random::uniform_real_distribution<Real> dis(0.1f, 1);
    std::vector<Real> t(100);
    std::vector<Eigen::Vector2d> y(100);
    t[0] = dis(gen);
    y[0][0] = dis(gen);
    y[0][1] = dis(gen);
    for (size_t i = 1; i < t.size(); ++i)
    {
        t[i] = t[i-1] + dis(gen);
        y[i][0] = dis(gen);
        y[i][1] = dis(gen);
    }

    std::vector<Eigen::Vector2d> y_copy = y;
    std::vector<Real> t_copy = t;
    boost::math::interpolators::vector_barycentric_rational<decltype(t), decltype(y)> interpolator(std::move(t), std::move(y));

    Eigen::Vector2d z;
    for (size_t i = 0; i < t_copy.size(); ++i)
    {
        interpolator(z, t_copy[i]);
        BOOST_CHECK_CLOSE(z[0], y_copy[i][0], 100*numeric_limits<Real>::epsilon());
        BOOST_CHECK_CLOSE(z[1], y_copy[i][1], 100*numeric_limits<Real>::epsilon());
    }
}

template<class Real>
void test_interpolation_condition_std_array()
{
    std::cout << "Testing interpolation condition for barycentric interpolation on std::array vectors of type "
              << boost::typeindex::type_id<Real>().pretty_name()  << "\n";
    std::mt19937 gen(4723);
    boost::random::uniform_real_distribution<Real> dis(0.1f, 1);
    std::vector<Real> t(100);
    std::vector<std::array<Real, 2>> y(100);
    t[0] = dis(gen);
    y[0][0] = dis(gen);
    y[0][1] = dis(gen);
    for (size_t i = 1; i < t.size(); ++i)
    {
        t[i] = t[i-1] + dis(gen);
        y[i][0] = dis(gen);
        y[i][1] = dis(gen);
    }

    std::vector<std::array<Real, 2>> y_copy = y;
    std::vector<Real> t_copy = t;
    boost::math::interpolators::vector_barycentric_rational<decltype(t), decltype(y)> interpolator(std::move(t), std::move(y));

    std::array<Real, 2> z;
    for (size_t i = 0; i < t_copy.size(); ++i)
    {
        interpolator(z, t_copy[i]);
        BOOST_CHECK_CLOSE(z[0], y_copy[i][0], 100*numeric_limits<Real>::epsilon());
        BOOST_CHECK_CLOSE(z[1], y_copy[i][1], 100*numeric_limits<Real>::epsilon());
    }
}

template<class Real>
void test_interpolation_condition_ublas()
{
    std::cout << "Testing interpolation condition for barycentric interpolation ublas vectors of type "
              << boost::typeindex::type_id<Real>().pretty_name()  << "\n";
    std::mt19937 gen(4723);
    boost::random::uniform_real_distribution<Real> dis(0.1f, 1);
    std::vector<Real> t(100);
    std::vector<boost::numeric::ublas::vector<Real>> y(100);
    t[0] = dis(gen);
    y[0].resize(2);
    y[0][0] = dis(gen);
    y[0][1] = dis(gen);
    for (size_t i = 1; i < t.size(); ++i)
    {
        t[i] = t[i-1] + dis(gen);
        y[i].resize(2);
        y[i][0] = dis(gen);
        y[i][1] = dis(gen);
    }

    std::vector<Real> t_copy = t;
    std::vector<boost::numeric::ublas::vector<Real>> y_copy = y;

    boost::math::interpolators::vector_barycentric_rational<decltype(t), decltype(y)> interpolator(std::move(t), std::move(y));

    boost::numeric::ublas::vector<Real> z(2);
    for (size_t i = 0; i < t_copy.size(); ++i)
    {
        interpolator(z, t_copy[i]);
        BOOST_CHECK_CLOSE(z[0], y_copy[i][0], 100*numeric_limits<Real>::epsilon());
        BOOST_CHECK_CLOSE(z[1], y_copy[i][1], 100*numeric_limits<Real>::epsilon());
    }
}

template<class Real>
void test_interpolation_condition_high_order()
{
    std::cout << "Testing interpolation condition in high order for barycentric interpolation on type " << boost::typeindex::type_id<Real>().pretty_name()  << "\n";
    std::mt19937 gen(5);
    boost::random::uniform_real_distribution<Real> dis(0.1f, 1);
    std::vector<Real> t(100);
    std::vector<Eigen::Vector2d> y(100);
    t[0] = dis(gen);
    y[0][0] = dis(gen);
    y[0][1] = dis(gen);
    for (size_t i = 1; i < t.size(); ++i)
    {
        t[i] = t[i-1] + dis(gen);
        y[i][0] = dis(gen);
        y[i][1] = dis(gen);
    }

    std::vector<Eigen::Vector2d> y_copy = y;
    std::vector<Real> t_copy = t;
    boost::math::interpolators::vector_barycentric_rational<decltype(t), decltype(y)> interpolator(std::move(t), std::move(y), 5);

    Eigen::Vector2d z;
    for (size_t i = 0; i < t_copy.size(); ++i)
    {
        interpolator(z, t_copy[i]);
        BOOST_CHECK_CLOSE(z[0], y_copy[i][0], 100*numeric_limits<Real>::epsilon());
        BOOST_CHECK_CLOSE(z[1], y_copy[i][1], 100*numeric_limits<Real>::epsilon());
    }
}

template<class Real>
void test_constant_eigen()
{
    std::cout << "Testing that constants are interpolated correctly using barycentric interpolation on Eigen vectors of type "
              << boost::typeindex::type_id<Real>().pretty_name() << "\n";

    std::mt19937 gen(6);
    boost::random::uniform_real_distribution<Real> dis(0.1f, 1);
    std::vector<Real> t(100);
    std::vector<Eigen::Vector2d> y(100);
    t[0] = dis(gen);
    Real constant0 = dis(gen);
    Real constant1 = dis(gen);
    y[0][0] = constant0;
    y[0][1] = constant1;
    for (size_t i = 1; i < t.size(); ++i)
    {
        t[i] = t[i-1] + dis(gen);
        y[i][0] = constant0;
        y[i][1] = constant1;
    }

    std::vector<Eigen::Vector2d> y_copy = y;
    std::vector<Real> t_copy = t;
    boost::math::interpolators::vector_barycentric_rational<decltype(t), decltype(y)> interpolator(std::move(t), std::move(y));

    Eigen::Vector2d z;
    for (size_t i = 0; i < t_copy.size(); ++i)
    {
        // Don't evaluate the constant at x[i]; that's already tested in the interpolation condition test.
        Real t = t_copy[i] + dis(gen);
        z = interpolator(t);
        BOOST_CHECK_CLOSE(z[0], constant0, 100*sqrt(numeric_limits<Real>::epsilon()));
        BOOST_CHECK_CLOSE(z[1], constant1, 100*sqrt(numeric_limits<Real>::epsilon()));
        Eigen::Vector2d zprime = interpolator.prime(t);
        Real zero_0 = zprime[0];
        Real zero_1 = zprime[1];
        BOOST_CHECK_SMALL(zero_0, sqrt(numeric_limits<Real>::epsilon()));
        BOOST_CHECK_SMALL(zero_1, sqrt(numeric_limits<Real>::epsilon()));
    }
}

template<class Real>
void test_constant_std_array()
{
    std::cout << "Testing that constants are interpolated correctly using barycentric interpolation on std::array vectors of type "
              << boost::typeindex::type_id<Real>().pretty_name() << "\n";

    std::mt19937 gen(6);
    boost::random::uniform_real_distribution<Real> dis(0.1f, 1);
    std::vector<Real> t(100);
    std::vector<std::array<Real, 2>> y(100);
    t[0] = dis(gen);
    Real constant0 = dis(gen);
    Real constant1 = dis(gen);
    y[0][0] = constant0;
    y[0][1] = constant1;
    for (size_t i = 1; i < t.size(); ++i)
    {
        t[i] = t[i-1] + dis(gen);
        y[i][0] = constant0;
        y[i][1] = constant1;
    }

    std::vector<std::array<Real,2>> y_copy = y;
    std::vector<Real> t_copy = t;
    boost::math::interpolators::vector_barycentric_rational<decltype(t), decltype(y)> interpolator(std::move(t), std::move(y));

    std::array<Real, 2> z;
    for (size_t i = 0; i < t_copy.size(); ++i)
    {
        // Don't evaluate the constant at x[i]; that's already tested in the interpolation condition test.
        Real t = t_copy[i] + dis(gen);
        z = interpolator(t);
        BOOST_CHECK_CLOSE(z[0], constant0, 100*sqrt(numeric_limits<Real>::epsilon()));
        BOOST_CHECK_CLOSE(z[1], constant1, 100*sqrt(numeric_limits<Real>::epsilon()));
        std::array<Real, 2> zprime = interpolator.prime(t);
        Real zero_0 = zprime[0];
        Real zero_1 = zprime[1];
        BOOST_CHECK_SMALL(zero_0, sqrt(numeric_limits<Real>::epsilon()));
        BOOST_CHECK_SMALL(zero_1, sqrt(numeric_limits<Real>::epsilon()));
    }
}

template<class Real>
void test_constant_high_order()
{
    std::cout << "Testing that constants are interpolated correctly using barycentric interpolation on type " << boost::typeindex::type_id<Real>().pretty_name() << "\n";

    std::mt19937 gen(6);
    boost::random::uniform_real_distribution<Real> dis(0.1f, 1);
    std::vector<Real> t(100);
    std::vector<Eigen::Vector2d> y(100);
    t[0] = dis(gen);
    Real constant0 = dis(gen);
    Real constant1 = dis(gen);
    y[0][0] = constant0;
    y[0][1] = constant1;
    for (size_t i = 1; i < t.size(); ++i)
    {
        t[i] = t[i-1] + dis(gen);
        y[i][0] = constant0;
        y[i][1] = constant1;
    }

    std::vector<Eigen::Vector2d> y_copy = y;
    std::vector<Real> t_copy = t;
    boost::math::interpolators::vector_barycentric_rational<decltype(t), decltype(y)> interpolator(std::move(t), std::move(y), 5);

    Eigen::Vector2d z;
    for (size_t i = 0; i < t_copy.size(); ++i)
    {
        // Don't evaluate the constant at x[i]; that's already tested in the interpolation condition test.
        Real t = t_copy[i] + dis(gen);
        z = interpolator(t);
        BOOST_CHECK_CLOSE(z[0], constant0, 100*sqrt(numeric_limits<Real>::epsilon()));
        BOOST_CHECK_CLOSE(z[1], constant1, 100*sqrt(numeric_limits<Real>::epsilon()));
        Eigen::Vector2d zprime = interpolator.prime(t);
        Real zero_0 = zprime[0];
        Real zero_1 = zprime[1];
        BOOST_CHECK_SMALL(zero_0, sqrt(numeric_limits<Real>::epsilon()));
        BOOST_CHECK_SMALL(zero_1, sqrt(numeric_limits<Real>::epsilon()));
    }
}

template<class Real>
void test_weights()
{
    std::cout << "Testing weights are calculated correctly using barycentric interpolation on type " << boost::typeindex::type_id<Real>().pretty_name() << "\n";

    std::mt19937 gen(9);
    boost::random::uniform_real_distribution<Real> dis(0.005, 0.01);
    std::vector<Real> t(100);
    std::vector<Eigen::Vector2d> y(100);
    t[0] = dis(gen);
    y[0][0] = dis(gen);
    y[0][1] = dis(gen);
    for (size_t i = 1; i < t.size(); ++i)
    {
        t[i] = t[i-1] + dis(gen);
        y[i][0] = dis(gen);
        y[i][1] = dis(gen);
    }

    std::vector<Eigen::Vector2d> y_copy = y;
    std::vector<Real> t_copy = t;
    boost::math::interpolators::detail::vector_barycentric_rational_imp<decltype(t), decltype(y)> interpolator(std::move(t), std::move(y), 1);

    for (size_t i = 1; i < t_copy.size() - 1; ++i)
    {
        Real w = interpolator.weight(i);
        Real w_expect = 1/(t_copy[i] - t_copy[i - 1]) + 1/(t_copy[i+1] - t_copy[i]);
        if (i % 2 == 0)
        {
            BOOST_CHECK_CLOSE(w, -w_expect, 0.00001);
        }
        else
        {
            BOOST_CHECK_CLOSE(w, w_expect, 0.00001);
        }
    }
}

BOOST_AUTO_TEST_CASE(vector_barycentric_rational)
{
    test_weights<double>();
    test_constant_eigen<double>();
    test_constant_std_array<double>();
    test_constant_high_order<double>();
    test_interpolation_condition_eigen<double>();
    test_interpolation_condition_ublas<double>();
    test_interpolation_condition_std_array<double>();
    test_interpolation_condition_high_order<double>();
    test_agreement_with_1d<double>();
}