#ifdef COMPILATION// -*-indent-tabs-mode:t;c-basic-offset:4;tab-width:4;-*-
$CXX $0 -o $0x -lboost_unit_test_framework \
`pkg-config --libs blas` \
`#-Wl,-rpath,/usr/local/Wolfram/Mathematica/12.0/SystemFiles/Libraries/Linux-x86-64 -L/usr/local/Wolfram/Mathematica/12.0/SystemFiles/Libraries/Linux-x86-64 -lmkl_intel_ilp64 -lmkl_sequential -lmkl_core` \
&&$0x&&rm $0x;exit
#endif
// © Alfredo A. Correa 2019-2020

#ifndef MULTI_ADAPTORS_BLAS_GEMM_HPP
#define MULTI_ADAPTORS_BLAS_GEMM_HPP

#include "../blas/core.hpp"
#include "../blas/numeric.hpp"
#include "../blas/operations.hpp"

#include "../../config/NODISCARD.hpp"

namespace boost{
namespace multi{
namespace blas{

using multi::blas::core::gemm;

template<class M> bool is_c_ordering(M const& m){
	return stride(rotated(m))==1 and size(m)!=1;
}

double const& conj(double const& d){return d;}
float const& conj(float const& f){return f;}

template<class A, std::enable_if_t<not is_conjugated<A>{}, int> =0>
auto gemm_base_aux(A&& a){return base(a);}

template<class A, std::enable_if_t<    is_conjugated<A>{}, int> =0>
auto gemm_base_aux(A&& a){return underlying(base(a));}

template<class A2D, class B2D, class C2D>
C2D&& gemm(typename std::decay_t<C2D>::element_type alpha, A2D const& a, B2D const& b, typename std::decay_t<C2D>::element_type beta, C2D&& c){
	assert( size(rotated(a)) == size(b) );
	assert( size(c) == size(a) );
	assert( size(rotated(b)) == size(rotated(c)) );
	auto base_a = gemm_base_aux(a);
	auto base_b = gemm_base_aux(b);
	auto base_c = gemm_base_aux(c);

	if(is_conjugated<C2D>{}) blas::gemm(conj(alpha), conj(a), conj(b), conj(beta), conj(c));
	else{
		if(is_c_ordering(c)){//gemm(alpha, transposed(b), transposed(a), beta, transposed(c));
			     if( is_c_ordering(a) and  is_c_ordering(b)){
				assert(!is_conjugated<A2D>{} and !is_conjugated<B2D>{});
				gemm('N', 'N', size(rotated(c)), size(a   ), size(b   ), &alpha, base_b, stride(b   ), base_a, stride(a   ), &beta, base_c, stride(c   ));}
			else if(!is_c_ordering(a) and  is_c_ordering(b)){
				assert(!is_conjugated<B2D>{});
				gemm('N', is_conjugated<A2D>{}?'C':'T', size(rotated(c)), size(a   ), size(b   ), &alpha, base_b, stride(b   ), base_a, stride(rotated(a)), &beta, base_c, stride(c   ));
			}
			else if( is_c_ordering(a) and !is_c_ordering(b)){
				assert(!is_conjugated<A2D>{});
				gemm(is_conjugated<B2D>{}?'C':'T', 'N', size(rotated(c)), size(a   ), size(b   ), &alpha, base_b, stride(rotated(b)), base_a, stride(a   ), &beta, base_c, stride(c   ));
			}else if(!is_c_ordering(a) and !is_c_ordering(b)){gemm(is_conjugated<B2D>{}?'C':'T', is_conjugated<A2D>{}?'C':'T', size(rotated(c)), size(a   ), size(b   ), &alpha, base_b, stride(rotated(b)), base_a, stride(rotated(a)), &beta, base_c, stride(c   ));}
		}else{
			using core::gemm;
				 if( is_c_ordering(a) and  is_c_ordering(b)){
				gemm(is_conjugated<A2D>{}?'C':'T', is_conjugated<B2D>{}?'C':'T', size(c   ), size(rotated(b)), size(rotated(a)), &alpha, base_a, stride(a   ), base_b, stride(b   ), &beta, base_c, stride(rotated(c)));
			}else if(!is_c_ordering(a) and  is_c_ordering(b)){
				if(is_conjugated<A2D>{} and size(a)==1){
					gemm('C', is_conjugated<B2D>{}?'C':'T', size(c   ), size(rotated(b)), size(rotated(a)), &alpha, base_a, size(rotated(a)), base_b, stride(b   ), &beta, base_c, stride(rotated(c)));
				}else{
					assert(not is_conjugated<A2D>{});
					gemm('N', is_conjugated<B2D>{}?'C':'T', size(c   ), size(rotated(b)), size(rotated(a)), &alpha, base_a, stride(rotated(a)), base_b, stride(b   ), &beta, base_c, stride(rotated(c)));
				}
			}
			else if( is_c_ordering(a) and !is_c_ordering(b)){
				if(is_conjugated<B2D>{} and size(rotated(b))==1){
					gemm(is_conjugated<A2D>{}?'C':'T', 'C', size(c   ), size(rotated(b)), size(rotated(a)), &alpha, base_a, stride(a   ), base_b, size(rotated(b)), &beta, base_c, stride(rotated(c)));
				}else{
					assert(not is_conjugated<B2D>{});
					gemm(is_conjugated<A2D>{}?'C':'T', 'N', size(c   ), size(rotated(b)), size(rotated(a)), &alpha, base_a, stride(a   ), base_b, stride(rotated(b)), &beta, base_c, stride(rotated(c)));
				}
			}else if(!is_c_ordering(a) and !is_c_ordering(b)){
				      if(not is_conjugated<A2D>{} and is_conjugated<B2D>{} and size(rotated(b))==1){
					gemm('N', 'C', size(c   ), size(rotated(b)), size(rotated(a)), &alpha, base_a, stride(rotated(a)), base_b, stride(b), &beta, base_c, stride(rotated(c)));
				}else if(is_conjugated<A2D>{} and size(a)==1 and is_conjugated<B2D>{} and size(rotated(b))==1){
					gemm('C', 'C', size(c   ), size(rotated(b)), size(rotated(a)), &alpha, base_a, size(rotated(a)), base_b, stride(b), &beta, base_c, stride(rotated(c)));
				}else{
					assert(not is_conjugated<A2D>{}); assert(not is_conjugated<B2D>{});
					gemm('N', 'N', size(c   ), size(rotated(b)), size(rotated(a)), &alpha, base_a, stride(rotated(a)), base_b, stride(rotated(b)), &beta, base_c, stride(rotated(c)));
				}
			}
		}
	}
	return std::forward<C2D>(c);
}

template<class AA, class A2D, class B2D, class C2D = typename A2D::decay_type>
NODISCARD("second argument is const")
auto gemm(AA a, A2D const& A, B2D const& B){
	assert(get_allocator(A) == get_allocator(B));
	return gemm(a, A, B, 0., C2D({size(A), size(rotated(B))}, get_allocator(A)));
}

template<class A2D, class B2D> auto gemm(A2D const& A, B2D const& B){return gemm(1., A, B);}

}}}

#if not __INCLUDE_LEVEL__ // _TEST_MULTI_ADAPTORS_BLAS_GEMM

#define BOOST_TEST_MODULE "C++ Unit Tests for Multi gemm"
#define BOOST_TEST_DYN_LINK
#include<boost/test/unit_test.hpp>

#include "../../array.hpp"
#include "../../utility.hpp"

#include <boost/timer/timer.hpp>

#include<complex>
#include<cassert>
#include<iostream>
#include<numeric>
#include<algorithm>

namespace multi = boost::multi;

template<class M> decltype(auto) print(M const& C){
	using std::cout;
	using multi::size;
	for(int i = 0; i != size(C); ++i){
		for(int j = 0; j != size(C[i]); ++j)
			cout<< C[i][j] <<' ';
		cout<<std::endl;
	}
	return cout<<std::endl;
}

BOOST_AUTO_TEST_CASE(multi_adaptors_blas_gemm_real_square){
	namespace blas = multi::blas;
	multi::array<double, 2> const a = {
		{ 1, 3},
		{ 9, 7},
	};
	multi::array<double, 2> const b = {
		{ 11, 12},
		{  7, 19},
	};
	{
		multi::array<double, 2> c({2, 2});
		blas::gemm(1., a, b, 0., c); // c=ab, c⸆=b⸆a⸆
		BOOST_REQUIRE( c[1][0] == 148 );
	}
	{
		multi::array<double, 2> c({2, 2});
		blas::gemm(1., blas::T(a), b, 0., c); // c=a⸆b, c⸆=b⸆a
		BOOST_REQUIRE(( c[1][1] == 169 and c[1][0] == 82 ));
	}
	{
		multi::array<double, 2> c({2, 2});
		blas::gemm(1., a, blas::T(b), 0., c); // c=ab⸆, c⸆=ba⸆
		BOOST_REQUIRE( c[1][0] == 183 );
	}
	{
		multi::array<double, 2> c({2, 2});
		blas::gemm(1., blas::T(a), blas::T(b), 0., c); // c=a⸆b⸆, c⸆=ba
		BOOST_REQUIRE( c[1][0] == 117 );
	}
	{
		multi::array<double, 2> c({2, 2});
		blas::gemm(1., blas::T(a), blas::T(b), 0., blas::T(c)); // c⸆=a⸆b⸆, c=ba
		BOOST_REQUIRE( c[0][1] == 117 );
	}
}

BOOST_AUTO_TEST_CASE(multi_adaptors_blas_gemm_real_nonsquare){
	namespace blas = multi::blas;
	multi::array<double, 2> const a = {
		{ 1, 3, 1},
		{ 9, 7, 1},
	};
	multi::array<double, 2> const b = {
		{ 11, 12, 1},
		{  7, 19, 1},
		{  1,  1, 1}
	};
	{
		multi::array<double, 2> c({2, 3});
		using multi::blas::gemm;
		blas::gemm(1., a, b, 0., c); // c=ab, c⸆=b⸆a⸆
		BOOST_REQUIRE( c[1][2] == 17 );
	}
}

using complex = std::complex<double>; complex const I{0,1};

BOOST_AUTO_TEST_CASE(multi_blas_gemm_nh){
	namespace blas = multi::blas;
	multi::array<complex, 2> const a = {
		{1.-2.*I, 9.-1.*I},
		{2.+3.*I, 1.-2.*I}
	};
	{
		multi::array<complex, 2> c({2, 2});
		blas::gemm(1., a, blas::H(a), 0., c); // c=aa†, c†=aa†
		BOOST_TEST_REQUIRE( c[1][0] == 7.-10.*I );
		BOOST_TEST_REQUIRE( c[0][1] == 7.+10.*I );
	}
}

BOOST_AUTO_TEST_CASE(multi_blas_gemm_elongated){
	using complex = std::complex<double>; complex const I{0,1};
	multi::array<complex, 2> const a = {
		{1.-2.*I, 9.-1.*I}
	};
	{
		multi::array<complex, 2> c({1, 1});
		using multi::blas::gemm;
		using multi::blas::hermitized;
		gemm(1., a, hermitized(a), 0., c); // c=aa†, c†=aa†
		BOOST_REQUIRE( c[0][0] == 87. + 0.*I );
	}
}

BOOST_AUTO_TEST_CASE(multi_adaptors_blas_gemm_complex_3x1_3x1_bisbis){
	using complex = std::complex<double>; complex const I{0,1};
	multi::array<complex, 2> const a = {
		{1. + 2.*I},
		{9. - 1.*I},
		{1. + 1.*I}
	};
	multi::array<complex, 2> const b = {
		{ 11. - 2.*I, 7. - 3.*I, 8. - 1.*I}
	};
	{
		auto ar = rotated(a).decay();
		multi::array<complex, 2> c({1, 1});
		using multi::blas::hermitized;
		using multi::blas::gemm;
		using multi::blas::is_c_ordering;

		BOOST_REQUIRE( size(hermitized(a)) == 1 );
		BOOST_REQUIRE( size(hermitized(b)[0]) == 1 );

		gemm(1., hermitized(a), hermitized(b), 0., c); // c=ab, c⸆=b⸆a⸆
		BOOST_REQUIRE( c[0][0] == 84.+7.*I );
	}
}

BOOST_AUTO_TEST_CASE(multi_adaptors_blas_gemm_real_empty){
	multi::array<double, 2> const a({0, 5});
	BOOST_REQUIRE( size(a) == 0 );
	BOOST_REQUIRE( size(rotated(a)) == 5 );
	BOOST_REQUIRE( a.empty() );
//	assert( rotated(a).empty() );
	multi::array<double, 2> const b;
	{
		multi::array<double, 2> c;//({3, 2});
		using multi::blas::gemm;
//		gemm(1., a, b, 0., c); // c=ab, c⸆=b⸆a⸆
	}
}

BOOST_AUTO_TEST_CASE(multi_adaptors_blas_gemm_real_nonsquare2){
	multi::array<double, 2> const a = {
		{ 1, 3},
		{ 9, 7},
		{ 1, 1}
	};
	multi::array<double, 2> const b = {
		{ 11, 12},
		{  7, 19}
	};
	{
		multi::array<double, 2> c({3, 2});
		using multi::blas::gemm;
		gemm(1., a, b, 0., c); // c=ab, c⸆=b⸆a⸆
		BOOST_REQUIRE( c[2][1] == 31 );
	}
	{
		multi::array<double, 2> c({2, 3});
		using multi::blas::gemm;
		gemm(1., a, b, 0., rotated(c)); // c=ab, c⸆=b⸆a⸆
		BOOST_REQUIRE( c[1][2] == 31 );
	}
	{
		auto ar = rotated(a).decay();
		multi::array<double, 2> c({3, 2});
		using multi::blas::gemm;
		gemm(1., rotated(ar), b, 0., c); // c=ab, c⸆=b⸆a⸆
		BOOST_REQUIRE( c[2][1] == 31 );
	}
	{
		auto ar = rotated(a).decay();
		multi::array<double, 2> c({2, 3});
		using multi::blas::gemm;
		gemm(1., rotated(ar), b, 0., rotated(c)); // c=ab, c⸆=b⸆a⸆
		BOOST_REQUIRE( c[1][2] == 31 );
	}
}

