// =======================================================================
// Copyright 2015 by Ireneusz Szcześniak
// Authors: Ireneusz Szcześniak
//
// 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)
// =======================================================================

// =======================================================================
// The custom Dijkstra call, which returns the optional path as a list
// of edges along with the cost of the path.  The search is stopped
// when the destination node is reached.
// =======================================================================

#ifndef BOOST_GRAPH_CUSTOM_DIJKSTRA_CALL
#define BOOST_GRAPH_CUSTOM_DIJKSTRA_CALL

#include <boost/graph/dijkstra_shortest_paths.hpp>
#include <boost/graph/visitors.hpp>
#include <boost/property_map/property_map.hpp>

#include <list>
#include <map>
#include <optional>

namespace boost {

  // =======================================================================
  // The function that traces back the result.  The predecessos map
  // should map a vertex to an edge.  Traditional Dijkstra maps a
  // vertex to a vertex.  However, we allow for multigraphs, and so we
  // care about a specific edge that led to a vertex.
  // =======================================================================

  template <typename Graph, typename WeightMap, typename PredMap>
  std::optional<std::pair<typename WeightMap::value_type,
                          std::list<typename Graph::edge_descriptor> > >
  trace(const Graph &g, WeightMap wm, PredMap pred,
        typename Graph::vertex_descriptor src,
        typename Graph::vertex_descriptor dst)
  {
    typedef typename Graph::vertex_descriptor vertex_descriptor;
    typedef typename Graph::edge_descriptor edge_descriptor;
    typedef typename WeightMap::value_type weight_type;
    typedef typename std::list<typename Graph::edge_descriptor> path_type;
    // The result type.
    typedef typename std::pair<weight_type, path_type> r_type;

    std::optional<r_type> result;

    if (src == dst)
      result = r_type();
    // Was the solution found?
    else if (pred[dst] != edge_descriptor())
      {
        // The result.
        r_type r;

        // Trace the solution to the source.
        vertex_descriptor c = dst;
        while (c != src)
          {
            const edge_descriptor &e = pred[c];
            // Increase the cost.
            r.first += get(wm, e);
            // Add the edge to the path.
            r.second.push_front(e);
            // Find the predecessing vertex.
            c = source(e, g);
          }

        result = std::move(r);
      }

    return result;
  }

  // =======================================================================
  // That's a helper function that uses BGL's Dijkstra.  There are two
  // important points:
  //
  // * we stop the search when we reach the dst vertex,
  //
  // * we predecessor map associates an edge with a vertex.
  //
  // =======================================================================
  template <typename Graph, typename WeightMap, typename IndexMap,
            typename PredMap>
  void
  stop_dijkstra_at_dst(const Graph &g,
                       typename Graph::vertex_descriptor src,
                       typename Graph::vertex_descriptor dst,
                       WeightMap wm, IndexMap im, PredMap pm)
  {
    // The type of the exception thrown by the cdc_visitor.
    struct exception {};

    struct visitor
    {
      typedef typename Graph::vertex_descriptor vertex_descriptor;
      typedef on_examine_vertex event_filter;
      visitor(vertex_descriptor dst): m_dst(dst) {}
      void operator()(vertex_descriptor v, const Graph& g) {
        if (v == m_dst)
          throw exception();
      }
      vertex_descriptor m_dst;
    };

    auto rep = record_edge_predecessors(pm, on_edge_relaxed());
    auto qat = visitor(dst);
    auto dv = make_dijkstra_visitor(std::make_pair(rep, qat));

    try
      {
        dijkstra_shortest_paths(g, src, weight_map(wm).
                                vertex_index_map(im).visitor(dv));
      }
    catch (exception) {}
  }

  // =======================================================================
  // The function that calls Dijkstra.  The function returns a list of
  // edges of the shortest path.
  // =======================================================================

  template <typename Graph, typename WeightMap, typename IndexMap>
  std::optional<std::pair<typename WeightMap::value_type,
                          std::list<typename Graph::edge_descriptor> > >
  custom_dijkstra_call(const Graph &g,
                       typename Graph::vertex_descriptor src,
                       typename Graph::vertex_descriptor dst,
                       WeightMap wm, IndexMap im)
  {
    typedef typename Graph::vertex_descriptor vertex_descriptor;
    typedef typename Graph::edge_descriptor edge_descriptor;

    std::map<vertex_descriptor, edge_descriptor> v2e;
    auto pred = make_assoc_property_map(v2e);
    stop_dijkstra_at_dst(g, src, dst, wm, im, pred);
    return trace(g, wm, pred, src, dst);
  }

} // boost

#endif /* BOOST_GRAPH_CUSTOM_DIJKSTRA_CALL */


//Copyright(c) 2016 Shuda Li[lishuda1980@gmail.com]
//
//Permission is hereby granted, free of charge, to any person obtaining a copy
//of this software and associated documentation files(the "Software"), to deal
//in the Software without restriction, including without limitation the rights
//to use, copy, modify, merge, publish, distribute, sublicense, and / or sell
//copies of the Software, and to permit persons to whom the Software is
//furnished to do so, subject to the following conditions :
//
//The above copyright notice and this permission notice shall be included in
//all copies or substantial portions of the Software.
//
//THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
//IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS
//FOR A PARTICULAR PURPOSE AND NON - INFRINGEMENT.IN NO EVENT SHALL THE AUTHORS OR
//COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
//LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
//OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
//SOFTWARE.
//

#ifndef BTL_Eigen_UTILITY_HEADER
#define BTL_Eigen_UTILITY_HEADER

//eigen-based helpers
#include "OtherUtil.hpp"
#include <Eigen/Dense>
#include <se3.hpp>
#include <opencv2/core.hpp>

namespace btl
{
namespace utility
{

using namespace Eigen;
using namespace Sophus;
using namespace btl::utility;

template< class T >
Eigen::Matrix< T, 4, 4 > inver(const Eigen::Matrix< T, 4, 4 >& F_ctw_){
	using namespace Eigen;
	Matrix< T, 4, 4 > F_wtc; F_wtc.setIdentity();

	Matrix< T, 3, 3 > R_trans = F_ctw_.block(0,0,3,3);
	Matrix< T, 3, 1 > Cw = F_ctw_.block(0,3,3,1);

	F_wtc.block(0,0,3,3) = R_trans.transpose();
	F_wtc.block(0,3,3,1) = -R_trans*Cw;
	return F_wtc;
}

template< class T >
void getCwVwFromPrj_Cam2World(const Eigen::Matrix< T, 4, 4 >&  Prj_ctw_,Eigen::Matrix< T, 3, 1 >* pCw_,Eigen::Matrix< T, 3, 1 >* pVw_){
	*pCw_ = Prj_ctw_.template block<3,1>(0,3); //4th column is camera centre
	*pVw_ = Prj_ctw_.template block<3,1>(0,0); //1st column is viewing direction
}

template< class T >
void getRTCVfromModelViewGL ( const Eigen::Matrix< T, 4, 4 >&  mMat_, Eigen::Matrix< T, 3, 3 >* pmR_, Eigen::Matrix< T, 3, 1 >* pvT_ )
{
    (* pmR_) ( 0, 0 ) =  mMat_ ( 0, 0 );   (* pmR_) ( 0, 1 ) =   mMat_ ( 0, 1 );  (* pmR_) ( 0, 2 ) = mMat_ ( 0, 2 );
    (* pmR_) ( 1, 0 ) = -mMat_ ( 1, 0 );   (* pmR_) ( 1, 1 ) = - mMat_ ( 1, 1 );  (* pmR_) ( 1, 2 ) = -mMat_ ( 1, 2 );
    (* pmR_) ( 2, 0 ) = -mMat_ ( 2, 0 );   (* pmR_) ( 2, 1 ) = - mMat_ ( 2, 1 );  (* pmR_) ( 2, 2 ) = -mMat_ ( 2, 2 );

	(*pvT_) ( 0 ) = mMat_ ( 0, 3 );
    (*pvT_) ( 1 ) = -mMat_ ( 1, 3 );
    (*pvT_) ( 2 ) = -mMat_ ( 2, 3 );

    return;
}

template< class T >
Eigen::Matrix< T, 4, 4 > setModelViewGLfromPrj(const Eigen::Transform<T, 3, Eigen::Affine> & Prj_)
{
	// column first for pGLMat_[16];
	// row first for Matrix3d;
	// pGLMat_[ 0] =  mR_(0,0); pGLMat_[ 4] =  mR_(0,1); pGLMat_[ 8] =  mR_(0,2); pGLMat_[12] =  vT_(0);
	// pGLMat_[ 1] = -mR_(1,0); pGLMat_[ 5] = -mR_(1,1); pGLMat_[ 9] = -mR_(1,2); pGLMat_[13] = -vT_(1);
	// pGLMat_[ 2] = -mR_(2,0); pGLMat_[ 6] = -mR_(2,1); pGLMat_[10] = -mR_(2,2); pGLMat_[14] = -vT_(2);
	// pGLMat_[ 3] =  0;        pGLMat_[ 7] =  0;        pGLMat_[11] =  0;        pGLMat_[15] = 1;

	Eigen::Matrix< T , 4, 4 > mMat;
	mMat.row( 0 ) =  Prj_.matrix().row( 0 );
	mMat.row( 1 ) = -Prj_.matrix().row( 1 );
	mMat.row( 2 ) = -Prj_.matrix().row( 2 );
	mMat.row( 3 ) =  Prj_.matrix().row( 3 );

	return mMat;
}

template< class T >
Eigen::Matrix< T , 4, 4 > setModelViewGLfromRTCV ( const SO3Group<T>& mR_, const Eigen::Matrix< T, 3, 1 >& vT_ )
{
    // column first for pGLMat_[16];
    // row first for Matrix3d;
    // pGLMat_[ 0] =  mR_(0,0); pGLMat_[ 4] =  mR_(0,1); pGLMat_[ 8] =  mR_(0,2); pGLMat_[12] =  vT_(0);
    // pGLMat_[ 1] = -mR_(1,0); pGLMat_[ 5] = -mR_(1,1); pGLMat_[ 9] = -mR_(1,2); pGLMat_[13] = -vT_(1);
    // pGLMat_[ 2] = -mR_(2,0); pGLMat_[ 6] = -mR_(2,1); pGLMat_[10] = -mR_(2,2); pGLMat_[14] = -vT_(2);
    // pGLMat_[ 3] =  0;        pGLMat_[ 7] =  0;        pGLMat_[11] =  0;        pGLMat_[15] = 1;

    Eigen::Matrix< T , 4, 4 > mMat;
    mMat ( 0, 0 ) =  mR_.matrix() ( 0, 0 ); mMat ( 0, 1 ) =  mR_.matrix() ( 0, 1 ); mMat ( 0, 2 ) =  mR_.matrix() ( 0, 2 ); mMat ( 0, 3 ) =  vT_ ( 0 );
    mMat ( 1, 0 ) = -mR_.matrix() ( 1, 0 ); mMat ( 1, 1 ) = -mR_.matrix() ( 1, 1 ); mMat ( 1, 2 ) = -mR_.matrix() ( 1, 2 ); mMat ( 1, 3 ) = -vT_ ( 1 );
    mMat ( 2, 0 ) = -mR_.matrix() ( 2, 0 ); mMat ( 2, 1 ) = -mR_.matrix() ( 2, 1 ); mMat ( 2, 2 ) = -mR_.matrix() ( 2, 2 ); mMat ( 2, 3 ) = -vT_ ( 2 );
    mMat ( 3, 0 ) =  0;            mMat ( 3, 1 ) =  0;            mMat ( 3, 2 ) =  0;            mMat ( 3, 3 ) =  1;

    return mMat;
}

template< class T >
Eigen::Matrix< T , 4, 4 > setModelViewGLfromRCCV ( const Eigen::Matrix< T, 3, 3 >& mR_, const Eigen::Matrix< T, 3, 1 >& vC_ )
{
	Eigen::Matrix< T, 3,1> eivT = -mR_.transpose()*vC_;
	return setModelViewGLfromRTCV(mR_,vC_);
}

template< class T1, class T2 >
void unprojectCamera2World ( const int& nX_, const int& nY_, const unsigned short& nD_, const Eigen::Matrix< T1, 3, 3 >& mK_, Eigen::Matrix< T2, 3, 1 >* pVec_ )
{
	//the pixel coordinate is defined w.r.t. opencv camera reference, which is defined as x-right, y-downward and z-forward. It's
	//a right hand system.
	//when rendering the point using opengl's camera reference which is defined as x-right, y-upward and z-backward. the
	//	glVertex3d ( Pt(0), -Pt(1), -Pt(2) );
	if ( nD_ > 400 ) {
		T2 dZ = nD_ / 1000.; //convert to meter
		T2 dX = ( nX_ - mK_ ( 0, 2 ) ) / mK_ ( 0, 0 ) * dZ;
		T2 dY = ( nY_ - mK_ ( 1, 2 ) ) / mK_ ( 1, 1 ) * dZ;
		( *pVec_ ) << dX + 0.0025, dY, dZ + 0.00499814; // the value is esimated using CCalibrateKinectExtrinsics::calibDepth()
		// 0.0025 by experience.
	}
	else {
		( *pVec_ ) << 0, 0, 0;
	}
}

template< class T >
void projectWorld2Camera ( const Eigen::Matrix< T, 3, 1 >& vPt_, const Eigen::Matrix3d& mK_, Eigen::Matrix< short, 2, 1>* pVec_  )
{
	// this is much faster than the function
	// eiv2DPt = mK * vPt; eiv2DPt /= eiv2DPt(2);
	( *pVec_ ) ( 0 ) = short ( mK_ ( 0, 0 ) * vPt_ ( 0 ) / vPt_ ( 2 ) + mK_ ( 0, 2 ) + 0.5 );
	( *pVec_ ) ( 1 ) = short ( mK_ ( 1, 1 ) * vPt_ ( 1 ) / vPt_ ( 2 ) + mK_ ( 1, 2 ) + 0.5 );
}

template< class T >
void convertPrj2Rnt(const Eigen::Transform< T, 3, Eigen::Affine >& Prj_, SO3Group< T >* pR_, Eigen::Matrix< T, 3, 1 >* pT_)
{
	*pR_ = SO3Group<T>(Prj_.linear());
	*pT_ = Prj_.translation();
	return;
}
template< class T >
Eigen::Transform< T, 3, Eigen::Affine > convertRnt2Prj(const SO3Group< T >& R_, const Eigen::Matrix< T, 3, 1 >& T_)
{
	Eigen::Transform< T, 3, Eigen::Affine > prj;
	prj.setIdentity();
	prj.linear() = R_.matrix();
	prj.translation() = T_;
	return prj;
}
template< class T >
void convertPrjInv2RpnC( const Eigen::Matrix< T, 4, 4 >& Prj_, Eigen::Matrix< T, 3, 3 >* pR_trans_, Eigen::Matrix< T, 3, 1 >* pT_)
{
	*pR_trans_ = Prj_.template block<3,3>(0,0);
	*pT_ = Prj_.template block<3,1>(0,3);
	return;
}
template< class T >
Eigen::Matrix< T, 4, 4 > convertRpnC2PrjInv(  const Eigen::Matrix< T, 3, 3 >& R_trans_, const Eigen::Matrix< T, 3, 1 >& C_ )
{
	Eigen::Matrix< T, 4, 4 > prj;
	prj.setIdentity();
	prj.template block<3,3>(0,0) = R_trans_;
	prj.template block<3,1>(0,3) = C_;
	return prj;
}

template< class T, int ROW, int COL >
T matNormL1 ( const Eigen::Matrix< T, ROW, COL >& eimMat1_, const Eigen::Matrix< T, ROW, COL >& eimMat2_ )
{
	Eigen::Matrix< T, ROW, COL > eimTmp = eimMat1_ - eimMat2_;
	Eigen::Matrix< T, ROW, COL > eimAbs = eimTmp.cwiseAbs();
	return (T) eimAbs.sum();
}

template< class T >
void setSkew( T x_, T y_, T z_, Eigen::Matrix< T, 3,3 >* peimMat_){
	*peimMat_ << 0, -z_, y_, z_, 0, -x_, -y_, x_, 0 ;
}

template< class T >
void setRotMatrixUsingExponentialMap( T x_, T y_, T z_, Eigen::Matrix< T, 3,3 >* peimR_ ){
	//http://opencv.itseez.com/modules/calib3d/doc/camera_calibration_and_3d_reconstruction.html?highlight=rodrigues#void Rodrigues(InputArray src, OutputArray dst, OutputArray jacobian)
	T theta = sqrt( x_*x_ + y_*y_ + z_*z_ );
	if(	theta < std::numeric_limits<T>::epsilon() ){
		*peimR_ = Eigen::Matrix< T, 3,3 >::Identity();
		return;
	}
	T sinTheta = sin(theta);
	T cosTheta = cos(theta);
	Eigen::Matrix< T, 3,3 > eimSkew;
	setSkew< T >(x_/theta,y_/theta,z_/theta,&eimSkew);
	*peimR_ = Eigen::Matrix< T, 3,3 >::Identity() + eimSkew*sinTheta + eimSkew*eimSkew*(1-cosTheta);
}

}//utility
}//btl
#endif


