/*
 [auto_generated]
 libs/numeric/odeint/test/velocity_verlet.cpp

 [begin_description]
 tba.
 [end_description]

 Copyright 2009-2012 Karsten Ahnert
 Copyright 2009-2012 Mario Mulansky

 Distributed under the Boost Software License, Version 1.0.
 (See accompanying file LICENSE_1_0.txt or
 copy at http://www.boost.org/LICENSE_1_0.txt)
 */

#include <boost/config.hpp>
#ifdef BOOST_MSVC
    #pragma warning(disable:4996)
#endif

#define BOOST_TEST_MODULE odeint_velocity_verlet

#define BOOST_FUSION_INVOKE_MAX_ARITY 15
#define BOOST_RESULT_OF_NUM_ARGS 15

#include <boost/numeric/odeint/config.hpp>

#include "resizing_test_state_type.hpp"

#include <boost/numeric/odeint/stepper/velocity_verlet.hpp>
#include <boost/numeric/odeint/algebra/fusion_algebra.hpp>

#include <boost/array.hpp>
#include <boost/test/unit_test.hpp>

#include <boost/units/systems/si/length.hpp>
#include <boost/units/systems/si/time.hpp>
#include <boost/units/systems/si/velocity.hpp>
#include <boost/units/systems/si/acceleration.hpp>
#include <boost/units/systems/si/io.hpp>

#include <boost/fusion/include/vector.hpp>
#include <boost/fusion/include/vector20.hpp>
#include <boost/fusion/container.hpp>

namespace fusion = boost::fusion;
namespace units = boost::units;
namespace si = boost::units::si;

typedef double value_type;
typedef units::quantity< si::time , value_type > time_type;
typedef units::unit< units::derived_dimension< units::time_base_dimension , 2 >::type , si::system > time_2;
typedef units::quantity< time_2 , value_type > time_2_type;
typedef units::quantity< si::length , value_type > length_type;
typedef units::quantity< si::velocity , value_type > velocity_type;
typedef units::quantity< si::acceleration , value_type > acceleration_type;
typedef fusion::vector< length_type , length_type > coor_vector;
typedef fusion::vector< velocity_type , velocity_type > velocity_vector;
typedef fusion::vector< acceleration_type , acceleration_type > accelartion_vector;

using namespace boost::unit_test;
using namespace boost::numeric::odeint;

size_t ode_call_count;

struct velocity_verlet_fixture
{
    velocity_verlet_fixture( void ) { ode_call_count = 0; adjust_size_count = 0; }
};

struct ode
{
    template< class CoorIn , class MomentumIn , class AccelerationOut , class Time >
    void operator()( const CoorIn &q , const MomentumIn &p , AccelerationOut &a , Time t ) const
    {
        a[0] = -q[0] - p[0];
        a[1] = -q[1] - p[1];
        ++ode_call_count;
    }
};

struct ode_units
{
    void operator()( coor_vector const &q , velocity_vector const &p , accelartion_vector &a , time_type t ) const
    {
        const units::quantity< si::frequency , value_type > omega = 1.0 * si::hertz;
        const units::quantity< si::frequency , value_type > friction = 0.001 * si::hertz;
        fusion::at_c< 0 >( a ) = omega * omega * fusion::at_c< 0 >( q ) - friction * fusion::at_c< 0 >( p );
        fusion::at_c< 1 >( a ) = omega * omega * fusion::at_c< 1 >( q ) - friction * fusion::at_c< 0 >( p );
        ++ode_call_count;
    }
};

template< class Q , class P >
void init_state( Q &q , P &p )
{
    q[0] = 1.0 ; q[1] = 0.5;
    p[0] = 2.0 ; p[1] = -1.0;
}

typedef boost::array< double , 2 > array_type;
typedef std::vector< double > vector_type;

typedef velocity_verlet< array_type > array_stepper;
typedef velocity_verlet< vector_type > vector_stepper;

template< typename Resizer >
struct get_resizer_test_stepper
{
    typedef velocity_verlet< test_array_type , test_array_type , double , test_array_type ,
        double , double , range_algebra , default_operations , Resizer > type;
};

BOOST_AUTO_TEST_SUITE( velocity_verlet_test )

BOOST_FIXTURE_TEST_CASE( test_with_array_ref , velocity_verlet_fixture )
{
    array_stepper stepper;
    array_type q , p ;
    init_state( q , p );
    stepper.do_step( ode() , std::make_pair( boost::ref( q ) , boost::ref( p ) ) , 0.0 , 0.01 );
    BOOST_CHECK_EQUAL( ode_call_count , size_t( 2 ) );
}

BOOST_FIXTURE_TEST_CASE( test_with_array_pair , velocity_verlet_fixture )
{
    array_stepper stepper;
    std::pair< array_type , array_type > xxx;
    init_state( xxx.first , xxx.second );
    stepper.do_step( ode() , xxx , 0.0 , 0.01 );
    BOOST_CHECK_EQUAL( ode_call_count , size_t( 2 ) );
}

BOOST_FIXTURE_TEST_CASE( test_with_vector_ref , velocity_verlet_fixture )
{
    vector_stepper stepper;
    vector_type q( 2 ) , p( 2 );
    init_state( q , p );
    stepper.do_step( ode() , std::make_pair( boost::ref( q ) , boost::ref( p ) ) , 0.0 , 0.01 );
    BOOST_CHECK_EQUAL( ode_call_count , size_t( 2 ) );
}

BOOST_FIXTURE_TEST_CASE( test_with_vector_pair , velocity_verlet_fixture )
{
    vector_stepper stepper;
    std::pair< vector_type , vector_type > x;
    x.first.resize( 2 ) ; x.second.resize( 2 );
    init_state( x.first , x.second );
    stepper.do_step( ode() , x , 0.0 , 0.01 );
    BOOST_CHECK_EQUAL( ode_call_count , size_t( 2 ) );
}

BOOST_FIXTURE_TEST_CASE( test_initial_resizer , velocity_verlet_fixture )
{
    typedef get_resizer_test_stepper< initially_resizer >::type stepper_type;
    std::pair< test_array_type , test_array_type > x;
    init_state( x.first , x.second );
    stepper_type stepper;
    stepper.do_step( ode() , x , 0.0 , 0.01 );
    stepper.do_step( ode() , x , 0.0 , 0.01 );
    BOOST_CHECK_EQUAL( adjust_size_count , size_t( 2 ) );
    BOOST_CHECK_EQUAL( ode_call_count , size_t( 3 ) );
}

BOOST_FIXTURE_TEST_CASE( test_always_resizer , velocity_verlet_fixture )
{
    typedef get_resizer_test_stepper< always_resizer >::type stepper_type;
    std::pair< test_array_type , test_array_type > x;
    init_state( x.first , x.second );
    stepper_type stepper;
    stepper.do_step( ode() , x , 0.0 , 0.01 );
    stepper.do_step( ode() , x , 0.0 , 0.01 );
    BOOST_CHECK_EQUAL( adjust_size_count , size_t( 4 ) );
    BOOST_CHECK_EQUAL( ode_call_count , size_t( 4 ) );    // attention: one more system call, since the size of the state has been changed
}

BOOST_FIXTURE_TEST_CASE( test_with_never_resizer , velocity_verlet_fixture )
{
    typedef get_resizer_test_stepper< never_resizer >::type stepper_type;
    std::pair< test_array_type , test_array_type > x;
    init_state( x.first , x.second );
    stepper_type stepper;
    stepper.do_step( ode() , x , 0.0 , 0.01 );
    stepper.do_step( ode() , x , 0.0 , 0.01 );
    BOOST_CHECK_EQUAL( adjust_size_count , size_t( 0 ) );
    BOOST_CHECK_EQUAL( ode_call_count , size_t( 3 ) );
}

BOOST_FIXTURE_TEST_CASE( test_reset , velocity_verlet_fixture )
{
    typedef get_resizer_test_stepper< initially_resizer >::type stepper_type;
    std::pair< test_array_type , test_array_type > x;
    init_state( x.first , x.second );
    stepper_type stepper;
    stepper.do_step( ode() , x , 0.0 , 0.01 );
    BOOST_CHECK_EQUAL( adjust_size_count , size_t( 2 ) );
    BOOST_CHECK_EQUAL( ode_call_count , size_t( 2 ) );
    stepper.do_step( ode() , x , 0.0 , 0.01 );
    BOOST_CHECK_EQUAL( adjust_size_count , size_t( 2 ) );
    BOOST_CHECK_EQUAL( ode_call_count , size_t( 3 ) );
    stepper.reset();
    BOOST_CHECK_EQUAL( adjust_size_count , size_t( 2 ) );
    BOOST_CHECK_EQUAL( ode_call_count , size_t( 3 ) );
    stepper.do_step( ode() , x , 0.0 , 0.01 );
    BOOST_CHECK_EQUAL( adjust_size_count , size_t( 2 ) );
    BOOST_CHECK_EQUAL( ode_call_count , size_t( 5 ) );
}

BOOST_FIXTURE_TEST_CASE( test_initialize1 , velocity_verlet_fixture )
{
    typedef get_resizer_test_stepper< initially_resizer >::type stepper_type;
    std::pair< test_array_type , test_array_type > x;
    init_state( x.first , x.second );
    stepper_type stepper;
    test_array_type ain;
    ode()( x.first , x.second , ain , 0.0 );
    BOOST_CHECK_EQUAL( adjust_size_count , size_t( 0 ) );
    stepper.initialize( ain );
    BOOST_CHECK_EQUAL( adjust_size_count , size_t( 2 ) );
    BOOST_CHECK_EQUAL( ode_call_count , size_t( 1 ) );
    stepper.do_step( ode() , x , 0.0 , 0.01 );
    BOOST_CHECK_EQUAL( adjust_size_count , size_t( 2 ) );
    BOOST_CHECK_EQUAL( ode_call_count , size_t( 2 ) );
}

BOOST_FIXTURE_TEST_CASE( test_initialize2 , velocity_verlet_fixture )
{
    typedef get_resizer_test_stepper< initially_resizer >::type stepper_type;
    std::pair< test_array_type , test_array_type > x;
    init_state( x.first , x.second );
    stepper_type stepper;
    stepper.initialize( ode() , x.first , x.second , 0.0 );
    BOOST_CHECK_EQUAL( adjust_size_count , size_t( 2 ) );
    BOOST_CHECK_EQUAL( ode_call_count , size_t( 1 ) );
    stepper.do_step( ode() , x , 0.0 , 0.01 );
    BOOST_CHECK_EQUAL( adjust_size_count , size_t( 2 ) );
    BOOST_CHECK_EQUAL( ode_call_count , size_t( 2 ) );
}

BOOST_FIXTURE_TEST_CASE( test_adjust_size , velocity_verlet_fixture )
{
    typedef get_resizer_test_stepper< initially_resizer >::type stepper_type;
    std::pair< test_array_type , test_array_type > x;
    init_state( x.first , x.second );
    stepper_type stepper;
    stepper.do_step( ode() , x , 0.0 , 0.01 );
    BOOST_CHECK_EQUAL( adjust_size_count , size_t( 2 ) );
    BOOST_CHECK_EQUAL( ode_call_count , size_t( 2 ) );
    stepper.adjust_size( x.first );
    BOOST_CHECK_EQUAL( adjust_size_count , size_t( 4 ) );
    BOOST_CHECK_EQUAL( ode_call_count , size_t( 2 ) );
    stepper.do_step( ode() , x , 0.0 , 0.01 );
    BOOST_CHECK_EQUAL( adjust_size_count , size_t( 4 ) );
    BOOST_CHECK_EQUAL( ode_call_count , size_t( 4 ) );
}

BOOST_FIXTURE_TEST_CASE( test_with_unit_pair , velocity_verlet_fixture )
{
    typedef velocity_verlet< coor_vector , velocity_vector , value_type , accelartion_vector ,
        time_type , time_2_type , fusion_algebra , default_operations > stepper_type;

    std::pair< coor_vector , velocity_vector > x;
    fusion::at_c< 0 >( x.first ) = 1.0 * si::meter;
    fusion::at_c< 1 >( x.first ) = 0.5 * si::meter;
    fusion::at_c< 0 >( x.second ) = 2.0 * si::meter_per_second;
    fusion::at_c< 1 >( x.second ) = -1.0 * si::meter_per_second;
    stepper_type stepper;
    stepper.do_step( ode_units() , x , 0.0 * si::second , 0.01 * si::second );
    BOOST_CHECK_EQUAL( ode_call_count , size_t( 2 ) );
}

BOOST_FIXTURE_TEST_CASE( test_with_unit_ref , velocity_verlet_fixture )
{
    typedef velocity_verlet< coor_vector , velocity_vector , value_type , accelartion_vector ,
        time_type , time_2_type , fusion_algebra , default_operations > stepper_type;

    coor_vector q;
    velocity_vector p;
    fusion::at_c< 0 >( q ) = 1.0 * si::meter;
    fusion::at_c< 1 >( q ) = 0.5 * si::meter;
    fusion::at_c< 0 >( p ) = 2.0 * si::meter_per_second;
    fusion::at_c< 1 >( p ) = -1.0 * si::meter_per_second;
    stepper_type stepper;
    stepper.do_step( ode_units() , std::make_pair( boost::ref( q ) , boost::ref( p ) ) , 0.0 * si::second , 0.01 * si::second );
    BOOST_CHECK_EQUAL( ode_call_count , size_t( 2 ) );
}