BOOST_AUTO_TEST_CASE(multi_adaptors_blas_gemm_real_2x3_3x2){
	multi::array<double, 2> const a = {
		{ 1, 3},
		{ 9, 4},
		{ 1, 5}
	};
	multi::array<double, 2> const b = {
		{ 11, 12},
		{  7, 19},
		{  8, 1 }
	};
	{
		multi::array<double, 2> c({2, 2});
		using multi::blas::gemm;
		gemm(1., rotated(a), b, 0., c); // c=a⸆b, c⸆=b⸆a
		BOOST_REQUIRE( c[1][0] == 101 );
		gemm(1., rotated(a), b, 0., rotated(c)); // c=a⸆b, c⸆=b⸆a
		BOOST_REQUIRE( c[0][1] == 101 );
	}
}

BOOST_AUTO_TEST_CASE(multi_adaptors_blas_gemm_real_1x3_3x2){
	multi::array<double, 2> const a = {
		{1, 9, 1}
	};
	multi::array<double, 2> const b = {
		{ 11, 12},
		{  7, 19},
		{  8, 1 }
	};
	{
		multi::array<double, 2> c({1, 2});
		using multi::blas::gemm;
		gemm(1., a, b, 0., c); // c=a⸆b, c⸆=b⸆a
		BOOST_REQUIRE( c[0][1] == 184 );
	}
	{
		multi::array<double, 2> c({2, 1});
		auto ar = rotated(a).decay();
		using multi::blas::gemm;
		gemm(1., rotated(ar), b, 0., rotated(c)); // c=a⸆b, c⸆=b⸆a
		BOOST_REQUIRE( c[1][0] == 184 );
	}
}

BOOST_AUTO_TEST_CASE(multi_adaptors_blas_gemm_real_2x3_3x1){
	multi::array<double, 2> const a = {
		{1, 9, 1},
		{3, 4, 5}
	};
	multi::array<double, 2> const b = {
		{ 11},
		{  7},
		{  8}
	};

	{
		multi::array<double, 2> c({1, 2});
		using multi::blas::gemm;
		gemm(1., a, b, 0., rotated(c)); // c⸆=ab, c=b⸆a⸆
		BOOST_REQUIRE( rotated(c)[1][0] == 101 );
		BOOST_REQUIRE( c[0][1] == 101 );
	}
	{
		multi::array<double, 2> c({2, 1});
		using multi::blas::gemm;
		gemm(1., a, b, 0., c); // c⸆=ab, c=b⸆a⸆
		BOOST_REQUIRE( rotated(c)[0][1] == 101 );
		BOOST_REQUIRE( c[1][0] == 101 );
	}
	{
		multi::array<double, 2> c({1, 2});
		auto ar = rotated(a).decay();
		using multi::blas::gemm;
		gemm(1., rotated(ar), b, 0., rotated(c)); // c⸆=ab, c⸆=b⸆a⸆
		BOOST_REQUIRE( c[0][1] == 101 );
	}
}

BOOST_AUTO_TEST_CASE(multi_adaptors_blas_gemm_real_1x3_3x1){
	multi::array<double, 2> const a = {
		{1, 9, 1}
	};
	multi::array<double, 2> const b = {
		{ 11},
		{  7},
		{  8}
	};
	{
		multi::array<double, 2> c({1, 1});
		using multi::blas::gemm;
		gemm(1., a, b, 0., c); // c=ab, c⸆=b⸆a⸆
		BOOST_REQUIRE( c[0][0] == 82 );
	}
	{
		multi::array<double, 2> c({1, 1});
		auto ar = rotated(a).decay();
		using multi::blas::gemm;
		gemm(1., rotated(ar), b, 0., c); // c=ab, c⸆=ba
		BOOST_REQUIRE( c[0][0] == 82 );
	}
	{
		multi::array<double, 2> c({1, 1});
		auto br = rotated(b).decay();
		using multi::blas::gemm;
		gemm(1., a, rotated(br), 0., c);
		BOOST_REQUIRE( c[0][0] == 82 );
	}
	{
		multi::array<double, 2> c({1, 1});
		auto br = rotated(b).decay();
		using multi::blas::gemm;
		using multi::blas::hermitized;
		gemm(1., a, hermitized(br), 0., c);
		BOOST_REQUIRE( c[0][0] == 82 );
	}
}

BOOST_AUTO_TEST_CASE(multi_adaptors_blas_gemm_complex_square){
	using complex = std::complex<double>; complex const I{0,1};
	multi::array<complex, 2> const a = {
		{ 1.+3.*I, 3.+2.*I},
		{ 9.+1.*I, 7.+1.*I},
	};
	multi::array<complex, 2> const b = {
		{11.+2.*I, 12.+4.*I},
		{ 7.+1.*I, 19.-9.*I},
	};
	{
		multi::array<complex, 2> c({2, 2});
		using multi::blas::gemm;
		gemm(1., a, b, 0., c); // c=ab, c⸆=b⸆a⸆
		BOOST_REQUIRE( c[1][0] == 145. + 43.*I );
	}
	{
		multi::array<complex, 2> c({2, 2});
		using multi::blas::transposed;
		using multi::blas::gemm;
		gemm(1., transposed(a), b, 0., c); // c=a⸆b, c⸆=b⸆a
		BOOST_REQUIRE(( c[1][1] == 170.-8.*I and c[1][0] == 77.+42.*I ));
	}
	{
		multi::array<complex, 2> c({2, 2});
		using multi::blas::transposed;
		using multi::blas::gemm;
		gemm(1., a, transposed(b), 0., c); // c=ab⸆, c⸆=ba⸆
		BOOST_REQUIRE( c[1][0] == 177.+69.*I );
	}
	{
		multi::array<complex, 2> c({2, 2});
//		using multi::blas::transposed;
//		using multi::blas::gemm;
//		gemm(1., transposed(a), transposed(b), 0., c); // c=a⸆b⸆, c⸆=ba
		using namespace multi::blas;
		gemm(1., T(a), T(b), 0., c); // c=a⸆b⸆, c⸆=ba
		BOOST_REQUIRE( c[1][0] == 109. + 68.*I );
	}
	{
		multi::array<complex, 2> c({2, 2});
		using multi::blas::transposed;
		using multi::blas::gemm;
		gemm(1., transposed(a), transposed(b), 0., transposed(c)); // c⸆=a⸆b⸆, c=ba
		BOOST_REQUIRE( c[0][1] == 109.+68.*I );
	}
}

BOOST_AUTO_TEST_CASE(multi_adaptors_blas_gemm_complex_1x3_3x1){
	using complex = std::complex<double>; complex const I{0,1};
	multi::array<complex, 2> const a = {
		{1. + 2.*I, 9. - 1.*I, 1. + 1.*I}
	};
	multi::array<complex, 2> const b = {
		{ 11. - 2.*I},
		{  7. - 3.*I},
		{  8. - 1.*I}
	};
	{
		multi::array<complex, 2> c({1, 1});
		using multi::blas::gemm;
		gemm(1., a, b, 0., c); // c=ab, c⸆=b⸆a⸆
		BOOST_REQUIRE( c[0][0] == 84.-7.*I );
	}
	{
		multi::array<complex, 2> c({1, 1});
		auto ar = rotated(a).decay();
		using multi::blas::gemm;
		gemm(1., rotated(ar), b, 0., c); // c=ab, c⸆=ba
		BOOST_REQUIRE( c[0][0] == 84.-7.*I );
	}
	{
		multi::array<complex, 2> c({1, 1});
		auto br = rotated(b).decay();
		using multi::blas::gemm;
		gemm(1., a, rotated(br), 0., c);
		BOOST_REQUIRE( c[0][0] == 84.-7.*I );
	}
	{
		multi::array<complex, 2> c({1, 1});
		auto br = rotated(b).decay();
		using multi::blas::gemm;
		using multi::blas::hermitized;
	//	gemm(1., a, hermitized(br), 0., rotated(c));
	//	BOOST_REQUIRE( c[0][0] == 84.-7.*I );
	}
}

BOOST_AUTO_TEST_CASE(multi_adaptors_blas_gemm_complex_hermitized_square){
	namespace blas = multi::blas;
	multi::array<complex, 2> const a = {
		{ 1.+3.*I, 3.+2.*I},
		{ 9.+1.*I, 7.+1.*I},
	};
	multi::array<complex, 2> const b = {
		{11.+2.*I, 12.+4.*I},
		{ 7.+1.*I, 19.-9.*I},
	};
	{
		multi::array<complex, 2> c({2, 2});
		blas::gemm(1., a, b, 0., c); // c=ab, c†=b†a†
		BOOST_REQUIRE( c[1][0] == 145. + 43.*I );
	}
	{
		multi::array<complex, 2> c({2, 2});
		blas::gemm(1., blas::H(a), blas::H(b), 0., c); // c=a†b†, c†=ba
		BOOST_REQUIRE( c[1][0] == 109. - 68.*I );
	}
	{
		multi::array<complex, 2> c({2, 2});
		blas::gemm(1., blas::H(a), blas::H(b), 0., blas::H(c)); // c†=a†b†, c=ba
		BOOST_REQUIRE( c[1][0] == 184. - 40.*I );
	}
	{
		multi::array<complex, 2> c({2, 2});
		blas::gemm(1., blas::H(a), b, 0., c); // c=a†b, c†=b†a
		BOOST_REQUIRE( c[1][0] == 87. - 16.*I );
	}
	{
		multi::array<complex, 2> c({2, 2});
		blas::gemm(1., a, blas::H(b), 0., c); // c=ab†, c†=ba†
		BOOST_REQUIRE( c[1][0] == 189. - 23.*I );
	}
	{
		multi::array<complex, 2> c({2, 2});
		blas::gemm(1., blas::H(a), blas::H(b), 0., c); // c=a†b†, c†=ba
		BOOST_REQUIRE( c[1][0] == 109. - 68.*I);
	}
	{
		multi::array<complex, 2> c({2, 2});
	//	blas::gemm(1., hermitized(a), hermitized(b), 0., rotated(c)); // c⸆=a†b†, c=b*a*
	//	print(rotated(c));
	//	BOOST_REQUIRE( c[0][1] == 109. - 68.*I );
	}
}

BOOST_AUTO_TEST_CASE(multi_adaptors_blas_gemm_complex_3x1_3x1){
	namespace blas = multi::blas;
	multi::array<complex, 2> const a = {
		{1. + 2.*I},
		{9. - 1.*I},
		{1. + 1.*I}
	};
	multi::array<complex, 2> const b = {
		{ 11. - 2.*I},
		{  7. - 3.*I},
		{  8. - 1.*I}
	};
	{
		multi::array<complex, 2> c({1, 1});
		blas::gemm(1., blas::H(a), b, 0., c); // c=ab, c⸆=b⸆a⸆
		BOOST_REQUIRE( c[0][0] == 80.-53.*I );
	}
	{
		multi::array<complex, 2> c({1, 1});
		multi::array<complex, 2> ha = blas::hermitized(a);
		blas::gemm(1., ha, b, 0., c);
		BOOST_REQUIRE( c[0][0] == 80.-53.*I );
		blas::gemm(1., blas::H(b), a, 0., c);
		BOOST_REQUIRE( c[0][0] == 80.+53.*I );
	}
}
BOOST_AUTO_TEST_CASE(multi_adaptors_blas_gemm_complex_1x3_3x2){
	namespace blas = multi::blas;
	multi::array<complex, 2> const a = {
		{1. + 2.*I, 9. - 1.*I, 1. + 1.*I}
	};
	multi::array<complex, 2> const b = {
		{ 11. - 2.*I, 5. + 2.*I},
		{  7. - 3.*I, 2. + 1.*I},
		{  8. - 1.*I, 1. + 1.*I}
	};
	{
		multi::array<complex, 2> c({1, 2});
		blas::gemm(1., a, b, 0., c); // c=ab, c⸆=b⸆a⸆
		BOOST_REQUIRE( c[0][1] == 20.+21.*I );
	}
	{
		auto ar = rotated(a).decay();
		multi::array<complex, 2> c({1, 2});
		blas::gemm(1., blas::H(ar), b, 0., c); // c=ab, c⸆=b⸆a⸆
		BOOST_REQUIRE( c[0][1] == 28.+3.*I );
	}
}

BOOST_AUTO_TEST_CASE(multi_adaptors_blas_gemm_complex_3x1_3x2){
	namespace blas = multi::blas;
	multi::array<complex, 2> const a = {
		{1. + 2.*I},
		{9. - 1.*I},
		{1. + 1.*I}
	};
	multi::array<complex, 2> const b = {
		{ 11. - 2.*I, 5. + 2.*I},
		{  7. - 3.*I, 2. + 1.*I},
		{  8. - 1.*I, 1. + 1.*I}
	};
	{
		auto ar = rotated(a).decay();
		multi::array<complex, 2> c({1, 2});
		blas::gemm(1., blas::H(a), b, 0., c); // c=ab, c⸆=b⸆a⸆
		BOOST_REQUIRE( c[0][1] == 28.+3.*I );
	}
}

BOOST_AUTO_TEST_CASE(multi_adaptors_blas_gemm_complex_3x2_3x2){
	namespace blas = multi::blas;
	multi::array<complex, 2> const a = {
		{1. + 2.*I, 5. + 2.*I},
		{9. - 1.*I, 9. + 1.*I},
		{1. + 1.*I, 2. + 2.*I}
	};
	multi::array<complex, 2> const b = {
		{ 11. - 2.*I, 5. + 2.*I},
		{  7. - 3.*I, 2. + 1.*I},
		{  8. - 1.*I, 1. + 1.*I}
	};
	{
		auto ar = rotated(a).decay();
		multi::array<complex, 2> c({2, 2});
		blas::gemm(1., blas::H(a), b, 0., c); // c=ab, c⸆=b⸆a⸆
		BOOST_REQUIRE( c[1][0] == 125.-84.*I );
	}
}

BOOST_AUTO_TEST_CASE(multi_adaptors_blas_gemm_complex_3x2_3x1){
	namespace blas = multi::blas;
	multi::array<complex, 2> const a = {
		{1. + 2.*I, 5. + 2.*I},
		{9. - 1.*I, 9. + 1.*I},
		{1. + 1.*I, 2. + 2.*I}
	};
	multi::array<complex, 2> const b = {
		{ 11. - 2.*I},
		{  7. - 3.*I},
		{  8. - 1.*I}
	};
	{
		auto ar = rotated(a).decay();
		multi::array<complex, 2> c({2, 1});
		blas::gemm(1., blas::H(a), b, 0., c); // c=ab, c⸆=b⸆a⸆
		BOOST_REQUIRE( c[1][0] == 125.-84.*I );
	}
}