///////////////////////////////////////////////////////////////////////////////
// BSD 3-Clause License
//
// Copyright (C) 2020-2021, LAAS-CNRS, University of Edinburgh
// Copyright note valid unless otherwise stated in individual files.
// All rights reserved.
///////////////////////////////////////////////////////////////////////////////

#ifndef CROCODDYL_CORE_MATHBASE_HPP_
#define CROCODDYL_CORE_MATHBASE_HPP_

#include <Eigen/Core>
#include <Eigen/Geometry>

namespace crocoddyl {

template <typename _Scalar>
struct MathBaseTpl {
 public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW

  typedef _Scalar Scalar;
  typedef Eigen::Matrix<Scalar, 2, 1> Vector2s;
  typedef Eigen::Matrix<Scalar, 3, 1> Vector3s;
  typedef Eigen::Matrix<Scalar, 4, 1> Vector4s;
  typedef Eigen::Matrix<Scalar, 6, 1> Vector6s;
  typedef Eigen::Matrix<Scalar, 2, 2> Matrix2s;
  typedef Eigen::Matrix<Scalar, 3, 3> Matrix3s;
  typedef Eigen::Matrix<Scalar, 4, 6> Matrix46s;
  typedef Eigen::Matrix<Scalar, 6, 6> Matrix6s;
  typedef Eigen::Matrix<Scalar, 1, 2> RowVector2s;

  typedef Eigen::Matrix<Scalar, Eigen::Dynamic, 3> MatrixX3s;
  typedef Eigen::Matrix<Scalar, Eigen::Dynamic, 6> MatrixX6s;
  typedef Eigen::Matrix<Scalar, 3, Eigen::Dynamic> Matrix3xs;
  typedef Eigen::Matrix<Scalar, 6, Eigen::Dynamic> Matrix6xs;

  typedef Eigen::Matrix<Scalar, Eigen::Dynamic, 1> VectorXs;
  typedef Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> MatrixXs;
  typedef Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> MatrixXsRowMajor;
  typedef Eigen::Array<Scalar, Eigen::Dynamic, 1> ArrayXs;
  typedef Eigen::Quaternion<Scalar> Quaternions;
  typedef Eigen::DiagonalMatrix<Scalar, Eigen::Dynamic> DiagonalMatrixXs;
};

}  // namespace crocoddyl

#endif


#include <iostream>
#include <time.h>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/registration/icp.h>
#include <pcl/registration/ia_ransac.h>
#include <pcl/point_types.h>
#include <pcl/features/normal_3d.h>
#include <pcl/point_types.h>
#include <pcl/features/pfh.h>
#include <pcl/filters/passthrough.h>
#include <pcl/visualization/cloud_viewer.h>
#include <limits>
#include <fstream>
#include <vector>
#include <Eigen/Core>
#include "pcl/point_cloud.h"
#include "pcl/kdtree/kdtree_flann.h"
#include "pcl/filters/passthrough.h"
#include "pcl/filters/voxel_grid.h"
#include "pcl/features/fpfh.h"

using namespace pcl;
using namespace std;

namespace pc{

PointCloud<FPFHSignature33>::Ptr getFeatures(PointCloud<PointXYZ>::Ptr cloud, PointCloud<Normal>::Ptr normals, double feature_radius) {

	PointCloud<FPFHSignature33>::Ptr features = PointCloud<FPFHSignature33>::Ptr(new PointCloud<FPFHSignature33>);
	search::KdTree<PointXYZ>::Ptr search_method_ptr = search::KdTree<PointXYZ>::Ptr(new search::KdTree<PointXYZ>);
	FPFHEstimation<PointXYZ, Normal, FPFHSignature33> fpfh_est;
	fpfh_est.setInputCloud(cloud);
	fpfh_est.setInputNormals(normals);
	fpfh_est.setSearchMethod(search_method_ptr);
	fpfh_est.setRadiusSearch(feature_radius);
	fpfh_est.compute(*features);
	return features;
}

PointCloud<Normal>::Ptr getNormals(PointCloud<PointXYZ>::Ptr incloud, double normals_radius) {

	PointCloud<Normal>::Ptr normalsPtr = PointCloud<Normal>::Ptr(new PointCloud<Normal>);
	NormalEstimation<PointXYZ, Normal> norm_est;
	norm_est.setInputCloud(incloud);
	norm_est.setRadiusSearch(normals_radius);
	norm_est.compute(*normalsPtr);
	return normalsPtr;
}

PointCloud<PointNormal>::Ptr getPointNormals(PointCloud<PointXYZ>::Ptr incloud, int k){

	PointCloud<PointNormal>::Ptr points_with_normals = PointCloud<PointNormal>::Ptr(new PointCloud<PointNormal>);
	NormalEstimation<PointXYZ, PointNormal> norm_est;
	pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ>());
	norm_est.setSearchMethod(tree);
	norm_est.setKSearch(k);
	norm_est.setInputCloud(incloud);
	norm_est.compute(*points_with_normals);
	pcl::copyPointCloud(*incloud, *points_with_normals);
	return points_with_normals;
}

}


/*
 * Copyright (c) 2013-2018 Masahide Kashiwagi (kashi@waseda.jp)
 */

#ifndef ODE_HPP
#define ODE_HPP

// ODE

#include <iostream>
#include <cmath>
#include <algorithm>
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/io.hpp>
#include <kv/interval.hpp>
#include <kv/rdouble.hpp>
#include <kv/interval-vector.hpp>
#include <kv/make-candidate.hpp>
#include <kv/psa.hpp>
#include <kv/ode-param.hpp>

#ifndef ODE_FAST
#define ODE_FAST 1
#endif

#ifndef ODE_STEP_COMPONENT
#define ODE_STEP_COMPONENT 0
#endif

#ifndef ODE_RESTART_RATIO
#define ODE_RESTART_RATIO 1
#endif

#ifndef ODE_COEF_MID
#define ODE_CORF_MID 0
#endif

namespace kv {

namespace ub = boost::numeric::ublas;

template <class T, class F>
int
ode(F f, ub::vector< interval<T> >& init, const interval<T>& start, interval<T>& end, const ode_param<T> p = ode_param<T>(), ub::vector< psa< interval<T> > >* result_psa = NULL) {
	int n = init.size();
	int i, j;

	ub::vector< psa< interval<T> > > x, y;
	psa< interval<T> > torg;
	psa< interval<T> > t;

	ub::vector< psa< interval<T> > > z, w;

	psa< interval<T> > temp;
	T m;
	ub::vector<T> newton_step;

	bool flag, resized;

	interval<T> deltat;
	ub::vector< interval<T> > result;

	T radius, radius_tmp;

	#if ODE_STEP_COMPONENT == 1
	ub::vector<T> tolerance(n);
	#else
	T tolerance;
	#endif

	int n_rad;

	#if ODE_RESTART_RATIO == 1
	T max_ratio;
	#endif

	int ret_val;
	interval<T> end2;
	int restart;

	bool save_mode, save_uh, save_rh;

	#if ODE_STEP_COMPONENT == 1
	for (i=0; i<n; i++) {
		tolerance(i) = std::max(T(1.), norm(init(i))) * p.epsilon;
	}
	#else
	m = 1.;
	for (i=0; i<n; i++) {
		m = std::max(m, norm(init(i)));
	}
	tolerance = m * p.epsilon;
	#endif

	x = init;
	torg.v.resize(2);
	torg.v(0) = start; torg.v(1) = 1.;

	save_mode = psa< interval<T> >::mode();
	save_uh = psa< interval<T> >::use_history();
	save_rh = psa< interval<T> >::record_history();
	psa< interval<T> >::mode() = 1;
	psa< interval<T> >::use_history() = false;
	psa< interval<T> >::record_history() = false;
	#if ODE_FAST == 1
	psa< interval<T> >::record_history() = true;
	psa< interval<T> >::history().clear();
	#endif
	for (j=0; j<p.order; j++) {
		#if ODE_FAST == 1
		if (j == 1) psa< interval<T> >::use_history() = true;
		#endif
		t = setorder(torg, j);
		y = f(x, t);
		for (i=0; i<n; i++) {
			y(i) = integrate(y(i));
			// set order preparing for constant function
			y(i) = setorder(y(i), j+1);
		}
		x = init + y;
	}

	if (p.autostep) {
		// use two non-zero coefficients of higher order term
		#if ODE_STEP_COMPONENT == 1
		radius = std::numeric_limits<T>::infinity();
		for (i=0; i<n; i++) {
			radius_tmp = 0.;
			n_rad = 0;
			for (j = p.order; j>=1; j--) {
				#if ODE_COEF_MID == 1
				using std::abs;
				m = abs(mid(x(i).v(j)));
				#else
				m = norm(x(i).v(j));
				#endif
				if (m == 0.) continue;
				radius_tmp = std::max(radius_tmp, (T)std::pow((double)m, 1./j));
				n_rad++;
				if (n_rad == 2) break;
			}
			radius = std::min(radius, std::pow((double)(tolerance(i)), 1./p.order) / radius_tmp);
		}
		#else // ODE_STEP_COMPONENT
		radius = 0.;
		n_rad = 0;
		for (j = p.order; j>=1; j--) {
			m = 0.;
			for (i=0; i<n; i++) {
				#if ODE_COEF_MID == 1
				using std::abs;
				m = std::max(m, abs(mid(x(i).v(j))));
				#else
				m = std::max(m, norm(x(i).v(j)));
				#endif
			}
			if (m == 0.) continue;
			radius = std::max(radius, (T)std::pow((double)m, 1./j));
			n_rad++;
			if (n_rad == 2) break;
		}
		radius = std::pow((double)tolerance, 1./p.order) / radius;
		#endif // ODE_STEP_COMPONENT
	}

	psa< interval<T> >::mode() = 2;

	restart = 0;
	resized = false;

	while (true) {
		if (p.autostep) {
			end2 = mid(start + radius);
			if (end2 >= end.lower()) {
				end2 = end;
				radius = mid(end2 - start);
				ret_val = 2;
			} else {
				ret_val = 1;
			}
		} else {
			end2 = end;
			ret_val = 2;
		}
		deltat = end2 - start;

		psa< interval<T> >::domain() = interval<T>(0., deltat.upper());

		z = x;
		t = setorder(torg, p.order);

		try {
			w = f(z, t);
		}
		catch (std::domain_error& e) {
			if (p.autostep && restart < p.restart_max) {
				psa< interval<T> >::use_history() = false;
				if (p.verbose == 1) {
					std::cout << "ode: radius changed: " << radius;
				}
				radius *= 0.5;
				if (p.verbose == 1) {
					std::cout << " -> " << radius << "\n";
				}
				restart++;
				continue;
			} else {
				throw std::domain_error("ode: evaluation error");
			}
		}

		for (i=0; i<n; i++) {
			temp = integrate(w(i));
			w(i) = setorder(temp, p.order);
		}
		w = init + w;

		newton_step.resize(n);
		for (i=0; i<n; i++) {
			newton_step(i) = norm(w(i).v(p.order) - z(i).v(p.order));
		}
		make_candidate(newton_step);
		for (i=0; i<n; i++) {
			z(i).v(p.order) += newton_step(i) * interval<T>(-1., 1.);
		}

		if (p.autostep && ret_val != 2 && resized == false) {
			resized = true;
			m = (std::numeric_limits<T>::min)();
			#if ODE_STEP_COMPONENT == 1
			for (i=0; i<n; i++) {
				m = std::max(m, (rad(eval(z(i), deltat)) - rad(init(i))) / tolerance(i));
			}
			#else
			for (i=0; i<n; i++) {
				m = std::max(m, rad(eval(z(i), deltat)) - rad(init(i)));
			}
			m = m / tolerance;
			#endif
			radius_tmp = radius / std::pow((double)m, 1. / p.order);
			if (radius_tmp >= radius && restart > 0) {
				// do nothing, not continue
			} else {
				radius = radius_tmp;
				continue;
			}
		}

		w = f(z, t);
		for (i=0; i<n; i++) {
			temp = integrate(w(i));
			w(i) = setorder(temp, p.order);
		}
		w = init + w;

		flag = true;
		#if ODE_RESTART_RATIO == 1
		max_ratio = 0.;
		#endif
		for (i=0; i<n; i++) {
			#if ODE_RESTART_RATIO == 1
			max_ratio = std::max(max_ratio, width(w(i).v(p.order)) / width(z(i).v(p.order)));
			#endif
			flag = flag && subset(w(i).v(p.order), z(i).v(p.order));
		}
		if (flag) break;

		if (!p.autostep || restart >= p.restart_max) {
			ret_val = 0;
			break;
		}
		if (p.verbose == 1) {
			std::cout << "ode: radius changed: " << radius;
		}
		#if ODE_RESTART_RATIO == 1
		radius *= std::max(std::min((T)0.5, (T)0.5 / max_ratio), (T)0.125);
		#else
		radius *= 0.5;
		#endif
		if (p.verbose == 1) {
			std::cout << " -> " << radius << "\n";
		}
		restart++;
	}

	if (ret_val != 0) {
		for (j=0; j<p.iteration; j++) {
			z = w;
			w = f(z, t);
			for (i=0; i<n; i++) {
				temp = integrate(w(i));
				w(i) = setorder(temp, p.order);
			}
			w = init + w;
			for (i=0; i<n; i++) {
				w(i).v(p.order) = intersect(w(i).v(p.order), z(i).v(p.order));
			}
		}

		result.resize(n);
		for (i=0; i<n; i++) {
			result(i) = eval(w(i), deltat);
		}

		init = result;
		if (ret_val == 1) end = end2;
		if (result_psa != NULL) *result_psa = w;
	}

	psa< interval<T> >::mode() = save_mode;
	psa< interval<T> >::use_history() = save_uh;
	psa< interval<T> >::record_history() = save_rh;

	return ret_val;
}

template <class T, class F>
int
odelong(F f, ub::vector< interval<T> >& init, const interval<T>& start, interval<T>& end, ode_param<T> p = ode_param<T>()) {

	ub::vector< interval<T> > x;
	interval<T> t, t1;
	int r;
	int ret_val = 0;

	x = init;
	t = start;
	p.set_autostep(true);
	while (1) {
		t1 = end;

		r = ode(f, x, t, t1, p);
		if (r == 0) {
			if (ret_val == 1) {
				init = x;
				end = t;
			}
			return ret_val;
		}
		ret_val = 1;
		if (p.verbose == 1) {
			std::cout << "t: " << t1 << "\n";
			std::cout << x << "\n";
		}
		if (r == 2) {
			init = x;
			return 2;
		}
		t = t1;
	}
}

} // namespace kv