BOOST_AUTO_TEST_SUITE_END()


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

#ifndef ALLSOL_HPP
#define ALLSOL_HPP

#include <iostream>
#include <list>
#include <limits>
#include <kv/interval.hpp>
#include <kv/rdouble.hpp>
#include <kv/interval-vector.hpp>
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/io.hpp>
#ifdef _OPENMP
#include <omp.h>
#endif
// #include <boost/random.hpp>
#include <kv/matrix-inversion.hpp>
#include <kv/autodif.hpp>

#ifndef EDGE_RATIO
#define EDGE_RATIO 0.9
#endif

// 0: Do not use TRIM
// 1: use TRIM and use slow trim algorithm
// 2: use TRIM and use fast bat slightly inefficient trim algorithm
// 3: use TRIM and use new trim algorithm

#ifndef USE_TRIM
#define USE_TRIM 3
#endif

#ifndef USE_ZERODIVIDE
#define USE_ZERODIVIDE 2
#endif

#ifndef USE_MULTITRIM
#define USE_MULTITRIM 0
#endif

#ifndef USE_AFFINEABS
#define USE_AFFINEABS 0
#endif

#ifndef USE_MIGSKIP
#define USE_MIGSKIP 1
#endif

#ifndef RECOVER_RATIO
#define RECOVER_RATIO 0.1
#endif

#ifndef USE_SLOWDIVIDE
#define USE_SLOWDIVIDE 1
#endif

#ifndef USE_FI
#define USE_FI 1
#endif

#ifndef USE_SUPERLINEAR
#define USE_SUPERLINEAR 1
#endif

#ifndef UNIFY_REST
#define UNIFY_REST 1
#endif

#ifndef EXISTENCE_RATIO
#define EXISTENCE_RATIO 0.8
#endif

#ifndef ITER_STOP_RATIO
#define ITER_STOP_RATIO 0.9
#endif

#ifndef ENABLE_INFINITY
#define ENABLE_INFINITY 1
#endif

#ifndef WEIGHTED_MAX
#define WEIGHTED_MAX 1
#endif

namespace kv {

namespace bn = boost::numeric;
namespace ub = boost::numeric::ublas;

namespace allsol_sub {

// return index of I_i which has maximum width

template <class T> int search_maxwidth (const ub::vector< interval<T> >& I) {
	int s = I.size();
	int i, mi;
	T m, tmp;

	m = 0.;
	for (i=0; i<s; i++) {
#if WEIGHTED_MAX == 1
		tmp = width(I(i)) / (1. + mag(I(i)) / (std::numeric_limits<T>::max)() * mag(I(i)));
#else
		tmp = width(I(i));
#endif
		if (tmp > m) {
			m = tmp; mi = i;
		}
	}

	return mi;
}

// return max width(I_i) / width(J_i)

template <class T> T widthratio_max (const ub::vector< interval<T> >& I, const ub::vector< interval<T> >& J) {
	int s = I.size();
	int i;
	T tmp, r;

	r = 0.;

	for (i=0; i<s; i++) {
		tmp = width(I(i)) / width(J(i));
		if (tmp > r) r = tmp;
	}

	return r;
}

// return min width(I_i) / width(J_i)

template <class T> T widthratio_min (const ub::vector< interval<T> >& I, const ub::vector< interval<T> >& J) {
	int s = I.size();
	int i;
	T tmp, r;

	r = std::numeric_limits<T>::max();

	for (i=0; i<s; i++) {
		tmp = width(I(i)) / width(J(i));
		if (tmp < r) r = tmp;
	}

	return r;
}

// J: original interval, I: shrinked interval
// If width(I(i)) < ratio * width(J(i)) then inflate I(i)
//  until width(I(i)) = ratio * width(J(i))
// Inflation outside J(i) is "not allowed".

template <class T> void recovery_inflation (ub::vector< interval<T> >& I, const ub::vector< interval<T> >& J, T ratio) {
	int s = I.size();
	int i;
	T tmp, tmp2, l, u;

	for (i=0; i<s; i++) {
		if ( width(I(i)) < ratio * width(J(i)) ) {
			tmp = mid(I(i));
			tmp2 = rad(J(i)) * ratio;
			l = tmp - tmp2;
			u = tmp + tmp2;
			if (l < J(i).lower()) {
				u += (J(i).lower() - l);
				l = J(i).lower();
			} else if (u > J(i).upper()) {
				l -= (u - J(i).upper());
				u = J(i).upper();
			}
			// to be sure that new I(i) includes original I(i)
			I(i) = interval<T>::hull(I(i), interval<T>(l, u));
		}
	}
}

// J: original interval, I: shrinked interval
// If width(I(i)) < ratio * width(J(i)) then inflate I(i)
//  until width(I(i)) = ratio * width(J(i))
// Inflation outside J(i) is "allowed".

template <class T> void recovery_inflation2 (ub::vector< interval<T> >& I, const ub::vector< interval<T> >& J, T ratio) {
	int s = I.size();
	int i;
	T tmp, tmp2, l, u;

	for (i=0; i<s; i++) {
		if ( width(I(i)) < ratio * width(J(i)) ) {
			tmp = mid(I(i));
			tmp2 = rad(J(i)) * ratio;
			l = tmp - tmp2;
			u = tmp + tmp2;
			// to be sure that new I(i) includes original I(i)
			I(i) = interval<T>::hull(I(i), interval<T>(l, u));
		}
	}
}

// **not used**
// return index of division such that norm of M seems to be smallest
// under the scaled norm u = rad(I:divided)

template <class T> int search_optimal_divide (const ub::matrix< interval<T> >& M, const ub::vector< interval<T> >& I) {
	int n = I.size();
	ub::vector<T> u(n), s(n);
	ub::matrix<T> m(n,n);
	int i, j, p, mp;
	T tmp, tmp2, tmp3;

	for (i=0; i<n; i++) u(i) = width(I(i));
	for (i=0; i<n; i++) {
		for (j=0; j<n; j++) {
			m(i,j) = norm(M(i,j));
		}
	}
	for (i=0; i<n; i++) {
		tmp = 0.;
		for (j=0; j<n; j++) {
			tmp += m(i,j) * u(j);
		}
		s(i) = tmp / u(i);
	}

	tmp3 = std::numeric_limits<T>::max();
	for (p=0; p<n; p++) {
		tmp2 = 0.;
		for (i=0; i<n; i++) {
			tmp = s(i) - m(i,p)*u(p)/u(i)/2.;
			if (i == p) tmp *= 2.;
			if (tmp > tmp2) tmp2 = tmp;
		}
		if (tmp2 < tmp3) {
			tmp3 = tmp2;
			mp = p;
		}
	}

	return mp;
}

// **not used**
// return index such that width(K(i)) is relatively smallest
// compared with width(I(i))

template <class T> int search_optimal_divide2 (const ub::vector< interval<T> >& K, const ub::vector< interval<T> >& I) {
	int n = I.size();
	T tmp, tmp2;
	int i, r;

	tmp2 = std::numeric_limits<T>::max();
	for (i=0; i<n; i++) {
		tmp =  width(K(i))/ width(I(i));
		if (tmp < tmp2) {
			tmp2 = tmp;
			r = i;
		}
	}

	return r;
}

#if ENABLE_INFINITY == 1

template <class T> T mid_infinity (const interval<T>& I) {
	if (I.upper() == std::numeric_limits<T>::infinity()) {
		if (I.lower() == -std::numeric_limits<T>::infinity()) {
			return T(0.);
		} else {
			return mid(interval<T>((std::numeric_limits<T>::max)(), I.lower()));
			#if 0
			if (I.lower() < 0.) {
				return T(0.);
			} else if (I.lower() == 0.) {
				return T(1.);
			} else {
				using std::sqrt;
				return sqrt((std::numeric_limits<T>::max)()) * sqrt(I.lower());
			}
			#endif
		}
	} else {
		if (I.lower() == -std::numeric_limits<T>::infinity()) {
			return mid(interval<T>(-(std::numeric_limits<T>::max)(), I.upper()));
			#if 0
			if (I.upper() > 0.) {
				return T(0.);
			} else if (I.upper() == 0.) {
				return T(-1.);
			} else {
				using std::sqrt;
				return -sqrt((std::numeric_limits<T>::max)()) * sqrt(-I.upper());
			}
			#endif
		} else {
			return mid(I);
		}
	}
}

template <class T> ub::vector<T> mid_infinity (const ub::vector< interval<T> >& I) {
	int n = I.size();
	int i;
	ub::vector<T> r(n);

	for (i=0; i<n; i++) {
		r(i) = mid_infinity(I(i));
	}

	return r;
}

template <class T> bool include_infinity (const interval<T>& I) {
	if (I.lower() == -std::numeric_limits<T>::infinity() || I.upper() == std::numeric_limits<T>::infinity()) return true;
	return false;
}

template <class T> bool include_infinity (const ub::vector< interval<T> >& I) {
	int n = I.size();
	int i;
	ub::vector<T> r(n);

	for (i=0; i<n; i++) {
		if (include_infinity(I(i))) return true;
	}

	return false;
}

template <class T> int search_maxwidth_infinity (const ub::vector< interval<T> >& I) {
	int s = I.size();
	int i, mi, r, tr;
	T m, tmp, tmp2;

	m = 0.;
	r = 0.;
	for (i=0; i<s; i++) {
		if (I(i).lower() == -std::numeric_limits<T>::infinity()) {
			if (I(i).upper() == std::numeric_limits<T>::infinity()) {
				return i;
			} else {
				tr = 1;
				tmp = I(i).upper();
			}
		} else {
			if (I(i).upper() == std::numeric_limits<T>::infinity()) {
				tr = 1;
				tmp = -I(i).lower();
			} else {
				tr = 0;
#if WEIGHTED_MAX == 1
				tmp = width(I(i)) / (1. + mag(I(i)) / (std::numeric_limits<T>::max)() * mag(I(i)));
#else
				tmp = width(I(i));
#endif
			}
		}
		if (tr > r || (tr == r &&  tmp > m)) {
			r = tr; m = tmp; mi = i;
		}
	}

	return mi;
}

template <class T> int search_maxwidth_finite (const ub::vector< interval<T> >& I) {
	int s = I.size();
	int i, mi;
	T m, tmp, tmp2;

	mi = -1;
	m = 0.;
	for (i=0; i<s; i++) {
		if (include_infinity(I(i))) continue;
#if WEIGHTED_MAX == 1
		tmp = width(I(i)) / (1. + mag(I(i)) / (std::numeric_limits<T>::max)() * mag(I(i)));
#else
		tmp = width(I(i));
#endif
		if (tmp > m) {
			m = tmp; mi = i;
		}
	}

	return mi;
}

#endif // ENABLE_INFINITY

// generate 1-d vector function from scalar function

template <class F>
struct MakeVec {
	F f;
	MakeVec(F f): f(f) {}