BOOST_AUTO_TEST_CASE(multi_adaptors_blas_gemm_complex_3x1_3x1_bis){
	namespace blas = multi::blas;
	multi::array<complex, 2> const a = {
		{1. + 2.*I},
		{9. - 1.*I},
		{1. + 1.*I}
	};
	multi::array<complex, 2> const b = {
		{ 11. - 2.*I},
		{  7. - 3.*I},
		{  8. - 1.*I}
	};
	{
		auto ar = rotated(a).decay();
		multi::array<complex, 2> c({1, 1});
		blas::gemm(1., blas::H(a), b, 0., c); // c=ab, c⸆=b⸆a⸆
		BOOST_REQUIRE( c[0][0] == 80.-53.*I );
	}
}

BOOST_AUTO_TEST_CASE(multi_adaptors_blas_gemm_real_square_automatic){
	namespace blas = multi::blas;
	multi::array<double, 2> const a = {
		{ 1., 3.},
		{ 9., 7.},
	};
	multi::array<double, 2> const b = {
		{ 11., 12.},
		{  7., 19.},
	};
	{
		multi::array<double, 2> c({2, 2});
		blas::gemm(1., a, b, 0., c); // c=ab, c⸆=b⸆a⸆
		BOOST_REQUIRE( c[1][0] == 148 and c[1][1] == 241 );
	}
	{
		multi::array<double, 2> c({2, 2});
		blas::gemm(1., a, blas::T(b), 0., c); // c=ab, c⸆=b⸆a⸆
		BOOST_REQUIRE( c[1][1] == 196. );
	}
	{
		multi::array<double, 2> c({2, 2});
		blas::gemm(1., blas::T(a), b, 0., c); // c=ab, c⸆=b⸆a⸆
		BOOST_REQUIRE(( c[1][1] == 169. and c[1][0] == 82. ));
	}
	{
		multi::array<double, 2> c({2, 2});
		blas::gemm(1., blas::T(a), blas::T(b), 0., c); // c=ab, c⸆=b⸆a⸆
		BOOST_REQUIRE( c[1][1] == 154. );
	}
}

BOOST_AUTO_TEST_CASE(multi_adaptors_blas_gemm_real_nonsquare_automatic){
	namespace blas = multi::blas;
	multi::array<double, 2> const a = {
		{ 1., 3., 1.},
		{ 9., 7., 1.},
	};
	multi::array<double, 2> const b = {
		{ 11., 12., 1.},
		{  7., 19., 1.},
		{  1.,  1., 1.}
	};
	{
		multi::array<double, 2> c({2, 3});
		blas::gemm(1., a, b, 0., c); // c=ab, c⸆=b⸆a⸆
		BOOST_REQUIRE( c[1][2] == 17. );
	}
}

BOOST_AUTO_TEST_CASE(multi_adaptors_blas_gemm_complex_square_automatic){
	namespace blas = multi::blas;
	multi::array<complex, 2> const a = {
		{ 1. + 2.*I, 3. - 3.*I},
		{ 9. + 1.*I, 7. + 4.*I},
	};
	multi::array<complex, 2> const b = {
		{ 11. + 1.*I, 12. + 1.*I},
		{  7. + 8.*I, 19. - 2.*I},
	};
	{
		multi::array<complex, 2> c({2, 2});
		blas::gemm(1., a, b, 0., c); // c=ab, c⸆=b⸆a⸆
		BOOST_REQUIRE( c[1][0] == complex(115, 104) );
	}
	{
		multi::array<complex, 2> c({2, 2});
		blas::gemm(1., a, blas::T(b), 0., c); // c=ab⸆, c⸆=ba⸆
		BOOST_REQUIRE( c[1][0] == complex(178, 75) );
	}
	{
		multi::array<complex, 2> c({2, 2});
		blas::gemm(1., blas::T(a), b, 0., c); // c=a⸆b, c⸆=b⸆a
		BOOST_REQUIRE(( c[1][1] == complex(180, 29) and c[1][0] == complex(53, 54) ));
	}
	{
		multi::array<complex, 2> c({2, 2});
		blas::gemm(1., blas::T(a), blas::T(b), 0., c); // c=ab, c⸆=b⸆a⸆
		BOOST_REQUIRE(( c[1][1] == complex(186, 65) and c[1][0] == complex(116, 25) ));
	}
	{
		multi::array<complex, 2> c({2, 2});
		blas::gemm(1., a, b, 0., c); // c=ab, c⸆=b⸆a⸆
		BOOST_REQUIRE( c[1][0] == complex(115, 104) );
	}
	{
		multi::array<complex, 2> c({2, 2});
		blas::gemm(1., blas::H(a), b, 0., c); // c=a†b, c†=b†a
		BOOST_REQUIRE( c[1][0] == complex(111, 64) and c[1][1] == complex(158, -51) );
	}
	{
		multi::array<complex, 2> c({2, 2});
		blas::gemm(1., a, blas::H(b), 0., c); // c=ab†, c†=ba†
		BOOST_REQUIRE( c[1][0] == complex(188, 43) and c[1][1] == complex(196, 25) );
	}
	{
		multi::array<complex, 2> c({2, 2});
		blas::gemm(1., blas::H(a), blas::H(b), 0., c); // c=a†b†, c†=ba
		BOOST_REQUIRE( c[1][0] == complex(116, -25) and c[1][1] == complex(186, -65) );
	}
	{
		multi::array<complex, 2> c({2, 2});
		blas::gemm(1., blas::T(a), blas::H(b), 0., c); // c=a⸆b†, c†=ba⸆†
		BOOST_REQUIRE( c[1][0] == complex(118, 5) and c[1][1] == complex(122, 45) );
	}
	{
		multi::array<complex, 2> c({2, 2});
		blas::gemm(1., blas::T(a), blas::T(b), 0., c); // c=a⸆b⸆, c⸆=ba
		BOOST_REQUIRE( c[1][0] == complex(116, 25) and c[1][1] == complex(186, 65) );
	}
}

BOOST_AUTO_TEST_CASE(multi_adaptors_blas_gemm_complex_nonsquare_automatic){
	namespace blas = multi::blas;
	multi::array<complex, 2> const a = {
		{ 1. + 2.*I, 3. - 3.*I, 1.-9.*I},
		{ 9. + 1.*I, 7. + 4.*I, 1.-8.*I},
	};
	multi::array<complex, 2> const b = {
		{ 11.+1.*I, 12.+1.*I, 4.+1.*I, 8.-2.*I},
		{  7.+8.*I, 19.-2.*I, 2.+1.*I, 7.+1.*I},
		{  5.+1.*I,  3.-1.*I, 3.+8.*I, 1.+1.*I}
	};
	{
		multi::array<complex, 2> c({2, 4});
		blas::gemm(1., a, b, 0., c); // c=ab, c⸆=b⸆a⸆
		BOOST_REQUIRE( c[1][2] == complex(112, 12) );
	}
}

#endif
#endif


/*
 Copyright (C) 2018 Quaternion Risk Management Ltd
 All rights reserved.

 This file is part of ORE, a free-software/open-source library
 for transparent pricing and risk analysis - http://opensourcerisk.org

 ORE is free software: you can redistribute it and/or modify it
 under the terms of the Modified BSD License.  You should have received a
 copy of the license along with this program.
 The license is also available online at <http://opensourcerisk.org>

 This program is distributed on the basis that it will form a useful
 contribution to risk analytics and model standardisation, but WITHOUT
 ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
 FITNESS FOR A PARTICULAR PURPOSE. See the license for more details.
*/

#include <boost/make_shared.hpp>
// clang-format off
#include <boost/test/unit_test.hpp>
#include <boost/test/data/test_case.hpp>
// clang-format on
#include <oret/datapaths.hpp>
#include <oret/toplevelfixture.hpp>

#include <ql/math/comparison.hpp>
#include <ql/math/interpolations/linearinterpolation.hpp>
#include <ql/math/interpolations/loginterpolation.hpp>
#include <ql/time/daycounters/actualactual.hpp>

#include <ored/configuration/curveconfigurations.hpp>
#include <ored/marketdata/commoditycurve.hpp>
#include <ored/marketdata/csvloader.hpp>
#include <ored/marketdata/curvespec.hpp>
#include <ored/marketdata/loader.hpp>
#include <ored/marketdata/todaysmarket.hpp>
#include <ored/utilities/csvfilereader.hpp>
#include <ored/utilities/to_string.hpp>

using namespace std;
using namespace boost::unit_test_framework;
using namespace QuantLib;
using namespace QuantExt;
using namespace ore::data;

namespace bdata = boost::unit_test::data;

namespace {

// List of curve configuration file names for data test case below
vector<string> curveConfigFiles = {"curveconfig_linear.xml",    "curveconfig_linear_flat.xml",
                                   "curveconfig_loglinear.xml", "curveconfig_loglinear_flat.xml",
                                   "curveconfig_cubic.xml",     "curveconfig_cubic_flat.xml"};

// The expected commodity curve
map<Date, Real> expectedCurve = {{Date(29, Jul, 2019), 1417.8998900}, {Date(30, Jul, 2019), 1417.9999450},
                                 {Date(31, Jul, 2019), 1418.1000000}, {Date(1, Aug, 2019), 1418.2000550},
                                 {Date(30, Aug, 2019), 1421.1016535}, {Date(30, Sep, 2019), 1424.1312750}};

// Pillars for interpolated curve tests (purposely leave out elements below spot to test interpolation there)
map<Date, Real> expectedInterpCurvePillars = {{Date(31, Jul, 2019), 1418.1000000},
                                              {Date(1, Aug, 2019), 1418.2000550},
                                              {Date(30, Aug, 2019), 1421.1016535},
                                              {Date(30, Sep, 2019), 1424.1312750}};

// Expected results for extrapolation below spot, interpolation and extrapolation beyond max date for the
// various interpolation methods
vector<Date> offPillarDates = {Date(29, Jul, 2019), Date(15, Sep, 2019), Date(1, Nov, 2019)};

map<string, vector<Real>> expectedInterpCurveOffPillars = {
    {"curveconfig_linear.xml", {1417.89989, 1422.6653291129, 1427.2586262258}},
    {"curveconfig_linear_flat.xml", {1418.1, 1422.6653291129, 1424.131275}},
    {"curveconfig_loglinear.xml", {1417.89991117635, 1422.66452345277, 1427.26540106042}},
    {"curveconfig_loglinear_flat.xml", {1418.1, 1422.66452345277, 1424.131275}},
    {"curveconfig_cubic.xml", {1417.89988981896, 1422.67192914531, 1427.25983144911}},
    {"curveconfig_cubic_flat.xml", {1418.1, 1422.67192914531, 1424.131275}}};

boost::shared_ptr<CommodityCurve> createCurve(const string& inputDir,
                                              const string& curveConfigFile = "curveconfig.xml") {

    // As of date
    Date asof(29, Jul, 2019);

    Conventions conventions;
    string filename = inputDir + "/conventions.xml";
    conventions.fromFile(TEST_INPUT_FILE(filename));
    CurveConfigurations curveConfigs;
    filename = inputDir + "/" + curveConfigFile;
    curveConfigs.fromFile(TEST_INPUT_FILE(filename));
    filename = inputDir + "/market.txt";
    CSVLoader loader(TEST_INPUT_FILE(filename), TEST_INPUT_FILE("fixings.txt"), false);

    // Commodity curve spec
    CommodityCurveSpec curveSpec("USD", "PM:XAUUSD");

    // Check commodity curve construction works
    boost::shared_ptr<CommodityCurve> curve;
    BOOST_REQUIRE_NO_THROW(curve =
                               boost::make_shared<CommodityCurve>(asof, curveSpec, loader, curveConfigs, conventions));

    return curve;
}

boost::shared_ptr<TodaysMarket> createTodaysMarket(const Date& asof, const string& inputDir) {

    Conventions conventions;
    conventions.fromFile(TEST_INPUT_FILE(string(inputDir + "/conventions.xml")));

    CurveConfigurations curveConfigs;
    curveConfigs.fromFile(TEST_INPUT_FILE(string(inputDir + "/curveconfig.xml")));

    TodaysMarketParameters todaysMarketParameters;
    todaysMarketParameters.fromFile(TEST_INPUT_FILE(string(inputDir + "/todaysmarket.xml")));

    string fixingsFile = inputDir + "/fixings_" + to_string(io::iso_date(asof)) + ".txt";
    CSVLoader loader(TEST_INPUT_FILE(string(inputDir + "/market.txt")), TEST_INPUT_FILE(fixingsFile), false);

    return boost::make_shared<TodaysMarket>(asof, todaysMarketParameters, loader, curveConfigs, conventions);
}

void checkCurve(const boost::shared_ptr<PriceTermStructure>& priceCurve, const map<Date, Real>& expectedValues) {

    for (const auto& kv : expectedValues) {
        Real price = priceCurve->price(kv.first);
        BOOST_CHECK_CLOSE(price, kv.second, 1e-12);
    }
}

} // namespace

BOOST_FIXTURE_TEST_SUITE(OREDataTestSuite, ore::test::TopLevelFixture)

BOOST_AUTO_TEST_SUITE(CommodityCurveTests)

BOOST_AUTO_TEST_CASE(testCommodityCurveTenorBasedOnTnPoints) {

    BOOST_TEST_MESSAGE("Testing commodity curve building with tenor based points quotes including ON and TN");

    boost::shared_ptr<CommodityCurve> curve = createCurve("tenor_based_on_tn_points");
    checkCurve(curve->commodityPriceCurve(), expectedCurve);
}

BOOST_AUTO_TEST_CASE(testCommodityCurveFixedDatePoints) {

    BOOST_TEST_MESSAGE("Testing commodity curve building with fixed date quotes");

    boost::shared_ptr<CommodityCurve> curve = createCurve("fixed_date_points");
    checkCurve(curve->commodityPriceCurve(), expectedCurve);
}