#endif // ODE_HPP


////////////////////////////////////////////////////////////////////////////
// lazy_list3_tests.cpp
//
// more tests on list<T>
//
////////////////////////////////////////////////////////////////////////////
/*=============================================================================
    Copyright (c) 2000-2003 Brian McNamara and Yannis Smaragdakis
    Copyright (c) 2001-2007 Joel de Guzman
    Copyright (c) 2015 John Fletcher

    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/phoenix/core/limits.hpp>

#include <boost/detail/lightweight_test.hpp>
#include <boost/phoenix/core.hpp>
#include <boost/phoenix/function/lazy_prelude.hpp>

int main()
{
    namespace phx = boost::phoenix;
    //using boost::phoenix::arg_names::arg1;
    //using boost::phoenix::arg_names::arg2;
    using namespace phx;

    list<int> l    = enum_from(2);
    list<int> ll   = take(4,l);
    list<int> lll  = take(12,l);
    list<int> l2   = enum_from_to(2,10);
    list<int> ll2  = take(4,l2);
    list<int> lll2 = take(12,l2);
    list<int> evens = filter(even,l);
    list<int> odds  = filter(odd,l);
    list<int> even4 = take(4,evens)();
    list<int> odd4  = take(4,odds)();
    list<int> itersome = take(4,iterate(dec,1))();
    list<int> repeatsome = take(4,repeat(1))();

    BOOST_TEST(last(ll)()   == 5);
    BOOST_TEST(last(lll)()  == 13);
    BOOST_TEST(last(ll2)()  == 5);
    BOOST_TEST(last(lll2)() == 10);
    BOOST_TEST(length(lll2)() == 9);
    BOOST_TEST(at_(even4,3)() == 8);
    BOOST_TEST(at_(odd4,2)()  == 7);
    BOOST_TEST(at_(itersome,3)()   == -3);
    BOOST_TEST(at_(repeatsome,3)() == 1);

    return boost::report_errors();
}


/////////////////////////////////////////////////////////////////////////////
//
// (C) Copyright Ion Gaztanaga 2007-2013
//
// 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)
//
// See http://www.boost.org/libs/intrusive for documentation.
//
/////////////////////////////////////////////////////////////////////////////
//[doc_treap_algorithms_code
#include <boost/intrusive/treap_algorithms.hpp>
#include <cassert>

struct my_node
{
   my_node(int i = 0, unsigned int priority = 0)
      :  prio_(priority), int_(i)
   {}
   my_node *parent_, *left_, *right_;
   int prio_;
   //other members
   int      int_;
};

//Define our own treap_node_traits
struct my_treap_node_traits
{
   typedef my_node                                    node;
   typedef my_node *                                  node_ptr;
   typedef const my_node *                            const_node_ptr;

   static node_ptr get_parent(const_node_ptr n)       {  return n->parent_;   }
   static void set_parent(node_ptr n, node_ptr parent){  n->parent_ = parent; }
   static node_ptr get_left(const_node_ptr n)         {  return n->left_;     }
   static void set_left(node_ptr n, node_ptr left)    {  n->left_ = left;     }
   static node_ptr get_right(const_node_ptr n)        {  return n->right_;    }
   static void set_right(node_ptr n, node_ptr right)  {  n->right_ = right;   }
};

struct node_ptr_compare
{  bool operator()(const my_node *a, const my_node *b) {  return a->int_ < b->int_;  }  };

struct node_ptr_priority
{  bool operator()(const my_node *a, const my_node *b) {  return a->prio_ < b->prio_;}  };

int main()
{
   typedef boost::intrusive::treap_algorithms<my_treap_node_traits> algo;
   my_node header, two(2, 5), three(3, 1);

   //Create an empty treap container:
   //"header" will be the header node of the tree
   algo::init_header(&header);

   //Now insert node "two" in the tree using the sorting functor
   algo::insert_equal_upper_bound(&header, &two, node_ptr_compare(), node_ptr_priority());

   //Now insert node "three" in the tree using the sorting functor
   algo::insert_equal_lower_bound(&header, &three, node_ptr_compare(), node_ptr_priority());

   //Now take the first node (the left node of the header)
   my_node *n = header.left_;
   assert(n == &two);

   //Now go to the next node
   n = algo::next_node(n);
   assert(n == &three);

   //Erase a node just using a pointer to it
   algo::unlink(&two, node_ptr_priority());

   //Erase a node using also the header (faster)
   algo::erase(&header, &three, node_ptr_priority());
   return 0;
}

//]


/**
 * @file random.hpp
 *
 * Miscellaneous math random-related routines.
 *
 * This file is part of mlpack 1.0.12.
 *
 * mlpack is free software; you may redstribute it and/or modify it under the
 * terms of the 3-clause BSD license.  You should have received a copy of the
 * 3-clause BSD license along with mlpack.  If not, see
 * http://www.opensource.org/licenses/BSD-3-Clause for more information.
 */
#ifndef __MY_MLPACK_CORE_MATH_RANDOM_HPP
#define __MY_MLPACK_CORE_MATH_RANDOM_HPP

#include <stdlib.h>
#include <math.h>
#include <float.h>

#include <boost/random.hpp>

namespace mips {
namespace math /** Miscellaneous math routines. */ {

// Annoying Boost versioning issues.
#include <boost/version.hpp>

#if BOOST_VERSION >= 104700
  // Global random object.
  extern boost::random::mt19937 randGen;
  // Global uniform distribution.
  extern boost::random::uniform_01<> randUniformDist;
  // Global normal distribution.
  extern boost::random::normal_distribution<> randNormalDist;
#else
  // Global random object.
  extern boost::mt19937 randGen;

  #if BOOST_VERSION >= 103900
    // Global uniform distribution.
    extern boost::uniform_01<> randUniformDist;
  #else
    // Pre-1.39 Boost.Random did not give default template parameter values.
    extern boost::uniform_01<boost::mt19937, double> randUniformDist;
  #endif

  // Global normal distribution.
  extern boost::normal_distribution<> randNormalDist;
#endif

/**
 * Set the random seed used by the random functions (Random() and RandInt()).
 * The seed is casted to a 32-bit integer before being given to the random
 * number generator, but a size_t is taken as a parameter for API consistency.
 *
 * @param seed Seed for the random number generator.
 */
inline void RandomSeed(const size_t seed)
{
  randGen.seed((uint32_t) seed);
  srand((unsigned int) seed);
}

/**
 * Generates a uniform random number between 0 and 1.
 */
inline double Random()
{
#if BOOST_VERSION >= 103900
  return randUniformDist(randGen);
#else
  // Before Boost 1.39, we did not give the random object when we wanted a
  // random number; that gets given at construction time.
  return randUniformDist();
#endif
}

/**
 * Generates a uniform random number in the specified range.
 */
inline double Random(const double lo, const double hi)
{
#if BOOST_VERSION >= 103900
  return lo + (hi - lo) * randUniformDist(randGen);
#else
  // Before Boost 1.39, we did not give the random object when we wanted a
  // random number; that gets given at construction time.
  return lo + (hi - lo) * randUniformDist();
#endif
}

/**
 * Generates a uniform random integer.
 */
inline int RandInt(const int hiExclusive)
{
#if BOOST_VERSION >= 103900
  return (int) std::floor((double) hiExclusive * randUniformDist(randGen));
#else
  // Before Boost 1.39, we did not give the random object when we wanted a
  // random number; that gets given at construction time.
  return (int) std::floor((double) hiExclusive * randUniformDist());
#endif
}

/**
 * Generates a uniform random integer.
 */
inline int RandInt(const int lo, const int hiExclusive)
{
#if BOOST_VERSION >= 103900
  return lo + (int) std::floor((double) (hiExclusive - lo)
                               * randUniformDist(randGen));
#else
  // Before Boost 1.39, we did not give the random object when we wanted a
  // random number; that gets given at construction time.
  return lo + (int) std::floor((double) (hiExclusive - lo)
                               * randUniformDist());
#endif

}

/**
 * Generates a normally distributed random number with mean 0 and variance 1.
 */
inline double RandNormal()
{
  return randNormalDist(randGen);
}

/**
 * Generates a normally distributed random number with specified mean and
 * variance.
 *
 * @param mean Mean of distribution.
 * @param variance Variance of distribution.
 */
inline double RandNormal(const double mean, const double variance)
{
  return variance * randNormalDist(randGen) + mean;
}

}; // namespace math
}; // namespace mlpack

#endif // __MLPACK_CORE_MATH_MATH_LIB_HPP


// 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)

// This implements the compactly supported cubic b spline algorithm described in
// Kress, Rainer. "Numerical analysis, volume 181 of Graduate Texts in Mathematics." (1998).
// Splines of compact support are faster to evaluate and are better conditioned than classical cubic splines.

// Let f be the function we are trying to interpolate, and s be the interpolating spline.
// The routine constructs the interpolant in O(N) time, and evaluating s at a point takes constant time.
// The order of accuracy depends on the regularity of the f, however, assuming f is
// four-times continuously differentiable, the error is of O(h^4).
// In addition, we can differentiate the spline and obtain a good interpolant for f'.
// The main restriction of this method is that the samples of f must be evenly spaced.
// Look for barycentric rational interpolation for non-evenly sampled data.
// Properties:
// - s(x_j) = f(x_j)
// - All cubic polynomials interpolated exactly

#ifndef BOOST_MATH_INTERPOLATORS_CUBIC_B_SPLINE_HPP
#define BOOST_MATH_INTERPOLATORS_CUBIC_B_SPLINE_HPP

#include <boost/math/interpolators/detail/cubic_b_spline_detail.hpp>