	template <class T> ub::vector<T> operator()(const ub::vector<T>& x) {
		ub::vector<T> r(1);
		r(0) = f(x(0));
		return r;
	}
};

} // namespace allsol_sub

// find all solution of f in I

template <class T, class F>
std::list< ub::vector< interval<T> > >
allsol (
F f,
const ub::vector< interval<T> >& I,
int verbose = 1,
T giveup = T(0.),
std::list< ub::vector < interval<T> > >* rest = NULL
)
{
	std::list< ub::vector < interval<T> > > targets;
	targets.push_back(I);
	return allsol_list(f, targets, verbose, giveup, rest);
}

// find all solution of f in targets (list of intervals)

template <class T, class F>
std::list< ub::vector< interval<T> > >
allsol_list (
F f,
std::list< ub::vector< interval<T> > > targets,
int verbose = 1,
T giveup = T(0.),
std::list< ub::vector < interval<T> > >* rest = NULL
)
{
	int s = (targets.front()).size();
	std::list< ub::vector< interval<T> > > solutions, solutions_big;
	int count_ne_test = 0;
	int count_ex_test = 0;
	int count_unknown = targets.size();
	int count_ne = 0;
	int count_ex = 0;
	int count_giveup = 0;

	#pragma omp parallel
	{

	ub::vector< interval<T> > I, fc, fi, C, CK, K, mvf, I1, I2, IR, Iorg, g;
	ub::vector<T> v;
	ub::matrix< interval<T> > fdi, M, L2;
	ub::matrix<T> L, R, E;
	typename std::list< ub::vector< interval<T> > >::iterator p, p2;
	int i, j, k, mi;
	T tmp, tmp2;
	T wmax;
	bool r, M_calculated, flag, flag2, flag3;
	interval<T> A, B, J, J2, Itmp;
#if USE_TRIM == 3
	ub::vector< interval<T> > A0, A1, A2; // for new trim algorithm
#endif // USE_TRIM == 3

	// boost::variate_generator<boost::mt19937, boost::uniform_int<> > rand (boost::mt19937(time(0)), boost::uniform_int<>(0, s-1));

	E = ub::identity_matrix<T>(s);

	while (true) {
		if (verbose >= 2) {
			#pragma omp critical (cout)
			{
			std::cout << "ne_test: " << count_ne_test << ", ex_test: " << count_ex_test << ", unknown: " << count_unknown << ", ne: " << count_ne << ", ex: " << count_ex << ", giveup: " << count_giveup << "    \r" << std::flush;
			}
		}

		#ifdef _OPENMP

		int iflag = 0;
		#pragma omp critical (targets)
		{
		if (count_unknown == 0) iflag = 2;
		else {
			if (targets.empty()) {
				iflag = 1;
			} else {
				I = targets.front();
				targets.pop_front();
			}
		}
		}
		if (iflag == 2)  break;
		if (iflag == 1) continue;

		#else // _OPENMP

		if (targets.empty()) break;
		I = targets.front();
		targets.pop_front();

		#endif // _OPENMP

		Iorg = I;

		// non-existence test

		#pragma omp atomic
		count_ne_test++;

#if USE_FI == 1
		try {
			fi = f(I);
		}
		catch (std::domain_error& e) {
			goto label;
		}

		if (!zero_in(fi)) {
			#pragma omp atomic
			count_ne++;
			#pragma omp critical (targets)
			{
			count_unknown--;
			}
			continue;
		}
#endif

		try {
			autodif< interval<T> >::split(f(autodif< interval<T> >::init(I)), fi, fdi);
		}
		catch (std::domain_error& e) {
			goto label;
		}

#if USE_FI != 1
		if (!zero_in(fi)) {
			#pragma omp atomic
			count_ne++;
			#pragma omp critical (targets)
			{
			count_unknown--;
			}
			continue;
		}
#endif

#if ENABLE_INFINITY == 1
		C = allsol_sub::mid_infinity(I);
#else
		C = mid(I);
#endif
		try {
			fc = f(C);
		}
		catch (std::domain_error& e) {
			goto label;
		}

		mvf = fc + prod(fdi, I - C);
		if (!zero_in(mvf)) {
			#pragma omp atomic
			count_ne++;
			#pragma omp critical (targets)
			{
			count_unknown--;
			}
			continue;
		}

#if ENABLE_INFINITY == 1
		if (allsol_sub::include_infinity(I)) goto label;
#endif

#if USE_TRIM >= 1
		// interval shrinking
		IR = I;
		flag = false; // non-existence in I turns out or not
		flag2 = false; // shrinking of I occurs or not
#if USE_ZERODIVIDE == 2
		flag3 = false; // division of I occurs or not
#endif

		// maximum width for using TRIM
		wmax = 0.;
		for (i=0; i<s; i++) {
			tmp = width(I(i));
			if (tmp > wmax) wmax = tmp;
		}
		wmax *= RECOVER_RATIO;

#if USE_TRIM == 3
		A0.resize(s);
		A1.resize(s);
		A2.resize(s);
#endif // USE_TRIM == 3

		for (i=0; i<s; i++) {

#if USE_TRIM == 3
			// prepare for new trim algorithm
			for (j=0; j<s; j++) {
				A0(j) = fdi(i,j) * (I(j)-C(j));
			}
			Itmp = 0.;
			for (j=0; j<s; j++) {
				A1(j) = Itmp;
				Itmp += A0(j);
			}
			Itmp = 0.;
			for (j=s-1; j>=0; j--) {
				A2(j) = Itmp;
				Itmp += A0(j);
			}
#endif // USE_TRIM == 3

			for (j=0; j<s; j++) {
				// do not use TRIM for narrow component
				if (width(I(j)) < wmax) continue;

				B = fdi(i, j);

#if USE_TRIM == 1
				// calculate A simply
				// simple but slow
				A = 0.;
				for (k=0; k<s; k++) {
					if (k == j) continue;
					A += fdi(i, k) * (I(k)-C(k));
				}
				A += fc(i);
#endif // USE_TRIM == 1
#if USE_TRIM == 2
				// old trim algorithm
				// calculate back A from mvf
				A = mvf(i);
				Itmp = B * (I(j)-C(j));
				rop<T>::begin();
				tmp = rop<T>::sub_down(A.lower(), Itmp.lower());
				tmp2 = rop<T>::sub_up(A.upper(), Itmp.upper());
				rop<T>::end();
				A.assign(tmp, tmp2);
#endif // USE_TRIM == 2
#if USE_TRIM == 3
				// new trim algorithm
				A = fc(i) + A1(j) + A2(j);
#endif // USE_TRIM == 3

				if (zero_in(B)) {
#if USE_ZERODIVIDE >= 1
					bool bdummy;
					if (rad(B) <= 0.) continue;
					J = C(j) - division_part1(A, B, bdummy);
					J2 = C(j) - division_part2(A, B);
					if (overlap(IR(j), J)) {
						if (overlap(IR(j), J2)) {
#if USE_ZERODIVIDE == 2
							if (overlap(J, J2)) continue;
							// interval division
							I1 = IR;
							I2 = IR;
							I1(j) = intersect(IR(j), J);
							I2(j) = intersect(IR(j), J2);
							allsol_sub::recovery_inflation(I1, Iorg, (T)RECOVER_RATIO);
							allsol_sub::recovery_inflation(I2, Iorg, (T)RECOVER_RATIO);
							#pragma omp critical (targets)
							{
							targets.push_back(I1);
							targets.push_back(I2);
							count_unknown += 1;
							}
							flag3 = true;
							break;
#else
							continue;
#endif
						} else {
							if (!subset(IR(j), J)) {
								IR(j) = intersect(IR(j), J);
								flag2 = true;
							}
						}
					} else {
						if (overlap(IR(j), J2)) {
							if (!subset(IR(j), J2)) {
								IR(j) = intersect(IR(j), J2);
								flag2 = true;
							}
						} else {
							flag = true;
							break;
						}
					}
#else
					continue;
#endif
				} else {
					J = C(j) - A/B;
					if (overlap(IR(j), J)) {
						if (!subset(IR(j), J)) {
							IR(j) = intersect(IR(j), J);
							flag2 = true;
						}
					} else {
						flag = true;
						break;
					}
				}
			} // loop j
			if (flag == true) break;
#if USE_ZERODIVIDE == 2
			if (flag3 == true) break;
#endif
		} // loop i

#if USE_ZERODIVIDE == 2
		// interval division occurs
		if (flag3 == true) continue;
#endif

		// non-existence in I turns out
		if (flag == true) {
			#pragma omp atomic
			count_ne++;
			#pragma omp critical (targets)
			{
			count_unknown--;
			}
			continue;
		}

		if (flag2 == true) {
			allsol_sub::recovery_inflation(IR, Iorg, (T)RECOVER_RATIO);
#if USE_MULTITRIM == 1
			// if radius of I is smaller than half on the
			// "division scheduled" index by interval shrinking,
			// skip the existence test and do interval
			// shrinking again.
			mi = allsol_sub::search_maxwidth(I);
			if (rad(IR(mi)) <= 0.5 * rad(I(mi))) {
				#pragma omp critical (targets)
				{
				targets.push_back(IR);
				}
				continue;
			}
#endif

			// renew I, C and f(C)
			// do not renew f'(I) because of calculation cost
			I = IR;
			C = mid(I);
			try {
				fc = f(C);
			}
			catch (std::domain_error& e) {
				goto label;
			}

			// re-check mvf
			mvf = fc + prod(fdi, I - C);
			if (!zero_in(mvf)) {
				#pragma omp atomic
				count_ne++;
				#pragma omp critical (targets)
				{
				count_unknown--;
				}
				continue;
			}
		}

#endif // USE_TRIM >= 1

#if USE_AFFINEABS == 1
		g.resize(s);
		for (i=0; i<s; i++) {
			g(i) = fc(i) / rad(mvf(i));
		}
		L2 = mid(fdi);
		I1 = fc + prod(fdi - L2, I - C);
		I2 = prod(g, L2);
		if (inner_prod(mag(g), mig(I1)) - inner_prod(mag(I2), rad(I)) > 0.) {
			#pragma omp atomic
			count_ne++;
			#pragma omp critical (targets)
			{
			count_unknown--;
			}
			continue;
		}
#endif

		// existence test

		M_calculated = false;

		// skip existence test using mig
#if USE_MIGSKIP >= 1
#if USE_MIGSKIP == 1
		// use "loose" condition
		// interval<T> is for avoiding compile error
		mvf = fc + prod(mig(fdi), interval<T>(1. + EDGE_RATIO) * (I - C));
#else
		// use "strict" condition
		mvf = fc + prod(mig(fdi), I - C);
#endif
		if (!zero_in(mvf)) goto label;
#endif

		L = mid(fdi);

#if 0
		// **not used**
		// below is same as mig?
		I1 = - fc + prod(L-fdi, I-C);
		I2 = prod(L, I-C);
		if (!subset(I1, I2)) goto label;
#endif

#if 0
		// **not used**
		// prod(mag(L) - mag(L - fdi), rad(I));
		L2 = L - fdi;
		v = prod(mag(L) - mag(L2), rad(I));

		flag = true;
		for (i=0; i<s; i++) {
			if (abs(fc(i)) > v(i)) {flag=false; break;}
		}
		if (flag == false) goto label;
#endif

		#pragma omp atomic
		count_ex_test++;

		r = invert(L, R);
		if (!r) goto label;

		M = E - prod(R, fdi);
		M_calculated = true;
		CK = C - prod(R, fc);
		K = CK +  prod(M, I - C);
		if (!overlap(K, I)) {
			#pragma omp atomic
			count_ne++;
			#pragma omp critical (targets)
			{
			count_unknown--;
			}
			continue;
		}

#if 0
		// **not used**
		// same condition as above !overlap(K, I)
		I1 = Rfc + prod(Rfdi, I - C);
		if (!zero_in(I1)) {
			count_ne++;
			continue;
		}
#endif

		if (proper_subset(K, I) && allsol_sub::widthratio_max(K, I) < EXISTENCE_RATIO ) {
			#pragma omp critical (solutions)
			{
			// check whether the solution is already found or not
			flag = true;
			p = solutions.begin();
			p2 = solutions_big.begin();
			while (p != solutions.end()) {
				if (overlap(K, *p)) {
					if (subset(K, *p2)||subset(*p, I)) {
						flag = false;
						break;
					}
					while (true) {
						C = mid(K);
						I1 = C - prod(R, f(C)) + prod(M, K - C);
						K = intersect(K, I1);
						if (subset(K, *p2)) {
							flag2 = true;
							break;
						}
						if (!overlap(K, *p)) {
							flag2 = false;
							break;
						}
					}
					if (flag2 == true) {
						flag = false;
						break;
					} else {
						/* never reach? */
						std::cout << "two overlap intervals includes different solutions";
					}
				}
				p++;
				p2++;
			}
			if (flag) { // new solution found
				if (verbose >= 1) {
					#pragma omp critical (cout)
					{
					std::cout << I << "(ex)\n";
					}
				}
				solutions_big.push_back(I);
				// iterative refinement
				while (1) {
					C = mid(K);
					#if USE_SUPERLINEAR == 1
					autodif< interval<T> >::split(f(autodif< interval<T> >::init(K)), fi, fdi);
					L = mid(fdi);
					r = invert(L, R);
					M = E - prod(R, fdi);
					#endif
					I1 = C - prod(R, f(C)) + prod(M, K - C);
					I1 = intersect(K, I1);
					tmp = allsol_sub::widthratio_min(I1, K);
					K = I1;
					if (tmp > ITER_STOP_RATIO) break;
				}
				solutions.push_back(K);
				count_ex++;
				if (verbose >= 1) {
					#pragma omp critical (cout)
					{
					std::cout << K << "(ex:improved)\n";
					}
				}
			}
			} // pragma omp critical (solutions)
			#pragma omp critical (targets)
			{
			count_unknown--;
			}
			continue;
		}

		// check the case that solution may exist near boundary.
		// If so, use K as next interval
#if ENABLE_INFINITY == 1
		if (!allsol_sub::include_infinity(I) && allsol_sub::widthratio_max(K, I) < EDGE_RATIO)
#else
		if (allsol_sub::widthratio_max(K, I) < EDGE_RATIO)
#endif
		{
			allsol_sub::recovery_inflation2(K, Iorg, (T)RECOVER_RATIO);
			#pragma omp critical (targets)
			{
			targets.push_back(K);
			}
			continue;
		}

		I = intersect(I, K);
		allsol_sub::recovery_inflation(I, Iorg, (T)RECOVER_RATIO);

		label:

		// divide interval

		// if (M_calculated) mi = search_optimal_divide(M, I);
		// if (M_calculated) mi = search_optimal_divide2(K, I);
		// else mi = allsol_sub::search_maxwidth(I);
		// mi = rand();

#if USE_SLOWDIVIDE == 1
#if ENABLE_INFINITY == 1
		if (!allsol_sub::include_infinity(I) && allsol_sub::widthratio_max(I, Iorg) <= 0.5)
#else
		if (allsol_sub::widthratio_max(I, Iorg) <= 0.5)
#endif
		{
			#pragma omp critical (targets)
			{
			targets.push_back(I);
			}
			continue;
		}
#endif

#ifdef USE_SLOWDIVIDE_OLD

		// if radius of I is smaller than half on the
		// "division scheduled" index by interval shrinking,
		// skip division.

		mi = allsol_sub::search_maxwidth(Iorg);
		if (rad(I(mi)) <= 0.5 * rad(Iorg(mi))) {
			#pragma omp critical (targets)
			{
			targets.push_back(I);
			}
			continue;
		}
#else
#if ENABLE_INFINITY == 1
		mi = allsol_sub::search_maxwidth_infinity(I);
#else
		mi = allsol_sub::search_maxwidth(I);
#endif
#endif

#if ENABLE_INFINITY == 1
		tmp = allsol_sub::mid_infinity(I(mi));
#else
		tmp = mid(I(mi));
#endif
		if (width(I(mi)) < giveup || tmp == I(mi).lower() || tmp == I(mi).upper()) {
			if (verbose >= 2) {
				std::cout << "too small interval (may be multiple root?):\n" << I << "\n";
			}
			if (rest != NULL) {
				#pragma omp critical (rest)
				{
				#if UNIFY_REST == 1
				I1 = I;
				while (true) {
					flag = false;
					p = (*rest).begin();
					while (p != (*rest).end()) {
						if (overlap(*p, I1)) {
							I1 = hull(I1, *p);
							p = (*rest).erase(p);
							// #pragma omp atomic
							// count_giveup--;
							flag = true;
							continue;
						}
						p++;
					}
					if (flag == false) break;
				}
				(*rest).push_back(I1);
				#else // UNIFY_REST == 1
				(*rest).push_back(I);
				#endif // UNIFY_REST == 1
				}
			}
			#pragma omp atomic
			count_giveup++;
			#pragma omp critical (targets)
			{
			count_unknown--;
			}
			continue;
		}

		I1 = I; I2 = I;
		I1(mi).assign(I1(mi).lower(), tmp);
		I2(mi).assign(tmp, I2(mi).upper());
#if ENABLE_INFINITY == 1
		ub::vector< interval<T> > I3;
		int mi2;
		if (allsol_sub::include_infinity(I1(mi))) {
			mi2 = allsol_sub::search_maxwidth_finite(I1);
			if (mi2 != -1) {
				I3 = I1;
				tmp = mid(I1(mi2));
				I1(mi2).assign(I1(mi2).lower(), tmp);
				I3(mi2).assign(tmp, I3(mi2).upper());
				#pragma omp critical (targets)
				{
				targets.push_back(I3);
				count_unknown += 1;
				}
			}
		}
		if (allsol_sub::include_infinity(I2(mi))) {
			mi2 = allsol_sub::search_maxwidth_finite(I2);
			if (mi2 != -1) {
				I3 = I2;
				tmp = mid(I2(mi2));
				I2(mi2).assign(I2(mi2).lower(), tmp);
				I3(mi2).assign(tmp, I3(mi2).upper());
				#pragma omp critical (targets)
				{
				targets.push_back(I3);
				count_unknown += 1;
				}
			}
		}
#endif
		#pragma omp critical (targets)
		{
		targets.push_back(I1);
		targets.push_back(I2);
		count_unknown += 1;
		}
	}

	} // pragma omp parallel

	if (verbose >= 1) {
			std::cout << "ne_test: " << count_ne_test << ", ex_test: " << count_ex_test << ", ne: " << count_ne << ", ex: " << count_ex << ", giveup: " << count_giveup << "    \n";
	}

	return solutions;
}

// allsol for 1-dimentional function

template <class T, class F>
std::list< interval<T> >
allsol (
F f,
const interval<T>& I,
int verbose = 1,
T giveup = T(0.),
std::list< interval<T> >* rest = NULL
)
{
	allsol_sub::MakeVec<F> g(f);
	ub::vector< interval<T> > I2(1);
	std::list< ub::vector< interval<T> > > rest2;
	typename std::list< ub::vector< interval<T> > >::iterator p1;
	std::list< ub::vector< interval<T> > >* rest_p;
	std::list< ub::vector< interval<T> > > r1;
	typename std::list< ub::vector< interval<T> > >::iterator p2;
	std::list< interval<T> > r2;

	I2(0) = I;
	if (rest == NULL) {
		rest_p = NULL;
	} else {
		rest_p = &rest2;
	}

	r1 = allsol(g, I2, verbose, giveup, rest_p);

	p2 = r1.begin();
	while (p2 != r1.end()) {
		r2.push_back((*(p2++))(0));
	}

	if (rest != NULL) {
		p1 = rest2.begin();
		while (p1 != rest2.end()) {
			(*rest).push_back((*(p1++))(0));
		}
	}

	return r2;
}

} // namespace kv