// Testing various interpolation methods
BOOST_DATA_TEST_CASE(testCommodityInterpolations, bdata::make(curveConfigFiles), curveConfigFile) {

    BOOST_TEST_MESSAGE("Testing with configuration file: " << curveConfigFile);

    boost::shared_ptr<CommodityCurve> curve = createCurve("different_interpolations", curveConfigFile);
    checkCurve(curve->commodityPriceCurve(), expectedInterpCurvePillars);

    BOOST_REQUIRE(expectedInterpCurveOffPillars.count(curveConfigFile) == 1);
    for (Size i = 0; i < offPillarDates.size(); i++) {
        Real price = curve->commodityPriceCurve()->price(offPillarDates[i]);
        Real expPrice = expectedInterpCurveOffPillars.at(curveConfigFile)[i];
        BOOST_CHECK_CLOSE(price, expPrice, 1e-12);
    }
}

// Basis test cases for data test below.
struct BasisTestCase {
    Date asof;
    string name;
    string curveName;
};

// List of basis test case directories for the data test case below.
vector<BasisTestCase> basisTestCases = {{Date(30, Sep, 2019), "wti_midland_cm", "NYMEX:FF"},
                                        {Date(30, Sep, 2019), "wti_midland_tm", "NYMEX:WTT"},
                                        {Date(30, Sep, 2019), "wti_midland_cm_base_ave", "NYMEX:FF"},
                                        {Date(30, Sep, 2019), "houston_ship_channel", "ICE:HXS"},
                                        {Date(23, Jan, 2020), "wti_midland_cm", "NYMEX:FF"},
                                        {Date(23, Jan, 2020), "wti_midland_tm", "NYMEX:WTT"},
                                        {Date(23, Jan, 2020), "wti_midland_cm_base_ave", "NYMEX:FF"},
                                        {Date(23, Jan, 2020), "houston_ship_channel", "ICE:HXS"}};

// Needed for BOOST_DATA_TEST_CASE below as it writes out the case.
ostream& operator<<(ostream& os, BasisTestCase testCase) {
    return os << "[" << io::iso_date(testCase.asof) << "," << testCase.name << "," << testCase.curveName << "]";
}

BOOST_DATA_TEST_CASE(testCommodityBasisCurve, bdata::make(basisTestCases), basisTestCase) {

    BOOST_TEST_MESSAGE("Testing commodity basis curve building " << basisTestCase << "...");

    Settings::instance().evaluationDate() = basisTestCase.asof;
    string dir = "basis/" + basisTestCase.name;

    boost::shared_ptr<TodaysMarket> tm;
    BOOST_REQUIRE_NO_THROW(tm = createTodaysMarket(basisTestCase.asof, dir));

    auto pts = tm->commodityPriceCurve(basisTestCase.curveName);

    for (const Date& d : pts->pillarDates()) {
        BOOST_TEST_MESSAGE(io::iso_date(d) << "," << fixed << setprecision(12) << pts->price(d));
    }

    // Tolerance for float comparison
    Real tol = 1e-12;

    // Read in the expected pillar results for the given date.
    vector<Date> expPillarDates;
    string filename = dir + "/expected_" + to_string(io::iso_date(basisTestCase.asof)) + ".csv";
    CSVFileReader reader(TEST_INPUT_FILE(filename), true, ",");
    BOOST_REQUIRE_EQUAL(reader.numberOfColumns(), 2);

    while (reader.next()) {
        // Get the expected expiry pillar date and price.
        Date expiry = parseDate(reader.get(0));
        Real price = parseReal(reader.get(1));
        expPillarDates.push_back(expiry);

        // Check the surface on the grid point.
        Real calcPrice = pts->price(expiry);
        BOOST_TEST_MESSAGE(io::iso_date(expiry) << "," << fixed << setprecision(12) << calcPrice);
        BOOST_CHECK_SMALL(price - calcPrice, tol);
    }

    vector<Date> calcPillarDates = pts->pillarDates();
    BOOST_CHECK_EQUAL_COLLECTIONS(expPillarDates.begin(), expPillarDates.end(), calcPillarDates.begin(),
                                  calcPillarDates.end());

    // Set up has flat extrapolation. Check it here.
    Real lastPrice = pts->price(calcPillarDates.back());
    Date extrapDate = calcPillarDates.back() + 1 * Years;
    Real extrapPrice = pts->price(extrapDate);
    BOOST_CHECK_SMALL(lastPrice - extrapPrice, tol);
}

BOOST_AUTO_TEST_SUITE_END()

BOOST_AUTO_TEST_SUITE_END()


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

 [begin_description]
 This file tests the use of the adams bashforth stepper.
 [end_description]

 Copyright 2011-2012 Karsten Ahnert
 Copyright 2011-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)
 */

// disable checked iterator warning for msvc

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

#define BOOST_TEST_MODULE odeint_adams_bashforth

#include <utility>

#include <boost/array.hpp>

#include <boost/test/unit_test.hpp>

#include <boost/mpl/list.hpp>
#include <boost/mpl/size_t.hpp>
#include <boost/mpl/range_c.hpp>

#include <boost/numeric/odeint/stepper/adams_bashforth.hpp>
#include <boost/numeric/odeint/stepper/runge_kutta4.hpp>

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

typedef double value_type;