namespace boost{ namespace math{

template <class Real>
class cubic_b_spline
{
public:
    // If you don't know the value of the derivative at the endpoints, leave them as nans and the routine will estimate them.
    // f[0] = f(a), f[length -1] = b, step_size = (b - a)/(length -1).
    template <class BidiIterator>
    cubic_b_spline(const BidiIterator f, BidiIterator end_p, Real left_endpoint, Real step_size,
                   Real left_endpoint_derivative = std::numeric_limits<Real>::quiet_NaN(),
                   Real right_endpoint_derivative = std::numeric_limits<Real>::quiet_NaN());
    cubic_b_spline(const Real* const f, size_t length, Real left_endpoint, Real step_size,
       Real left_endpoint_derivative = std::numeric_limits<Real>::quiet_NaN(),
       Real right_endpoint_derivative = std::numeric_limits<Real>::quiet_NaN());

    cubic_b_spline() = default;
    Real operator()(Real x) const;

    Real prime(Real x) const;

private:
    std::shared_ptr<detail::cubic_b_spline_imp<Real>> m_imp;
};

template<class Real>
cubic_b_spline<Real>::cubic_b_spline(const Real* const f, size_t length, Real left_endpoint, Real step_size,
                                     Real left_endpoint_derivative, Real right_endpoint_derivative) : m_imp(std::make_shared<detail::cubic_b_spline_imp<Real>>(f, f + length, left_endpoint, step_size, left_endpoint_derivative, right_endpoint_derivative))
{
}

template <class Real>
template <class BidiIterator>
cubic_b_spline<Real>::cubic_b_spline(BidiIterator f, BidiIterator end_p, Real left_endpoint, Real step_size,
   Real left_endpoint_derivative, Real right_endpoint_derivative) : m_imp(std::make_shared<detail::cubic_b_spline_imp<Real>>(f, end_p, left_endpoint, step_size, left_endpoint_derivative, right_endpoint_derivative))
{
}

template<class Real>
Real cubic_b_spline<Real>::operator()(Real x) const
{
    return m_imp->operator()(x);
}

template<class Real>
Real cubic_b_spline<Real>::prime(Real x) const
{
    return m_imp->prime(x);
}

}}
#endif


// 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 <iostream>
#include <cmath>
#include <complex>
#include <boost/numeric/mtl/vector/dense_vector.hpp>
#include <boost/numeric/mtl/vector/map_view.hpp>
#include <boost/numeric/mtl/operation/print.hpp>
#include <boost/numeric/mtl/operation/set_to_zero.hpp>
#include <boost/numeric/mtl/operation/conj.hpp>
#include <boost/numeric/mtl/operation/real.hpp>
#include <boost/numeric/mtl/operation/imag.hpp>
#include <boost/numeric/mtl/operation/scale.hpp>

#if 0
#include <boost/numeric/mtl/operation/hermitian.hpp>
#endif

using std::cout;  using std::complex;

typedef complex<double> ct;

double value(double)
{
    return 7.0;
}

complex<double> value(complex<double>)
{
    return ct(7.0, 1.0);
}

// scaled value
double svalue(double)
{
    return 14.0;
}

ct svalue(ct)
{
    return ct(14.0, 2.0);
}

// conjugated value
double cvalue(double)
{
    return 7.0;
}

ct cvalue(ct)
{
    return ct(7.0, -1.0);
}

// complex scaled value
ct csvalue(double)
{
    return ct(0.0, 7.0);
}

ct csvalue(ct)
{
    return ct(-1.0, 7.0);
}

template <typename Vector>
void test(Vector& vector, const char* name)
{
    using mtl::real; using mtl::imag;

    set_to_zero(vector);
    typename Vector::value_type ref(0);

    vector[2]= value(ref);
    vector[4]= value(ref) + 1.0;
    vector[5]= value(ref) + 2.0;

#if 0 // When sparse vectors are used there should be an inserter class for vectors too
    {
	inserter<Vector>  ins(vector);
	ins(2) << value(ref);
	ins(4) << value(ref) + 1.0;
	ins(5) << value(ref) + 2.0;
    }
#endif

    cout << "\n\n" << name << "\n";
    cout << "Original vector:\n" << vector << "\n";

    mtl::vec::scaled_view<double, Vector>  scaled_vector(2.0, vector);
    cout << "vector  scaled with 2.0\n" << scaled_vector << "\n";
    MTL_THROW_IF(scaled_vector(2) != svalue(ref), mtl::runtime_error("scaling wrong"));

    mtl::vec::conj_view<Vector>  conj_vector(vector);
    cout << "conjugated vector\n" << conj_vector << "\n";
    MTL_THROW_IF(conj_vector(2) != cvalue(ref), mtl::runtime_error(" wrong"));

    mtl::vec::scaled_view<ct, Vector>  cscaled_vector(ct(0.0, 1.0), vector);
    cout << "vector scaled with i (complex(0, 1))\n" << cscaled_vector << "\n";
    MTL_THROW_IF(cscaled_vector(2) != csvalue(ref), mtl::runtime_error("complex scaling wrong"));

#if 0 // transposition of vector is not an issue (yet)
    mtl::vec::hermitian_view<Vector>  hermitian_vector(vector);
    cout << "Hermitian vector (conjugate transposed)\n" << hermitian_vector << "\n";
    if (hermitian_vector(3, 2) != cvalue(ref))
	throw "conjugate transposing  wrong";
#endif

    cout << "vector  scaled with 2.0 (free function)\n" << scale(2.0, vector) << "\n";
    MTL_THROW_IF(scale(2.0, vector)(2) != svalue(ref), mtl::runtime_error("scaling wrong"));

#if defined(__GNUC__) && __GNUC__ == 4 && (__GNUC_MINOR__ >= 3 && __GNUC_MINOR__ <= 6)
    cout << "conjugated vector (free function) \n" << mtl::conj(vector) << "\n";
    MTL_THROW_IF(mtl::conj(vector)[2] != cvalue(ref), mtl::runtime_error("conjugating wrong"));

    cout << "real vector (free function) \n" << mtl::vec::real(vector) << "\n";
    MTL_THROW_IF(mtl::vec::real(vector)[2] != real(value(ref)), mtl::runtime_error("real part wrong"));

    cout << "imag vector (free function) \n" << mtl::vec::imag(vector) << "\n";
    MTL_THROW_IF(mtl::vec::imag(vector)[2] != imag(value(ref)), mtl::runtime_error("imag part wrong"));
#else
    cout << "conjugated vector (free function) \n" << conj(vector) << "\n";
    MTL_THROW_IF(conj(vector)[2] != cvalue(ref), mtl::runtime_error("conjugating wrong"));

    cout << "real vector (free function) \n" << real(vector) << "\n";
    MTL_THROW_IF(real(vector)[2] != real(value(ref)), mtl::runtime_error("real part wrong"));

    cout << "imag vector (free function) \n" << imag(vector) << "\n";
    MTL_THROW_IF(imag(vector)[2] != imag(value(ref)), mtl::runtime_error("imag part wrong"));
#endif

    cout << "vector scaled with i (complex(0, 1)) (free function)\n" << scale(ct(0.0, 1.0), vector) << "\n";
    MTL_THROW_IF(scale(ct(0.0, 1.0), vector)(2) != csvalue(ref), mtl::runtime_error("complex scaling wrong"));

#if 0 // transposition of vector is not an issue (yet)
    cout << "Hermitian  vector (conjugate transposed) (free function)\n" << hermitian(vector) << "\n";
    if (hermitian(vector)(3, 2) != cvalue(ref))
	throw "conjugate transposing wrong";
#endif

}

int main(int argc, char* argv[])
{
    unsigned size= 7;
    if (argc > 1) size= atoi(argv[1]);

    mtl::dense_vector<double>                                 dv(size);
    mtl::dense_vector<complex<double> >                       drc(size);

    test(dv, "Dense double vector");
    test(drc, "Dense complex vector");

    return 0;
}


// Copyright (C) 2010  Davis E. King (davis@dlib.net)
// License: Boost Software License   See LICENSE.txt for the full license.

#include "tester.h"
#include <dlib/matrix.h>
#include <dlib/rand.h>
#include <vector>
#include <sstream>

namespace
{
    using namespace test;
    using namespace dlib;
    using namespace std;
    dlib::logger dlog("test.symmetric_matrix_cache");

    class test_symmetric_matrix_cache : public tester
    {
        /*!
            WHAT THIS OBJECT REPRESENTS
                This object represents a unit test.  When it is constructed
                it adds itself into the testing framework.
        !*/
    public:
        test_symmetric_matrix_cache (
        ) :
            tester (
                "test_symmetric_matrix_cache",       // the command line argument name for this test
                "Run tests on the symmetric_matrix_cache function.", // the command line argument description
                0                     // the number of command line arguments for this test
            )
        {
        }

        dlib::rand rnd;

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

        template <typename EXP1, typename EXP2>
        void test_colm_exp (
            const matrix_exp<EXP1>& m1,
            const matrix_exp<EXP2>& m2
        )
        {
            for (long i = 0; i < m1.nc(); ++i)
            {

                typename colm_exp<EXP1>::type c1 = colm(m1,i);
                typename colm_exp<EXP2>::type c2 = colm(m2,i);

                DLIB_TEST(equal(c1 , c2));
                DLIB_TEST(equal(colm(m1,i) , c2));
                DLIB_TEST(equal(c1 , colm(m2,i)));
                DLIB_TEST(equal(colm(m1,i) , colm(m2,i)));
            }

            // Get a bunch of columns at once to test out the reference
            // counting and automatic cache expansion built into the symmetric_matrix_cache.
            // This test verifies that, for example, getting column 3 doesn't stomp on
            // any of the previous columns.
            typename colm_exp<EXP1>::type c1_0 = colm(m1,0);
            typename colm_exp<EXP1>::type c1_1 = colm(m1,1);
            typename colm_exp<EXP1>::type c1_2 = colm(m1,2);
            typename colm_exp<EXP1>::type c1_3 = colm(m1,3);
            typename colm_exp<EXP1>::type c1_4 = colm(m1,4);
            typename colm_exp<EXP1>::type c1_5 = colm(m1,5);

            typename colm_exp<EXP2>::type c2_0 = colm(m2,0);
            typename colm_exp<EXP2>::type c2_1 = colm(m2,1);
            typename colm_exp<EXP2>::type c2_2 = colm(m2,2);
            typename colm_exp<EXP2>::type c2_3 = colm(m2,3);
            typename colm_exp<EXP2>::type c2_4 = colm(m2,4);
            typename colm_exp<EXP2>::type c2_5 = colm(m2,5);

            DLIB_TEST(equal(c1_0, c2_0));
            DLIB_TEST(equal(c1_1, c2_1));
            DLIB_TEST(equal(c1_2, c2_2));
            DLIB_TEST(equal(c1_3, c2_3));
            DLIB_TEST(equal(c1_4, c2_4));
            DLIB_TEST(equal(c1_5, c2_5));
        }

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

        template <typename EXP1, typename EXP2>
        void test_rowm_exp (
            const matrix_exp<EXP1>& m1,
            const matrix_exp<EXP2>& m2
        )
        {
            for (long i = 0; i < m1.nc(); ++i)
            {

                typename rowm_exp<EXP1>::type r1 = rowm(m1,i);
                typename rowm_exp<EXP2>::type r2 = rowm(m2,i);

                DLIB_TEST(equal(r1 , r2));
                DLIB_TEST(equal(rowm(m1,i) , r2));
                DLIB_TEST(equal(r1 , rowm(m2,i)));
                DLIB_TEST(equal(rowm(m1,i) , rowm(m2,i)));
            }

            // Get a bunch of rows at once to test out the reference
            // counting and automatic cache expansion built into the symmetric_matrix_cache.
            // This test verifies that, for example, getting row 3 doesn't stomp on
            // any of the previous rows.
            typename rowm_exp<EXP1>::type r1_0 = rowm(m1,0);
            typename rowm_exp<EXP1>::type r1_1 = rowm(m1,1);
            typename rowm_exp<EXP1>::type r1_2 = rowm(m1,2);
            typename rowm_exp<EXP1>::type r1_3 = rowm(m1,3);
            typename rowm_exp<EXP1>::type r1_4 = rowm(m1,4);
            typename rowm_exp<EXP1>::type r1_5 = rowm(m1,5);

            typename rowm_exp<EXP2>::type r2_0 = rowm(m2,0);
            typename rowm_exp<EXP2>::type r2_1 = rowm(m2,1);
            typename rowm_exp<EXP2>::type r2_2 = rowm(m2,2);
            typename rowm_exp<EXP2>::type r2_3 = rowm(m2,3);
            typename rowm_exp<EXP2>::type r2_4 = rowm(m2,4);
            typename rowm_exp<EXP2>::type r2_5 = rowm(m2,5);

            DLIB_TEST(equal(r1_0, r2_0));
            DLIB_TEST(equal(r1_1, r2_1));
            DLIB_TEST(equal(r1_2, r2_2));
            DLIB_TEST(equal(r1_3, r2_3));
            DLIB_TEST(equal(r1_4, r2_4));
            DLIB_TEST(equal(r1_5, r2_5));
        }

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

        template <typename EXP1, typename EXP2>
        void test_diag_exp (
            const matrix_exp<EXP1>& m1,
            const matrix_exp<EXP2>& m2
        )
        {

            typename diag_exp<EXP1>::type c1 = diag(m1);
            typename diag_exp<EXP2>::type c2 = diag(m2);

            DLIB_TEST(equal(c1 , c2));
            DLIB_TEST(equal(diag(m1) , c2));
            DLIB_TEST(equal(c1 , diag(m2)));
            DLIB_TEST(equal(diag(m1) , diag(m2)));
        }

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