#endif // ALLSOL_HPP


//---------------------------------------------------------------------------//
// Copyright (c) 2013 Kyle Lutz <kyle.r.lutz@gmail.com>
//
// 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://kylelutz.github.com/compute for more information.
//---------------------------------------------------------------------------//

#define BOOST_TEST_MODULE TestTypeTraits
#include <boost/test/unit_test.hpp>

#include <set>
#include <list>
#include <vector>

#include <boost/type_traits.hpp>
#include <boost/static_assert.hpp>

#include <boost/compute/types.hpp>
#include <boost/compute/type_traits.hpp>
#include <boost/compute/iterator/buffer_iterator.hpp>
#include <boost/compute/iterator/constant_iterator.hpp>
#include <boost/compute/detail/is_buffer_iterator.hpp>
#include <boost/compute/detail/is_contiguous_iterator.hpp>

namespace bc = boost::compute;

BOOST_AUTO_TEST_CASE(scalar_type)
{
    BOOST_STATIC_ASSERT((boost::is_same<bc::scalar_type<bc::int_>::type, int>::value));
    BOOST_STATIC_ASSERT((boost::is_same<bc::scalar_type<bc::int2_>::type, int>::value));
    BOOST_STATIC_ASSERT((boost::is_same<bc::scalar_type<bc::float_>::type, float>::value));
    BOOST_STATIC_ASSERT((boost::is_same<bc::scalar_type<bc::float4_>::type, float>::value));
}

BOOST_AUTO_TEST_CASE(vector_size)
{
    BOOST_STATIC_ASSERT(bc::vector_size<bc::int_>::value == 1);
    BOOST_STATIC_ASSERT(bc::vector_size<bc::int2_>::value == 2);
    BOOST_STATIC_ASSERT(bc::vector_size<bc::float_>::value == 1);
    BOOST_STATIC_ASSERT(bc::vector_size<bc::float4_>::value == 4);
}

BOOST_AUTO_TEST_CASE(is_vector_type)
{
    BOOST_STATIC_ASSERT(bc::is_vector_type<bc::int_>::value == false);
    BOOST_STATIC_ASSERT(bc::is_vector_type<bc::int2_>::value == true);
    BOOST_STATIC_ASSERT(bc::is_vector_type<bc::float_>::value == false);
    BOOST_STATIC_ASSERT(bc::is_vector_type<bc::float4_>::value == true);
}

BOOST_AUTO_TEST_CASE(make_vector_type)
{
    BOOST_STATIC_ASSERT((boost::is_same<bc::make_vector_type<cl_uint, 2>::type, bc::uint2_>::value));
    BOOST_STATIC_ASSERT((boost::is_same<bc::make_vector_type<int, 4>::type, bc::int4_>::value));
    BOOST_STATIC_ASSERT((boost::is_same<bc::make_vector_type<float, 8>::type, bc::float8_>::value));
    BOOST_STATIC_ASSERT((boost::is_same<bc::make_vector_type<bc::char_, 16>::type, bc::char16_>::value));
}

BOOST_AUTO_TEST_CASE(is_fundamental_type)
{
    BOOST_STATIC_ASSERT((bc::is_fundamental<int>::value == true));
    BOOST_STATIC_ASSERT((bc::is_fundamental<bc::int_>::value == true));
    BOOST_STATIC_ASSERT((bc::is_fundamental<bc::int2_>::value == true));
    BOOST_STATIC_ASSERT((bc::is_fundamental<float>::value == true));
    BOOST_STATIC_ASSERT((bc::is_fundamental<bc::float_>::value == true));
    BOOST_STATIC_ASSERT((bc::is_fundamental<bc::float4_>::value == true));

    BOOST_STATIC_ASSERT((bc::is_fundamental<std::pair<int, float> >::value == false));
    BOOST_STATIC_ASSERT((bc::is_fundamental<std::complex<float> >::value == false));
}

BOOST_AUTO_TEST_CASE(type_name)
{
    // scalar types
    BOOST_CHECK(std::strcmp(bc::type_name<bc::char_>(), "char") == 0);
    BOOST_CHECK(std::strcmp(bc::type_name<bc::uchar_>(), "uchar") == 0);
    BOOST_CHECK(std::strcmp(bc::type_name<bc::short_>(), "short") == 0);
    BOOST_CHECK(std::strcmp(bc::type_name<bc::ushort_>(), "ushort") == 0);
    BOOST_CHECK(std::strcmp(bc::type_name<bc::int_>(), "int") == 0);
    BOOST_CHECK(std::strcmp(bc::type_name<bc::uint_>(), "uint") == 0);
    BOOST_CHECK(std::strcmp(bc::type_name<bc::long_>(), "long") == 0);
    BOOST_CHECK(std::strcmp(bc::type_name<bc::ulong_>(), "ulong") == 0);
    BOOST_CHECK(std::strcmp(bc::type_name<bc::float_>(), "float") == 0);
    BOOST_CHECK(std::strcmp(bc::type_name<bc::double_>(), "double") == 0);
    BOOST_CHECK(std::strcmp(bc::type_name<bool>(), "bool") == 0);

    // vector types
    BOOST_CHECK(std::strcmp(bc::type_name<bc::char16_>(), "char16") == 0);
    BOOST_CHECK(std::strcmp(bc::type_name<bc::uint4_>(), "uint4") == 0);
    BOOST_CHECK(std::strcmp(bc::type_name<bc::ulong8_>(), "ulong8") == 0);
    BOOST_CHECK(std::strcmp(bc::type_name<bc::float2_>(), "float2") == 0);
    BOOST_CHECK(std::strcmp(bc::type_name<bc::double4_>(), "double4") == 0);
}

BOOST_AUTO_TEST_CASE(is_contiguous_iterator)
{
    using boost::compute::detail::is_contiguous_iterator;

    BOOST_STATIC_ASSERT(is_contiguous_iterator<int *>::value == true);
    BOOST_STATIC_ASSERT(is_contiguous_iterator<std::vector<int>::iterator>::value == true);
    BOOST_STATIC_ASSERT(is_contiguous_iterator<std::vector<int>::const_iterator>::value == true);
    BOOST_STATIC_ASSERT(is_contiguous_iterator<std::list<int>::iterator>::value == false);
    BOOST_STATIC_ASSERT(is_contiguous_iterator<std::set<int>::iterator>::value == false);
    BOOST_STATIC_ASSERT(is_contiguous_iterator<std::insert_iterator<std::set<int> > >::value == false);
    BOOST_STATIC_ASSERT(is_contiguous_iterator<std::back_insert_iterator<std::vector<int> > >::value == false);
}

BOOST_AUTO_TEST_CASE(is_buffer_iterator)
{
    using boost::compute::detail::is_buffer_iterator;

    BOOST_STATIC_ASSERT(is_buffer_iterator<boost::compute::buffer_iterator<int> >::value == true);
    BOOST_STATIC_ASSERT(is_buffer_iterator<boost::compute::constant_iterator<int> >::value == false);
}

BOOST_AUTO_TEST_CASE(is_device_iterator)
{
    using boost::compute::is_device_iterator;

    BOOST_STATIC_ASSERT(is_device_iterator<boost::compute::buffer_iterator<int> >::value == true);
    BOOST_STATIC_ASSERT(is_device_iterator<const boost::compute::buffer_iterator<int> >::value == true);
    BOOST_STATIC_ASSERT(is_device_iterator<boost::compute::constant_iterator<int> >::value == true);
    BOOST_STATIC_ASSERT(is_device_iterator<const boost::compute::constant_iterator<int> >::value == true);
    BOOST_STATIC_ASSERT(is_device_iterator<float *>::value == false);
    BOOST_STATIC_ASSERT(is_device_iterator<const float *>::value == false);
    BOOST_STATIC_ASSERT(is_device_iterator<std::vector<int>::iterator>::value == false);
    BOOST_STATIC_ASSERT(is_device_iterator<const std::vector<int>::iterator>::value == false);
}