struct lorenz
{
    template< class State , class Deriv , class Value >
    void operator()( const State &_x , Deriv &_dxdt , const Value &dt ) const
    {
        const value_type sigma = 10.0;
        const value_type R = 28.0;
        const value_type b = 8.0 / 3.0;

        typename boost::range_iterator< const State >::type x = boost::begin( _x );
        typename boost::range_iterator< Deriv >::type dxdt = boost::begin( _dxdt );

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

template< class State >
class rk4_decorator
{
public:

    size_t do_count;

    template< class System , class StateIn , class DerivIn , class StateOut >
    void do_step_dxdt_impl( System system , const StateIn &in , const DerivIn &dxdt , value_type t , StateOut &out , value_type dt )
    {
        m_stepper.do_step( system , in , dxdt , t , out , dt );
        ++do_count;
    }

    template< class System , class StateInOut , class DerivIn >
    void do_step_dxdt_impl( System system , StateInOut &x , const DerivIn &dxdt , value_type t , value_type dt )
    {
        m_stepper.do_step( system , x , dxdt , t , dt );
        ++do_count;
    }

    runge_kutta4< State > m_stepper;

private:

};

BOOST_AUTO_TEST_SUITE( adams_bashforth_test )

BOOST_AUTO_TEST_CASE( test_adams_bashforth_coefficients )
{
    detail::adams_bashforth_coefficients< value_type , 1 > c1;
    detail::adams_bashforth_coefficients< value_type , 2 > c2;
    detail::adams_bashforth_coefficients< value_type , 3 > c3;
    detail::adams_bashforth_coefficients< value_type , 4 > c4;
    detail::adams_bashforth_coefficients< value_type , 5 > c5;
    detail::adams_bashforth_coefficients< value_type , 6 > c6;
    detail::adams_bashforth_coefficients< value_type , 7 > c7;
    detail::adams_bashforth_coefficients< value_type , 8 > c8;
}

BOOST_AUTO_TEST_CASE( test_rotating_buffer )
{
    const size_t N = 5;
    detail::rotating_buffer< size_t , N > buffer;
    for( size_t i=0 ; i<N ; ++i ) buffer[i] = i;

    for( size_t i=0 ; i<N ; ++i )
        BOOST_CHECK_EQUAL( buffer[i] , i );

    buffer.rotate();

    for( size_t i=1 ; i<N ; ++i )
        BOOST_CHECK_EQUAL( buffer[i] , i - 1 );
    BOOST_CHECK_EQUAL( buffer[0] , size_t( N-1 ) );
}

BOOST_AUTO_TEST_CASE( test_copying )
{
    typedef boost::array< double , 1 > state_type;
    typedef adams_bashforth< 2 , state_type > stepper_type;

    stepper_type s1;
    s1.step_storage()[0].m_v[0] = 1.5;
    s1.step_storage()[1].m_v[0] = 2.25;

    stepper_type s2( s1 );
    BOOST_CHECK_CLOSE( s1.step_storage()[0].m_v[0] , s2.step_storage()[0].m_v[0] , 1.0e-14 );
    BOOST_CHECK_CLOSE( s1.step_storage()[1].m_v[0] , s2.step_storage()[1].m_v[0] , 1.0e-14 );
    BOOST_CHECK( ( &(s1.step_storage()[0]) ) != ( &(s2.step_storage()[0]) ) );

    stepper_type s3;
    state_type *p1 = &( s3.step_storage()[0].m_v ) , *p2 = &( s3.step_storage()[1].m_v );
    s3 = s1;
    BOOST_CHECK( p1 == ( &( s3.step_storage()[0].m_v ) ) );
    BOOST_CHECK( p2 == ( &( s3.step_storage()[1].m_v ) ) );

    BOOST_CHECK_CLOSE( s1.step_storage()[0].m_v[0] , s3.step_storage()[0].m_v[0] , 1.0e-14 );
    BOOST_CHECK_CLOSE( s1.step_storage()[1].m_v[0] , s3.step_storage()[1].m_v[0] , 1.0e-14 );
}

typedef boost::mpl::range_c< size_t , 1 , 6 > vector_of_steps;

BOOST_AUTO_TEST_CASE_TEMPLATE( test_init_and_steps , step_type , vector_of_steps )
{
    const static size_t steps = step_type::value;
    typedef boost::array< value_type , 3 > state_type;

    adams_bashforth< steps , state_type > stepper;
    state_type x = {{ 10.0 , 10.0 , 10.0 }};
    const value_type dt = 0.01;
    value_type t = 0.0;

    stepper.initialize( lorenz() , x , t , dt );
    BOOST_CHECK_CLOSE( t , value_type( steps - 1 ) * dt , 1.0e-14 );

    stepper.do_step( lorenz() , x , t , dt );
}

BOOST_AUTO_TEST_CASE( test_instantiation )
{
    typedef boost::array< double , 3 > state_type;
    adams_bashforth< 1 , state_type > s1;
    adams_bashforth< 2 , state_type > s2;
    adams_bashforth< 3 , state_type > s3;
    adams_bashforth< 4 , state_type > s4;
    adams_bashforth< 5 , state_type > s5;
    adams_bashforth< 6 , state_type > s6;
    adams_bashforth< 7 , state_type > s7;
    adams_bashforth< 8 , state_type > s8;

    state_type x = {{ 10.0 , 10.0 , 10.0 }};
    value_type t = 0.0 , dt = 0.01;
    s1.do_step( lorenz() , x , t , dt );
    s2.do_step( lorenz() , x , t , dt );
    s3.do_step( lorenz() , x , t , dt );
    s4.do_step( lorenz() , x , t , dt );
    s5.do_step( lorenz() , x , t , dt );
    s6.do_step( lorenz() , x , t , dt );
//    s7.do_step( lorenz() , x , t , dt );
//    s8.do_step( lorenz() , x , t , dt );
}

BOOST_AUTO_TEST_CASE( test_auto_initialization )
{
    typedef boost::array< double , 3 > state_type;
    state_type x = {{ 10.0 , 10.0 , 10.0 }};

    adams_bashforth< 3 , state_type , value_type , state_type , value_type , range_algebra , default_operations ,
                     initially_resizer , rk4_decorator< state_type > > adams;

    adams.initializing_stepper().do_count = 0;
    adams.do_step( lorenz() , x , 0.0 , x , 0.1 );
    BOOST_CHECK_EQUAL( adams.initializing_stepper().do_count , size_t( 1 ) );

    adams.do_step( lorenz() , x , 0.0 , x , 0.1 );
    BOOST_CHECK_EQUAL( adams.initializing_stepper().do_count , size_t( 2 ) );

    adams.do_step( lorenz() , x , 0.0 , x , 0.1 );
    BOOST_CHECK_EQUAL( adams.initializing_stepper().do_count , size_t( 2 ) );

    adams.do_step( lorenz() , x , 0.0 , x , 0.1 );
    BOOST_CHECK_EQUAL( adams.initializing_stepper().do_count , size_t( 2 ) );

    adams.reset();

    adams.do_step( lorenz() , x , 0.0 , x , 0.1 );
    BOOST_CHECK_EQUAL( adams.initializing_stepper().do_count , size_t( 3 ) );

    adams.do_step( lorenz() , x , 0.0 , x , 0.1 );
    BOOST_CHECK_EQUAL( adams.initializing_stepper().do_count , size_t( 4 ) );
}

BOOST_AUTO_TEST_CASE( test_manual_initialization )
{
    typedef boost::array< double , 3 > state_type;
    state_type x = {{ 10.0 , 10.0 , 10.0 }};

    adams_bashforth< 3 , state_type , value_type , state_type , value_type , range_algebra , default_operations ,
        initially_resizer , rk4_decorator< state_type > > adams;

    adams.initializing_stepper().do_count = 0;
    double t = 0.0 , dt = 0.1;
    adams.initialize( lorenz() , x , t , dt );
    BOOST_CHECK_EQUAL( adams.initializing_stepper().do_count , size_t( 2 ) );

    adams.do_step( lorenz() , x , 0.0 , x , 0.1 );
    BOOST_CHECK_EQUAL( adams.initializing_stepper().do_count , size_t( 2 ) );
}

BOOST_AUTO_TEST_SUITE_END()


#include <type_traits>

#include <Eigen/Dense>
#include <gtest/gtest.h>

#include "drake/common/autodiff.h"
#include "drake/common/cond.h"
#include "drake/common/eigen_types.h"
#include "drake/common/extract_double.h"
#include "drake/common/test_utilities/eigen_matrix_compare.h"

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

namespace drake {
namespace common {
namespace {

// Test nominal behavior of `AutoDiff`, checking implicit and explicit casting.
GTEST_TEST(AutodiffOverloadsTest, Casting) {
  VectorXd dx(3);
  dx << 0, 1, 2;
  VectorXd dx0(3);
  dx0.setZero();

  // No derivatives supplied.
  AutoDiffXd x(1);
  EXPECT_EQ(x.value(), 1);
  EXPECT_EQ(x.derivatives().size(), 0);
  // Persists.
  x = 2;
  EXPECT_EQ(x.value(), 2);
  EXPECT_EQ(x.derivatives().size(), 0);
  // Update derivative.
  x.derivatives() = dx;
  EXPECT_EQ(x.value(), 2);
  EXPECT_EQ(x.derivatives(), dx);
  // Implicitly castable, resets derivatives to zero.
  x = 10;
  EXPECT_EQ(x.value(), 10);
  EXPECT_EQ(x.derivatives(), dx0);
  // Resets derivative size.
  x = AutoDiffXd(10);
  EXPECT_EQ(x.value(), 10);
  EXPECT_EQ(x.derivatives().size(), 0);

  // Only explicitly castable to double.
  double xs = x.value();
  EXPECT_EQ(xs, 10);

  // Can mix zero-size derivatives: interpreted as constants.
  x = AutoDiffXd(2, dx);
  AutoDiffXd y(3);
  AutoDiffXd z = x * y;
  EXPECT_EQ(z.value(), 6);
  EXPECT_EQ(z.derivatives().size(), 3);

  // The following causes failure, because of mixed derivative size.
  // y.derivatives() = VectorXd::Zero(2);
  // EXPECT_EQ((x * y).value(), 10);
}

// Tests ExtractDoubleOrThrow on autodiff.
GTEST_TEST(AutodiffOverloadsTest, ExtractDouble) {
  // On autodiff.
  Eigen::AutoDiffScalar<Eigen::Vector2d> x;
  x.value() = 1.0;
  EXPECT_EQ(ExtractDoubleOrThrow(x), 1.0);

  // A double still works, too.
  double y = 1.0;
  EXPECT_EQ(ExtractDoubleOrThrow(y), 1.0);
}

// Tests correctness of nexttoward.
GTEST_TEST(AutodiffOverloadsTest, NextToward) {
  const long double inf = std::numeric_limits<long double>::infinity();
  const double eps = std::numeric_limits<double>::epsilon();
  Eigen::AutoDiffScalar<Eigen::Vector2d> x;
  x.value() = 1.0;
  EXPECT_EQ(nexttoward(x, inf) - 1, eps);
}

// Tests correctness of isfinite.
GTEST_TEST(AutodiffOverloadsTest, IsFinite) {
  Eigen::AutoDiffScalar<Eigen::Vector2d> x;
  x.value() = 1.0 / 0.0;
  EXPECT_EQ(isfinite(x), false);
  x.value() = 0.0;
  EXPECT_EQ(isfinite(x), true);
  x.derivatives()[0] = 1.0 / 0.0;  // The derivatives() are ignored.
  EXPECT_EQ(isfinite(x), true);
}

// Tests correctness of isinf.
GTEST_TEST(AutodiffOverloadsTest, IsInf) {
  Eigen::AutoDiffScalar<Eigen::Vector2d> x;
  x.value() = 1.0 / 0.0;
  EXPECT_EQ(isinf(x), true);
  x.value() = 0.0;
  EXPECT_EQ(isinf(x), false);
  x.derivatives()[0] = 1.0 / 0.0;  // The derivatives() are ignored.
  EXPECT_EQ(isinf(x), false);
}

// Tests correctness of isnan.
GTEST_TEST(AutodiffOverloadsTest, IsNaN) {
  Eigen::AutoDiffScalar<Eigen::Vector2d> x;
  x.value() = 0.0 / 0.0;
  EXPECT_EQ(isnan(x), true);
  x.value() = 0.0;
  EXPECT_EQ(isnan(x), false);
}

GTEST_TEST(AutodiffOverloadsTests, CopySign) {
  Eigen::AutoDiffScalar<Eigen::Vector2d> x, y, z;
  x.derivatives() = Eigen::VectorXd::Unit(2, 0);
  y.derivatives() = Eigen::VectorXd::Unit(2, 1);

  // Positive, positive.
  x.value() = 1.1;
  y.value() = 2.5;
  z = copysign(x, y);
  EXPECT_DOUBLE_EQ(z.value(), x.value());
  EXPECT_TRUE(CompareMatrices(z.derivatives(), x.derivatives()));

  // Positive, negative.
  x.value() = 1.1;
  y.value() = -2.5;
  z = copysign(x, y);
  EXPECT_DOUBLE_EQ(z.value(), -x.value());
  EXPECT_TRUE(CompareMatrices(z.derivatives(), -x.derivatives()));

  // Negative, positive.
  x.value() = -1.1;
  y.value() = 2.5;
  z = copysign(x, y);
  EXPECT_DOUBLE_EQ(z.value(), -x.value());
  EXPECT_TRUE(CompareMatrices(z.derivatives(), -x.derivatives()));

  // Negative, negative.
  x.value() = -1.1;
  y.value() = -2.5;
  z = copysign(x, y);
  EXPECT_DOUBLE_EQ(z.value(), x.value());
  EXPECT_TRUE(CompareMatrices(z.derivatives(), x.derivatives()));

  // Test w/ double y (Negative, positive).
  z = copysign(x, 2.5);
  EXPECT_DOUBLE_EQ(z.value(), -x.value());
  EXPECT_TRUE(CompareMatrices(z.derivatives(), -x.derivatives()));
}

// Tests that pow(AutoDiffScalar, AutoDiffScalar) applies the chain rule.
GTEST_TEST(AutodiffOverloadsTest, Pow) {
  Eigen::AutoDiffScalar<Eigen::Vector2d> x;
  x.value() = 1.1;
  x.derivatives() = Eigen::VectorXd::Unit(2, 0);
  Eigen::AutoDiffScalar<Eigen::Vector2d> y;
  y.value() = 2.5;
  y.derivatives() = Eigen::VectorXd::Unit(2, 1);

  x = x * (y + 2);
  EXPECT_DOUBLE_EQ(4.95, x.value());

  // The derivative of x with respect to its original value is y + 2 = 4.5.
  EXPECT_DOUBLE_EQ(4.5, x.derivatives()[0]);
  // The derivative of x with respect to y is x = 1.1.
  EXPECT_DOUBLE_EQ(1.1, x.derivatives()[1]);

  // The following should be the same as `pow(x, y)`, but we want to test this
  // one to check Drake's pow(ADS<DerType&>, ADS<DerType&>) works.
  auto z = pow(x + 0.0, y + 0.0);
  // z is x^y = 4.95^2.5 ~= 54.51.
  EXPECT_DOUBLE_EQ(std::pow(4.95, 2.5), z.value());
  // ∂z/∂x is y*x^(y-1) = 2.5 * 4.95^1.5 ~= 27.53.
  const double dzdx = 2.5 * std::pow(4.95, 1.5);
  // ∂z/∂y is (x^y)*ln(x) = (4.95^2.5)*ln(4.95) ~= 87.19.
  const double dzdy = std::pow(4.95, 2.5) * std::log(4.95);
  // By the chain rule, dz/dv is 27.53 * xgrad + 87.19 * ygrad
  EXPECT_DOUBLE_EQ(dzdx * 4.5 + dzdy * 0.0, z.derivatives()[0]);
  EXPECT_DOUBLE_EQ(dzdx * 1.1 + dzdy * 1.0, z.derivatives()[1]);
}

// Tests that pow(AutoDiffScalar, AutoDiffScalar) computes sane derivatives for
// base^exponent at the corner case base = 0, exponent.derivatives() = {0}.
GTEST_TEST(AutodiffOverloadsTest, PowEmptyExponentDerivative) {
  Eigen::AutoDiffScalar<Vector1d> x;
  x.value() = 0.;
  x.derivatives() = Eigen::VectorXd::Unit(1, 0);
  Eigen::AutoDiffScalar<Vector1d> y;
  y.value() = 2.5;
  y.derivatives() = Eigen::VectorXd::Zero(1);

  const auto z = pow(x, y);
  EXPECT_DOUBLE_EQ(0., z.value());
  // ∂z/∂v = ∂z/∂x, which is y*x^(y-1) = 0 (as opposed to NAN, had the chain
  // rule been applied).
  EXPECT_TRUE(CompareMatrices(Vector1d{0.}, z.derivatives()));
}

GTEST_TEST(AutodiffOverloadsTest, IfThenElse1) {
  using DerType = Eigen::Vector2d;
  Eigen::AutoDiffScalar<DerType> x{10.0, DerType{2, 1}};
  Eigen::AutoDiffScalar<DerType> y{if_then_else(x >= 0, x * x, x * x * x)};
  EXPECT_DOUBLE_EQ(y.value(), x.value() * x.value());
  EXPECT_DOUBLE_EQ(y.derivatives()[0], 2 * 2 * x.value());
  EXPECT_DOUBLE_EQ(y.derivatives()[1], 2 * x.value());
}

GTEST_TEST(AutodiffOverloadsTest, IfThenElse2) {
  using DerType = Eigen::Vector2d;
  Eigen::AutoDiffScalar<DerType> x{-10.0, DerType{2, 1}};
  Eigen::AutoDiffScalar<DerType> y{if_then_else(x >= 0, x * x, x * x * x)};
  EXPECT_DOUBLE_EQ(y.value(), x.value() * x.value() * x.value());
  EXPECT_DOUBLE_EQ(y.derivatives()[0], 2 * 3 * x.value() * x.value());
  EXPECT_DOUBLE_EQ(y.derivatives()[1], 3 * x.value() * x.value());
}

GTEST_TEST(AutodiffOverloadsTest, IfThenElse3) {
  using DerType = Eigen::Vector2d;
  Eigen::AutoDiffScalar<DerType> x{10.0, DerType{2, 1}};
  Eigen::AutoDiffScalar<DerType> y{if_then_else(x >= 0, x + 1, x)};
  EXPECT_DOUBLE_EQ(y.value(), x.value() + 1);
  EXPECT_DOUBLE_EQ(y.derivatives()[0], 2 * 1);
  EXPECT_DOUBLE_EQ(y.derivatives()[1], 1);
}

GTEST_TEST(AutodiffOverloadsTest, IfThenElse4) {
  using DerType = Eigen::Vector2d;
  Eigen::AutoDiffScalar<DerType> x{-10.0, DerType{2, 1}};
  Eigen::AutoDiffScalar<DerType> y{if_then_else(x >= 0, x, x)};
  EXPECT_DOUBLE_EQ(y.value(), x.value());
  EXPECT_DOUBLE_EQ(y.derivatives()[0], 2);
  EXPECT_DOUBLE_EQ(y.derivatives()[1], 1);
}

GTEST_TEST(AutodiffOverloadsTest, IfThenElse5) {
  using DerType = Eigen::Vector2d;
  Eigen::AutoDiffScalar<DerType> x{-10.0, DerType{2, 1}};
  Eigen::AutoDiffScalar<DerType> y{if_then_else(x >= 0, -x, -x * x * x)};
  EXPECT_DOUBLE_EQ(y.value(), -x.value() * x.value() * x.value());
  EXPECT_DOUBLE_EQ(y.derivatives()[0], 2 * -3 * x.value() * x.value());
  EXPECT_DOUBLE_EQ(y.derivatives()[1], -3 * x.value() * x.value());
}

GTEST_TEST(AutodiffOverloadsTest, IfThenElse6) {
  using DerType = Eigen::Vector2d;
  Eigen::AutoDiffScalar<DerType> x{10.0, DerType{2, 1}};
  Eigen::AutoDiffScalar<DerType> y{if_then_else(x >= 0, -x, x * x * x)};
  EXPECT_DOUBLE_EQ(y.value(), -x.value());
  EXPECT_DOUBLE_EQ(y.derivatives()[0], 2 * -1);
  EXPECT_DOUBLE_EQ(y.derivatives()[1], -1);
}

GTEST_TEST(AutodiffOverloadsTest, IfThenElse7) {
  using DerType = Eigen::Vector2d;
  Eigen::AutoDiffScalar<DerType> x{5.0, DerType{2, 1}};
  Eigen::AutoDiffScalar<DerType> y{10.0, DerType{2, 1}};
  Eigen::AutoDiffScalar<DerType> z{if_then_else(true, x * x, y * y * y)};
  EXPECT_DOUBLE_EQ(z.value(), x.value() * x.value());
  EXPECT_DOUBLE_EQ(z.derivatives()[0], 2 * 2 * x.value());
  EXPECT_DOUBLE_EQ(z.derivatives()[1], 2 * x.value());
}

GTEST_TEST(AutodiffOverloadsTest, IfThenElse8) {
  using DerType = Eigen::Vector2d;
  Eigen::AutoDiffScalar<DerType> x{5.0, DerType{2, 1}};
  Eigen::AutoDiffScalar<DerType> y{10.0, DerType{2, 1}};
  Eigen::AutoDiffScalar<DerType> z{if_then_else(false, x * x, y * y * y)};
  EXPECT_DOUBLE_EQ(z.value(), y.value() * y.value() * y.value());
  EXPECT_DOUBLE_EQ(z.derivatives()[0], 2 * 3 * y.value() * y.value());
  EXPECT_DOUBLE_EQ(z.derivatives()[1], 3 * y.value() * y.value());
}

GTEST_TEST(AutodiffOverloadsTest, Cond1) {
  using DerType = Eigen::Vector2d;
  Eigen::AutoDiffScalar<DerType> x{-10.0, DerType{2, 1}};
  Eigen::AutoDiffScalar<DerType> y{cond(x >= 0, x * x, x * x * x)};
  EXPECT_DOUBLE_EQ(y.value(), x.value() * x.value() * x.value());
  EXPECT_DOUBLE_EQ(y.derivatives()[0], 2 * 3 * x.value() * x.value());
  EXPECT_DOUBLE_EQ(y.derivatives()[1], 3 * x.value() * x.value());
}

GTEST_TEST(AutodiffOverloadsTest, Cond2) {
  using DerType = Eigen::Vector2d;
  Eigen::AutoDiffScalar<DerType> x{10.0, DerType{2, 1}};
  Eigen::AutoDiffScalar<DerType> y{cond(x >= 0, x * x, x * x * x)};
  EXPECT_DOUBLE_EQ(y.value(), x.value() * x.value());
  EXPECT_DOUBLE_EQ(y.derivatives()[0], 2 * 2 * x.value());
  EXPECT_DOUBLE_EQ(y.derivatives()[1], 2 * x.value());
}

GTEST_TEST(AutodiffOverloadsTest, Cond3) {
  using DerType = Eigen::Vector2d;
  Eigen::AutoDiffScalar<DerType> x{10.0, DerType{2, 1}};
  // clang-format off
  Eigen::AutoDiffScalar<DerType> y{cond(x >= 10, 10 * x * x,
                                        x >= 5, 5 * x * x * x,
                                        x >= 3, -3 * x,
                                        x * x)};
  // clang-format on
  EXPECT_DOUBLE_EQ(y.value(), 10 * x.value() * x.value());
  EXPECT_DOUBLE_EQ(y.derivatives()[0], 2 * 10 * 2 * x.value());
  EXPECT_DOUBLE_EQ(y.derivatives()[1], 10 * 2 * x.value());
}

GTEST_TEST(AutodiffOverloadsTest, Cond4) {
  using DerType = Eigen::Vector2d;
  Eigen::AutoDiffScalar<DerType> x{8.0, DerType{2, 1}};
  // clang-format off
  Eigen::AutoDiffScalar<DerType> y{cond(x >= 10, 10 * x * x,
                                        x >= 5, 5 * x * x * x,
                                        x >= 3, -3 * x,
                                        x * x)};
  // clang-format on
  EXPECT_DOUBLE_EQ(y.value(), 5 * x.value() * x.value() * x.value());
  EXPECT_DOUBLE_EQ(y.derivatives()[0], 2 * 5 * 3 * x.value() * x.value());
  EXPECT_DOUBLE_EQ(y.derivatives()[1], 5 * 3 * x.value() * x.value());
}

GTEST_TEST(AutodiffOverloadsTest, Cond5) {
  using DerType = Eigen::Vector2d;
  Eigen::AutoDiffScalar<DerType> x{4.0, DerType{2, 1}};
  // clang-format off
  Eigen::AutoDiffScalar<DerType> y{cond(x >= 10, 10 * x * x,
                                        x >= 5, 5 * x * x * x,
                                        x >= 3, -3 * x,
                                        x * x)};
  // clang-format on
  EXPECT_DOUBLE_EQ(y.value(), -3 * x.value());
  EXPECT_DOUBLE_EQ(y.derivatives()[0], 2 * -3);
  EXPECT_DOUBLE_EQ(y.derivatives()[1], -3);
}

GTEST_TEST(AutodiffOverloadsTest, Cond6) {
  using DerType = Eigen::Vector2d;
  Eigen::AutoDiffScalar<DerType> x{2.0, DerType{2, 1}};
  // clang-format off
  Eigen::AutoDiffScalar<DerType> y{cond(x >= 10, 10 * x * x,
                                        x >= 5, 5 * x * x * x,
                                        x >= 3, -3 * x,
                                        x * x)};
  // clang-format on
  EXPECT_DOUBLE_EQ(y.value(), x.value() * x.value());
  EXPECT_DOUBLE_EQ(y.derivatives()[0], 2 * 2 * x.value());
  EXPECT_DOUBLE_EQ(y.derivatives()[1], 2 * x.value());
}

GTEST_TEST(AutodiffOverloadsTest, Cond7) {
  using DerType = Eigen::Vector2d;
  Eigen::AutoDiffScalar<DerType> x{10.0, DerType{2, 1}};
  Eigen::AutoDiffScalar<DerType> y{2.0, DerType{4, 2}};
  // clang-format off
  Eigen::AutoDiffScalar<DerType> z{cond(x >= 10, 10 * x * x,
                                        x >= 5, 5 * y * y * y,
                                        x * x)};
  // clang-format on
  EXPECT_DOUBLE_EQ(z.value(), 10 * x.value() * x.value());
  EXPECT_DOUBLE_EQ(z.derivatives()[0], 2 * 10 * 2 * x.value());
  EXPECT_DOUBLE_EQ(z.derivatives()[1], 10 * 2 * x.value());
}

GTEST_TEST(AutodiffOverloadsTest, Cond8) {
  using DerType = Eigen::Vector2d;
  Eigen::AutoDiffScalar<DerType> x{7.0, DerType{2, 1}};
  Eigen::AutoDiffScalar<DerType> y{2.0, DerType{4, 2}};
  // clang-format off
  Eigen::AutoDiffScalar<DerType> z{cond(x >= 10, 10 * x * x,
                                        x >= 5, 5 * y * y * y,
                                        x * x)};
  // clang-format on
  EXPECT_DOUBLE_EQ(z.value(), 5 * y.value() * y.value() * y.value());
  EXPECT_DOUBLE_EQ(z.derivatives()[0], 4 * 5 * 3 * y.value() * y.value());
  EXPECT_DOUBLE_EQ(z.derivatives()[1], 2 * 5 * 3 * y.value() * y.value());
}

GTEST_TEST(AutodiffOverloadsTest, Cond9) {
  using DerType = Eigen::Vector2d;
  Eigen::AutoDiffScalar<DerType> x{3, DerType{2, 1}};
  Eigen::AutoDiffScalar<DerType> y{2.0, DerType{4, 2}};
  // clang-format off
  Eigen::AutoDiffScalar<DerType> z{cond(x >= 10, 10 * x * x,
                                        x >= 5, 5 * y * y * y,
                                        x * x)};
  // clang-format on
  EXPECT_DOUBLE_EQ(z.value(), x.value() * x.value());
  EXPECT_DOUBLE_EQ(z.derivatives()[0], 2 * 2 * x.value());
  EXPECT_DOUBLE_EQ(z.derivatives()[1], 2 * x.value());
}

// This is just a sanity check to make sure that Eigen::NumTraits::Literal
// is the right way to dig through an AutoDiffScalar to find the underlying
// floating point type. If this compiles it succeeds.
GTEST_TEST(AutodiffOverloadsTest, CheckEigenLiteral) {
  using DerTyped = Eigen::Vector2d;
  using DerTypef = Eigen::Vector2f;
  using Td = Eigen::AutoDiffScalar<DerTyped>;
  using Tf = Eigen::AutoDiffScalar<DerTypef>;

  using Literald = typename Eigen::NumTraits<Td>::Literal;
  using Literalf = typename Eigen::NumTraits<Tf>::Literal;

  static_assert(std::is_same<Literald, double>::value &&
                    std::is_same<Literalf, float>::value,
                "Eigen::NumTraits<T>::Literal didn't behave as expected.");
}

GTEST_TEST(AutodiffOverloadsTest, DummyValueX) {
  using T = Eigen::AutoDiffScalar<Eigen::VectorXd>;
  const T dummy_xd = dummy_value<T>::get();
  const double value = dummy_xd.value();
  EXPECT_TRUE(std::isnan(value));
  const Eigen::VectorXd derivatives = dummy_xd.derivatives();
  EXPECT_EQ(derivatives.rows(), 0);
}

GTEST_TEST(AutodiffOverloadsTest, DummyValue2) {
  using T = Eigen::AutoDiffScalar<Eigen::Vector2d>;
  const T dummy_2d = dummy_value<T>::get();
  const double value = dummy_2d.value();
  EXPECT_TRUE(std::isnan(value));
  const Eigen::Vector2d derivatives = dummy_2d.derivatives();
  EXPECT_EQ(derivatives.rows(), 2);
  EXPECT_TRUE(std::isnan(derivatives(0)));
  EXPECT_TRUE(std::isnan(derivatives(1)));
}

}  // namespace
}  // namespace common
}  // namespace drake


// Copyright (c) 2010 Daniel Pfeifer

#ifndef __VMML_VECTOR_TRAITS_HPP__
#define __VMML_VECTOR_TRAITS_HPP__

#include <vmmlib/vector.hpp>
#include <boost/la/vector_traits.hpp>

namespace boost
{
namespace la
{

template<size_t M, typename T>
struct vector_traits<vmml::vector<M, T> >
{
	typedef vmml::vector<M, T> vector_type;

	static const int dim = M;

	typedef T scalar_type;

	template<int I>
	static scalar_type r(const vector_type& v)
	{
		BOOST_STATIC_ASSERT(I >= 0);
		BOOST_STATIC_ASSERT(I < dim);
		return v.array[I];
	}

	template<int I>
	static scalar_type& w(vector_type& v)
	{
		BOOST_STATIC_ASSERT(I >= 0);
		BOOST_STATIC_ASSERT(I < dim);
		return v.array[I];
	}

	static scalar_type ir(int i, const vector_type& v)
	{
		BOOST_ASSERT(i >= 0);
		BOOST_ASSERT(i < dim);
		return v.array[i];
	}

	static scalar_type& iw(int i, vector_type& v)
	{
		BOOST_ASSERT(i >= 0);
		BOOST_ASSERT(i < dim);
		return v.array[i];
	}
};

} // namespace la
} // namespace boost

#endif /* VMML_VECTOR_TRAITS_HPP */


/* Copyright (c) 2017, United States Government, as represented by the
 * Administrator of the National Aeronautics and Space Administration.
 *
 * All rights reserved.
 *
 * The Astrobee platform is 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 <camera/camera_params.h>

#include <Eigen/Geometry>
#include <ceres/rotation.h>
#include <gtest/gtest.h>

#include <functional>
#include <list>
#include <memory>
#include <string>
#include <vector>

#define EXPECT_VECTOR3D_NEAR(p1, p2, t) EXPECT_NEAR(p1[0], p2[0], t); EXPECT_NEAR(p1[1], p2[1], t); \
  EXPECT_NEAR(p1[2], p2[2], t);

#define EXPECT_VECTOR2D_NEAR(p1, p2, t) EXPECT_NEAR(p1[0], p2[0], t); EXPECT_NEAR(p1[1], p2[1], t);

void Function(camera::CameraParameters const& params) {
  EXPECT_VECTOR2D_NEAR(Eigen::Vector2i(1200, 600), params.GetDistortedSize(), 1e-3);
  EXPECT_VECTOR2D_NEAR(Eigen::Vector2i(600, 300), params.GetDistortedHalfSize(), 1e-3);
  EXPECT_VECTOR2D_NEAR(Eigen::Vector2i(1200, 600), params.GetUndistortedSize(), 1e-3);
  EXPECT_VECTOR2D_NEAR(Eigen::Vector2i(600, 300), params.GetUndistortedHalfSize(), 1e-3);
  EXPECT_VECTOR2D_NEAR(Eigen::Vector2d(300, 200), params.GetFocalVector(), 1e-3);
  EXPECT_VECTOR2D_NEAR(Eigen::Vector2d(660.7, 500.3), params.GetOpticalOffset(), 1e-3);
  EXPECT_VECTOR2D_NEAR(Eigen::Vector2i(50, 100), params.GetCropOffset(), 1e-3);
}

TEST(camera_params, creation) {
  camera::CameraParameters params(
      Eigen::Vector2i(1200, 600),
      Eigen::Vector2d(300, 200),
      Eigen::Vector2d(660.7, 500.3));
  EXPECT_VECTOR2D_NEAR(params.GetDistortedHalfSize(), Eigen::Vector2i(600, 300), 1e-3);
  EXPECT_VECTOR2D_NEAR(params.GetUndistortedHalfSize(), Eigen::Vector2i(600, 300), 1e-3);
  EXPECT_EQ(0, params.GetDistortion().size());
  EXPECT_NEAR(250, params.GetFocalLength(), 1e-3);

  // Make RAW and DISTORT be different frames
  params.SetCropOffset(Eigen::Vector2i(50, 100));
  EXPECT_VECTOR2D_NEAR(params.GetCropOffset(), Eigen::Vector2i(50, 100), 1e-3);

  // Verify we get the correct intrinsic matrix
  Eigen::Matrix3d k = params.GetIntrinsicMatrix<camera::RAW>();
  EXPECT_VECTOR3D_NEAR(k.diagonal(), Eigen::Vector3d(300, 200, 1), 1e-3);
  EXPECT_VECTOR3D_NEAR(k.col(2), Eigen::Vector3d(710.7, 600.3, 1), 1e-3);

  k = params.GetIntrinsicMatrix<camera::DISTORTED>();
  EXPECT_VECTOR3D_NEAR(k.diagonal(), Eigen::Vector3d(300, 200, 1), 1e-3);
  EXPECT_VECTOR3D_NEAR(k.col(2), Eigen::Vector3d(660.7, 500.3, 1), 1e-3);

  k = params.GetIntrinsicMatrix<camera::DISTORTED_C>();
  EXPECT_VECTOR3D_NEAR(k.diagonal(), Eigen::Vector3d(300, 200, 1), 1e-3);
  EXPECT_VECTOR3D_NEAR(k.col(2), Eigen::Vector3d(60.7, 200.3, 1), 1e-3);

  k = params.GetIntrinsicMatrix<camera::UNDISTORTED>();
  EXPECT_VECTOR3D_NEAR(k.diagonal(), Eigen::Vector3d(300, 200, 1), 1e-3);
  EXPECT_VECTOR3D_NEAR(k.col(2), Eigen::Vector3d(600, 300, 1), 1e-3);

  k = params.GetIntrinsicMatrix<camera::UNDISTORTED_C>();
  EXPECT_VECTOR3D_NEAR(k.diagonal(), Eigen::Vector3d(300, 200, 1), 1e-3);
  EXPECT_VECTOR3D_NEAR(k.col(2), Eigen::Vector3d(0, 0, 1), 1e-3);

  // See what happens when we do a copy
  camera::CameraParameters params2 = params;
  EXPECT_VECTOR2D_NEAR(Eigen::Vector2i(1200, 600), params2.GetDistortedSize(), 1e-3);
  EXPECT_VECTOR2D_NEAR(Eigen::Vector2i(600, 300), params2.GetDistortedHalfSize(), 1e-3);
  EXPECT_VECTOR2D_NEAR(Eigen::Vector2i(1200, 600), params2.GetUndistortedSize(), 1e-3);
  EXPECT_VECTOR2D_NEAR(Eigen::Vector2i(600, 300), params2.GetUndistortedHalfSize(), 1e-3);
  EXPECT_VECTOR2D_NEAR(Eigen::Vector2d(300, 200), params2.GetFocalVector(), 1e-3);
  EXPECT_VECTOR2D_NEAR(Eigen::Vector2d(660.7, 500.3), params2.GetOpticalOffset(), 1e-3);
  EXPECT_VECTOR2D_NEAR(Eigen::Vector2i(50, 100), params2.GetCropOffset(), 1e-3);

  camera::CameraParameters params3(params);
  EXPECT_VECTOR2D_NEAR(Eigen::Vector2i(1200, 600), params3.GetDistortedSize(), 1e-3);
  EXPECT_VECTOR2D_NEAR(Eigen::Vector2i(600, 300), params3.GetDistortedHalfSize(), 1e-3);
  EXPECT_VECTOR2D_NEAR(Eigen::Vector2i(1200, 600), params3.GetUndistortedSize(), 1e-3);
  EXPECT_VECTOR2D_NEAR(Eigen::Vector2i(600, 300), params3.GetUndistortedHalfSize(), 1e-3);
  EXPECT_VECTOR2D_NEAR(Eigen::Vector2d(300, 200), params3.GetFocalVector(), 1e-3);
  EXPECT_VECTOR2D_NEAR(Eigen::Vector2d(660.7, 500.3), params3.GetOpticalOffset(), 1e-3);
  EXPECT_VECTOR2D_NEAR(Eigen::Vector2i(50, 100), params3.GetCropOffset(), 1e-3);

  params3.SetOpticalOffset(Eigen::Vector2d(99, -100));
  EXPECT_VECTOR2D_NEAR(Eigen::Vector2d(99, -100), params3.GetOpticalOffset(), 1e-3);
  EXPECT_VECTOR2D_NEAR(Eigen::Vector2d(660.7, 500.3), params.GetOpticalOffset(), 1e-3);

  // Verify that we don't have memory alignment issues with shared ptrs
  std::shared_ptr<camera::CameraParameters> params4;
  params4.reset(new camera::CameraParameters(params));
  EXPECT_VECTOR2D_NEAR(Eigen::Vector2i(1200, 600), params4->GetDistortedSize(), 1e-3);
  EXPECT_VECTOR2D_NEAR(Eigen::Vector2i(600, 300), params4->GetDistortedHalfSize(), 1e-3);
  EXPECT_VECTOR2D_NEAR(Eigen::Vector2i(1200, 600), params4->GetUndistortedSize(), 1e-3);
  EXPECT_VECTOR2D_NEAR(Eigen::Vector2i(600, 300), params4->GetUndistortedHalfSize(), 1e-3);
  EXPECT_VECTOR2D_NEAR(Eigen::Vector2d(300, 200), params4->GetFocalVector(), 1e-3);
  EXPECT_VECTOR2D_NEAR(Eigen::Vector2d(660.7, 500.3), params4->GetOpticalOffset(), 1e-3);
  EXPECT_VECTOR2D_NEAR(Eigen::Vector2i(50, 100), params4->GetCropOffset(), 1e-3);

  // Maybe alignment problems happen when passing to a function
  Function(params);

  auto f_bind = std::bind(Function, params);
  f_bind();

  std::function<void(camera::CameraParameters const&)> f_bind2 = Function;
  f_bind2(params);

  // Maybe the alignment problem is an STL container?
  std::list<camera::CameraParameters, Eigen::aligned_allocator<camera::CameraParameters> > list;
  list.emplace_back(params);
  list.emplace_back(params);
}

TEST(camera_params, conversion) {
  //       raw d d_c u u_c
  // 1      --->
  // 2      <---
  // 3            <------
  // 4            ------>
  // 5               --->
  // 6               <---
  // 7         ------>
  // 8         <------
  // 9         --------->
  // 10        <---------

  Eigen::VectorXd distortion(1);
  distortion[0] = 0.7;  // FOV model
  camera::CameraParameters params(
      Eigen::Vector2i(1200, 600),
      Eigen::Vector2d(300, 300),
      Eigen::Vector2d(660.5, 500.3), distortion);
  params.SetCropOffset(Eigen::Vector2i(25, 50));

  // RAW to DISTORTED conversions
  Eigen::Vector2d raw = Eigen::Vector2d::Zero(), output = Eigen::Vector2d::Zero();
  params.Convert<camera::RAW, camera::DISTORTED>(raw, &output);
  EXPECT_NEAR(-25, output[0], 1e-6);
  EXPECT_NEAR(-50, output[1], 1e-6);
  params.Convert<camera::DISTORTED, camera::RAW>(output, &output);
  EXPECT_NEAR(0, output[0], 1e-6);
  EXPECT_NEAR(0, output[1], 1e-6);

  // forward backward via 3, 4
  Eigen::Vector2d input(-40, 77.3), output2;
  params.Convert<camera::UNDISTORTED_C, camera::DISTORTED_C>(input, &output);
  params.Convert<camera::DISTORTED_C, camera::UNDISTORTED_C>(output, &output2);
  EXPECT_NEAR(input[0], output2[0], 1e-6);
  EXPECT_NEAR(input[1], output2[1], 1e-6);

  // forward backward via 7, 8
  input << 233.4, 368.9;
  params.Convert<camera::UNDISTORTED, camera::DISTORTED>(input, &output);
  params.Convert<camera::DISTORTED, camera::UNDISTORTED>(output, &output2);
  EXPECT_NEAR(input[0], output2[0], 1e-6);
  EXPECT_NEAR(input[1], output2[1], 1e-6);

  // forward backward via 9, 10
  input << 542.4, 182.5;
  params.Convert<camera::DISTORTED, camera::UNDISTORTED_C>(input, &output);
  params.Convert<camera::UNDISTORTED_C, camera::DISTORTED>(output, &output2);
  EXPECT_NEAR(input[0], output2[0], 1e-6);
  EXPECT_NEAR(input[1], output2[1], 1e-6);

  // forward through 5, 10, 7
  input << 43.6, -213.9;
  params.Convert<camera::UNDISTORTED, camera::UNDISTORTED_C>(input, &output2);
  params.Convert<camera::UNDISTORTED_C, camera::DISTORTED>(output2, &output);
  params.Convert<camera::DISTORTED, camera::UNDISTORTED>(output, &output2);
  EXPECT_NEAR(input[0], output2[0], 1e-6);
  EXPECT_NEAR(input[1], output2[1], 1e-6);

  // forward through 8, 9, 6
  input << 542.4, 182.5;
  params.Convert<camera::UNDISTORTED, camera::DISTORTED>(input, &output2);
  params.Convert<camera::DISTORTED, camera::UNDISTORTED_C>(output2, &output);
  params.Convert<camera::UNDISTORTED_C, camera::UNDISTORTED>(output, &output2);
  EXPECT_NEAR(input[0], output2[0], 1e-6);
  EXPECT_NEAR(input[1], output2[1], 1e-6);
}

// Run all the tests that were declared with TEST()
int main(int argc, char** argv) {
  testing::InitGoogleTest(&argc, argv);
  return RUN_ALL_TESTS();
}


#include <algorithm>
#include <cmath>

#include <boost/format.hpp>

#include "Color.hpp"
#include "Math.hpp"

using boost::format;
using std::floor;
using std::sort;
using std::string;
using std::vector;

Color::Color(Color::Name name) :
	a(0xFF)
{
	r = (static_cast<unsigned int>(name) & 0xFF0000) >> 16;
	g = (static_cast<unsigned int>(name) & 0x00FF00) >> 8;
	b = static_cast<unsigned int>(name) & 0x0000FF;
}

Color::Color(ubyte r, ubyte g, ubyte b, ubyte a)
{
	init(r, g, b);
	this->a = a;
}

Color Color::gradient(Color color1, Color color2, float t)
{
	Color c;
	c.r = static_cast<ubyte>(lerp<float>(color1.r, color2.r, t));
	c.g = static_cast<ubyte>(lerp<float>(color1.g, color2.g, t));
	c.b = static_cast<ubyte>(lerp<float>(color1.b, color2.b, t));
	return c;
}

Color Color::hsv(float h, float s, float v)
{
	Color color;

	if( s == 0.0f )
		color = Color::rgb(v, v, v);
	else
	{
		h *= 5; // Sector 0 to 5
		int i = floor(h);
		float f = h - i;
		float p = v * (1.0f - s);
		float q = v * (1.0f - s * f);
		float t = v * (1.0f - s * (1.0f - f));

		switch( i )
		{
			case 0:
				color = Color::rgb(v, t, p);
				break;

			case 1:
				color = Color::rgb(q, v, p);
				break;

			case 2:
				color = Color::rgb(p, v, t);
				break;

			case 3:
				color = Color::rgb(p, q, v);
				break;

			case 4:
				color = Color::rgb(t, p, v);
				break;

			default:
			case 5:
				color = Color::rgb(v, p, q);
				break;
		}
	}

	return color;
}

void Color::init(ubyte r, ubyte g, ubyte b)
{
	this->r = r;
	this->g = g;
	this->b = b;
}

float Color::normalizedA() const
{
	return static_cast<float>(this->a) / 255.0f;
}

float Color::normalizedB() const
{
	return static_cast<float>(this->b) / 255.0f;
}

float Color::normalizedG() const
{
	return static_cast<float>(this->g) / 255.0f;
}

float Color::normalizedR() const
{
	return static_cast<float>(this->r) / 255.0f;
}

Color Color::rgb(float r, float g, float b)
{
	Color color;
	color.r = floor(r * 255.0f);
	color.g = floor(g * 255.0f);
	color.b = floor(b * 255.0f);
	return color;
}

unsigned int Color::toInteger() const
{
	return (a & 0xFF000000) + (r & 0x00FF0000) + (g & 0x0000FF00) + (b & 0x000000FF);
}

string Color::toString() const
{
	return (format("#%02x%02x%02x%02x") %
		static_cast<unsigned short>(a) %
		static_cast<unsigned short>(r) %
		static_cast<unsigned short>(g) %
		static_cast<unsigned short>(b)).str();
}

bool Gradient::ColorPoint::operator < (const Gradient::ColorPoint& p) const
{
	return (this->location < p.location);
}

Gradient::Gradient(Color minColor, Color maxColor)
{
	addColor(minColor, 0.0f);
	addColor(maxColor, 1.0f);
}

void Gradient::addColor(Color color, float location)
{
	ColorPoint p;
	p.color = color;
	p.location = location;
	this->colorArray.push_back(p);
	sort(this->colorArray.begin(), this->colorArray.end());
}

Color Gradient::getColor(float t)
{
	if( t <= 0.0 )
		return this->colorArray.front().color;
	if( t >= 1.0 )
		return this->colorArray.back().color;

	for( vector<ColorPoint>::iterator it = this->colorArray.begin(); it != this->colorArray.end(); ++it )
	{
		if( (*it).location > t )
		{
			vector<ColorPoint>::iterator j = it;
			j--;
			ColorPoint& p1 = (*j);
			ColorPoint& p2 = (*it);
			return Color::gradient(p1.color, p2.color, (t - p1.location) / (p2.location - p1.location));
		}
	}

	//Default
	return Color::BLACK;
}


//==================================================================================================
/*
  Copyright 2017 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)
*/
//==================================================================================================
//! [transform_reduce2]
#include <boost/simd/algorithm/transform_reduce.hpp>
#include <boost/simd/function/toint.hpp>
#include <iostream>
#include <vector>

namespace bs =  boost::simd;

struct f
{
  template < class T >
  auto operator()(const T & a, const T & b) -> decltype(bs::toint(a*b))
  {
    return bs::toint(a)*bs::toint(b);
  }
};

int main()
{
  std::vector<float> d{1.5f, 2.5f, 3.5f, 4.5f, 5.5f, 6.5f, 7.5f, 8.5f, 9.5f, 10.5f} ;
  std::vector<float> e{1.5f, 2.5f, 3.5f, 4.5f, 5.5f, 6.5f, 7.5f, 8.5f, 9.5f, 10.5f} ;

  auto s1 = bs::transform_reduce( d.data(), d.data()+10, e.data(), f(), 0, bs::plus);
  std::cout << "s1 = " << s1 << std::endl;

  return 0;
}
//! [transform_reduce2]


//==============================================================================
//         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
//==============================================================================
/*!
 * \file
**/
#ifndef BOOST_SIMD_IEEE_FUNCTIONS_EXPONENT_HPP_INCLUDED
#define BOOST_SIMD_IEEE_FUNCTIONS_EXPONENT_HPP_INCLUDED
#include <boost/simd/include/functor.hpp>
#include <boost/dispatch/include/functor.hpp>

/*!
 * \ingroup boost_simd_ieee
 * \defgroup boost_simd_ieee_exponent exponent
 *
 * \par Description
 * Returns the exponent of the floating input.
 * \par
 * The sign \f$ \pm \f$ , exponent e and mantissa m of a floating point entry a are related by
 * \f$a = \pm m\times 2^e\f$, with m between zero and one
 *
 * \par Header file
 *
 * \code
 * #include <nt2/include/functions/exponent.hpp>
 * \endcode
 *
 *
 * \synopsis
 *
 * \code
 * namespace boost::simd
 * {
 *   template <class A0>
 *     meta::call<tag::exponent_(A0)>::type
 *     exponent(const A0 & a0);
 * }
 * \endcode
 *
 * \param a0 the unique parameter of exponent
 *
 * \return an integer value
 *
 * \par Notes
 * In SIMD mode, this function acts elementwise on the inputs vectors elements
 * \par
 *
**/

namespace boost { namespace simd { namespace tag
  {
    /*!
     * \brief Define the tag exponent_ of functor exponent
     *        in namespace boost::simd::tag for toolbox boost.simd.ieee
    **/
    struct exponent_ : ext::elementwise_<exponent_> { typedef ext::elementwise_<exponent_> parent; };
  }
  BOOST_DISPATCH_FUNCTION_IMPLEMENTATION(tag::exponent_, exponent, 1)
} }

#endif

// modified by jt the 25/12/2010


#include "opencv2/opencv.hpp"
#include <cstdio>
#include <vector>
#include <deque>
#include <iostream>

#include "opencv2/core/core_c.h" // shame, but needed for using dlib
#include <dlib/image_processing.h>
#include <dlib/opencv/cv_image.h>

#include "pico/picornt.hpp"

using namespace std;

//
// stolen from charuco ..
//
void drawAxis(cv::InputOutputArray _image, cv::InputArray _cameraMatrix, cv::InputArray _distCoeffs,
              cv::InputArray _rvec, cv::InputArray _tvec, float length) {

    CV_Assert(_image.getMat().total() != 0 &&
              (_image.getMat().channels() == 1 || _image.getMat().channels() == 3));
    CV_Assert(length > 0);

    // project axis points
    std::vector< cv::Point3f > axisPoints;
    axisPoints.push_back(cv::Point3f(0, 0, 0));
    axisPoints.push_back(cv::Point3f(length, 0, 0));
    axisPoints.push_back(cv::Point3f(0, length, 0));
    axisPoints.push_back(cv::Point3f(0, 0, length));
    std::vector< cv::Point2f > imagePoints;
    cv::projectPoints(axisPoints, _rvec, _tvec, _cameraMatrix, _distCoeffs, imagePoints);

    // draw axis lines
    cv::line(_image, imagePoints[0], imagePoints[1], cv::Scalar(0, 0, 255), 3);
    cv::line(_image, imagePoints[0], imagePoints[2], cv::Scalar(0, 255, 0), 3);
    cv::line(_image, imagePoints[0], imagePoints[3], cv::Scalar(255, 0, 0), 3);
}

//
// e.g. to smooth landmarks
//
template<class T>
struct Ipol
{
    size_t N;
    std::deque<T> q;

    Ipol(size_t n=10) : N(n) {}

    T operator ()(T t)
    {
        q.push_back(t);
        if (q.size() > N)
            q.pop_front();

        T acc(0,0);// = 0;
        for (size_t i=0; i<q.size(); ++i)
            acc += q[i];
        return acc / double(q.size());
    }
};

struct Head
{
    dlib::shape_predictor sp;
    vector<cv::Point3d> pts3d;
    vector<Ipol <cv::Point2d> > ipol;
    cv::Mat rvec,tvec;
    cv::Mat camMatrix;

    Head()
    {
        dlib::deserialize("c:/p/ocv/old/data/shape_predictor_68_face_landmarks.dat") >> sp;

        cv::FileStorage fs("c:/p/ocv/old/data/mdl.yml.gz", cv::FileStorage::READ);
        cv::Mat mdl;
        fs["mdl"] >> mdl;
        fs.release();

        //// if you want to see the 3d model ..
        //cv::Mat ch[3];
        //cv::split(mdl, ch);
        //cv::Mat_<double> depth;
        //cv::normalize(ch[1], depth, -100);
        //cv::imshow("head1", depth);

        // get 2d reference points from image
        vector<cv::Point2d> pts2d;
        cv::Mat meanI = cv::imread("c:/p/ocv/old/data/reference_320_320.png", 0);
        getkp2d(meanI, pts2d, cv::Rect(80,80, 160,160), false);

        // get 3d reference points from model
        for(size_t k=0; k<pts2d.size(); k++)
        {
            cv::Vec3d pm = mdl.at<cv::Vec3d>(int(pts2d[k].y), int(pts2d[k].x));
            cv::Point3d p(pm[0], pm[2], -pm[1]);
            pts3d.push_back(p);
        }
    }

    void getkp2d(const cv::Mat &I, vector<cv::Point2d> &pts2d, const cv::Rect &r, int interpolate=0)
    {
        dlib::rectangle rec(r.x, r.y, r.x+r.width, r.y+r.height);
        dlib::full_object_detection shape = sp(dlib::cv_image<uchar>(I), rec);

        pts2d.clear();
        for(size_t k=0; k<shape.num_parts(); k++)
        {
            cv::Point2d p(shape.part(k).x(), shape.part(k).y());
            pts2d.push_back(p);
        }
        if (interpolate > 0)
        {
            if (ipol.empty() || (ipol[0].N != interpolate))
                ipol = vector<Ipol <cv::Point2d> >(pts2d.size(), interpolate);

            for(size_t k=0; k<pts2d.size(); k++)
            {
                pts2d[k] = ipol[k](pts2d[k]);
            }
        }
    }

    cv::Mat pnp(const cv::Size &s, vector<cv::Point2d> &pts2d)
    {
        // camMatrix based on img size
        int max_d = std::max(s.width,s.height);
        camMatrix = (cv::Mat_<double>(3,3) <<
            max_d,   0, s.width/2.0,
            0,     max_d, s.height/2.0,
            0,   0,        1.0);

        // 2d -> 3d correspondence
        cv::solvePnP(pts3d, pts2d, camMatrix, cv::Mat(1,4,CV_64F,0.0), rvec, tvec, false, cv::SOLVEPNP_EPNP);
        //cerr << "rot " << rvec.t() *180/CV_PI << endl;
        //cerr << "tra " << tvec.t() << endl;

        // get 3d rot mat
        cv::Mat rotM(3, 3, CV_64F);
        cv::Rodrigues(rvec, rotM);

        // push tvec to transposed Mat
        cv::Mat rotMT = rotM.t();
        rotMT.push_back(tvec.reshape(1, 1));

        // transpose back, and multiply
        return camMatrix * rotMT.t();
    }
    void draw(cv::Mat &I, const vector<cv::Point2d> &pts, const cv::Rect &r)
    {
        for (size_t i=0; i<pts.size()-1; i++)
        {
            cv::line(I,pts[i],pts[i+1],cv::Scalar(200));
            cv::circle(I,pts[i],3,cv::Scalar(0,200));
        }
        drawAxis(I, camMatrix, cv::Mat(1,4,CV_64F,0.0), rvec, tvec, 80);
    }
};

int main()
{
    cerr << "press 'esc.' to stop, "<<endl<<"\t'd' to decrease or, 'D' to increase landmark interpolation." << endl;

    Head head;
    cv::VideoCapture cap(0);
    int denoise = 5;
    while(cap.isOpened())
    {
        cv::Mat frame;
        if (! cap.read(frame))
            break;
        cv::Mat gray;
        cv::cvtColor(frame, gray, cv::COLOR_BGR2GRAY);
        vector<cv::Point2d> pts2d; // 2d model from landmarks
        vector<cv::Vec4f>   obj;   // pico object: (confidence, x, y, width)
        int n = pico::findObjects(gray, obj);
        if (n > 0)
        {
            const cv::Vec4f &o = obj[0];
            float conf = o[0];
            if (conf < 4.0f) continue;
            int w = int(o[3])/2; // half width
            cv::Rect r(int(o[2])-w, int(o[1])-w, 2*w,2*w);
            //cv::rectangle(frame, r, cv::Scalar(0, 4*conf, 0), 1);

            head.getkp2d(gray, pts2d, r, denoise);
            cv::Mat p = head.pnp(frame.size(), pts2d);
            head.draw(frame, pts2d, r);
        }

        cv::imshow("box",frame);
        int k = cv::waitKey(10);
        if (k == 27)   break;
        if (k == 'd') if(denoise>0) denoise--, cerr << "denoise " << denoise << endl;
        if (k == 'D') denoise++, cerr << "denoise " << denoise << endl;
    }
    return 0;
}


/**
 * @file GLoggerTest.cpp
 */

/********************************************************************************
 *
 * This file is part of the Geneva library collection. The following license
 * applies to this file:
 *
 * ------------------------------------------------------------------------------
 * 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.
 * ------------------------------------------------------------------------------
 *
 * Note that other files in the Geneva library collection may use a different
 * license. Please see the licensing information in each file.
 *
 ********************************************************************************
 *
 * Geneva was started by Dr. Rüdiger Berlich and was later maintained together
 * with Dr. Ariel Garcia under the auspices of Gemfony scientific. For further
 * information on Gemfony scientific, see http://www.gemfomy.eu .
 *
 * The majority of files in Geneva was released under the Apache license v2.0
 * in February 2020.
 *
 * See the NOTICE file in the top-level directory of the Geneva library
 * collection for a list of contributors and copyright information.
 *
 ********************************************************************************/

// Global checks, defines and includes needed for all of Geneva
#include "common/GGlobalDefines.hpp"

// Standard headers go here
#include <iostream>
#include <map>

// Boost headers go here
#include <boost/test/unit_test.hpp>
#include <boost/test/tools/floating_point_comparison.hpp>
#include <boost/math/constants/constants.hpp>
#include <boost/assign/list_of.hpp>
#include <boost/assign/list_inserter.hpp>

// Geneva headers go here
#include "common/GFormulaParserT.hpp"

using namespace boost::assign;
using namespace Gem::Common;
using namespace std;

#define testFormula(FORMULA)\
{\
	std::string formula( #FORMULA );\
	GFormulaParserT<double> f(formula);\
	double fp_val = FORMULA;\
	double parse_val = f();\
	BOOST_CHECK_CLOSE(parse_val, fp_val, 0.001);\
}\

#define testFormulaFailure(FORMULA, EXCEPTION)\
{\
	std::string formula( #FORMULA );\
	GFormulaParserT<double> f(formula);\
	BOOST_CHECK_THROW(f(), EXCEPTION );\
}\

int test_main(int argc, char** const argv) {
	{ // Test replacement of variables and constants (1)
		std::map<std::string, std::vector<double>> parameterValues;

		std::string formula("sin({{0}})/{{1}}");

		std::vector<double> list0 = boost::assign::list_of(4.34343434343434);
		std::vector<double> list1 = boost::assign::list_of(8.98989898989899);

		parameterValues["0"] = list0;
		parameterValues["1"] = list1;

		GFormulaParserT<double> f(formula);

		double fp_val = sin(4.34343434343434)/8.98989898989899;
		double parse_val = f(parameterValues);

		BOOST_CHECK_CLOSE(parse_val, fp_val, 0.001);
	}

	{ // Test replacement of variables and constants (1)
		std::map<std::string, std::vector<double>> parameterValues;

		std::string formula("fabs(sin({{0}})/max({{1}}, 0.000001))");

		std::vector<double> list0 = boost::assign::list_of(4.34343434343434);
		std::vector<double> list1 = boost::assign::list_of(8.98989898989899);

		parameterValues["0"] = list0;
		parameterValues["1"] = list1;

		GFormulaParserT<double> f(formula);

		double fp_val = fabs(sin(4.34343434343434)/max(8.98989898989899, 0.000001));
		double parse_val = f(parameterValues);

		BOOST_CHECK_CLOSE(parse_val, fp_val, 0.001);
	}

	{ // Test replacement of variables and constants (1)
		std::map<std::string, std::vector<double>> parameterValues;

		std::string formula("sin({{var0[2]}})/{{var1}}");

		std::vector<double> list0 = boost::assign::list_of(1.5)(2.5)(3.5);
		std::vector<double> list1 = boost::assign::list_of(8.98989898989899);

		parameterValues["var0"] = list0;
		parameterValues["var1"] = list1;

		GFormulaParserT<double> f(formula);

		double fp_val = sin(list0[2])/list1[0];
		double parse_val = f(parameterValues);

		BOOST_CHECK_CLOSE(parse_val, fp_val, 0.001);
	}

	{ // Test replacement of variables and constants (2)
		std::map<std::string, std::vector<double>> parameterValues;
		std::map<std::string, double> userConstants;

		std::string formula("gem*sin({{var1}})*cos(pi)");

		std::vector<double> var1list = boost::assign::list_of(2.);
		parameterValues["var1"] = var1list;
		userConstants["gem"] = -1.;

		GFormulaParserT<double> f(formula, userConstants);

		double fp_val = -1.*sin(2.)*cos(boost::math::constants::pi<double>());
		double parse_val = f(parameterValues);

		BOOST_CHECK_CLOSE(parse_val, fp_val, 0.001);
	}

	// Test constants
	{
		std::string formula("pi");

		GFormulaParserT<double> f(formula);

		double fp_val = boost::math::constants::pi<double>();
		double parse_val = f();

		BOOST_CHECK_CLOSE(parse_val, fp_val, 0.001);
	}

	{
		std::string formula("e");

		GFormulaParserT<double> f(formula);

		double fp_val = boost::math::constants::e<double>();
		double parse_val = f();

		BOOST_CHECK_CLOSE(parse_val, fp_val, 0.001);
	}

	// Test simple calculations
	testFormula(1.234);
	testFormula(1.2e3);
	testFormula(2e-03);
	testFormula(-1);
	testFormula(1*2+3*4);
	testFormula(1*(2+3)*4);
	testFormula((1*2)+(3*4));
	testFormula(-(1));
	testFormula(-(-1));
	testFormula(+(-1));
	testFormula(+(+1));

	// Test functions
	testFormula(fabs(-1.0));
	testFormula(acos(1.0));
	testFormula(asin(1.0));
	testFormula(atan(1.0));
	testFormula(ceil(0.5));
	testFormula(cos(1.0));
	testFormula(cosh(1.0));
	testFormula(exp(1.0));
	testFormula(floor(1.0));
	testFormula(log(1.0));
	testFormula(log10(1.0));
	testFormula(sin(1.0));
	testFormula(sinh(1.0));
	testFormula(sqrt(1.0));
	testFormula(tan(1.0));
	testFormula(tanh(1.0));
	testFormula(pow(2.0, 3.0));
	testFormula(max(2.0, 3.0));
	testFormula(min(2.0, 3.0));

	// Test synthesized formulas
	testFormula(sinh(1.0)*sin(1.0));

	// Test failures in formulas
	testFormulaFailure(1/0, gemfony_exception);
	testFormulaFailure(1/0, Gem::Common::math_logic_error);
	testFormulaFailure(1/0, Gem::Common::division_by_0);

	testFormulaFailure(acos(-2), gemfony_exception);
	testFormulaFailure(acos(-2), Gem::Common::math_logic_error);
	testFormulaFailure(acos(-2), Gem::Common::acos_invalid_range<double>);

	testFormulaFailure(acos(+2), gemfony_exception);
	testFormulaFailure(acos(+2), Gem::Common::math_logic_error);
	testFormulaFailure(acos(+2), Gem::Common::acos_invalid_range<double>);

	testFormulaFailure(asin(-2), gemfony_exception);
	testFormulaFailure(asin(-2), Gem::Common::math_logic_error);
	testFormulaFailure(asin(-2), Gem::Common::asin_invalid_range<double>);

	testFormulaFailure(asin(+2), gemfony_exception);
	testFormulaFailure(asin(+2), Gem::Common::math_logic_error);
	testFormulaFailure(asin(+2), Gem::Common::asin_invalid_range<double>);

	testFormulaFailure(log(0), gemfony_exception);
	testFormulaFailure(log(0), Gem::Common::math_logic_error);
	testFormulaFailure(log(0), Gem::Common::log_negative_value<double>);

	testFormulaFailure(log(-1), gemfony_exception);
	testFormulaFailure(log(-1), Gem::Common::math_logic_error);
	testFormulaFailure(log(-1), Gem::Common::log_negative_value<double>);

	testFormulaFailure(log10(0), gemfony_exception);
	testFormulaFailure(log10(0), Gem::Common::math_logic_error);
	testFormulaFailure(log10(0), Gem::Common::log10_negative_value<double>);

	testFormulaFailure(log10(-1), gemfony_exception);
	testFormulaFailure(log10(-1), Gem::Common::math_logic_error);
	testFormulaFailure(log10(-1), Gem::Common::log10_negative_value<double>);

	testFormulaFailure(sqrt(-1), gemfony_exception);
	testFormulaFailure(sqrt(-1), Gem::Common::math_logic_error);
	testFormulaFailure(sqrt(-1), Gem::Common::sqrt_negative_value<double>);

	return 0;
}