        void test_stuff (
            long csize
        )
        {
            print_spinner();
            dlog << LINFO << "csize: "<< csize;
            matrix<double> m = randm(10,10,rnd);

            m = make_symmetric(m);

            DLIB_TEST(equal(symmetric_matrix_cache<float>(m, csize), matrix_cast<float>(m)));
            DLIB_TEST(equal(symmetric_matrix_cache<double>(m, csize), matrix_cast<double>(m)));

            dlog << LINFO << "test colm/rowm";

            for (long i = 0; i < m.nr(); ++i)
            {
                DLIB_TEST(equal(colm(symmetric_matrix_cache<float>(m, csize),i), colm(matrix_cast<float>(m),i)));
                DLIB_TEST(equal(rowm(symmetric_matrix_cache<float>(m, csize),i), rowm(matrix_cast<float>(m),i)));
                // things are supposed to be symmetric
                DLIB_TEST(equal(colm(symmetric_matrix_cache<float>(m, csize),i), trans(rowm(matrix_cast<float>(m),i))));
                DLIB_TEST(equal(rowm(symmetric_matrix_cache<float>(m, csize),i), trans(colm(matrix_cast<float>(m),i))));
            }

            dlog << LINFO << "test diag";
            DLIB_TEST(equal(diag(symmetric_matrix_cache<float>(m,csize)), diag(matrix_cast<float>(m))));

            test_colm_exp(symmetric_matrix_cache<float>(m,csize), matrix_cast<float>(m));
            test_rowm_exp(symmetric_matrix_cache<float>(m,csize), matrix_cast<float>(m));
            test_diag_exp(symmetric_matrix_cache<float>(m,csize), matrix_cast<float>(m));

            test_colm_exp(tmp(symmetric_matrix_cache<float>(m,csize)), tmp(matrix_cast<float>(m)));
            test_rowm_exp(symmetric_matrix_cache<float>(m,csize), tmp(matrix_cast<float>(m)));
            test_diag_exp(tmp(symmetric_matrix_cache<float>(m,csize)), tmp(matrix_cast<float>(m)));
        }

        void perform_test (
        )
        {

            for (int itr = 0; itr < 5; ++itr)
            {
                test_stuff(0);
                test_stuff(1);
                test_stuff(2);
            }

        }
    };

    // Create an instance of this object.  Doing this causes this test
    // to be automatically inserted into the testing framework whenever this cpp file
    // is linked into the project.  Note that since we are inside an unnamed-namespace
    // we won't get any linker errors about the symbol a being defined multiple times.
    test_symmetric_matrix_cache a;

}


// The MIT License (MIT)
//
// Copyright (c) 2015 Markus Herb
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in
// all copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
// THE SOFTWARE.

/**
 * author: li chunpeng  <lichunpeng@stu.xidian.edu.cn>
 * create: 2018-10-11
 */
#ifndef _URANUS_MATRIX_HPP_
#define _URANUS_MATRIX_HPP_

#include <cmath>
#include <Eigen/Dense>
#include <Eigen/Eigenvalues>

namespace uranus
{
	const int Dynamic = Eigen::Dynamic;

	/**
	 * @class lcp::Matrix
	 * @brief Template type for matrices
	 * @param T The numeric scalar type
	 * @param rows The number of rows
	 * @param cols The number of columns
	 */
	template<int rows, int cols>
	using Matrix = Eigen::Matrix<double, rows, cols>;

	/**
	 * @brief Template type for vectors
	 * @param T The numeric scalar type
	 * @param N The vector dimension
	 */
	template<int N>
	using Vector = Eigen::Matrix<double, N, 1>;

	/**
	 * @class lcp::SquareMatrix
	 * @brief Template type representing a square matrix
	 * @param T The numeric scalar type
	 * @param N The dimensionality of the Matrix
	 */
	template<int N>
	using SquareMatrix = Eigen::Matrix<double, N, N>;

	/**
	 * @class uranus::setZeroMat
	 * @brief set all element zero for vector or SquareMatrix
	 * @param Type the input template
	 * @param N The dimensionality of vector or SquareMatrix
	 */
	template<typename Type, int N>
	inline bool setZero(Type& rhs)
	{
		if (std::is_same<Type, Vector<N>>::value)
		{
			for (int i = 0; i < N; ++i) rhs(i) = 0;
			return true;
		}
		else if (std::is_same<Type, SquareMatrix<N>>::value)
		{
			for (int i = 0; i < N*N; ++i) rhs(i) = 0;
			return true;
		}
		else
			return false;
	}

	/**
	 * @brief calu N dim Norm number
	 * @param Type the input template
	 * @param N The dimensionality of vector or SquareMatrix
	 */
	template<typename Type, int N>
	double Norm(const Type& lhs, const Type& rhs)
	{
		double _exp = 1.0 / N;
		double sum = 0;
		for (size_t i = 0; i < N; ++i)
			sum += pow(std::abs(lhs(i) - rhs(i)), N);
		return pow(sum, _exp);
	}
}

#endif // _URANUS_MATRIX_HPP_


// Copyright Louis Dionne 2013-2017
// Distributed under the Boost Software License, Version 1.0.
// (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)

#include <boost/hana/assert.hpp>
#include <boost/hana/equal.hpp>
#include <boost/hana/ext/boost/mpl/list.hpp>
#include <boost/hana/ext/std/integral_constant.hpp>
#include <boost/hana/fold_left.hpp>
#include <boost/hana/if.hpp>
#include <boost/hana/integral_constant.hpp>
#include <boost/hana/plus.hpp>
#include <boost/hana/type.hpp>

#include <boost/mpl/list.hpp>
#include <type_traits>
namespace hana = boost::hana;
namespace mpl = boost::mpl;

auto types = mpl::list<long, float, short, float, long, long double>{};
auto number_of_floats = hana::fold_left(types, hana::int_c<0>, [](auto count, auto t) {
    return hana::if_(hana::trait<std::is_floating_point>(t),
        count + hana::int_c<1>,
        count
    );
});

BOOST_HANA_CONSTANT_CHECK(number_of_floats == hana::int_c<3>);

int main() { }


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

using namespace std;
using namespace Eigen;

int main()
{
  Array33f m;
  m << 1,2,3,
       4,5,6,
       7,8,9;
  Array<float,5,5> n = Array<float,5,5>::Constant(0.6);
  n.block(1,1,3,3) = m;
  cout << "n = " << endl << n << endl << endl;
  Array33f res = n.block(0,0,3,3) * m;
  cout << "res =" << endl << res << endl;
}


//[ Calc3
//  Copyright 2008 Eric Niebler. 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)
//
// This example enhances the arithmetic expression evaluator
// in calc2.cpp by using a proto transform to calculate the
// number of arguments an expression requires and using a
// compile-time assert to guarantee that the right number of
// arguments are actually specified.

#include <iostream>
#include <boost/mpl/int.hpp>
#include <boost/mpl/assert.hpp>
#include <boost/mpl/min_max.hpp>
#include <boost/proto/core.hpp>
#include <boost/proto/context.hpp>
#include <boost/proto/transform.hpp>
namespace mpl = boost::mpl;
namespace proto = boost::proto;
using proto::_;

// Will be used to define the placeholders _1 and _2
template<typename I> struct placeholder : I {};

// This grammar basically says that a calculator expression is one of:
//   - A placeholder terminal
//   - Some other terminal
//   - Some non-terminal whose children are calculator expressions
// In addition, it has transforms that say how to calculate the
// expression arity for each of the three cases.
struct CalculatorGrammar
  : proto::or_<

        // placeholders have a non-zero arity ...
        proto::when< proto::terminal< placeholder<_> >, proto::_value >

        // Any other terminals have arity 0 ...
      , proto::when< proto::terminal<_>, mpl::int_<0>() >

        // For any non-terminals, find the arity of the children and
        // take the maximum. This is recursive.
      , proto::when< proto::nary_expr<_, proto::vararg<_> >
             , proto::fold<_, mpl::int_<0>(), mpl::max<CalculatorGrammar, proto::_state>() > >

    >
{};

// Simple wrapper for calculating a calculator expression's arity.
// It specifies mpl::int_<0> as the initial state. The data, which
// is not used, is mpl::void_.
template<typename Expr>
struct calculator_arity
  : boost::result_of<CalculatorGrammar(Expr)>
{};

template<typename Expr>
struct calculator_expression;

// Tell proto how to generate expressions in the calculator_domain
struct calculator_domain
  : proto::domain<proto::generator<calculator_expression> >
{};

// Define a calculator context, for evaluating arithmetic expressions
// (This is as before, in calc1.cpp and calc2.cpp)
struct calculator_context
  : proto::callable_context< calculator_context const >
{
    // The values bound to the placeholders
    double d[2];

    // The result of evaluating arithmetic expressions
    typedef double result_type;

    explicit calculator_context(double d1 = 0., double d2 = 0.)
    {
        d[0] = d1;
        d[1] = d2;
    }

    // Handle the evaluation of the placeholder terminals
    template<typename I>
    double operator ()(proto::tag::terminal, placeholder<I>) const
    {
        return d[ I() - 1 ];
    }
};

// Wrap all calculator expressions in this type, which defines
// operator () to evaluate the expression.
template<typename Expr>
struct calculator_expression
  : proto::extends<Expr, calculator_expression<Expr>, calculator_domain>
{
    typedef
        proto::extends<Expr, calculator_expression<Expr>, calculator_domain>
    base_type;

    explicit calculator_expression(Expr const &expr = Expr())
      : base_type(expr)
    {}

    BOOST_PROTO_EXTENDS_USING_ASSIGN(calculator_expression<Expr>)

    // Override operator () to evaluate the expression
    double operator ()() const
    {
        // Assert that the expression has arity 0
        BOOST_MPL_ASSERT_RELATION(0, ==, calculator_arity<Expr>::type::value);
        calculator_context const ctx;
        return proto::eval(*this, ctx);
    }

    double operator ()(double d1) const
    {
        // Assert that the expression has arity 1
        BOOST_MPL_ASSERT_RELATION(1, ==, calculator_arity<Expr>::type::value);
        calculator_context const ctx(d1);
        return proto::eval(*this, ctx);
    }

    double operator ()(double d1, double d2) const
    {
        // Assert that the expression has arity 2
        BOOST_MPL_ASSERT_RELATION(2, ==, calculator_arity<Expr>::type::value);
        calculator_context const ctx(d1, d2);
        return proto::eval(*this, ctx);
    }
};

// Define some placeholders (notice they're wrapped in calculator_expression<>)
calculator_expression<proto::terminal< placeholder< mpl::int_<1> > >::type> const _1;
calculator_expression<proto::terminal< placeholder< mpl::int_<2> > >::type> const _2;

// Now, our arithmetic expressions are immediately executable function objects:
int main()
{
    // Displays "5"
    std::cout << (_1 + 2.0)( 3.0 ) << std::endl;

    // Displays "6"
    std::cout << ( _1 * _2 )( 3.0, 2.0 ) << std::endl;

    // Displays "0.5"
    std::cout << ( (_1 - _2) / _2 )( 3.0, 2.0 ) << std::endl;

    // This won't compile because the arity of the
    // expression doesn't match the number of arguments
    // ( (_1 - _2) / _2 )( 3.0 );

    return 0;
}
//]


#define BOOST_TEST_MODULE pcraster aguila util
#include <boost/test/unit_test.hpp>
#include "com_util.h"

BOOST_AUTO_TEST_CASE(smallest_divisor)
{
  using namespace com;

  BOOST_CHECK(smallestDivisor(10, 1) == 1);
  BOOST_CHECK(smallestDivisor(10, 2) == 2);
  BOOST_CHECK(smallestDivisor(10, 3) == 5);
  BOOST_CHECK(smallestDivisor(10, 4) == 5);
  BOOST_CHECK(smallestDivisor(10, 6) == 10);
  BOOST_CHECK(smallestDivisor(10, 10) == 10);
}

BOOST_AUTO_TEST_CASE(largest_divisor)
{
  using namespace com;

  BOOST_CHECK(largestDivisor(10, 10) == 10);
  BOOST_CHECK(largestDivisor(10, 9) == 5);
  BOOST_CHECK(largestDivisor(10, 6) == 5);
  BOOST_CHECK(largestDivisor(10, 5) == 5);
  BOOST_CHECK(largestDivisor(10, 4) == 2);
  BOOST_CHECK(largestDivisor(10, 2) == 2);
  BOOST_CHECK(largestDivisor(10, 1) == 1);
}


// smooth_feedback: Control theory on Lie groups
// https://github.com/pettni/smooth_feedback
//
// Licensed under the MIT License <http://opensource.org/licenses/MIT>.
//
// Copyright (c) 2021 Petter Nilsson
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in all
// copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.

#ifndef SMOOTH__FEEDBACK__COLLOCATION__MESH_HPP_
#define SMOOTH__FEEDBACK__COLLOCATION__MESH_HPP_

/**
 * @file
 * @brief Refinable Legendre-Gauss-Radau mesh of time interval [0, 1]
 */

#include <Eigen/Core>
#include <Eigen/LU>
#include <smooth/internal/utils.hpp>
#include <smooth/polynomial/quadrature.hpp>

#include <ranges>
#include <span>
#include <vector>

#include "smooth/feedback/traits.hpp"
#include "smooth/feedback/utils/sparse.hpp"