//==============================================================================
//         Copyright 2003 - 2013   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2013   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
//==============================================================================
#include <nt2/hyperbolic/include/functions/asinh.hpp>

#include <nt2/sdk/functor/meta/call.hpp>
#include <boost/simd/sdk/simd/native.hpp>
#include <nt2/sdk/unit/tests/type_expr.hpp>
#include <nt2/sdk/unit/tests/ulp.hpp>
#include <nt2/include/functions/splat.hpp>
#include <nt2/sdk/unit/module.hpp>
#include <boost/simd/sdk/config.hpp>
#include <boost/simd/sdk/simd/io.hpp>
#include <boost/math/special_functions/asinh.hpp>

#include <nt2/include/functions/rec.hpp>
#include <nt2/include/constants/zero.hpp>
#include <nt2/include/constants/inf.hpp>
#include <nt2/include/constants/minf.hpp>
#include <nt2/include/constants/nan.hpp>
#include <nt2/include/constants/valmax.hpp>
#include <nt2/include/constants/valmax.hpp>
#include <nt2/include/constants/eps.hpp>
#include <nt2/include/constants/sqrteps.hpp>
#include <nt2/include/constants/oneosqrteps.hpp>
#include <nt2/include/constants/two.hpp>

NT2_TEST_CASE_TPL ( asinh,  NT2_SIMD_REAL_TYPES)
{
  using nt2::asinh;
  using nt2::tag::asinh_;
  using boost::simd::native;
  typedef BOOST_SIMD_DEFAULT_EXTENSION  ext_t;
  typedef native<T,ext_t>                  vT;
  typedef typename nt2::meta::call<asinh_(vT)>::type r_t;
  typedef vT wished_r_t;

  // return type conformity test
  NT2_TEST_TYPE_IS(r_t, wished_r_t);

  // specific values tests
#ifndef BOOST_SIMD_NO_INVALIDS
  NT2_TEST_ULP_EQUAL(asinh(nt2::Inf<vT>()), nt2::Inf<r_t>(), 0.5);
  NT2_TEST_ULP_EQUAL(asinh(nt2::Minf<vT>()), nt2::Minf<r_t>(), 0.5);
  NT2_TEST_ULP_EQUAL(asinh(nt2::Nan<vT>()), nt2::Nan<r_t>(), 0.5);
#endif
 NT2_TEST_ULP_EQUAL(asinh(nt2::Zero<vT>()), nt2::Zero<r_t>(), 0.5);
 NT2_TEST_ULP_EQUAL(asinh(nt2::Valmax<vT>()), nt2::splat<vT>(boost::math::asinh(nt2::Valmax<T>())), 0.5);
 NT2_TEST_ULP_EQUAL(asinh(nt2::rec(nt2::Sqrteps<vT>())*nt2::Two<vT>()),  nt2::splat<vT>(boost::math::asinh(nt2::rec(nt2::Sqrteps<T>())*2)), 0.5);
 NT2_TEST_ULP_EQUAL(asinh(nt2::Eps<vT>()), nt2::Eps<vT>(), 0.5);

 for(T i=T(0.1); i <= T(1.1); i+= T(0.5))
 {
   vT ii =  nt2::splat<vT>(i);
   vT ri =  nt2::splat<vT>(nt2::rec(i));
   NT2_TEST_ULP_EQUAL(asinh(ii), nt2::splat<vT>(boost::math::asinh(i)), 0.5);
   NT2_TEST_ULP_EQUAL(asinh(ri), nt2::splat<vT>(boost::math::asinh(nt2::rec(i))), 0.5);
 }

}


/*
 * Copyright 2008 by Tommi Rantala <tt.rantala@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 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.
 */

/* Implements a multi-way mergesort based on the loser tree.
 */

#include "routine.h"
#include "util/debug.h"
#include <cassert>
#include <cstring>
#include <boost/array.hpp>

static inline int
cmp(const unsigned char* a, const unsigned char* b)
{
	assert(a != 0);
	assert(b != 0);
	return strcmp(reinterpret_cast<const char*>(a),
	              reinterpret_cast<const char*>(b));
}

#include "losertree.h"

void mergesort_4way(unsigned char**, size_t, unsigned char**);

template <unsigned K>
static void
mergesort_losertree(unsigned char** strings, size_t n, unsigned char** tmp)
{
	if (n < 0x10000) {
		mergesort_4way(strings, n, tmp);
		return;
	}
	debug() << __func__ << "(), n="<<n<<"\n";
	const size_t split = size_t(double(n) / double(K));
	boost::array<std::pair<unsigned char**, size_t>, K> ranges;
	for (unsigned i=0; i < K-1; ++i) {
		ranges[i] = std::make_pair(strings+i*split, split);
	}
	ranges[K-1] = std::make_pair(strings+(K-1)*split, n-(K-1)*split);
	for (unsigned i=0; i < K; ++i) {
		mergesort_losertree<K>(ranges[i].first, ranges[i].second,
				tmp+(ranges[i].first-strings));
	}
	unsigned char** result = tmp;
	loser_tree<unsigned char*> tree(ranges.begin(), ranges.end());
	while (tree._nonempty_streams) { *result++ = tree.min(); }
	(void) memcpy(strings, tmp, n*sizeof(unsigned char*));
}

void mergesort_losertree_64way(unsigned char** strings, size_t n)
{
	unsigned char** tmp = static_cast<unsigned char**>(
			malloc(n*sizeof(unsigned char*)));
	mergesort_losertree<64>(strings, n, tmp);
	free(tmp);
}
void mergesort_losertree_128way(unsigned char** strings, size_t n)
{
	unsigned char** tmp = static_cast<unsigned char**>(
			malloc(n*sizeof(unsigned char*)));
	mergesort_losertree<128>(strings, n, tmp);
	free(tmp);
}
void mergesort_losertree_256way(unsigned char** strings, size_t n)
{
	unsigned char** tmp = static_cast<unsigned char**>(
			malloc(n*sizeof(unsigned char*)));
	mergesort_losertree<256>(strings, n, tmp);
	free(tmp);
}
void mergesort_losertree_512way(unsigned char** strings, size_t n)
{
	unsigned char** tmp = static_cast<unsigned char**>(
			malloc(n*sizeof(unsigned char*)));
	mergesort_losertree<512>(strings, n, tmp);
	free(tmp);
}
void mergesort_losertree_1024way(unsigned char** strings, size_t n)
{
	unsigned char** tmp = static_cast<unsigned char**>(
			malloc(n*sizeof(unsigned char*)));
	mergesort_losertree<1024>(strings, n, tmp);
	free(tmp);
}

ROUTINE_REGISTER_SINGLECORE(mergesort_losertree_64way,
		"64way loser tree based mergesort")
ROUTINE_REGISTER_SINGLECORE(mergesort_losertree_128way,
		"128way loser tree based mergesort")
ROUTINE_REGISTER_SINGLECORE(mergesort_losertree_256way,
		"256way loser tree based mergesort")
ROUTINE_REGISTER_SINGLECORE(mergesort_losertree_512way,
		"512way loser tree based mergesort")
ROUTINE_REGISTER_SINGLECORE(mergesort_losertree_1024way,
		"1024way loser tree based mergesort")

void mergesort_4way_parallel(unsigned char**, size_t, unsigned char**);

template <unsigned K>
static void
mergesort_losertree_parallel(unsigned char** strings, size_t n, unsigned char** tmp)
{
	if (n < 0x10000) {
		mergesort_4way_parallel(strings, n, tmp);
		return;
	}
	debug() << __func__ << "(), n="<<n<<"\n";
	const size_t split = size_t(double(n) / double(K));
	boost::array<std::pair<unsigned char**, size_t>, K> ranges;
	for (unsigned i=0; i < K-1; ++i) {
		ranges[i] = std::make_pair(strings+i*split, split);
	}
	ranges[K-1] = std::make_pair(strings+(K-1)*split, n-(K-1)*split);
#pragma omp parallel for
	for (unsigned i=0; i < K; ++i) {
		mergesort_losertree_parallel<K>(ranges[i].first, ranges[i].second,
				tmp+(ranges[i].first-strings));
	}
	unsigned char** result = tmp;
	loser_tree<unsigned char*> tree(ranges.begin(), ranges.end());
	while (tree._nonempty_streams) { *result++ = tree.min(); }
	(void) memcpy(strings, tmp, n*sizeof(unsigned char*));
}

void mergesort_losertree_64way_parallel(unsigned char** strings, size_t n)
{
	unsigned char** tmp = static_cast<unsigned char**>(
			malloc(n*sizeof(unsigned char*)));
	mergesort_losertree_parallel<64>(strings, n, tmp);
	free(tmp);
}
void mergesort_losertree_128way_parallel(unsigned char** strings, size_t n)
{
	unsigned char** tmp = static_cast<unsigned char**>(
			malloc(n*sizeof(unsigned char*)));
	mergesort_losertree_parallel<128>(strings, n, tmp);
	free(tmp);
}
void mergesort_losertree_256way_parallel(unsigned char** strings, size_t n)
{
	unsigned char** tmp = static_cast<unsigned char**>(
			malloc(n*sizeof(unsigned char*)));
	mergesort_losertree_parallel<256>(strings, n, tmp);
	free(tmp);
}
void mergesort_losertree_512way_parallel(unsigned char** strings, size_t n)
{
	unsigned char** tmp = static_cast<unsigned char**>(
			malloc(n*sizeof(unsigned char*)));
	mergesort_losertree_parallel<512>(strings, n, tmp);
	free(tmp);
}
void mergesort_losertree_1024way_parallel(unsigned char** strings, size_t n)
{
	unsigned char** tmp = static_cast<unsigned char**>(
			malloc(n*sizeof(unsigned char*)));
	mergesort_losertree_parallel<1024>(strings, n, tmp);
	free(tmp);
}

ROUTINE_REGISTER_MULTICORE(mergesort_losertree_64way_parallel,
		"Parallel 64way loser tree based mergesort")
ROUTINE_REGISTER_MULTICORE(mergesort_losertree_128way_parallel,
		"Parallel 128way loser tree based mergesort")
ROUTINE_REGISTER_MULTICORE(mergesort_losertree_256way_parallel,
		"Parallel 256way loser tree based mergesort")
ROUTINE_REGISTER_MULTICORE(mergesort_losertree_512way_parallel,
		"Parallel 512way loser tree based mergesort")
ROUTINE_REGISTER_MULTICORE(mergesort_losertree_1024way_parallel,
		"Parallel 1024way loser tree based mergesort")


// Eigen tutorial -- Block operations
// SOurce:
// http://eigen.tuxfamily.org/dox/group__TutorialBlockOperations.html
#include <iostream>
#include <Eigen/Dense>

using namespace Eigen;

int main(void)
{
    MatrixXf m(4,4);
    m << 1, 2, 3, 4,
         5, 6, 7, 8,
         9,10,11,12,
        13,14,15,16;
    std::cout << "Block in the middle" << std::endl;
    std::cout << m.block<2,2>(1,1) << '\n' << std::endl;
    for (int i = 0; i <= 3; i++) {
        std::cout << "Block of size " << i << "x" << i << std::endl;
        std::cout << m.block(0,0,i,i) << '\n' << std::endl;
    }

    // block can also be used as lvalues, meaning that you can assign to a block
    Array22f r;
    r << 1,2,
         3,4;
    Array44f a = Array44f::Constant(0.6);
    std::cout << "Here is the array a:\n" << a << '\n' << std::endl;
    a.block<2,2>(1,1) = r;
    std::cout << "Here is now a with m copied into its central 2x2 block:\n"
        << a << '\n' << std::endl;
    a.block(0,0,2,3) = a.block(2,1,2,3);
    std::cout << "Here is now a with bottom-right 2x3 copied into top-left 2x2 block:\n"
        << a << '\n' << std::endl;

    // Columns and rows
    MatrixXf u(3,3);
    u << 1,2,3,
         4,5,6,
         7,8,9;
    std::cout << "Here is thematrix u:\n" << u << std::endl;
    std::cout << "2nd Row: " << u.row(1) << std::endl;
    u.col(2) += 3 * u.col(0);
    std::cout << "After adding 3 times the first column into the third column,"
        << "the matrix u is:\n" << u << std::endl;

    // Corner-related operations
    MatrixXf w(4,4);
    w << 1, 2, 3, 4,
         5, 6, 7, 8,
         9,10,11,12,
        13,14,15,16;
    std::cout << "w.leftCols(2) =\n" << m.leftCols(2) << '\n' << std::endl;
    std::cout << "w.bottomRows<2>() =\n" <<  w.bottomRows<2>() << '\n' << std::endl;
    w.topLeftCorner(1,3) = w.bottomRightCorner(3,1).transpose();
    std::cout << "After assignment, w =\n" << w << std::endl;

    // Block operations for vectors
    ArrayXf y(6);
    y << 1,2,3,4,5,6;
    std::cout << "y.head(3) = \n" << y.head(3) << '\n' << std::endl;
    std::cout << "y.tail<3>() =\n" << y.tail<3>() << "\n\n";
    y.segment(1,4) *= 2;
    std::cout << "after 'y.segment(1,4) *= 2', y =\n" << y << std::endl;

    return 0;
}