namespace smooth::feedback {

using smooth::utils::zip;

using std::views::iota, std::views::drop, std::views::reverse, std::views::take,
  std::views::transform, std::views::join;

namespace detail {

/**
 * @brief Legendre-Gauss-Radau nodes including an extra node at +1.
 */
template<std::size_t K, std::size_t I = 8>
constexpr std::pair<std::array<double, K + 1>, std::array<double, K + 1>> lgr_plus_one()
{
  auto lgr_norm = ::smooth::lgr_nodes<K, I>();

  std::array<double, K + 1> ns, ws;
  for (auto i = 0u; i < K; ++i) {
    ns[i] = lgr_norm.first[i];
    ws[i] = lgr_norm.second[i];
  }
  ns[K] = 1;
  ws[K] = 0;
  return {ns, ws};
}

}  // namespace detail

/**
 * @brief Collocation mesh of interval [0, 1].
 * @tparam _Kmin minimal number of collocation points per interval
 * @tparam _Kmax maximal number of collocation points per interval
 *
 * [0, 1] is divided into non-overlapping intervals I_i, and each interval I_i has K_i LGR
 * collocation points.
 */
template<std::size_t _Kmin = 5, std::size_t _Kmax = 10>
  requires(_Kmin <= _Kmax)
class Mesh
{
  using MatMap = Eigen::Map<const Eigen::Matrix<double, -1, -1, Eigen::RowMajor>>;

public:
  /// @brief Minimal number of collocation points per interval
  static constexpr auto Kmin = _Kmin;
  /// @brief Maximal number of collocation points per interval
  static constexpr auto Kmax = _Kmax;

  /**
   * @brief Create a mesh consisting of a single interval [0, 1].
   *
   * @param Kmin minimal polynomial degree in mesh
   * @param Kmax maximal polynomial degree in mesh
   *
   * @note It must hold that kKmin <= Kmin <= Kmax <= kKmax, where kKmin and kKmax are compile-time
   * constants that define which LGR nodes to pre-compute.
   *
   * @note Allocates heap memory.
   */
  inline Mesh() : intervals_(1, Interval{.K = Kmin, .tau0 = 0.}) {}

  /**
   * @brief Create a mesh consisting of a n intervals of equal size over [0, 1].
   *
   * @param n number of intervals. If n==0, only a single interval is created.
   * @param k polynomial degree for all intervals
   *
   * @note Allocates heap memory.
   */
  inline Mesh(const std::size_t n, const std::size_t k = Kmin)
  {
    assert(Kmin <= k && k <= Kmax + 1);

    if (n < 2) {
      intervals_.emplace_back(k, 0.);
    } else {
      const double dx = 1. / static_cast<double>(n);
      intervals_.reserve(n);
      for (std::size_t i = 0; i < n; ++i) {
        intervals_.emplace_back(k, static_cast<double>(i) * dx);
      }
    }
  }

  /**
   * @brief Number of intervals in mesh.
   */
  inline std::size_t N_ivals() const { return intervals_.size(); }

  /**
   * @brief Number of collocation points in mesh.
   */
  inline std::size_t N_colloc() const
  {
    return std::accumulate(
      intervals_.begin(), intervals_.end(), 0u, [](std::size_t curr, const auto & x) {
        return curr + x.K;
      });
  }

  /**
   * @brief Number of collocation points in interval i.
   *
   * @note This is also equal to the polynomial degree inside interval i, since the polynomial is
   * fitted with an "extra" point belonging to the subsequent interval.
   */
  inline std::size_t N_colloc_ival(std::size_t i) const
  {
    assert(i < intervals_.size());
    return intervals_[i].K;
  }

  /**
   * @brief Refine interval using the ph strategy.
   *
   * @param i index of interval to refine
   * @param D target number of collocation points in refined interval
   *
   * If D > Kmax, or current degree > Kmax    then the interval is divided into
   *                                          n = max(2, ceil(D / Kmin)) intervals with deg Kmin
   * If D < current degree,                   then nothing is done.
   * If D <= Kmax,                            then the polynomial degree is increased to D.
   *
   * @note May allocate heap memory due to vector resizing if the number of intervals is increased.
   */
  inline void refine_ph(std::size_t i, std::size_t D)
  {
    assert(i < intervals_.size());
    if (D > Kmax || intervals_[i].K > Kmax) {
      // refine by splitting interval into n intervals, each with degree Kmin_
      std::size_t n = std::max<std::size_t>(2u, (D + Kmin - 1) / Kmin);

      const double tau0 = intervals_[i].tau0;
      const double tauf = i + 1 < intervals_.size() ? intervals_[i + 1].tau0 : 1.;
      const double taum = (tauf - tau0) / n;

      while (n-- > 1) {
        intervals_.insert(intervals_.begin() + i + 1, Interval{.K = Kmin, .tau0 = tau0 + n * taum});
      }
    } else if (D < intervals_[i].K) {
      return;
    } else if (D <= Kmax) {
      // refine by increasing degree in interval
      intervals_[i].K = D;
    }
  }

  /**
   * @brief Refine intervals in mesh to satisfy a target error criterion.
   * @param errs relative errors for all intervals (@see mesh_dyn_error())
   * @param target_err target relative error
   */
  inline void refine_errors(std::ranges::sized_range auto && errs, double target_err)
  {
    const auto N = N_ivals();

    assert(N == std::size_t(std::ranges::size(errs)));

    for (const auto & [i, e] : zip(iota(0u, N) | reverse, errs | reverse)) {

      const auto Ki = N_colloc_ival(i);

      if (e > target_err) {
        const auto Ktarget = Ki + std::lround(std::log(e / target_err) / std::log(Ki) + 1);
        refine_ph(i, Ktarget);
      }
    }
  }

  /**
   * @brief Set the number of collocation points in interval i to K
   * @param i interval index
   * @param K number of collocation points s.t. (Kmin <= K <= Kmax + 1)
   */
  inline void set_N_colloc_ival(std::size_t i, std::size_t K)
  {
    assert(Kmin <= K);
    assert(K <= Kmax + 1);
    intervals_[i].K = K;
  }

  /**
   * @brief Interval nodes (as range of doubles).
   *
   * @note Includes extra point at 1, i.e. size of returned range is equal to N_colloc_ival()+1
   */
  inline auto interval_nodes(std::size_t i) const
  {
    const std::size_t k = intervals_[i].K;

    assert(Kmin <= k && k <= Kmax + 1);

    std::span<const double> sp;

    utils::static_for<Kmax + 2 - Kmin>([&](auto ivar) {
      static constexpr auto K = Kmin + ivar;
      if (K == k) {
        static constexpr auto nw_ext_s = detail::lgr_plus_one<K>();

        sp = std::span<const double>(nw_ext_s.first.data(), k + 1);
      }
    });

    const double tau0 = intervals_[i].tau0;
    const double tauf = i + 1 < intervals_.size() ? intervals_[i + 1].tau0 : 1.;
    const double al   = (tauf - tau0) / 2;

    return transform(std::move(sp), [tau0, al](double d) -> double { return tau0 + al * (d + 1); });
  }

  /**
   * @brief Nodes (as range of doubles).
   *
   * @note Includes extra point at 1, i.e. size of returned range is equal to N_colloc()+1
   *
   * @note The result is an input range
   */
  inline auto all_nodes() const
  {
    const auto n_ivals = N_ivals();
    auto all_views     = iota(0u, n_ivals) | transform([this, n_ivals = n_ivals](auto i) {
                       const auto n_ival    = N_colloc_ival(i);
                       const int64_t n_take = i + 1 < n_ivals ? n_ival : n_ival + 1;
                       return interval_nodes(i) | take(n_take);
                     });

    return join(std::move(all_views));
  }

  /**
   * @brief Interval weights (as range of doubles)
   *
   * @note Includes zero weight at 1, i.e. size of returned range is equal to N_colloc_ival()+1
   */
  inline auto interval_weights(std::size_t i) const
  {
    const std::size_t k = intervals_[i].K;

    assert(Kmin <= k && k <= Kmax + 1);

    std::span<const double> sp;

    utils::static_for<Kmax + 2 - Kmin>([&](auto ivar) {
      static constexpr auto K = Kmin + ivar;
      if (K == k) {
        static constexpr auto nw_ext_s = detail::lgr_plus_one<K>();

        sp = std::span<const double>(nw_ext_s.second.data(), k + 1);
      }
    });

    const double tau0 = intervals_[i].tau0;
    const double tauf = i + 1 < intervals_.size() ? intervals_[i + 1].tau0 : 1.;
    const double al   = (tauf - tau0) / 2;

    return transform(std::move(sp), [al](double d) -> double { return al * d; });
  }

  /**
   * @brief Weights (as range of doubles)
   *
   * @note Includes zero weight at 1, i.e. size of returned range is equal to N_colloc()+1
   *
   * @note The result is an input range
   */
  inline auto all_weights() const
  {
    const auto n_ivals = N_ivals();
    auto all_views     = iota(0u, n_ivals) | transform([this, n_ivals = n_ivals](auto i) {
                       const auto n_ival    = N_colloc_ival(i);
                       const int64_t n_take = i + 1 < n_ivals ? n_ival : n_ival + 1;
                       return interval_weights(i) | take(n_take);
                     });

    return join(std::move(all_views));
  }

  /**
   * @brief Interval differentiation matrix w.r.t. [0, 1] timescale.
   *
   * Returns a \f$ (K+1 \times K) \f$ matrix \f$ D \f$ s.t.
   * \f[
   *   \begin{bmatrix} y'(\tau_{i, 0}) & y'(\tau_{i, 1}) & \cdots & y'(\tau_{i, K-1}) \end{bmatrix}
   *  =
   *   \begin{bmatrix} y(\tau_{i, 0}) & y(\tau_{i, 1}) & \cdots & y(\tau_{i, K}) \end{bmatrix} D
   * \f],
   * where \f$ y(\cdot) \in \mathbb{R}^{d \times 1} \f$ is a Lagrange polynomial in interval i.
   *
   * @note Allocates heap memory for return value.
   */
  inline Eigen::MatrixXd interval_diffmat(std::size_t i) const
  {
    const std::size_t k = intervals_[i].K;

    const double tau0 = intervals_[i].tau0;
    const double tauf = i + 1 < intervals_.size() ? intervals_[i + 1].tau0 : 1.;

    Eigen::MatrixXd ret(k + 1, k);

    utils::static_for<Kmax + 2 - Kmin>([&](auto ivar) {
      static constexpr auto K = Kmin + ivar;
      if (K == k) {
        static constexpr auto nw_ext_s = detail::lgr_plus_one<K>();
        static constexpr auto B_ext_s  = lagrange_basis<K>(nw_ext_s.first);
        static constexpr auto D_ext_s =
          polynomial_basis_derivatives<K, K + 1>(B_ext_s, nw_ext_s.first)
            .template block<K + 1, K>(0, 0);
        ret = MatMap(D_ext_s[0].data(), k + 1, k);
      }
    });

    ret *= 2. / (tauf - tau0);
    return ret;
  }

  /**
   * @brief Interval differentiation matrix (unscaled).
   *
   * Returns a Map D_us and a scalar alpha s.t. D = alpha * D_us
   *
   * @see interval_diffmat
   */
  inline std::pair<double, MatMap> interval_diffmat_unscaled(std::size_t i) const
  {
    const std::size_t k = intervals_[i].K;

    const double tau0 = intervals_[i].tau0;
    const double tauf = i + 1 < intervals_.size() ? intervals_[i + 1].tau0 : 1.;

    MatMap ret(nullptr, 0, 0);

    utils::static_for<Kmax + 2 - Kmin>([&](auto ivar) {
      static constexpr auto K = Kmin + ivar;
      if (K == k) {
        static constexpr auto nw_ext_s = detail::lgr_plus_one<K>();
        static constexpr auto B_ext_s  = lagrange_basis<K>(nw_ext_s.first);
        static constexpr auto D_ext_s =
          polynomial_basis_derivatives<K, K + 1>(B_ext_s, nw_ext_s.first)
            .template block<K + 1, K>(0, 0);
        // Eigen maps don't have copy constructors so use placement new
        new (&ret) MatMap(D_ext_s[0].data(), k + 1, k);
      }
    });

    return {2. / (tauf - tau0), ret};
  }

  /**
   * @brief Interval integration matrix w.r.t. [0, 1] timescale.
   *
   * Returns a \f$ (K \times K) \f$ matrix \f$ I \f$ s.t.
   * \f[
   *   \begin{bmatrix}
   *      y(\tau_{i, 1}) & y(\tau_{i, 2}) & \cdots & y(\tau_{i, K})
   *   \end{bmatrix}
   *  = y(\tau_{i, 0}) \begin{bmatrix} 1 & \ldots & 1 \end{bmatrix}
   *    + \begin{bmatrix}
   *        \dot y(\tau_{i, 0}) & \dot y(\tau_{i, 1}) & \cdots & \dot y(\tau_{i, K-1})
   *      \end{bmatrix} I
   * \f],
   * where \f$ y(\cdot) \in \mathbb{R}^{d \times 1} \f$ is a Lagrange
   * polynomial in interval i.
   *
   * @note Allocates heap memory for return value.
   *
   * @note Performs a matrix inverse.
   */
  inline Eigen::MatrixXd interval_intmat(std::size_t i) const
  {
    const std::size_t k = intervals_[i].K;
    return interval_diffmat(i).block(1, 0, k, k).inverse();
  }

  /**
   * @brief Find interval index that contains t
   */
  inline std::size_t interval_find(double t) const
  {
    if (t < 0) { return 0; }
    if (t > 1) { return intervals_.size() - 1; }
    auto it = utils::binary_interval_search(
      intervals_, t, [](const auto & ival, double _t) { return ival.tau0 <=> _t; });
    if (it != intervals_.end()) { return std::distance(intervals_.begin(), it); }
    return 0;
  }

  /**
   * @brief Increase the degree of all intervals by one (up to maximal degree Kmax + 1)
   */
  void increase_degrees()
  {
    for (auto & ival : intervals_) { ival.K = std::min(ival.K + 1, Kmax + 1); }
  }

  /**
   * @brief Decrease the degree of all intervals by one (down to minimal degree Kmin)
   */
  void decrease_degrees()
  {
    for (auto & ival : intervals_) { ival.K = std::max(ival.K - 1, Kmin); }
  }

  /**
   * @brief Evaluate a function
   *
   * @tparam RetT return value type
   *
   * @param t time value in [0, 1]
   * @param r values for the collocation points (size N [extend=false] or N+1 [extend=true])
   * @param p derivative to evaluate
   * @param extend set to true if a value is provided for t=+1
   */
  template<smooth::traits::RnType RetT>
  RetT eval(double t, std::ranges::range auto && r, std::size_t p = 0, bool extend = true) const
  {
    const std::size_t ival = interval_find(t);
    const std::size_t k    = intervals_[ival].K;

    const double tau0 = intervals_[ival].tau0;
    const double tauf = ival + 1 < intervals_.size() ? intervals_[ival + 1].tau0 : 1.;

    const double u = 2 * (t - tau0) / (tauf - tau0) - 1;

    int64_t N_before = 0;
    for (auto i = 0u; i < ival; ++i) { N_before += intervals_[i].K; }

    // initialize output variable
    RetT ret = RetT::Zero(dof(*std::ranges::begin(r)));

    utils::static_for<Kmax + 2 - Kmin>([&](auto i) {
      static constexpr auto K = Kmin + i;
      if (K == k) {
        if (extend || ival + 1 < intervals_.size()) {
          static constexpr auto nw_ext_s = detail::lgr_plus_one<K>();
          static constexpr auto B_ext_s  = lagrange_basis<K>(nw_ext_s.first);  // K+1 x K+1
          const auto U                   = monomial_derivative<K>(u, p);       // 1 x K+1
          const auto W                   = U * B_ext_s;                        // 1 x K+1

          for (const auto & [w, v] : zip(std::span(W[0].data(), k + 1), r | drop(N_before))) {
            ret += w * v;
          }
        } else {
          static constexpr auto nw_s = lgr_nodes<K>();
          static constexpr auto B_s  = lagrange_basis<K - 1>(nw_s.first);  // K x K
          const auto U               = monomial_derivative<K - 1>(u, p);   // 1 x K
          const auto W               = U * B_s;                            // 1 x K

          for (const auto & [w, v] : zip(std::span(W[0].data(), k), r | drop(N_before))) {
            ret += w * v;
          }
        }
      }
    });

    return ret;
  }

private:
  struct Interval
  {
    /// @brief Polynomial degree in interval
    std::size_t K;
    /// @brief Start of interval on [0, 1] timescale
    double tau0;
  };

  /// @brief Mesh intervals
  std::vector<Interval> intervals_;
};

/// @brief MeshType is a specialization of Mesh
template<typename T>
concept MeshType = traits::is_specialization_of_sizet_v<std::decay_t<T>, Mesh>;

}  // namespace smooth::feedback

#endif  // SMOOTH__FEEDBACK__COLLOCATION__MESH_HPP_


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

#include <boost/simd/include/functor.hpp>
#include <boost/simd/constant/register.hpp>
#include <boost/simd/constant/hierarchy.hpp>

namespace boost { namespace simd
{
  namespace tag
  {
   /*!
     @brief Eps generic tag

     Represents the Eps constant in generic contexts.

     @par Models:
        Hierarchy
   **/
    BOOST_SIMD_CONSTANT_REGISTER(Eps,double,1,0X34000000,0x3CB0000000000000ULL)
  }
  namespace ext
  {
   template<class Site, class... Ts>
   BOOST_FORCEINLINE generic_dispatcher<tag::Eps, Site> dispatching_Eps(adl_helper, boost::dispatch::meta::unknown_<Site>, boost::dispatch::meta::unknown_<Ts>...)
   {
     return generic_dispatcher<tag::Eps, Site>();
   }
   template<class... Args>
   struct impl_Eps;
  }
  /*!
    Generates two times the machine epsilon.

    @par Semantic:

    @code
    T r = Eps<T>();
    @endcode

    is similar to:

    @code
    if T is integral
      r = T(1)
    else if T is double
      r =  pow(2.0, -53);
    else if T is float
      r =  pow(2.0f, -23);
    @endcode

  **/
  BOOST_SIMD_CONSTANT_IMPLEMENTATION(boost::simd::tag::Eps, Eps)
} }

#include <boost/simd/constant/common.hpp>

#endif


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

#include <iostream>
#include <nt2/include/functions/quadl.hpp>
#include <nt2/integration/output.hpp>
#include <nt2/integration/options.hpp>
#include <nt2/sdk/unit/tests.hpp>
#include <nt2/sdk/unit/module.hpp>
#include <boost/fusion/tuple.hpp>
#include <boost/lambda/lambda.hpp>
#include <boost/bind.hpp>
#include <nt2/include/functions/sqr.hpp>
#include <nt2/include/functions/rowvect.hpp>
#include <nt2/include/functions/exp.hpp>
#include <nt2/include/functions/log.hpp>
#include <nt2/include/functions/oneminus.hpp>
#include <nt2/include/functions/expm1.hpp>
#include <nt2/include/functions/sqr.hpp>
#include <nt2/include/functions/zeros.hpp>
#include <nt2/include/functions/globalsum.hpp>
#include <nt2/include/functions/globalmax.hpp>
#include <nt2/include/functions/mul_i.hpp>
#include <nt2/include/functions/linspace.hpp>
#include <nt2/include/functions/dist.hpp>
#include <nt2/include/functions/rec.hpp>
#include <nt2/include/functions/minusone.hpp>
#include <nt2/include/functions/dot.hpp>
#include <nt2/include/functions/ones.hpp>
#include <nt2/include/constants/half.hpp>
#include <nt2/include/constants/eps.hpp>
#include <nt2/include/constants/sqrteps.hpp>
#include <nt2/include/constants/one.hpp>
#include <nt2/include/constants/two.hpp>
#include <nt2/include/constants/pi.hpp>
#include <nt2/include/constants/sqrteps.hpp>
#include <nt2/include/constants/pio_2.hpp>
#include <nt2/table.hpp>
struct f
{
  template < class X > inline
  X operator()(const X & x ) const
  {
    return x;
  }
};

struct g
{
  template < class X > inline
  X operator()(const X & x ) const
  {
    return nt2::oneminus(x)/nt2::log(x);
  }
};

struct h
{
  template < class X> inline
  typename nt2::meta::as_complex<X>::type operator()(const X & x ) const
  {
    return nt2::exp(nt2::mul_i(x));
  }
};
struct k
{
  template < class X> inline
  X operator()(const X & x ) const
  {
    return nt2::rec(nt2::minusone(x+x));
  }
};

NT2_TEST_CASE_TPL( integ_cplx_out, NT2_REAL_TYPES )
{
  using nt2::quadl;
  using nt2::options;
  using nt2::integration::output;
  typedef typename nt2::meta::as_complex<T>::type cT;
  typedef nt2::table<T> tab_t;
  tab_t x =  nt2::linspace(-nt2::Pio_2<T>(), nt2::Pio_2<T>(), 2);
  BOOST_AUTO_TPL(res, (quadl(h(), x)));
  std::cout << "Integrals:" << res.integrals << ") with error " << res.errors
            << " after " << res.eval_count <<  " evaluations\n";
  NT2_TEST_LESSER_EQUAL(nt2::dist(res.integrals(1), nt2::Two<cT>()), nt2::Quadlabstol<T>());
}

NT2_TEST_CASE_TPL( integ_cplx_inout, NT2_REAL_TYPES )
{
  using nt2::quadl;
  using nt2::options;
  using nt2::integration::output;
  typedef typename nt2::meta::as_complex<T>::type cT;
  typedef nt2::table<cT> tab_t;
  cT cx[] = { cT(0, 0), cT(1, 1),cT(1, -1),cT(0, 0)};
  tab_t x(nt2::of_size(1, 4), &cx[0], &cx[4]);
  BOOST_AUTO_TPL(res, (quadl(k(), x)));
  std::cout << "Integrals:" << res.integrals << ") with error " << res.errors
            << " after " << res.eval_count <<  " evaluations and cvce is "<< res.successful << "\n";
  NT2_TEST_LESSER_EQUAL(nt2::dist(res.integrals(nt2::end_), std::complex<T>(0, -nt2::Pi<T>())), nt2::Quadlabstol<T>());
}

NT2_TEST_CASE_TPL( integ_cplx_inout2, NT2_REAL_TYPES )
{
  using nt2::quadl;
  using nt2::options;
  using nt2::integration::output;
  typedef typename nt2::meta::as_complex<T>::type cT;
  typedef nt2::table<cT> tab_t;
  cT cx[] = { std::complex<T>(0, 0), std::complex<T>(1, 1),std::complex<T>(1, -1),std::complex<T>(0, 0)};
  tab_t x(nt2::of_size(1, 4), &cx[0], &cx[4]);
  BOOST_AUTO_TPL(res, (quadl(k(), std::complex<T>(0, 0), std::complex<T>(0, 0),
                                     options[nt2::range::waypoints_ =x,
                                             nt2::range::return_waypoints_ = true,
                                             nt2::tolerance::abstol_ = nt2::Sqrteps<T>(),
                                             nt2::range::singular_a_ = true
                                       ] )));
  std::cout << "Integrals:" << res.integrals << ") with error " << res.errors
            << " after " << res.eval_count <<  " evaluations and cvce is "<< res.successful << "\n";
  NT2_TEST_LESSER_EQUAL(nt2::dist(res.integrals(nt2::end_), std::complex<T>(0, -nt2::Pi<T>())),  nt2::Sqrteps<T>());
}

NT2_TEST_CASE_TPL( integ_functor_, NT2_REAL_TYPES )
{
  using nt2::quadl;
  using nt2::options;
  using nt2::integration::output;
  typedef nt2::table<T> tab_t;
  typedef typename nt2::meta::as_logical<T>::type lT;
  tab_t x = nt2::_(T(0), T(5), T(5));
  tab_t x0= nt2::_(T(0), T(5), T(5));
  NT2_DISPLAY(x);
  //output<tab_t,T>
  BOOST_AUTO_TPL(res, (quadl(f(), T(0), T(5))));
  std::cout << "Integrals:" << res.integrals << " with error " << res.errors
            << " after " << res.eval_count <<  " evaluations\n";

  NT2_TEST_LESSER_EQUAL(nt2::globalmax(nt2::dist(res.integrals, nt2::sqr(T(5))*nt2::Half<T>())), T(1.0e-5));
}

NT2_TEST_CASE_TPL( integ_functor__, NT2_REAL_TYPES )
{
  using nt2::quadl;
  using nt2::options;
  using nt2::integration::output;
  typedef nt2::table<T> tab_t;
  typedef typename nt2::meta::as_logical<T>::type lT;
  tab_t x = nt2::_(T(0), T(5), T(5));
  tab_t x0= nt2::_(T(0), T(5), T(5));
  NT2_DISPLAY(x);
  //output<tab_t,T>
  BOOST_AUTO_TPL(res, (quadl(f(), T(0), T(5), options [ nt2::tolerance::abstol_ = T(1.0e-5),
                                                              nt2::range::waypoints_ =x,
                                                              nt2::range::return_waypoints_ = true]))); //nt2::_(T(0), T(5), T(5)) ]));
  std::cout << "Integrals:" << res.integrals << " with error " << res.errors
            << " after " << res.eval_count <<  " evaluations\n";

  NT2_TEST_LESSER_EQUAL(nt2::globalmax(nt2::dist(res.integrals, nt2::sqr(x)*nt2::Half<T>())), T(1.0e-5));
}

NT2_TEST_CASE_TPL( integ_functorc, NT2_REAL_TYPES )
{
  using nt2::quadl;
  using nt2::options;
  using nt2::integration::output;
  typedef nt2::table<T> tab_t;
  typedef typename nt2::meta::as_logical<T>::type lT;
  tab_t x = nt2::_(T(0), T(1), T(5));
  NT2_DISPLAY(x);
  //output<tab_t,T>
  BOOST_AUTO_TPL(res, (quadl(f(), T(0), T(5), options [ nt2::tolerance::abstol_ = T(1.0e-5),
                                                              nt2::range::return_waypoints_ = true,
                                                              nt2::range::waypoints_ = nt2::_(T(0), T(1), T(5)) ])));
  std::cout << "Integrals:" << res.integrals << " with error " << res.errors
            << " after " << res.eval_count <<  " evaluations\n";

  NT2_TEST_LESSER_EQUAL(nt2::globalmax(nt2::dist(res.integrals, nt2::sqr(x)*nt2::Half<T>())), T(1.0e-5));
}

NT2_TEST_CASE_TPL( integ_functorb, NT2_REAL_TYPES )
{
  using nt2::quadl;
  using nt2::options;
  using nt2::integration::output;
  typedef nt2::table<T> tab_t;
  typedef typename nt2::meta::as_logical<T>::type lT;
  tab_t x = nt2::_(T(0), T(5), T(5));
  NT2_DISPLAY(x);
  //output<tab_t,T>
  BOOST_AUTO_TPL(res, (quadl(f(), x, options [ nt2::tolerance::abstol_ = T(1.0e-5),
                                                     nt2::range::return_waypoints_ = true])));
  std::cout << "Integrals:" << res.integrals << " with error " << res.errors
            << " after " << res.eval_count <<  " evaluations\n";

  NT2_TEST_LESSER_EQUAL(nt2::globalmax(nt2::dist(res.integrals, nt2::sqr(x)*nt2::Half<T>())), T(1.0e-5));
}

NT2_TEST_CASE_TPL( integ_functor0, NT2_REAL_TYPES )
{
  using nt2::quadl;
  using nt2::options;
  using nt2::integration::output;
  typedef nt2::table<T> tab_t;
  tab_t x = nt2::_(T(0), T(1), T(1));
  output<tab_t,T> res =  (quadl(g(), x, options [nt2::range::singular_a_ = true,
                                                       nt2::range::singular_b_ = true]));
  std::cout << "Integrals:" << res.integrals << " with error " << res.errors
            << " after " << res.eval_count <<  " evaluations\n";

  NT2_TEST_LESSER_EQUAL(nt2::dist(res.integrals(nt2::end_), -nt2::log(T(2))),nt2::Quadlabstol<T>());

}