// 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/detail/has_duplicates.hpp>
#include <boost/hana/integral_constant.hpp>
namespace hana = boost::hana;

static_assert(!hana::detail::has_duplicates<>::value, "");

static_assert(!hana::detail::has_duplicates<
    hana::int_<0>
>::value, "");

static_assert(!hana::detail::has_duplicates<
    hana::int_<0>, hana::int_<1>
>::value, "");

static_assert(!hana::detail::has_duplicates<
    hana::int_<0>, hana::int_<1>, hana::int_<2>
>::value, "");

static_assert(hana::detail::has_duplicates<
    hana::int_<0>, hana::int_<0>, hana::int_<2>
>::value, "");

static_assert(hana::detail::has_duplicates<
    hana::int_<0>, hana::int_<1>, hana::int_<0>
>::value, "");

static_assert(hana::detail::has_duplicates<
    hana::int_<0>, hana::int_<1>, hana::int_<2>, hana::int_<1>
>::value, "");

static_assert(hana::detail::has_duplicates<
    hana::int_<0>, hana::int_<1>, hana::int_<2>, hana::int_<2>
>::value, "");

static_assert(hana::detail::has_duplicates<
    hana::int_<0>, hana::int_<1>, hana::int_<2>, hana::int_<1>, hana::int_<1>
>::value, "");

static_assert(hana::detail::has_duplicates<
    hana::int_<0>, hana::int_<1>, hana::int_<2>, hana::int_<1>, hana::int_<2>
>::value, "");

// Make sure it uses deep equality
static_assert(hana::detail::has_duplicates<
    hana::int_<0>, hana::long_<0>, hana::int_<2>, hana::int_<3>
>::value, "");

static_assert(hana::detail::has_duplicates<
    hana::int_<0>, hana::int_<1>, hana::int_<2>, hana::long_<1>
>::value, "");

int main() { }


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

#include "../unittest_config.h"

#include "clotho/utility/popcount.hpp"

#define TEST_PC( x, e) \
    BOOST_REQUIRE_MESSAGE( popcount( x ) == e, "Unexpected popcount: pc(" << x << ") =  pc(0x" << std::hex << x << ") = " << std::dec << popcount(x) << " !=  " << e)

BOOST_AUTO_TEST_SUITE( test_utility )

/// Basic two's complement test cases
BOOST_AUTO_TEST_CASE( test_popcount_two_comp ) {
    int x = -129;   // 0xffffff7f

    TEST_PC( x, 31 );

    char y = -51;   // 0xCD
    TEST_PC( y, 5);

    long z = -129 * 0x0000000100000000;   // 0xffffff7f00000000
    TEST_PC( z, 31);

    unsigned int a = -1;
    TEST_PC( a, 32 );
}

BOOST_AUTO_TEST_SUITE_END()


#include "compi.hpp"

#include <complex>
#include <iostream>

#include <boost/math/quadrature/sinh_sinh.hpp>

extern "C" {
    #include "integration_routines.h"
}
#include "integration_routines_template.hpp"
#include "IntegrandFunctionWrapper.hpp"

struct SinhSinhParameters: public RoutineParametersBase {

    SinhSinhParameters(PyObject* routine_args, PyObject* routine_kwargs){
        constexpr auto keywords = generate_keyword_list<IntegralRange::infinite>();

        if(!PyArg_ParseTupleAndKeywords(routine_args,routine_kwargs,"O|OO$pId", const_cast<char**>(keywords.data()),
            &integrand,
            &args,&kw,
            &full_output,&max_levels,&tolerance)){
                throw could_not_parse_arguments("Unable to parse Python args to C variables");
        }
    }

    struct result_type: public RoutineParametersBase::result_type {
        size_t levels;
    };
};

auto run_integration_routine(const compi_internal::IntegrandFunctionWrapper& f, const SinhSinhParameters& parameters){
    SinhSinhParameters::result_type result;

    boost::math::quadrature::sinh_sinh<Real> integrator{static_cast<size_t>(parameters.max_levels)};

    result.result = integrator.integrate(f,parameters.tolerance,&result.err,&result.l1,&result.levels);

    return result;
}

extern "C" PyObject* sinh_sinh(PyObject* self, PyObject* args, PyObject* kwargs){
    return integration_routine<SinhSinhParameters>(args,kwargs);
}


//
// Copyright (c) 2018 CNRS, INRIA
//

#include "pinocchio/multibody/model.hpp"
#include "pinocchio/multibody/data.hpp"
#include "pinocchio/algorithm/jacobian.hpp"
#include "pinocchio/algorithm/joint-configuration.hpp"
#include "pinocchio/algorithm/kinematics.hpp"
#include "pinocchio/algorithm/kinematics-derivatives.hpp"
#include "pinocchio/algorithm/rnea.hpp"
#include "pinocchio/algorithm/rnea-derivatives.hpp"
#include "pinocchio/algorithm/aba.hpp"
#include "pinocchio/algorithm/aba-derivatives.hpp"
#include "pinocchio/algorithm/crba.hpp"
#include "pinocchio/parsers/sample-models.hpp"

#include <iostream>

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

BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE)

BOOST_AUTO_TEST_CASE(test_aba_derivatives)
{
  using namespace Eigen;
  using namespace pinocchio;

  Model model;
  buildModels::humanoidRandom(model);

  Data data(model), data_ref(model);

  model.lowerPositionLimit.head<3>().fill(-1.);
  model.upperPositionLimit.head<3>().fill(1.);
  VectorXd q = randomConfiguration(model);
  VectorXd v(VectorXd::Random(model.nv));
  VectorXd tau(VectorXd::Random(model.nv));
  VectorXd a(aba(model,data_ref,q,v,tau));

  MatrixXd aba_partial_dq(model.nv,model.nv); aba_partial_dq.setZero();
  MatrixXd aba_partial_dv(model.nv,model.nv); aba_partial_dv.setZero();
  Data::RowMatrixXs aba_partial_dtau(model.nv,model.nv); aba_partial_dtau.setZero();

  computeABADerivatives(model, data, q, v, tau, aba_partial_dq, aba_partial_dv, aba_partial_dtau);
  computeRNEADerivatives(model,data_ref,q,v,a);
  for(Model::JointIndex k = 1; k < (Model::JointIndex)model.njoints; ++k)
  {
    BOOST_CHECK(data.oMi[k].isApprox(data_ref.oMi[k]));
    BOOST_CHECK(data.v[k].isApprox(data_ref.v[k]));
    BOOST_CHECK(data.ov[k].isApprox(data_ref.ov[k]));
    BOOST_CHECK(data.oa_gf[k].isApprox(data_ref.oa_gf[k]));
    BOOST_CHECK(data.of[k].isApprox(data_ref.of[k]));
    BOOST_CHECK(data.oYcrb[k].isApprox(data_ref.oYcrb[k]));
    BOOST_CHECK(data.doYcrb[k].isApprox(data_ref.doYcrb[k]));
  }

  computeJointJacobians(model,data_ref,q);
  BOOST_CHECK(data.J.isApprox(data_ref.J));

  aba(model,data_ref,q,v,tau);
  BOOST_CHECK(data.ddq.isApprox(data_ref.ddq));

  computeMinverse(model,data_ref,q);
  data_ref.Minv.triangularView<Eigen::StrictlyLower>()
  = data_ref.Minv.transpose().triangularView<Eigen::StrictlyLower>();

  BOOST_CHECK(aba_partial_dtau.isApprox(data_ref.Minv));

  BOOST_CHECK(data.J.isApprox(data_ref.J));
  BOOST_CHECK(data.dJ.isApprox(data_ref.dJ));
  BOOST_CHECK(data.dVdq.isApprox(data_ref.dVdq));
  BOOST_CHECK(data.dAdq.isApprox(data_ref.dAdq));
  BOOST_CHECK(data.dAdv.isApprox(data_ref.dAdv));
  BOOST_CHECK(data.dtau_dq.isApprox(data_ref.dtau_dq));
  BOOST_CHECK(data.dtau_dv.isApprox(data_ref.dtau_dv));

  MatrixXd aba_partial_dq_fd(model.nv,model.nv); aba_partial_dq_fd.setZero();
  MatrixXd aba_partial_dv_fd(model.nv,model.nv); aba_partial_dv_fd.setZero();
  MatrixXd aba_partial_dtau_fd(model.nv,model.nv); aba_partial_dtau_fd.setZero();

  Data data_fd(model);
  VectorXd a0 = aba(model,data_fd,q,v,tau);
  VectorXd v_eps(VectorXd::Zero(model.nv));
  VectorXd q_plus(model.nq);
  VectorXd a_plus(model.nv);
  const double alpha = 1e-8;
  for(int k = 0; k < model.nv; ++k)
  {
    v_eps[k] += alpha;
    q_plus = integrate(model,q,v_eps);
    a_plus = aba(model,data_fd,q_plus,v,tau);

    aba_partial_dq_fd.col(k) = (a_plus - a0)/alpha;
    v_eps[k] -= alpha;
  }
  BOOST_CHECK(aba_partial_dq.isApprox(aba_partial_dq_fd,sqrt(alpha)));

  VectorXd v_plus(v);
  for(int k = 0; k < model.nv; ++k)
  {
    v_plus[k] += alpha;
    a_plus = aba(model,data_fd,q,v_plus,tau);

    aba_partial_dv_fd.col(k) = (a_plus - a0)/alpha;
    v_plus[k] -= alpha;
  }
  BOOST_CHECK(aba_partial_dv.isApprox(aba_partial_dv_fd,sqrt(alpha)));

  VectorXd tau_plus(tau);
  for(int k = 0; k < model.nv; ++k)
  {
    tau_plus[k] += alpha;
    a_plus = aba(model,data_fd,q,v,tau_plus);

    aba_partial_dtau_fd.col(k) = (a_plus - a0)/alpha;
    tau_plus[k] -= alpha;
  }
  BOOST_CHECK(aba_partial_dtau.isApprox(aba_partial_dtau_fd,sqrt(alpha)));
}

BOOST_AUTO_TEST_CASE(test_aba_minimal_argument)
{
  using namespace Eigen;
  using namespace pinocchio;

  Model model;
  buildModels::humanoidRandom(model);

  Data data(model), data_ref(model);

  model.lowerPositionLimit.head<3>().fill(-1.);
  model.upperPositionLimit.head<3>().fill(1.);
  VectorXd q = randomConfiguration(model);
  VectorXd v(VectorXd::Random(model.nv));
  VectorXd tau(VectorXd::Random(model.nv));
  VectorXd a(aba(model,data_ref,q,v,tau));

  MatrixXd aba_partial_dq(model.nv,model.nv); aba_partial_dq.setZero();
  MatrixXd aba_partial_dv(model.nv,model.nv); aba_partial_dv.setZero();
  Data::RowMatrixXs aba_partial_dtau(model.nv,model.nv); aba_partial_dtau.setZero();

  computeABADerivatives(model, data_ref, q, v, tau, aba_partial_dq, aba_partial_dv, aba_partial_dtau);

  computeABADerivatives(model, data, q, v, tau);

  BOOST_CHECK(data.J.isApprox(data_ref.J));
  BOOST_CHECK(data.dJ.isApprox(data_ref.dJ));
  BOOST_CHECK(data.dVdq.isApprox(data_ref.dVdq));
  BOOST_CHECK(data.dAdq.isApprox(data_ref.dAdq));
  BOOST_CHECK(data.dAdv.isApprox(data_ref.dAdv));
  BOOST_CHECK(data.dtau_dq.isApprox(data_ref.dtau_dq));
  BOOST_CHECK(data.dtau_dv.isApprox(data_ref.dtau_dv));
  BOOST_CHECK(data.Minv.isApprox(aba_partial_dtau));
  BOOST_CHECK(data.ddq_dq.isApprox(aba_partial_dq));
  BOOST_CHECK(data.ddq_dv.isApprox(aba_partial_dv));
}