NT2_TEST_CASE_TPL( integ_tag, NT2_REAL_TYPES )
{
  using nt2::quadl;
  using nt2::options;
  using nt2::integration::output;
  typedef nt2::table<T> tab_t;
  typedef typename nt2::meta::as_logical<T>::type lT;
  tab_t x = nt2::_(T(0), T(5));
  NT2_DISPLAY(x);
  //output<tab_t,T>
  BOOST_AUTO_TPL(res, (quadl(nt2::functor<nt2::tag::exp_>(), x)));
  std::cout << "Integrals: " << res.integrals << " with " << res.errors
            << " after " << res.eval_count <<  " evaluations\n";
  NT2_DISPLAY(x);
  NT2_DISPLAY(res.integrals);
  NT2_DISPLAY(nt2::expm1(x));

  NT2_TEST_LESSER_EQUAL(nt2::globalmax(nt2::dist(res.integrals, nt2::expm1(T(5)))), nt2::Sqrteps<T>());
}

NT2_TEST_CASE_TPL( integ_tag_r, NT2_REAL_TYPES )
{
  using nt2::quadl;
  using nt2::options;
  using nt2::integration::output;
  typedef nt2::table<T> tab_t;
  typedef typename nt2::meta::as_logical<T>::type lT;
  tab_t x = nt2::_(T(5), T(-1), T(4));
  NT2_DISPLAY(x);
  //output<tab_t,T>
  BOOST_AUTO_TPL(res, (quadl(nt2::functor<nt2::tag::exp_>(), T(5), T(4))));
  std::cout << "Integrals: " << res.integrals << " with " << res.errors
            << " after " << res.eval_count <<  " evaluations\n";
  NT2_DISPLAY(x);
  NT2_DISPLAY(res.integrals);
  NT2_DISPLAY( nt2::exp(x)-nt2::exp(T(5)));
  NT2_TEST_LESSER_EQUAL(nt2::globalmax(nt2::dist(res.integrals, nt2::exp(x(nt2::end_))-nt2::exp(T(5)))), nt2::Sqrteps<T>());
}

NT2_TEST_CASE_TPL( integ_tag_reverse, NT2_REAL_TYPES )
{
  using nt2::quadl;
  using nt2::options;
  using nt2::integration::output;
  typedef nt2::table<T> tab_t;
  typedef typename nt2::meta::as_logical<T>::type lT;
  tab_t x = nt2::_(T(5), T(-1), T(0));
  NT2_DISPLAY(x);
  //output<tab_t,T>
  BOOST_AUTO_TPL(res, (quadl(nt2::functor<nt2::tag::exp_>(), x)));
  std::cout << "Integrals: " << res.integrals << " with " << res.errors
            << " after " << res.eval_count <<  " evaluations\n";
  NT2_DISPLAY(x);
  NT2_DISPLAY(res.integrals);
  NT2_DISPLAY( nt2::exp(x)-nt2::exp(T(5)));
  NT2_TEST_LESSER_EQUAL(nt2::globalmax(nt2::dist(res.integrals, nt2::exp(x(nt2::end_))-nt2::exp(T(5)))), nt2::Sqrteps<T>());
}


/*
 * Copyright Andrey Semashev 2020
 *
 * Distributed under the Boost Software License, Version 1.0.
 * (See accompanying file LICENSE_1_0.txt or copy at
 * https://www.boost.org/LICENSE_1_0.txt)
 */
/*!
 * \file popcount.hpp
 *
 * This header defines \c popcount algorithm, which counts the number of non-zero bits in an integer.
 */

#ifndef BOOST_BIT_OPS_COUNTING_POPCOUNT_HPP_INCLUDED_
#define BOOST_BIT_OPS_COUNTING_POPCOUNT_HPP_INCLUDED_

#include <boost/cstdint.hpp>
#include <boost/bit_ops/detail/config.hpp>
#include <boost/bit_ops/detail/int_sizes.hpp>
#include <boost/bit_ops/detail/type_traits/enable_if.hpp>
#include <boost/bit_ops/detail/type_traits/integral_constant.hpp>
#include <boost/bit_ops/detail/type_traits/is_integral.hpp>
#include <boost/bit_ops/detail/type_traits/is_unsigned.hpp>

#if defined(_MSC_VER)
#include <intrin.h>
#endif

namespace boost {
namespace bit_ops {

namespace detail {

#if defined(BOOST_BIT_OPS_DETAIL_HAS_BUILTIN_POPCOUNT)

template< typename T, unsigned int N >
inline typename bit_ops::detail::enable_if< N <= BOOST_BIT_OPS_DETAIL_SIZEOF_INT, unsigned int >::type popcount(T value, bit_ops::detail::integral_constant< unsigned int, N >) BOOST_NOEXCEPT
{
    return static_cast< unsigned int >(__builtin_popcount(value));
}

#if BOOST_BIT_OPS_DETAIL_SIZEOF_LONG > BOOST_BIT_OPS_DETAIL_SIZEOF_INT

template< typename T, unsigned int N >
inline typename bit_ops::detail::enable_if< N == BOOST_BIT_OPS_DETAIL_SIZEOF_LONG, unsigned int >::type popcount(T value, bit_ops::detail::integral_constant< unsigned int, N >) BOOST_NOEXCEPT
{
    return static_cast< unsigned int >(__builtin_popcountl(value));
}

#endif // BOOST_BIT_OPS_DETAIL_SIZEOF_LONG > BOOST_BIT_OPS_DETAIL_SIZEOF_INT

#if BOOST_BIT_OPS_DETAIL_SIZEOF_LLONG > BOOST_BIT_OPS_DETAIL_SIZEOF_LONG

template< typename T, unsigned int N >
inline typename bit_ops::detail::enable_if< N == BOOST_BIT_OPS_DETAIL_SIZEOF_LLONG, unsigned int >::type popcount(T value, bit_ops::detail::integral_constant< unsigned int, N >) BOOST_NOEXCEPT
{
    return static_cast< unsigned int >(__builtin_popcountll(value));
}

#endif // BOOST_BIT_OPS_DETAIL_SIZEOF_LLONG > BOOST_BIT_OPS_DETAIL_SIZEOF_LONG

#elif defined(BOOST_BIT_OPS_DETAIL_HAS_POPCNT)

#if defined(BOOST_BIT_OPS_DETAIL_HAS_POPCNT16)

template< typename T, unsigned int N >
inline typename bit_ops::detail::enable_if< N <= 2u, unsigned int >::type popcount(T value, bit_ops::detail::integral_constant< unsigned int, N >) BOOST_NOEXCEPT
{
    return static_cast< unsigned int >(__popcnt16(value));
}

template< typename T, unsigned int N >
inline typename bit_ops::detail::enable_if< N == 4u, unsigned int >::type popcount(T value, bit_ops::detail::integral_constant< unsigned int, N >) BOOST_NOEXCEPT
{
    return static_cast< unsigned int >(__popcnt(value));
}

#else // defined(BOOST_BIT_OPS_DETAIL_HAS_POPCNT16)

template< typename T, unsigned int N >
inline typename bit_ops::detail::enable_if< N <= 4u, unsigned int >::type popcount(T value, bit_ops::detail::integral_constant< unsigned int, N >) BOOST_NOEXCEPT
{
    return static_cast< unsigned int >(__popcnt(value));
}

#endif // defined(BOOST_BIT_OPS_DETAIL_HAS_POPCNT16)

template< typename T, unsigned int N >
inline typename bit_ops::detail::enable_if< N == 8u, unsigned int >::type popcount(T value, bit_ops::detail::integral_constant< unsigned int, N >) BOOST_NOEXCEPT
{
#if defined(BOOST_BIT_OPS_DETAIL_HAS_POPCNT64)
    return static_cast< unsigned int >(__popcnt64(value));
#else
    return static_cast< unsigned int >(__popcnt(static_cast< unsigned int >(value)) + __popcnt(static_cast< unsigned int >(value >> 32u)));
#endif
}

#else

template< typename T >
inline unsigned int popcount(T value, bit_ops::detail::integral_constant< unsigned int, 1u >) BOOST_NOEXCEPT
{
    value = (value & 0x55u) + ((value >> 1u) & 0x55u);
    value = (value & 0x33u) + ((value >> 2u) & 0x33u);
    value = value + (value >> 4u);
    return static_cast< unsigned int >(value & 15u);
}

template< typename T >
inline unsigned int popcount(T value, bit_ops::detail::integral_constant< unsigned int, 2u >) BOOST_NOEXCEPT
{
    value = (value & 0x5555u) + ((value >> 1u) & 0x5555u);
    value = (value & 0x3333u) + ((value >> 2u) & 0x3333u);
    value = (value + (value >> 4u)) & 0x0F0Fu;
    value = value + (value >> 8u);
    return static_cast< unsigned int >(value & 31u);
}

template< typename T >
inline unsigned int popcount(T value, bit_ops::detail::integral_constant< unsigned int, 4u >) BOOST_NOEXCEPT
{
    value = (value & 0x55555555u) + ((value >> 1u) & 0x55555555u);
    value = (value & 0x33333333u) + ((value >> 2u) & 0x33333333u);
    value = (value + (value >> 4u)) & 0x0F0F0F0Fu;
    value = (value + (value >> 8u)) & 0x00FF00FFu;
    value = value + (value >> 16u);
    return static_cast< unsigned int >(value & 63u);
}

template< typename T >
inline unsigned int popcount(T value, bit_ops::detail::integral_constant< unsigned int, 8u >) BOOST_NOEXCEPT
{
    value = (value & UINT64_C(0x5555555555555555)) + ((value >> 1u) & UINT64_C(0x5555555555555555));
    value = (value & UINT64_C(0x3333333333333333)) + ((value >> 2u) & UINT64_C(0x3333333333333333));
    value = (value + (value >> 4u)) & UINT64_C(0x0F0F0F0F0F0F0F0F);
    value = (value + (value >> 8u)) & UINT64_C(0x00FF00FF00FF00FF);
    value = value + (value >> 16u);
    value = value + (value >> 32u);
    return static_cast< unsigned int >(value & 127u);
}

#endif

} // namespace detail

//! Returns the number of non-zero bits in \a value
template< typename T >
inline typename bit_ops::detail::enable_if<
    bit_ops::detail::is_integral< T >::value && bit_ops::detail::is_unsigned< T >::value,
    unsigned int
>::type popcount(T value) BOOST_NOEXCEPT
{
    return bit_ops::detail::popcount(value, bit_ops::detail::integral_constant< unsigned int, sizeof(T) >());
}

} // namespace bit_ops
} // namespace boost

#endif // BOOST_BIT_OPS_COUNTING_POPCOUNT_HPP_INCLUDED_


//==================================================================================================
/*!

  Copyright 2015 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)
*/
//==================================================================================================
#include <boost/simd/function/scalar/dot.hpp>
#include <simd_test.hpp>
#include <boost/dispatch/meta/as_integer.hpp>
#include <boost/simd/constant/inf.hpp>
#include <boost/simd/constant/minf.hpp>
#include <boost/simd/constant/mone.hpp>
#include <boost/simd/constant/nan.hpp>
#include <boost/simd/constant/one.hpp>
#include <boost/simd/constant/zero.hpp>

STF_CASE_TPL (" dot",  STF_IEEE_TYPES)
{
  namespace bs = boost::simd;
  using bs::dot;

  // specific values tests
  STF_EQUAL(dot(bs::Inf<T>(), bs::Inf<T>()), bs::Inf<T>());
  STF_EQUAL(dot(bs::Minf<T>(), bs::Minf<T>()), bs::Inf<T>());
  STF_EQUAL(dot(bs::Mone<T>(), bs::Mone<T>()), bs::One<T>());
  STF_IEEE_EQUAL(dot(bs::Nan<T>(), bs::Nan<T>()), bs::Nan<T>());
  STF_EQUAL(dot(bs::One<T>(), bs::One<T>()), bs::One<T>());
  STF_EQUAL(dot(bs::Zero<T>(), bs::Zero<T>()), bs::Zero<T>());
} // end of test for floating_


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

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

 * See LICENSE for the license information

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

/**
 * @file testErrors.cpp
 * @date Feb 20, 2010
 * @author Frank Dellaert
 */

#include <boost/assign/std/list.hpp> // for +=
using namespace boost::assign;

#include <CppUnitLite/TestHarness.h>
#include <gtsam/base/Testable.h>
#include <gtsam/linear/Errors.h>

using namespace std;
using namespace gtsam;

/* ************************************************************************* */
TEST( Errors, arithmetic )
{
  Errors e;
  e += Vector_(2,1.0,2.0), Vector_(3,3.0,4.0,5.0);
  DOUBLES_EQUAL(1+4+9+16+25,dot(e,e),1e-9);

  axpy(2.0,e,e);
  Errors expected;
  expected += Vector_(2,3.0,6.0), Vector_(3,9.0,12.0,15.0);
  CHECK(assert_equal(expected,e));
}

/* ************************************************************************* */
int main() {
  TestResult tr;
  return TestRegistry::runAllTests(tr);
}
/* ************************************************************************* */


#include <iostream>
#include <{{ cookiecutter.project_name }}Utils.h>
#include <fmt/core.h> //packagemanager downloaded library.
#define ARMA_DONT_USE_WRAPPER
#define ARMA_DONT_USE_BLAS
#define ARMA_DONT_USE_LAPACK
//#define ARMA_USE_BLAS
#include <armadillo> //system wide installed package.

using namespace std;
using namespace arma;

int main()
{
  auto message = fmt::format("The answer is {}", sum(1,2));
  std::cout << message <<std::endl;
  mat v = randu<mat>(3,3);
  mat b = randu<mat>(3,3);
  std::cout << v*b << std::endl;
return EXIT_SUCCESS;
}