BOOST_AUTO_TEST_CASE(test_aba_derivatives_fext)
{
  using namespace Eigen;
  using namespace pinocchio;

  Model model;
  buildModels::humanoidRandom(model);

  Data data(model), data_ref(model);

  model.lowerPositionLimit.head<3>().fill(-1.);
  model.upperPositionLimit.head<3>().fill(1.);
  VectorXd q = randomConfiguration(model);
  VectorXd v(VectorXd::Random(model.nv));
  VectorXd tau(VectorXd::Random(model.nv));
  VectorXd a(aba(model,data_ref,q,v,tau));

  typedef container::aligned_vector<Force> ForceVector;
  ForceVector fext((size_t)model.njoints);
  for(ForceVector::iterator it = fext.begin(); it != fext.end(); ++it)
    (*it).setRandom();

  MatrixXd aba_partial_dq(model.nv,model.nv); aba_partial_dq.setZero();
  MatrixXd aba_partial_dv(model.nv,model.nv); aba_partial_dv.setZero();
  Data::RowMatrixXs aba_partial_dtau(model.nv,model.nv); aba_partial_dtau.setZero();

  computeABADerivatives(model, data, q, v, tau, fext,
                        aba_partial_dq, aba_partial_dv, aba_partial_dtau);

  aba(model,data_ref,q,v,tau,fext);
//  updateGlobalPlacements(model, data_ref);
//  for(size_t k =1; k < (size_t)model.njoints; ++k)
//  {
//    BOOST_CHECK(data.oMi[k].isApprox(data_ref.oMi[k]));
//    BOOST_CHECK(daita.of[k].isApprox(data_ref.oMi[k].act(data.f[k])));
//
//  }
  BOOST_CHECK(data.ddq.isApprox(data_ref.ddq));

  computeABADerivatives(model,data_ref,q,v,tau);
  BOOST_CHECK(aba_partial_dv.isApprox(data_ref.ddq_dv));
  BOOST_CHECK(aba_partial_dtau.isApprox(data_ref.Minv));

  MatrixXd aba_partial_dq_fd(model.nv,model.nv); aba_partial_dq_fd.setZero();
  MatrixXd aba_partial_dv_fd(model.nv,model.nv); aba_partial_dv_fd.setZero();
  MatrixXd aba_partial_dtau_fd(model.nv,model.nv); aba_partial_dtau_fd.setZero();

  Data data_fd(model);
  const VectorXd a0 = aba(model,data_fd,q,v,tau,fext);
  VectorXd v_eps(VectorXd::Zero(model.nv));
  VectorXd q_plus(model.nq);
  VectorXd a_plus(model.nv);
  const double alpha = 1e-8;
  for(int k = 0; k < model.nv; ++k)
  {
    v_eps[k] += alpha;
    q_plus = integrate(model,q,v_eps);
    a_plus = aba(model,data_fd,q_plus,v,tau,fext);

    aba_partial_dq_fd.col(k) = (a_plus - a0)/alpha;
    v_eps[k] -= alpha;
  }
  BOOST_CHECK(aba_partial_dq.isApprox(aba_partial_dq_fd,sqrt(alpha)));

  VectorXd v_plus(v);
  for(int k = 0; k < model.nv; ++k)
  {
    v_plus[k] += alpha;
    a_plus = aba(model,data_fd,q,v_plus,tau,fext);

    aba_partial_dv_fd.col(k) = (a_plus - a0)/alpha;
    v_plus[k] -= alpha;
  }
  BOOST_CHECK(aba_partial_dv.isApprox(aba_partial_dv_fd,sqrt(alpha)));

  VectorXd tau_plus(tau);
  for(int k = 0; k < model.nv; ++k)
  {
    tau_plus[k] += alpha;
    a_plus = aba(model,data_fd,q,v,tau_plus,fext);

    aba_partial_dtau_fd.col(k) = (a_plus - a0)/alpha;
    tau_plus[k] -= alpha;
  }
  BOOST_CHECK(aba_partial_dtau.isApprox(aba_partial_dtau_fd,sqrt(alpha)));

  // test the shortcut
  Data data_shortcut(model);
  computeABADerivatives(model,data_shortcut,q,v,tau,fext);
  BOOST_CHECK(data_shortcut.ddq_dq.isApprox(aba_partial_dq));
  BOOST_CHECK(data_shortcut.ddq_dv.isApprox(aba_partial_dv));
  BOOST_CHECK(data_shortcut.Minv.isApprox(aba_partial_dtau));
}

BOOST_AUTO_TEST_CASE(test_multiple_calls)
{
  using namespace Eigen;
  using namespace pinocchio;

  Model model;
  buildModels::humanoidRandom(model);

  Data data1(model), data2(model);

  model.lowerPositionLimit.head<3>().fill(-1.);
  model.upperPositionLimit.head<3>().fill(1.);
  VectorXd q = randomConfiguration(model);
  VectorXd v(VectorXd::Random(model.nv));
  VectorXd tau(VectorXd::Random(model.nv));

  computeABADerivatives(model,data1,q,v,tau);
  data2 = data1;

  for(int k = 0; k < 20; ++k)
  {
    computeABADerivatives(model,data1,q,v,tau);
  }

  BOOST_CHECK(data1.J.isApprox(data2.J));
  BOOST_CHECK(data1.dJ.isApprox(data2.dJ));
  BOOST_CHECK(data1.dVdq.isApprox(data2.dVdq));
  BOOST_CHECK(data1.dAdq.isApprox(data2.dAdq));
  BOOST_CHECK(data1.dAdv.isApprox(data2.dAdv));

  BOOST_CHECK(data1.dFdq.isApprox(data2.dFdq));
  BOOST_CHECK(data1.dFdv.isApprox(data2.dFdv));
  BOOST_CHECK(data1.dFda.isApprox(data2.dFda));

  BOOST_CHECK(data1.dtau_dq.isApprox(data2.dtau_dq));
  BOOST_CHECK(data1.dtau_dv.isApprox(data2.dtau_dv));

  BOOST_CHECK(data1.ddq_dq.isApprox(data2.ddq_dq));
  BOOST_CHECK(data1.ddq_dv.isApprox(data2.ddq_dv));
  BOOST_CHECK(data1.Minv.isApprox(data2.Minv));
}

BOOST_AUTO_TEST_CASE(test_aba_derivatives_vs_kinematics_derivatives)
{
  using namespace Eigen;
  using namespace pinocchio;

  Model model;
  buildModels::humanoidRandom(model);

  Data data(model), data_ref(model);

  model.lowerPositionLimit.head<3>().fill(-1.);
  model.upperPositionLimit.head<3>().fill(1.);
  VectorXd q = randomConfiguration(model);
  VectorXd v(VectorXd::Random(model.nv));
  VectorXd a(VectorXd::Random(model.nv));

  VectorXd tau = rnea(model,data_ref,q,v,a);

  /// Check againt computeGeneralizedGravityDerivatives
  MatrixXd aba_partial_dq(model.nv,model.nv); aba_partial_dq.setZero();
  MatrixXd aba_partial_dv(model.nv,model.nv); aba_partial_dv.setZero();
  MatrixXd aba_partial_dtau(model.nv,model.nv); aba_partial_dtau.setZero();

  computeABADerivatives(model,data,q,v,tau,aba_partial_dq,aba_partial_dv,aba_partial_dtau);
  computeForwardKinematicsDerivatives(model,data_ref,q,v,a);

  BOOST_CHECK(data.J.isApprox(data_ref.J));
  BOOST_CHECK(data.dJ.isApprox(data_ref.dJ));

  for(size_t k = 1; k < (size_t)model.njoints; ++k)
  {
    BOOST_CHECK(data.oMi[k].isApprox(data_ref.oMi[k]));
    BOOST_CHECK(data.ov[k].isApprox(data_ref.ov[k]));
    BOOST_CHECK(data.oa[k].isApprox(data_ref.oa[k]));
  }
}

BOOST_AUTO_TEST_SUITE_END()


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

#ifndef BOOST_MATH_HANKEL_HPP
#define BOOST_MATH_HANKEL_HPP

#include <boost/math/special_functions/math_fwd.hpp>
#include <boost/math/special_functions/bessel.hpp>

namespace boost{ namespace math{

namespace detail{

template <class T, class Policy>
std::complex<T> hankel_imp(T v, T x, const bessel_no_int_tag&, const Policy& pol, int sign)
{
   BOOST_MATH_STD_USING
   static const char* function = "boost::math::cyl_hankel_1<%1%>(%1%,%1%)";

   if(x < 0)
   {
      bool isint_v = floor(v) == v;
      T j, y;
      bessel_jy(v, -x, &j, &y, need_j | need_y, pol);
      std::complex<T> cx(x), cv(v);
      std::complex<T> j_result, y_result;
      if(isint_v)
      {
         int s = (iround(v) & 1) ? -1 : 1;
         j_result = j * s;
         y_result = T(s) * (y - (2 / constants::pi<T>()) * (log(-x) - log(cx)) * j);
      }
      else
      {
         j_result = pow(cx, v) * pow(-cx, -v) * j;
         T p1 = pow(-x, v);
         std::complex<T> p2 = pow(cx, v);
         y_result = p1 * y / p2
            + (p2 / p1 - p1 / p2) * j / tan(constants::pi<T>() * v);
      }
      // multiply y_result by i:
      y_result = std::complex<T>(-sign * y_result.imag(), sign * y_result.real());
      return j_result + y_result;
   }

   if(x == 0)
   {
      if(v == 0)
      {
         // J is 1, Y is -INF
         return std::complex<T>(1, sign * -policies::raise_overflow_error<T>(function, 0, pol));
      }
      else
      {
         // At least one of J and Y is complex infinity:
         return std::complex<T>(policies::raise_overflow_error<T>(function, 0, pol), sign * policies::raise_overflow_error<T>(function, 0, pol));
      }
   }

   T j, y;
   bessel_jy(v, x, &j, &y, need_j | need_y, pol);
   return std::complex<T>(j, sign * y);
}

template <class T, class Policy>
std::complex<T> hankel_imp(int v, T x, const bessel_int_tag&, const Policy& pol, int sign);

template <class T, class Policy>
inline std::complex<T> hankel_imp(T v, T x, const bessel_maybe_int_tag&, const Policy& pol, int sign)
{
   BOOST_MATH_STD_USING  // ADL of std names.
   int ival = detail::iconv(v, pol);
   if(0 == v - ival)
   {
      return hankel_imp(ival, x, bessel_int_tag(), pol, sign);
   }
   return hankel_imp(v, x, bessel_no_int_tag(), pol, sign);
}

template <class T, class Policy>
inline std::complex<T> hankel_imp(int v, T x, const bessel_int_tag&, const Policy& pol, int sign)
{
   BOOST_MATH_STD_USING
   if((std::abs(v) < 200) && (x > 0))
      return std::complex<T>(bessel_jn(v, x, pol), sign * bessel_yn(v, x, pol));
   return hankel_imp(static_cast<T>(v), x, bessel_no_int_tag(), pol, sign);
}

template <class T, class Policy>
inline std::complex<T> sph_hankel_imp(T v, T x, const Policy& pol, int sign)
{
   BOOST_MATH_STD_USING
   return constants::root_half_pi<T>() * hankel_imp(v + 0.5f, x, bessel_no_int_tag(), pol, sign) / sqrt(std::complex<T>(x));
}

} // namespace detail

template <class T1, class T2, class Policy>
inline std::complex<typename detail::bessel_traits<T1, T2, Policy>::result_type> cyl_hankel_1(T1 v, T2 x, const Policy& pol)
{
   BOOST_FPU_EXCEPTION_GUARD
   typedef typename detail::bessel_traits<T1, T2, Policy>::result_type result_type;
   typedef typename detail::bessel_traits<T1, T2, Policy>::optimisation_tag tag_type;
   typedef typename policies::evaluation<result_type, Policy>::type value_type;
   return policies::checked_narrowing_cast<std::complex<result_type>, Policy>(detail::hankel_imp<value_type>(v, static_cast<value_type>(x), tag_type(), pol, 1), "boost::math::cyl_hankel_1<%1%>(%1%,%1%)");
}

template <class T1, class T2>
inline std::complex<typename detail::bessel_traits<T1, T2, policies::policy<> >::result_type> cyl_hankel_1(T1 v, T2 x)
{
   return cyl_hankel_1(v, x, policies::policy<>());
}

template <class T1, class T2, class Policy>
inline std::complex<typename detail::bessel_traits<T1, T2, Policy>::result_type> cyl_hankel_2(T1 v, T2 x, const Policy& pol)
{
   BOOST_FPU_EXCEPTION_GUARD
   typedef typename detail::bessel_traits<T1, T2, Policy>::result_type result_type;
   typedef typename detail::bessel_traits<T1, T2, Policy>::optimisation_tag tag_type;
   typedef typename policies::evaluation<result_type, Policy>::type value_type;
   return policies::checked_narrowing_cast<std::complex<result_type>, Policy>(detail::hankel_imp<value_type>(v, static_cast<value_type>(x), tag_type(), pol, -1), "boost::math::cyl_hankel_1<%1%>(%1%,%1%)");
}

template <class T1, class T2>
inline std::complex<typename detail::bessel_traits<T1, T2, policies::policy<> >::result_type> cyl_hankel_2(T1 v, T2 x)
{
   return cyl_hankel_2(v, x, policies::policy<>());
}

template <class T1, class T2, class Policy>
inline std::complex<typename detail::bessel_traits<T1, T2, Policy>::result_type> sph_hankel_1(T1 v, T2 x, const Policy&)
{
   BOOST_FPU_EXCEPTION_GUARD
   typedef typename detail::bessel_traits<T1, T2, Policy>::result_type result_type;
   typedef typename policies::evaluation<result_type, Policy>::type value_type;
   typedef typename policies::normalise<
      Policy,
      policies::promote_float<false>,
      policies::promote_double<false>,
      policies::discrete_quantile<>,
      policies::assert_undefined<> >::type forwarding_policy;

   return policies::checked_narrowing_cast<std::complex<result_type>, Policy>(detail::sph_hankel_imp<value_type>(static_cast<value_type>(v), static_cast<value_type>(x), forwarding_policy(), 1), "boost::math::sph_hankel_1<%1%>(%1%,%1%)");
}

template <class T1, class T2>
inline std::complex<typename detail::bessel_traits<T1, T2, policies::policy<> >::result_type> sph_hankel_1(T1 v, T2 x)
{
   return sph_hankel_1(v, x, policies::policy<>());
}

template <class T1, class T2, class Policy>
inline std::complex<typename detail::bessel_traits<T1, T2, Policy>::result_type> sph_hankel_2(T1 v, T2 x, const Policy&)
{
   BOOST_FPU_EXCEPTION_GUARD
   typedef typename detail::bessel_traits<T1, T2, Policy>::result_type result_type;
   typedef typename policies::evaluation<result_type, Policy>::type value_type;
   typedef typename policies::normalise<
      Policy,
      policies::promote_float<false>,
      policies::promote_double<false>,
      policies::discrete_quantile<>,
      policies::assert_undefined<> >::type forwarding_policy;

   return policies::checked_narrowing_cast<std::complex<result_type>, Policy>(detail::sph_hankel_imp<value_type>(static_cast<value_type>(v), static_cast<value_type>(x), forwarding_policy(), -1), "boost::math::sph_hankel_1<%1%>(%1%,%1%)");
}

template <class T1, class T2>
inline std::complex<typename detail::bessel_traits<T1, T2, policies::policy<> >::result_type> sph_hankel_2(T1 v, T2 x)
{
   return sph_hankel_2(v, x, policies::policy<>());
}

}} // namespaces

#endif // BOOST_MATH_HANKEL_HPP


#include <boost/math/distributions/bernoulli.hpp>


#include "lookIncFromSr.h"

#include <pybind11/eigen.h>
#include <pybind11/numpy.h>
#include <pybind11/stl.h>

#include <Eigen/Dense>

#include <isce3/core/Ellipsoid.h>
#include <isce3/core/Orbit.h>
#include <isce3/geometry/DEMInterpolator.h>
#include <isce3/geometry/geometry.h>

// Aliases
namespace py = pybind11;
using isce3::geometry::DEMInterpolator;
using namespace isce3::core;

// Functions binding
void addbinding_look_inc_from_sr(pybind11::module& m)
{

    m.def(
            "look_inc_ang_from_slant_range",
            [](double slant_range, const Orbit& orbit,
                    std::optional<double> az_time = {},
                    const DEMInterpolator& dem_interp = {},
                    const Ellipsoid& ellips = {}) {
                return lookIncAngFromSlantRange(
                        slant_range, orbit, az_time, dem_interp, ellips);
            },
            py::arg("slant_range"), py::arg("orbit"),
            py::arg("az_time") = std::nullopt,
            py::arg_v("dem_interp", DEMInterpolator(), "0.0"),
            py::arg_v("ellips", Ellipsoid(), "WGS84"),
            R"(
Estimate look angle (off-nadir angle) and local incidence angle at a desired slant range
from orbit(spacecraft/antenna statevector) and at a certain relative azimuth time.

Parameters
----------
slant_range : float
    true slant range in meters from antenna phase center (or spacecraft position) to the ground.
orbit : isce3.core.orbit
az_time : float, optional
    relative azimuth time in seconds w.r.t reference epoch time of orbit object.
    If not speficied, the mid time of orbit will be used as azimuth time.
dem_interp : isce3.geometry.DEMInterpolator, default=0.0
ellips : isce3.core.Ellipsoid, default=WGS84

Returns
-------
float
    Look angle or off-nadir angle in (rad)
float
    Incidence angle in (rad)

Raises
------
RuntimeError
    for bad-value look angle or incidence angles

Notes
-----
See references [1]_ and [2]_ for the equations to calculate
look angle and incidence angle, respectivelty.

References
----------
..[1] https://en.wikipedia.org/wiki/Law_of_cosines
..[2] https://en.wikipedia.org/wiki/Law_of_sines
)");

    m.def(
            "look_inc_ang_from_slant_range",
            [](const Eigen::Ref<const Eigen::ArrayXd>& slant_range,
                    const Orbit& orbit, std::optional<double> az_time = {},
                    const DEMInterpolator& dem_interp = {},
                    const Ellipsoid& ellips = {}) {
                return lookIncAngFromSlantRange(
                        slant_range, orbit, az_time, dem_interp, ellips);
            },
            py::arg("slant_range"), py::arg("orbit"),
            py::arg("az_time") = std::nullopt,
            py::arg_v("dem_interp", DEMInterpolator(), "0.0"),
            py::arg_v("ellips", Ellipsoid(), "WGS84"),
            R"(
Estimate look angles (off-nadir angle) and local incidence angles at desired slant ranges
from orbit(spacecraft/antenna statevector) and at a certain relative azimuth time.

Parameters
----------
slant_range : numpy.ndarray(float)
    Array of slant ranges in meters from antenna phase center (or spacecraft position) to the ground.
orbit : isce3.core.orbit
az_time : float, optional
    relative azimuth time in seconds w.r.t reference epoch time of orbit object.
    If not speficied, the mid time of orbit will be used as azimuth time.
dem_interp : isce3.geometry.DEMInterpolator, default=0.0
ellips : isce3.core.Ellipsoid, default=WGS84

Returns
-------
numpy.ndarray(float)
    Look angles or off-nadir angles in (rad)
numpy.ndarray(float)
    Incidence angles in (rad)

Raises
------
RuntimeError
    for bad-value look angle or incidence angles

Notes
-----
See references [1]_ and [2]_ for the equations to calculate
look angle and incidence angle, respectivelty.

References
----------
..[1] https://en.wikipedia.org/wiki/Law_of_cosines
..[2] https://en.wikipedia.org/wiki/Law_of_sines
)");
}


#include "coredsp/filter.h"
#include "coredsp/noise.h"
#include "coreutil/cpu.h"
#include <boost/preprocessor.hpp>
#include <functional>
#include <chrono>
#include <vector>

typedef std::chrono::steady_clock Clock;
typedef Clock::time_point TimePoint;
typedef Clock::duration Duration;

static unsigned iteration_count = 64 * 1024;

struct Benchmark {
  const char *name {};
  std::function<float()> fn;
  double run() const;
};

double Benchmark::run() const {
  TimePoint tstart = Clock::now();
  this->fn();
  Duration dur = Clock::now() - tstart;
  auto ns = std::chrono::duration_cast<std::chrono::nanoseconds>(dur);
  return ns.count() * 1e-9;
}

#define FIR_NCOEF_MIN 2
#define FIR_NCOEF_LIMIT 256

static constexpr unsigned nfir = FIR_NCOEF_LIMIT - FIR_NCOEF_MIN;

#define DECL_FIR_FLT(z, n, t)                               \
  static coredsp::FIR<n, coreutil::simd_t<float>> firf##n;  \
  static coredsp::FIR<n, coreutil::simd_t<double>> fird##n;
#define DECL_FIRG_FLT(z, n, t)                                 \
  static coredsp::FIRg<coreutil::simd_t<float>> firgf##n(n);   \
  static coredsp::FIRg<coreutil::simd_t<double>> firgd##n(n);
BOOST_PP_REPEAT_FROM_TO(FIR_NCOEF_MIN, FIR_NCOEF_LIMIT, DECL_FIR_FLT,);
BOOST_PP_REPEAT_FROM_TO(FIR_NCOEF_MIN, FIR_NCOEF_LIMIT, DECL_FIRG_FLT,);

static Benchmark bfirf_simd[nfir], bfirf_scalar[nfir];
static Benchmark bfirgf_simd[nfir], bfirgf_scalar[nfir];
static Benchmark bfird_simd[nfir], bfird_scalar[nfir];
static Benchmark bfirgd_simd[nfir], bfirgd_scalar[nfir];

template <class T> std::function<float()> create_fir_simd_fn(T &filter) {
  return [&filter]() -> float {
    float out {};
    coredsp::WhiteNoise source;
    for (unsigned i = 0; i < iteration_count; ++i) {
      filter.in(source.tick());
      out = filter.impl_simd();
    }
    return out;
  };
}

template <class T> std::function<float()> create_fir_scalar_fn(T &filter) {
  return [&filter]() -> float {
    float out {};
    coredsp::WhiteNoise source;
    for (unsigned i = 0; i < iteration_count; ++i) {
      filter.in(source.tick());
      out = filter.impl_scalar();
    }
    return out;
  };
}

int main() {
  coreutil::disable_denormals();

#define DEF_FIR_BENCHMARK(z, n, t)                                      \
  bfirf_simd[n - FIR_NCOEF_MIN] = Benchmark{"FIR<" #n "> simd float", create_fir_simd_fn(firf##n)}; \
  bfirf_scalar[n - FIR_NCOEF_MIN] = Benchmark{"FIR<" #n "> scalar float", create_fir_scalar_fn(firf##n)}; \
  bfirgf_simd[n - FIR_NCOEF_MIN] = Benchmark{"FIRg<" #n "> simd float", create_fir_simd_fn(firgf##n)}; \
  bfirgf_scalar[n - FIR_NCOEF_MIN] = Benchmark{"FIRg<" #n "> scalar float", create_fir_scalar_fn(firgf##n)}; \
  bfird_simd[n - FIR_NCOEF_MIN] = Benchmark{"FIR<" #n "> simd double", create_fir_simd_fn(fird##n)}; \
  bfird_scalar[n - FIR_NCOEF_MIN] = Benchmark{"FIR<" #n "> scalar double", create_fir_scalar_fn(fird##n)}; \
  bfirgd_simd[n - FIR_NCOEF_MIN] = Benchmark{"FIRg<" #n "> simd double", create_fir_simd_fn(firgd##n)}; \
  bfirgd_scalar[n - FIR_NCOEF_MIN] = Benchmark{"FIRg<" #n "> scalar double", create_fir_scalar_fn(firgd##n)};

  BOOST_PP_REPEAT_FROM_TO(FIR_NCOEF_MIN, FIR_NCOEF_LIMIT, DEF_FIR_BENCHMARK,);

  setlinebuf(stdout);

  for (unsigned i = 0; i < nfir; ++i) {
    double tsimdf = bfirf_simd[i].run();
    double tscalarf = bfirf_scalar[i].run();
    double tgsimdf = bfirgf_simd[i].run();
    double tgscalarf = bfirgf_scalar[i].run();
    double tsimdd = bfird_simd[i].run();
    double tscalard = bfird_scalar[i].run();
    double tgsimdd = bfirgd_simd[i].run();
    double tgscalard = bfirgd_scalar[i].run();

    printf("%u %f %f %f %f %f %f %f %f\n", i + FIR_NCOEF_MIN,
           tsimdf, tscalarf, tgsimdf, tgscalarf,
           tsimdd, tscalard, tgsimdd, tgscalard);
  }

  return 0;
}


#pragma once
#pragma once

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

#include "Geometry.hpp"

#include <vector>

namespace pointcloudhandler {

		class Geometry3D : public Geometry {
		public:
			~Geometry3D() override
			{
			}

		protected:
			Geometry3D(GeometryType type) : Geometry(type, 3)
			{
			}

		public:
			Geometry3D& Clear() override = 0;
			bool IsEmpty() const override = 0;
			virtual Eigen::Vector3d GetMinBound() const = 0;
			virtual Eigen::Vector3d GetMaxBound() const = 0;
			virtual Eigen::Vector3d GetCenter() const = 0;

			virtual Geometry3D& Transform(const Eigen::Matrix4d& transformation) = 0;
			virtual Geometry3D& Translate(const Eigen::Vector3d& translation, bool relative = true) = 0;
			virtual Geometry3D& Scale(const double scale, bool center = true) = 0;
			virtual Geometry3D& Rotate(const Eigen::Matrix3d& R, bool center = true) = 0;

		protected:
			Eigen::Vector3d ComputeMinBound(const std::vector<Eigen::Vector3d>& points) const;
			Eigen::Vector3d ComputeMaxBound(const std::vector<Eigen::Vector3d>& points) const;
			Eigen::Vector3d ComputeCenter(const std::vector<Eigen::Vector3d>& points) const;

			void TransformPoints(const Eigen::Matrix4d& transformation,std::vector<Eigen::Vector3d>& points) const;
			void TranslatePoints(const Eigen::Vector3d& translation,std::vector<Eigen::Vector3d>& points,bool relative) const;
			void ScalePoints(const double scale, std::vector<Eigen::Vector3d>& points, bool center) const;
			void RotatePoints(const Eigen::Matrix3d& R, std::vector<Eigen::Vector3d>& points, bool center) const;
		};

}  // namespace pointcloudhandler