//  Copyright John Maddock 2006.
//  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)
//
// Basic sanity check that header <boost/math/special_functions/bessel.hpp>
// #includes all the files that it needs to.
//
#include <boost/math/special_functions/bessel_prime.hpp>
//
// Note this header includes no other headers, this is
// important if this test is to be meaningful:
//
#include "test_compile_result.hpp"

void compile_and_link_test()
{
   check_result<float>(boost::math::cyl_bessel_j_prime<float>(f, f));
   check_result<double>(boost::math::cyl_bessel_j_prime<double>(d, d));
#ifndef BOOST_MATH_NO_LONG_DOUBLE_MATH_FUNCTIONS
   check_result<long double>(boost::math::cyl_bessel_j_prime<long double>(l, l));
#endif

   check_result<float>(boost::math::cyl_neumann_prime<float>(f, f));
   check_result<double>(boost::math::cyl_neumann_prime<double>(d, d));
#ifndef BOOST_MATH_NO_LONG_DOUBLE_MATH_FUNCTIONS
   check_result<long double>(boost::math::cyl_neumann_prime<long double>(l, l));
#endif

   check_result<float>(boost::math::cyl_bessel_i_prime<float>(f, f));
   check_result<double>(boost::math::cyl_bessel_i_prime<double>(d, d));
#ifndef BOOST_MATH_NO_LONG_DOUBLE_MATH_FUNCTIONS
   check_result<long double>(boost::math::cyl_bessel_i_prime<long double>(l, l));
#endif

   check_result<float>(boost::math::cyl_bessel_k_prime<float>(f, f));
   check_result<double>(boost::math::cyl_bessel_k_prime<double>(d, d));
#ifndef BOOST_MATH_NO_LONG_DOUBLE_MATH_FUNCTIONS
   check_result<long double>(boost::math::cyl_bessel_k_prime<long double>(l, l));
#endif

   check_result<float>(boost::math::sph_bessel_prime<float>(u, f));
   check_result<double>(boost::math::sph_bessel_prime<double>(u, d));
#ifndef BOOST_MATH_NO_LONG_DOUBLE_MATH_FUNCTIONS
   check_result<long double>(boost::math::sph_bessel_prime<long double>(u, l));
#endif

   check_result<float>(boost::math::sph_neumann_prime<float>(u, f));
   check_result<double>(boost::math::sph_neumann_prime<double>(u, d));
#ifndef BOOST_MATH_NO_LONG_DOUBLE_MATH_FUNCTIONS
   check_result<long double>(boost::math::sph_neumann_prime<long double>(u, l));
#endif
}


/*
 * dynamics_structure.cpp
 *
 *  Created on: Jan 20, 2021
 *      Author: talhakavuncu
 */

#include <iostream>
#include <unsupported/Eigen/AdolcForward>
#include <adolc/adolc.h>
#include <Eigen/Dense>
#include "dynamics_structure.h"
namespace Unicycle_Dynamics
{
	unicycle_state_tensor dynamics(const unicycle_state_tensor & x,const unicycle_input_tensor & u)
	{
		unicycle_state_tensor x_dot;

	//	std::cout<<"no issues"<<std::endl;
	//	std::cout<<x.rows()<<std::endl;
		adouble px,py,theta,v,omega,a;
	//	px=x[0];
	//	py=x[1];
	//
	//	theta=x[2];
	//
	//	v=x[3];
	//
	//	omega=u[0];
	//	a=u[1];
	//
	//	x_dot[0]=v*cos(theta);
	//	x_dot[1]=v*sin(theta);
	//	x_dot[2]=omega;
	//	x_dot[3]=a;

	//	px=x[0];
	//	py=x[1];
	//
	//	theta=x[2];
	//
	//	v=x[3];
	//
	//	omega=u[0];
	//	a=u[1];

		x_dot[0]=x[3]*cos(x[2]);
		x_dot[1]=x[3]*sin(x[2]);
		x_dot[2]=u[0];
		x_dot[3]=u[1];

		return x_dot;

	};
	unicycle2_state_tensor dynamics_2(const unicycle2_state_tensor & x,const unicycle2_input_tensor & u)
	{
		unicycle2_state_tensor x_dot;

		for(int i=0;i<2;++i)
			x_dot.segment(4*i,4)=dynamics(x.segment(4*i,4),u.segment(2*i,2));

		return x_dot;
	};
	unicycle4_state_tensor dynamics_4(const unicycle4_state_tensor & x,const unicycle4_input_tensor & u)
	{
		unicycle4_state_tensor x_dot;

		for(int i=0;i<4;++i)
			x_dot.segment(4*i,4)=dynamics(x.segment(4*i,4),u.segment(2*i,2));

		return x_dot;
	};

	unicycle3_state_tensor dynamics_3(const unicycle3_state_tensor & x,const unicycle3_input_tensor & u)
	{
		unicycle3_state_tensor x_dot;
		for(int i=0;i<3;++i)
			x_dot.segment(4*i,4)=dynamics(x.segment(4*i,4),u.segment(2*i,2));
//	//	std::cout<<"no issues"<<std::endl;
//	//	std::cout<<x.rows()<<std::endl;
//		adouble px1,py1,theta1,v1,omega1,a1;
//		adouble px2,py2,theta2,v2,omega2,a2;
//		adouble px3,py3,theta3,v3,omega3,a3;
//		px1=x[0];
//		py1=x[1];
//		theta1=x[2];
//		v1=x[3];
//		omega1=u[0];
//		a1=u[1];
//
//		px2=x[4];
//		py2=x[5];
//		theta2=x[6];
//		v2=x[7];
//		omega2=u[2];
//		a2=u[3];
//
//
//		px3=x[8];
//		py3=x[9];
//		theta3=x[10];
//		v3=x[11];
//		omega3=u[4];
//		a3=u[5];
//
//		x_dot[0]=v1*cos(theta1);
//		x_dot[1]=v1*sin(theta1);
//		x_dot[2]=omega1;
//		x_dot[3]=a1;
//
//		x_dot[4]=v2*cos(theta2);
//		x_dot[5]=v2*sin(theta2);
//		x_dot[6]=omega2;
//		x_dot[7]=a2;
//
//		x_dot[8]=v3*cos(theta3);
//		x_dot[9]=v3*sin(theta3);
//		x_dot[10]=omega3;
//		x_dot[11]=a3;

		return x_dot;

	};

	unicycle6_state_tensor dynamics_6(const unicycle6_state_tensor & x,const unicycle6_input_tensor & u)
	{
		unicycle6_state_tensor x_dot;
		for(int i=0;i<6;++i)
			x_dot.segment(4*i,4)=dynamics(x.segment(4*i,4),u.segment(2*i,2));

		return x_dot;
	}
}

namespace Drone_Dynamics{

drone_state_tensor dynamics(const drone_state_tensor & X,const drone_input_tensor & u)
{
	drone_state_tensor X_dot;
	adouble x,y,z,phi,theta,psi,x_d,y_d,z_d,phi_d,theta_d,psi_d,w1,w2,w3,w4,thrust;
	x=X[0];
	y=X[1];
	z=X[2];
	phi=X[3];
	theta=X[4];
	psi=X[5];
	x_d=X[6];
	y_d=X[7];
	z_d=X[8];
	phi_d=X[9];
	theta_d=X[10];
	psi_d=X[11];

	w1=u[0];
	w2=u[1];
	w3=u[2];
	w4=u[3];

	Eigen::Matrix<adouble,3,3> Rsb,mat,mat_inv,I,I_inv;

	Rsb<<cos(theta)*cos(psi), cos(theta)*sin(psi), -sin(theta),
		-cos(phi)*sin(psi)+sin(theta)*cos(psi)*sin(phi), cos(psi)*cos(phi)+sin(theta)*sin(psi)*sin(phi), sin(phi)*cos(theta),
		sin(phi)*sin(psi)+cos(phi)*sin(theta)*cos(psi), -sin(phi)*cos(psi)+cos(phi)*sin(theta)*sin(psi), cos(theta)*cos(phi);

	mat<<1,0,-sin(theta),
		 0,cos(phi),sin(phi)*cos(theta),
		 0,-sin(phi),cos(theta)*cos(phi);

	mat_inv<<1,sin(phi)*tan(theta),cos(phi)*tan(theta),
			0,cos(phi),-sin(phi),
			0,sin(phi)/cos(theta),cos(phi)/cos(theta);

	I<<I_xx,0,0,
		0,I_yy,0,
		0,0,I_zz;

	I_inv<<1/I_xx,0,0,
		0,1/I_yy,0,
		0,0,1/I_zz;

	thrust=C_T*(w1*w1 + w2*w2 + w3*w3 + w4*w4);

	Eigen::Matrix<adouble,3,1> angle_dots,Omega_in,v_dots,gravity_term,thrust_term,pos_dots,Omega_b,L_b,Moments,angle_ddots,temp;
	Eigen::Matrix<adouble,3,3> skew1,skew2;
	pos_dots<<x_d,y_d,z_d;
	gravity_term<<0,0,g;
	thrust_term<<0,0,thrust/mass;
	angle_dots<<phi_d,theta_d,psi_d;
	Omega_in=Rsb*(mat*angle_dots);
//	temp=Omega_in.cross(pos_dots);
	skew1<<0,-Omega_in[2],Omega_in[1],
			Omega_in[2],0,-Omega_in[0],
			-Omega_in[1],Omega_in[0],0;
	v_dots=Rsb*thrust_term-gravity_term-skew1*pos_dots;//Omega_in.cross(pos_dots);

	X_dot[0]=x_d;
	X_dot[1]=y_d;
	X_dot[2]=z_d;
	X_dot[3]=phi_d;
	X_dot[4]=theta_d;
	X_dot[5]=psi_d;
	X_dot[6]=v_dots[0];
	X_dot[7]=v_dots[1];
	X_dot[8]=v_dots[2];

	Omega_b=mat*angle_dots;

	L_b=I*Omega_b;

	Moments<<d*C_T/sqrt(2)*(-w1*w1-w2*w2+w3*w3+w4*w4),
			d*C_T/sqrt(2)*(-w1*w1+w2*w2+w3*w3-w4*w4),
			C_D*(-w1*w1+w2*w2-w3*w3+w4*w4);

	skew2<<0,-Omega_b[2],Omega_b[1],
			Omega_b[2],0,-Omega_b[0],
			-Omega_b[1],Omega_b[0],0;

	angle_ddots=mat_inv*(I_inv*(Moments-skew2*L_b));//mat_inv*(I_inv*(Moments-Omega_b.cross(L_b))); //Euler's eqn.

	X_dot[9]=angle_ddots[0];
	X_dot[10]=angle_ddots[1];
	X_dot[11]=angle_ddots[2];

	return X_dot;

};

drone2_state_tensor dynamics_2(const drone2_state_tensor & X,const drone2_input_tensor & u)
{
	drone2_state_tensor X_dot;
	drone_state_tensor X_dot1,X_dot2;
	X_dot1=dynamics(X.segment(0, 12),u.segment(0,4));
	X_dot2=dynamics(X.segment(12, 12),u.segment(4,4));
	X_dot<<X_dot1,X_dot2;
	return X_dot;

//	drone2_state_tensor X_dot;
//	adouble x1,y1,z1,phi1,theta1,psi1,x_d1,y_d1,z_d1,phi_d1,theta_d1,psi_d1,w11,w21,w31,w41,thrust1;
//	adouble x2,y2,z2,phi2,theta2,psi2,x_d2,y_d2,z_d2,phi_d2,theta_d2,psi_d2,w12,w22,w32,w42,thrust2;
//	x1=X[0];
//	y1=X[1];
//	z1=X[2];
//	phi1=X[3];
//	theta1=X[4];
//	psi1=X[5];
//	x_d1=X[6];
//	y_d1=X[7];
//	z_d1=X[8];
//	phi_d1=X[9];
//	theta_d1=X[10];
//	psi_d1=X[11];
//
//	w11=u[0];
//	w21=u[1];
//	w31=u[2];
//	w41=u[3];
//
//	x2=X[12];
//	y2=X[13];
//	z2=X[14];
//	phi2=X[15];
//	theta2=X[16];
//	psi2=X[17];
//	x_d2=X[18];
//	y_d2=X[19];
//	z_d2=X[20];
//	phi_d2=X[21];
//	theta_d2=X[22];
//	psi_d2=X[23];
//
//	w12=u[4];
//	w22=u[5];
//	w32=u[6];
//	w42=u[7];
//
//	Eigen::Matrix<adouble,3,3> Rsb1,mat1,mat_inv1,I,I_inv,
//	Rsb2,mat2,mat_inv2;
//
//	Rsb1<<cos(theta1)*cos(psi1), cos(theta1)*sin(psi1), -sin(theta1),
//		-cos(phi1)*sin(psi1)+sin(theta1)*cos(psi1)*sin(phi1), cos(psi1)*cos(phi1)+sin(theta1)*sin(psi1)*sin(phi1), sin(phi1)*cos(theta1),
//		sin(phi1)*sin(psi1)+cos(phi1)*sin(theta1)*cos(psi1), -sin(phi1)*cos(psi1)+cos(phi1)*sin(theta1)*sin(psi1), cos(theta1)*cos(phi1);
//
//	mat1<<1,0,-sin(theta1),
//		 0,cos(phi1),sin(phi1)*cos(theta1),
//		 0,-sin(phi1),cos(theta1)*cos(phi1);
//
//	mat_inv1<<1,sin(phi1)*tan(theta1),cos(phi1)*tan(theta1),
//			0,cos(phi1),-sin(phi1),
//			0,sin(phi1)/cos(theta1),cos(phi1)/cos(theta1);
//
//
//
//	Rsb2<<cos(theta2)*cos(psi2), cos(theta2)*sin(psi2), -sin(theta2),
//		-cos(phi2)*sin(psi2)+sin(theta2)*cos(psi2)*sin(phi2), cos(psi2)*cos(phi2)+sin(theta2)*sin(psi2)*sin(phi2), sin(phi2)*cos(theta2),
//		sin(phi2)*sin(psi2)+cos(phi2)*sin(theta2)*cos(psi2), -sin(phi2)*cos(psi2)+cos(phi2)*sin(theta2)*sin(psi2), cos(theta2)*cos(phi2);
//
//	mat2<<1,0,-sin(theta2),
//		 0,cos(phi2),sin(phi2)*cos(theta2),
//		 0,-sin(phi2),cos(theta2)*cos(phi2);
//
//	mat_inv2<<1,sin(phi2)*tan(theta2),cos(phi2)*tan(theta2),
//			0,cos(phi2),-sin(phi2),
//			0,sin(phi2)/cos(theta2),cos(phi2)/cos(theta2);
//
//
//	I<<I_xx,0,0,
//		0,I_yy,0,
//		0,0,I_zz;
//
//	I_inv<<1/I_xx,0,0,
//		0,1/I_yy,0,
//		0,0,1/I_zz;
//
//
//
//
//
//	thrust1=C_T*(w11*w11 + w21*w21 + w31*w31 + w41*w41);
//	thrust2=C_T*(w12*w12 + w22*w22 + w32*w32 + w42*w42);
//
//	Eigen::Matrix<adouble,3,1> angle_dots1,Omega_in1,v_dots1,gravity_term1,thrust_term1,pos_dots1,Omega_b1,L_b1,Moments1,angle_ddots1,temp1;
//	Eigen::Matrix<adouble,3,1> angle_dots2,Omega_in2,v_dots2,gravity_term2,thrust_term2,pos_dots2,Omega_b2,L_b2,Moments2,angle_ddots2,temp2;
//	Eigen::Matrix<adouble,3,3> skew11,skew21,skew12,skew22;
//	pos_dots1<<x_d1,y_d1,z_d1;
//	gravity_term1<<0,0,g;
//	thrust_term1<<0,0,thrust1/mass;
//	angle_dots1<<phi_d1,theta_d1,psi_d1;
//	Omega_in1=Rsb1*(mat1*angle_dots1);
////	temp=Omega_in.cross(pos_dots);
//	skew11<<0,-Omega_in1[2],Omega_in1[1],
//			Omega_in1[2],0,-Omega_in1[0],
//			-Omega_in1[1],Omega_in1[0],0;
//	v_dots1=Rsb1*thrust_term1-gravity_term1-skew11*pos_dots1;//Omega_in.cross(pos_dots);
//
//	pos_dots2<<x_d2,y_d2,z_d2;
//	gravity_term2<<0,0,g;
//	thrust_term2<<0,0,thrust2/mass;
//	angle_dots2<<phi_d2,theta_d2,psi_d2;
//	Omega_in2=Rsb2*(mat2*angle_dots2);
////	temp=Omega_in.cross(pos_dots);
//	skew12<<0,-Omega_in2[2],Omega_in2[1],
//			Omega_in2[2],0,-Omega_in2[0],
//			-Omega_in2[1],Omega_in2[0],0;
//	v_dots2=Rsb2*thrust_term2-gravity_term2-skew12*pos_dots2;//Omega_in.cross(pos_dots);
//
//
//
//
//
//	X_dot[0]=x_d1;
//	X_dot[1]=y_d1;
//	X_dot[2]=z_d1;
//	X_dot[3]=phi_d1;
//	X_dot[4]=theta_d1;
//	X_dot[5]=psi_d1;
//	X_dot[6]=v_dots1[0];
//	X_dot[7]=v_dots1[1];
//	X_dot[8]=v_dots1[2];
//
//	X_dot[12]=x_d2;
//	X_dot[13]=y_d2;
//	X_dot[14]=z_d2;
//	X_dot[15]=phi_d2;
//	X_dot[16]=theta_d2;
//	X_dot[17]=psi_d2;
//	X_dot[18]=v_dots2[0];
//	X_dot[19]=v_dots2[1];
//	X_dot[20]=v_dots2[2];
//
//	Omega_b1=mat1*angle_dots1;
//	Omega_b2=mat2*angle_dots2;
//
//	L_b1=I*Omega_b1;
//	L_b2=I*Omega_b2;
//
//	Moments1<<d*C_T/sqrt(2)*(-w11*w11-w21*w21+w31*w31+w41*w41),
//			d*C_T/sqrt(2)*(-w11*w11+w21*w21+w31*w31-w41*w41),
//			C_D*(-w11*w11+w21*w21-w31*w31+w41*w41);
//
//	Moments2<<d*C_T/sqrt(2)*(-w12*w12-w22*w22+w32*w32+w42*w42),
//			d*C_T/sqrt(2)*(-w12*w12+w22*w22+w32*w32-w42*w42),
//			C_D*(-w12*w12+w22*w22-w32*w32+w42*w42);
//
//	skew21<<0,-Omega_b1[2],Omega_b1[1],
//			Omega_b1[2],0,-Omega_b1[0],
//			-Omega_b1[1],Omega_b1[0],0;
//
//	skew22<<0,-Omega_b2[2],Omega_b2[1],
//			Omega_b2[2],0,-Omega_b2[0],
//			-Omega_b2[1],Omega_b2[0],0;
//
//
//	angle_ddots1=mat_inv1*(I_inv*(Moments1-skew21*L_b1));//mat_inv*(I_inv*(Moments-Omega_b.cross(L_b))); //Euler's eqn.
//
//
//	angle_ddots2=mat_inv2*(I_inv*(Moments2-skew22*L_b2));
//
//
//	X_dot[9]=angle_ddots1[0];
//	X_dot[10]=angle_ddots1[1];
//	X_dot[11]=angle_ddots1[2];
//
//	X_dot[21]=angle_ddots2[0];
//	X_dot[22]=angle_ddots2[1];
//	X_dot[23]=angle_ddots2[2];
//
//	return X_dot;

}

}

namespace Single_Integrator_3D
{
	state_tensor dynamics(const state_tensor & x, const input_tensor & u)
	{
		return u;
	};

	state_tensor2 dynamics2(const state_tensor2 & x, const input_tensor2 & u)
	{
		return u;
	}
}
namespace Double_Integrator_3D
{
	state_tensor dynamics(const state_tensor & x, const input_tensor & u)
	{
		state_tensor x_dot;
		x_dot[0]=x[3];
		x_dot[1]=x[4];
		x_dot[2]=x[5];
		x_dot[3]=u[0]/mb;
		x_dot[4]=u[1]/mb;
		x_dot[5]=u[2]/mb;

		return x_dot;
	};
//
//	state_tensor2 dynamics2(const state_tensor2 & x, const input_tensor2 & u)
//	{
//		return u;
//	}
}
namespace Drone_First_Order_Dynamics
{
	typedef Eigen::Matrix<adouble,6,1> drone_state_tensor;
	typedef Eigen::Matrix<adouble,6,1> drone_input_tensor;

	typedef Eigen::Matrix<adouble,6*2,1> drone2_state_tensor;
	typedef Eigen::Matrix<adouble,6*2,1> drone2_input_tensor;

//	const double C_T=3.1582*1e-10;
//	const double C_D=7.9379*1e-12;
//	const double g=9.80665;
//	const double d=39.73*1e-3;
//	const double I_xx=1.395*1e-5;
//	const double I_yy=1.436*1e-5;
//	const double I_zz=2.173*1e-5;
//	const double mass=0.033;
	drone_state_tensor dynamics(const drone_state_tensor & X,const drone_input_tensor & input)
	{
		adouble x,y,z,phi,theta,psi,u,v,w,p,q,r,x_d,y_d,z_d,phi_d,theta_d,psi_d;
		drone_state_tensor x_dot;
		u=input[0];
		v=input[1];
		w=input[2];
		p=input[3];
		q=input[4];
		r=input[5];
		Eigen::Matrix<adouble,3,3> Rsb,mat,mat_inv;
		Eigen::Matrix<adouble,3,1> v_b,w_b,v_i,euler_d;

		v_b<<u,v,w;
		w_b<<p,q,r;

		Rsb<<cos(theta)*cos(psi), cos(theta)*sin(psi), -sin(theta),
				-cos(phi)*sin(psi)+sin(theta)*cos(psi)*sin(phi), cos(psi)*cos(phi)+sin(theta)*sin(psi)*sin(phi), sin(phi)*cos(theta),
				sin(phi)*sin(psi)+cos(phi)*sin(theta)*cos(psi), -sin(phi)*cos(psi)+cos(phi)*sin(theta)*sin(psi), cos(theta)*cos(phi);

//		mat<<1,0,-sin(theta),
//			 0,cos(phi),sin(phi)*cos(theta),
//			 0,-sin(phi),cos(theta)*cos(phi);

		mat_inv<<1,sin(phi)*tan(theta),cos(phi)*tan(theta),
				0,cos(phi),-sin(phi),
				0,sin(phi)/cos(theta),cos(phi)/cos(theta);

		v_i=Rsb*v_b;
		euler_d=mat_inv*w_b;

		x_d=v_i[0];
		y_d=v_i[1];
		z_d=v_i[2];

		phi_d=euler_d[0];
		theta_d=euler_d[1];
		psi_d=euler_d[2];

//		x_d=w*(sin(phi)*sin(psi)+cos(phi)*cos(psi)*sin(theta))-
//				v*(cos(phi)*sin(psi)-cos(psi)*sin(phi)*sin(theta))+
//				u*(cos(psi)*cos(theta));
//
//		y_d=v*(cos(phi)*cos(psi)+sin(phi)*sin(psi)*sin(theta))-
//				w*(cos(psi)*sin(phi)-cos(phi)*sin(psi)*sin(theta))+
//				u*(cos(theta)*sin(psi));
//		z_d=w*(cos(phi)*cos(theta))-u*(sin(theta))+v*(cos(theta)*sin(phi));
//
//		phi_d=p+r*(cos(phi)*tan(theta))+q*(sin(phi)*tan(theta));
//		theta_d=q*cos(phi)-r*sin(phi);
//		psi_d=r*cos(phi)/cos(theta)+q*sin(phi)/cos(theta);
//
		x_dot<<x_d,y_d,z_d,phi_d,theta_d,psi_d;

		return x_dot;
	}

	drone2_state_tensor dynamics_2(const drone2_state_tensor & x,const drone2_input_tensor & u)
	{
		drone2_state_tensor X_dot;
		drone_state_tensor X_dot1,X_dot2;
		X_dot1=dynamics(x.segment(0, 6),u.segment(0,6));
		X_dot2=dynamics(x.segment(6, 6),u.segment(6,6));
		X_dot<<X_dot1,X_dot2;
		return X_dot;
	}

}


#ifndef INCLUDED_STDDEFX
#include "stddefx.h"
#define INCLUDED_STDDEFX
#endif

#ifndef INCLUDED_COM_RIMAP
#include "com_rimap.h"
#define INCLUDED_COM_RIMAP
#endif

#ifndef INCLUDED_BOOST_MATH_SPECIAL_FUNCTIONS_ROUND
#include <boost/math/special_functions/round.hpp>
#define INCLUDED_BOOST_MATH_SPECIAL_FUNCTIONS_ROUND
#endif

/*!
  \file
  brief

  more elaborated
*/

//------------------------------------------------------------------------------
// DEFINITION OF STATIC CLASS MEMBERS
//------------------------------------------------------------------------------

//------------------------------------------------------------------------------
// DEFINITION OF CLASS MEMBERS
//------------------------------------------------------------------------------

com::RIMap::RIMap()

  : d_r1(0.0), d_r2(0.0), d_i1(0), d_i2(0), d_conv(0.0)

{
}

com::RIMap::RIMap(REAL8 r1, REAL8 r2, int i1, int i2)
{
  setRealRange(r1, r2);
  setIntRange(i1, i2);
}

com::RIMap::~RIMap()
{
}

void com::RIMap::recalc()
{
  if(d_r1 != d_r2)
    d_conv = static_cast<REAL8>(d_i2 - d_i1) / (d_r2 - d_r1);
  else
    d_conv = 0.0;
}

void com::RIMap::setRealRange(REAL8 r1, REAL8 r2)
{
  d_r1 = r1;
  d_r2 = r2;
  recalc();
}

void com::RIMap::setIntRange(int i1, int i2)
{
  d_i1 = i1;
  d_i2 = i2;
  recalc();
}

REAL8 com::RIMap::r1() const
{
  return d_r1;
}

REAL8 com::RIMap::r2() const
{
  return d_r2;
}

int com::RIMap::i1() const
{
  return d_i1;
}

int com::RIMap::i2() const
{
  return d_i2;
}

int com::RIMap::transform(REAL8 v) const
{
  return d_i1 + boost::math::iround((v - d_r1) * d_conv);
}

REAL8 com::RIMap::transform(int v) const
{
  if(d_conv == 0.0)
    return 0.0;
  else
    return d_r1 + static_cast<double>(v - d_i1) / d_conv;
}

bool com::RIMap::inRange(REAL8 r) const
{
  return d_r1 <= r && r <= d_r2;
}

bool com::RIMap::inRange(int i) const
{
  return d_i1 <= i && i <= d_i2;
}

//------------------------------------------------------------------------------
// DEFINITION OF FREE OPERATORS
//------------------------------------------------------------------------------

//------------------------------------------------------------------------------
// DEFINITION OF FREE FUNCTIONS
//------------------------------------------------------------------------------

//------------------------------------------------------------------------------
// DOCUMENTATION OF ENUMERATIONS
//------------------------------------------------------------------------------

//------------------------------------------------------------------------------
// DOCUMENTATION OF INLINE FUNCTIONS
//------------------------------------------------------------------------------

//------------------------------------------------------------------------------
// DOCUMENTATION OF PURE VIRTUAL FUNCTIONS
//------------------------------------------------------------------------------


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

  Copyright 2015 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#include <boost/simd/constant/sqrt_2.hpp>
#include <boost/simd/constant/two.hpp>
#include <boost/simd/as.hpp>
#include <simd_test.hpp>

STF_CASE_TPL( "Check sqrt_2 behavior for integral types"
            , (std::uint8_t)(std::uint16_t)(std::uint32_t)(std::uint64_t)
              (std::int8_t)(std::int16_t)(std::int32_t)(std::int64_t)
            )
{
  using boost::simd::as;
  using boost::simd::detail::sqrt_2;
  using boost::simd::Sqrt_2;

  STF_TYPE_IS(decltype(Sqrt_2<T>()), T);
  STF_EQUAL(Sqrt_2<T>(), T(1));
  STF_EQUAL(sqrt_2( as(T{}) ),T(1));
}

STF_CASE_TPL( "Check sqrt_2 behavior for floating types"
            , (double)(float)
            )
{
  using boost::simd::as;
  using boost::simd::detail::sqrt_2;
  using boost::simd::Sqrt_2;
  using boost::simd::Two;

  STF_TYPE_IS(decltype(Sqrt_2<T>()), T);
  auto z1 = Sqrt_2<T>();
  STF_ULP_EQUAL(z1*z1, Two<T>(), 0.5);
  auto z2 = sqrt_2( as(T{}) );
  STF_ULP_EQUAL(z2*z2, Two<T>(), 0.5);

}


// -*- C++ -*-

// The MIT License (MIT)
//
// Copyright (c) 2021 Alexander Samoilov
//
// 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.

#pragma once

#include <pybind11/pybind11.h>
#include <pybind11/eigen.h>
#include <pybind11/stl.h>
#include <pybind11/stl_bind.h>
#include <pybind11/numpy.h>
#include <Eigen/Dense>

/**
 *  @brief Contains fundamental type defintions used by the project.
 *
 */

namespace py = pybind11;

namespace detail {

template <typename T> inline T sqr(T a) { return a*a; }

inline double id(size_t i, size_t j) { return i==j ? 1.0 : 0.0; }

inline bool is_odd(size_t i) { return i&1; }

inline bool is_even(size_t i) { return !is_odd(i); }

}

using RowVectorXd = Eigen::RowVectorXd;

using RowMatrixXd = Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>;
// Use RowMatrixXd instead of MatrixXd
// see https://pybind11.readthedocs.io/en/stable/advanced/cast/eigen.html#storage-orders for more details


#include <cstdlib>
#include <algorithm>
#include <iostream>
#include <vector>
#include <Eigen/Dense>
#include <Eigen/Sparse>
#include <gtest/gtest.h>
#include "arpaca.hpp"

namespace arpaca {

template<typename T> struct PrecisionThreshold;

template<> struct PrecisionThreshold<float> { static const float value; };
template<> struct PrecisionThreshold<double> { static const double value; };

const float PrecisionThreshold<float>::value = 1e-3f;
const double PrecisionThreshold<double>::value = 1e-10;

std::vector<int> MakeRandomSequence(int n, int k)
{
  std::vector<int> seq;
  seq.reserve(n);
  for (int i = 0; i < n; ++i) seq.push_back(i);
  std::random_shuffle(seq.begin(), seq.end());
  seq.resize(k);
  return seq;
}

double GetRandomValue(double range)
{
  return 2 * range * std::rand() / RAND_MAX - range;
}

template<typename Scalar>
Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic>
MakeSymmetrixRandomMatrix(int n, int k, Scalar range)
{
  Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> X(n, n);
  X.setZero();

  std::vector<int> idx = MakeRandomSequence(n*n, 2*k);
  for (size_t i = 0; i < idx.size(); ++i)
    X(idx[i]/n, idx[i]%n) = GetRandomValue(range);

  return (X + X.transpose()) / 2;
}

template<typename Scalar>
Eigen::SparseMatrix<Scalar, Eigen::RowMajor>
MakeSparse(const Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic>& X)
{
  Eigen::SparseMatrix<Scalar, Eigen::RowMajor> S(X.rows(), X.cols());

  for (int i = 0; i < S.rows(); ++i) {
    S.startVec(i);
    for (int j = 0; j < S.cols(); ++j)
      if (X(i, j) != 0)
        S.insertBack(i, j) = X(i, j);
  }

  S.finalize();
  return S;
}

struct RandomMatrixTestParameter {
  int n;
  int k;
  int num_eigenvalues;
  int num_lanczos_vectors;
};

RandomMatrixTestParameter MakeParameter(int n, int k, int ne, int nlv)
{
  RandomMatrixTestParameter p = { n, k, ne, nlv };
  return p;
}

class RandomMatrixTest
    : public testing::TestWithParam<RandomMatrixTestParameter> {
 protected:
  template<typename Scalar>
  void TestOfScalar()
  {
    typedef Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> Matrix;
    typedef Eigen::SparseMatrix<Scalar, Eigen::RowMajor> SparseMatrix;
    typedef Eigen::Matrix<Scalar, Eigen::Dynamic, 1> Vector;

    RandomMatrixTestParameter p = GetParam();

    Matrix X = MakeSymmetrixRandomMatrix(p.n, p.k, Scalar(10.0));
    SparseMatrix S = MakeSparse(X);

    Eigen::SelfAdjointEigenSolver<Matrix> X_eigen(S);
    SymmetricEigenSolver<Scalar> S_eigen =
        Solve(S, p.num_eigenvalues, ALGEBRAIC_LARGEST, p.num_lanczos_vectors);

    Vector X_eigenvalues = X_eigen.eigenvalues().bottomRows(p.num_eigenvalues);
    Matrix X_eigenvectors =
        X_eigen.eigenvectors().rightCols(p.num_eigenvalues);
    Vector S_eigenvalues = S_eigen.MoveEigenvalues();
    Matrix S_eigenvectors = S_eigen.MoveEigenvectors();

    {
      const Scalar diff_mean =
          (X_eigenvalues - S_eigenvalues).cwiseAbs().mean();
      EXPECT_TRUE(diff_mean < PrecisionThreshold<Scalar>::value)
          << "eigenvalues diff_mean: " << diff_mean;
    }

    {
      Scalar diff_sum = 0.0;
      for (int i = 0; i < p.num_eigenvalues; ++i) {
        // Since each eigenvector given by X and S may have different signs,
        // we have to check either same or opposite.
        const Scalar diff1 =
            (X_eigenvectors.col(i) - S_eigenvectors.col(i)).cwiseAbs().mean();
        const Scalar diff2 =
            (X_eigenvectors.col(i) + S_eigenvectors.col(i)).cwiseAbs().mean();
        diff_sum += std::min(diff1, diff2);
      }
      const Scalar diff_mean = diff_sum / p.num_eigenvalues;

      EXPECT_TRUE(diff_mean < PrecisionThreshold<Scalar>::value)
          << "eigenvectors diff_mean: " << diff_mean;
    }

    std::cout << "info: " << S_eigen.GetInfo() << std::endl;
    std::cout << "# actual iterations: "
              << S_eigen.num_actual_iterations() << std::endl;
    std::cout << "# converged eigenvalues: "
              << S_eigen.num_converged_eigenvalues() << std::endl;
  }
};

TEST_P(RandomMatrixTest, Double)
{
  TestOfScalar<double>();
}

TEST_P(RandomMatrixTest, Float)
{
  TestOfScalar<float>();
}

INSTANTIATE_TEST_CASE_P(
    SizeTest,
    RandomMatrixTest,
    testing::Values(MakeParameter(100, 1000, 10, 50),
                    MakeParameter(1000, 10000, 100, 500),
                    MakeParameter(100, 1000, 50, 0)));

}  // namespace arpaca


#include <Eigen/Core>
#include "densecrf.h"
#include "pairwise.h"
#include "densecrf_wrapper.h"

DenseCRFWrapper::DenseCRFWrapper(int npixels, int nlabels, int nrank)
: m_npixels(npixels), m_nlabels(nlabels), m_nrank(nrank) {
	m_crf = new DenseCRF(npixels, nlabels);
}

DenseCRFWrapper::~DenseCRFWrapper() {
	delete m_crf;
}

int DenseCRFWrapper::npixels() { return m_npixels; }
int DenseCRFWrapper::nlabels() { return m_nlabels; }

void DenseCRFWrapper::add_pairwise_energy(float* pairwise_costs_ptr, float* features_ptr, int nfeatures) {
	m_crf->addPairwiseEnergy(
		Eigen::Map<const Eigen::MatrixXf>(features_ptr, nfeatures, m_npixels),
		new MatrixCompatibility(
			Eigen::Map<const Eigen::MatrixXf>(pairwise_costs_ptr, m_nlabels, m_nlabels)
		),
		DIAG_KERNEL,
		NORMALIZE_SYMMETRIC
	);
}

void DenseCRFWrapper::add_comparison_energy(float* pair_comp_ptr, float* label_comp_ptr) {

	PairwisePotential* comp_potential_ptr = new PairwisePotential(new MatrixCompatibility(
		Eigen::Map<const Eigen::MatrixXf>(label_comp_ptr, m_nlabels, m_nlabels) ),
		Eigen::Map<const Eigen::MatrixXf>(pair_comp_ptr, m_npixels, m_npixels));
	m_crf->addPairwiseEnergy(comp_potential_ptr);
}

void DenseCRFWrapper::add_comparison_energy_nystrom(float* ns_left_ptr, float* ns_right_ptr, float* label_comp_ptr) {

	PairwisePotential* comp_potential_ns_ptr = new PairwisePotential(new MatrixCompatibility(
		Eigen::Map<const Eigen::MatrixXf>(label_comp_ptr, m_nlabels, m_nlabels) ),
		Eigen::Map<const Eigen::MatrixXf>(ns_left_ptr, m_npixels, m_nrank),
		Eigen::Map<const Eigen::MatrixXf>(ns_right_ptr, m_nrank, m_npixels) );
	m_crf->addPairwiseEnergy(comp_potential_ns_ptr);
}

void DenseCRFWrapper::set_unary_energy(float* unary_costs_ptr) {
	m_crf->setUnaryEnergy(
		Eigen::Map<const Eigen::MatrixXf>(
			unary_costs_ptr, m_nlabels, m_npixels)
	);
}

void DenseCRFWrapper::map(int n_iters, int* labels) {
	VectorXs labels_vec = m_crf->map(n_iters);
	for (int i = 0; i < m_npixels; i ++)
		labels[i] = labels_vec(i);
}

void DenseCRFWrapper::inference(int n_iters, float* Q) {
	MatrixXf Q_vec = m_crf->inference(n_iters);
	memcpy(Q, Q_vec.data(), Q_vec.cols()*Q_vec.rows()*sizeof(float));
}


//---------------------------------------------------------------------------//
// 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://boostorg.github.com/compute for more information.
//---------------------------------------------------------------------------//

#ifndef BOOST_COMPUTE_ALGORITHM_DETAIL_RANDOM_FILL_HPP
#define BOOST_COMPUTE_ALGORITHM_DETAIL_RANDOM_FILL_HPP

#include <iterator>

#include <boost/compute/command_queue.hpp>
#include <boost/compute/random/default_random_engine.hpp>
#include <boost/compute/random/uniform_real_distribution.hpp>

namespace boost {
namespace compute {
namespace detail {

template<class OutputIterator, class Generator>
inline void random_fill(OutputIterator first,
                        OutputIterator last,
                        Generator &g,
                        command_queue &queue)
{
    g.fill(first, last, queue);
}

template<class OutputIterator>
inline void
random_fill(OutputIterator first,
            OutputIterator last,
            typename std::iterator_traits<OutputIterator>::value_type lo,
            typename std::iterator_traits<OutputIterator>::value_type hi,
            command_queue &queue)
{
    typedef typename
        std::iterator_traits<OutputIterator>::value_type value_type;
    typedef typename
        boost::compute::default_random_engine engine_type;
    typedef typename
        boost::compute::uniform_real_distribution<value_type> distribution_type;

    engine_type engine(queue);
    distribution_type generator(lo, hi);
    generator.fill(first, last, engine, queue);
}

} // end detail namespace
} // end compute namespace
} // end boost namespace

#endif // BOOST_COMPUTE_ALGORITHM_DETAIL_RANDOM_FILL_HPP


/* boost random/geometric_distribution.hpp header file
 *
 * Copyright Jens Maurer 2000-2001
 * Permission to use, copy, modify, sell, and distribute this software
 * is hereby granted without fee provided that the above copyright notice
 * appears in all copies and that both that copyright notice and this
 * permission notice appear in supporting documentation,
 *
 * Jens Maurer makes no representations about the suitability of this
 * software for any purpose. It is provided "as is" without express or
 * implied warranty.
 *
 * See http://www.boost.org for most recent version including documentation.
 *
 * $Id: geometric_distribution.hpp 11696 2001-11-14 21:53:38Z jmaurer $
 *
 * Revision history
 *  2001-02-18  moved to individual header files
 */

#ifndef BOOST_RANDOM_GEOMETRIC_DISTRIBUTION_HPP
#define BOOST_RANDOM_GEOMETRIC_DISTRIBUTION_HPP

#include <cmath>          // std::log
#include <cassert>
#include <boost/random/uniform_01.hpp>

namespace boost {

#if defined(__GNUC__) && (__GNUC__ < 3)
// Special gcc workaround: gcc 2.95.x ignores using-declarations
// in template classes (confirmed by gcc author Martin v. Loewis)
  using std::log;
#endif

// geometric distribution: p(i) = (1-p) * pow(p, i-1)   (integer)
template<class UniformRandomNumberGenerator, class IntType = int>
class geometric_distribution
{
public:
  typedef UniformRandomNumberGenerator base_type;
  typedef IntType result_type;

  geometric_distribution(base_type & rng, double p)
    : _rng(rng)
  {
    assert(0.0 < p && p < 1.0);
#ifndef BOOST_NO_STDC_NAMESPACE
    using std::log;
#endif
    _log_p = log(p);
  }
  // compiler-generated copy ctor is fine
  // uniform_01 cannot be assigned, neither can this class

  result_type operator()()
  {
#ifndef BOOST_NO_STDC_NAMESPACE
    using std::log;
#endif
    return IntType (log(1-_rng()) / _log_p) + 1;
  }

#ifndef BOOST_NO_OPERATORS_IN_NAMESPACE
  friend bool operator==(const geometric_distribution& x,
                         const geometric_distribution& y)
  { return x._log_p == y._log_p && x._rng == y._rng; }
#else
  // Use a member function
  bool operator==(const geometric_distribution& rhs) const
  { return _log_p == rhs._log_p && _rng == rhs._rng;  }
#endif
private:
  uniform_01<base_type> _rng;
  typename uniform_01<base_type>::result_type _log_p;
};

} // namespace boost

#endif // BOOST_RANDOM_GEOMETRIC_DISTRIBUTION_HPP


/*
Author: Andrej Karpathy (http://cs.stanford.edu/~karpathy/)
1 May 2012
BSD licence
*/

#include <string>
#include <vector>
#include <stdlib.h>

#include <Eigen/Eigen>
#include <eigenlibsvm/svm_utils.h>
#include <eigenlibsvm/eigen_extensions.h>

using namespace std;
using namespace esvm;

// run from build folder of the project (see path below)
int main (int argc, char** argv) {

  Eigen::MatrixXf X;
  Eigen::MatrixXf y;
  eigen_extensions::loadASCII("../test/svmtestx.eig.txt", &X);
  eigen_extensions::loadASCII("../test/svmtesty.eig.txt", &y);

  // Classify
  cout << X.topRows(5) << endl;
  cout << y.topRows(5) << endl;
  vector<int> yhat;
  SVMClassifier svm;
  svm.train(X, y);
  svm.test(X, yhat);

  Eigen::MatrixXf w;
  float b;
  svm.getw(w, b);
  Eigen::MatrixXf margin= ((X * w).array() + b).matrix(); // ahh eigen...

  // Evaluate accuracy and print results
  int match=0;
  for(int i=0;i<yhat.size();i++) {
    if(yhat[i]==(int)y(i)) match++; else printf("WRONG! ");
    printf("y= %d, yhat= %d margin= %f\n", (int)y(i), yhat[i], margin(i));
  }
  printf("MATLAB cross-check: last margin should be around -3.107\n");
  printf("Accuracy= %f. From MATLAB cross-check, expect this to be around 0.945\n", 1.0*match/yhat.size());

  // Save the model
  svm.saveModel("temp.svmmodel");

  // Test loading model in new instance of SVMClassifier
  SVMClassifier svm2;
  svm2.loadModel("temp.svmmodel");
  yhat.clear();
  svm2.test(X, yhat);
  match=0;
  for(int i=0;i<yhat.size();i++) if(yhat[i]==(int)y(i)) match++;
  printf("Accuracy= %f from loaded model. Should be 0.945 again.\n", 1.0*match/yhat.size());

  printf("you may want to rm the temporary file temp.svmmodel\n");
}


#include <boost/test/unit_test.hpp>

#include <smtrat-modules/SATModule/mcsat/BaseBackend.h>
#include <smtrat-common/smtrat-common.h>

#include <carl-model/evaluation/ModelEvaluation.h>
#include <smtrat-mcsat/assignments/arithmetic/AssignmentFinder.h>
#include <smtrat-mcsat/explanations/nlsat/Explanation.h>

using namespace smtrat;

BOOST_AUTO_TEST_SUITE(Test_AssignmentFinder);

/**
 * Tests Example 3 from the NLSAT-Paper
 */
BOOST_AUTO_TEST_CASE(Test_NLSATPaper_Ex3)
{

	carl::Variable x = carl::freshRealVariable("x");
	carl::Variable y = carl::freshRealVariable("y");

	// Original constraints
	FormulaT c1(Poly(x)+Rational(1), carl::Relation::LEQ);
	FormulaT c2(Poly(x)-Rational(1), carl::Relation::GEQ);
	FormulaT c3(Poly(x)+y, carl::Relation::GREATER);
	FormulaT c4(Poly(x)-y, carl::Relation::GREATER);
	// Learnt from first conflict
	FormulaT c5(Poly(x), carl::Relation::LESS);

	// Explanation for first conflict
	FormulaT ex1(carl::FormulaType::OR, { c3.negated(), c4.negated(), c5.negated() });
	// Explanation for second conflict
	FormulaT ex2(carl::FormulaType::OR, { c1.negated(), c5 });

	mcsat::MCSATBackend<mcsat::MCSATSettingsNL> nlsat;

	// Decide x+1<=0
	SMTRAT_LOG_INFO("smtrat.test.nlsat", "Decide " << c1);
	nlsat.pushConstraint(c1);

	// T-Decide
	SMTRAT_LOG_INFO("smtrat.test.nlsat", "T-Decide " << x << " = -2");
	{
		auto res = nlsat.findAssignment(x);
		BOOST_CHECK(carl::variant_is_type<mcsat::ModelValues>(res));
		auto value = boost::get<mcsat::ModelValues>(res);
		BOOST_CHECK(value.size() == 1);
		BOOST_CHECK(value[0].first == x);
		BOOST_CHECK(value[0].second == ModelValue(Rational(-1)));
	}
	FormulaT fx(Poly(x) + Rational(2), carl::Relation::EQ);
	nlsat.pushAssignment(x, Rational(-2), fx);

	// Propagate x+y>0
	SMTRAT_LOG_INFO("smtrat.test.nlsat", "Propagate " << c3);
	nlsat.pushConstraint(c3);

	// Check whether x-y>0 is feasible, propagate x-y<=0
	SMTRAT_LOG_INFO("smtrat.test.nlsat", "T-Propagate " << c4.negated());
	{
		auto res = nlsat.isInfeasible(y, c4);
		BOOST_CHECK(carl::variant_is_type<FormulasT>(res));
		auto r = boost::get<FormulasT>(res);
		auto explanation = nlsat.explain(y, r);
		BOOST_CHECK(boost::get<FormulaT>(&explanation) != nullptr);
		BOOST_CHECK(ex1 == boost::get<FormulaT>(explanation));
	}
	nlsat.pushConstraint(c4.negated());

	// Backtrack assignment
	SMTRAT_LOG_INFO("smtrat.test.nlsat", "Backtrack " << x);
	nlsat.popAssignment(x);

	// Check whether x>0 is feasible, propagate x<=0
	SMTRAT_LOG_INFO("smtrat.test.nlsat", "T-Propagate " << c5);
	{
		auto res = nlsat.isInfeasible(x, c5.negated());
		BOOST_CHECK(carl::variant_is_type<FormulasT>(res));
		auto r = boost::get<FormulasT>(res);
		auto explanation = nlsat.explain(x, r);
		BOOST_CHECK(boost::get<FormulaT>(&explanation) != nullptr);
		BOOST_CHECK(ex2 == boost::get<FormulaT>(explanation));
	}
}

BOOST_AUTO_TEST_CASE(CoverComputation)
{
	carl::Variable a = carl::freshRealVariable("a");
	carl::Variable b = carl::freshRealVariable("b");
	Poly p = Poly(20)*a*a*b - Poly(a)*a*a*a*b - Poly(120)*b;
	carl::UnivariatePolynomial<Rational> q(a, {
		Rational(900),
		Rational(0),
		Rational(-2550),
		Rational(0),
		Rational(1055)/2,
		Rational(0),
		Rational(-40),
		Rational(0),
		Rational(1)
	});
	carl::Interval<Rational> i(Rational(633)/1025, carl::BoundType::STRICT, Rational(2533)/4096, carl::BoundType::STRICT);
	FormulaT f(p, carl::Relation::LESS);

	std::cout << "Constructing RAN on " << q << " / " << i << std::endl;
	carl::RealAlgebraicNumber<Rational> ran(q, i);
	std::cout << "-> " << ran << std::endl;

	Model model;
	model.assign(a, ran);
	model.assign(b, Rational(-3));

	auto res = carl::model::evaluate(f, model);
	std::cout << f << " on " << model << " -> " << res << std::endl;

	model.assign(b, Rational(1));
	res = carl::model::evaluate(f, model);
	std::cout << f << " on " << model << " -> " << res << std::endl;
}

BOOST_AUTO_TEST_CASE(AssignmentFinderBug) {
	// assign variable b with constraints (a ! > rootExpr(1 + 3*__z^3 + -3*b^3 + 3*__z^6 + -6*__z^3*b^3 + 3*b^6 + __z^9 + -3*__z^6*b^3 + 3*__z^3*b^6 + -1*b^9, 1, __z)) under a = 2

	carl::Variable a = carl::freshRealVariable("a");
	carl::Variable b = carl::freshRealVariable("b");

	Model model;
	model.assign(a, Rational(2));

	const carl::Variable& z = MultivariateRootT::var();
	Poly poly = Poly(Rational(1)) + Rational(3)*z*z*z - Rational(3)*b*b*b + Rational(3)*z*z*z*z*z*z - Rational(6)*z*z*z*b*b*b
				+ Rational(3)*b*b*b*b*b*b + z*z*z*z*z*z*z*z*z - Rational(3)*z*z*z*z*z*z*b*b*b + Rational(3)*z*z*z*b*b*b*b*b*b
				- b*b*b*b*b*b*b*b*b;
	MultivariateRootT mvroot(poly, 1);
	VariableComparisonT varcomp(a, mvroot, carl::Relation::GREATER, true);
	FormulaT formula(varcomp);
	std::cout << "Looking at " << varcomp << std::endl;

	// proof that an assignment for b exist
	Model model2 = model;
	model2.assign(b, Rational(3));
	auto res = carl::model::evaluate(formula, model2);
	BOOST_CHECK(res.isBool());
	BOOST_CHECK(res.asBool());

	// call assignment finder
	mcsat::Bookkeeping bookkeeping;
	bookkeeping.pushConstraint(formula);
	bookkeeping.pushAssignment(a, Rational(2), FormulaT(carl::FormulaType::TRUE));
	mcsat::arithmetic::AssignmentFinder af;
	auto afres = af(bookkeeping, b);
	BOOST_CHECK(afres);
	BOOST_CHECK(carl::variant_is_type<mcsat::ModelValues>(*afres));

	FormulaT tmp;
	{
		bookkeeping.pushAssignment(b, Rational(0), FormulaT(carl::FormulaType::TRUE));
		mcsat::nlsat::Explanation expl;
		auto ex = expl(bookkeeping, b, { formula });
		assert(ex);
		std::cout << *ex << std::endl;
		tmp = boost::get<FormulaT>(*ex).subformulas()[2];
		bookkeeping.popAssignment(b);
	}
	std::cout << "tmp = " << tmp << std::endl;
	bookkeeping.pushConstraint(tmp);
	{
		mcsat::arithmetic::AssignmentFinder af;
		auto afres = af(bookkeeping, b);
		BOOST_CHECK(afres);
		BOOST_CHECK(carl::variant_is_type<mcsat::ModelValues>(*afres));
		std::cout << *afres << std::endl;
	}
}

BOOST_AUTO_TEST_SUITE_END();


/**
 *           c++11-only implementation of the L-BFGS-B algorithm
 *
 * Copyright (c) 2014 Patrick Wieschollek
 *               https://github.com/PatWie/LBFGSB
 * All rights reserved.
 *
 * Permission is hereby granted, free of charge, to any person obtaining a copy
 * of this software and associated documentation files (the "Software"), to deal
 * in the Software without restriction, including without limitation the rights
 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 * copies of the Software, and to permit persons to whom the Software is
 * furnished to do so, subject to the following conditions:

 * The above copyright notice and this permission notice shall be included in all
 * copies or substantial portions of the Software.

 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
 * SOFTWARE.
 */

#ifndef LBFGSB_H_
#define LBFGSB_H_

#include "meta.h"

#include <list>
#include <stdio.h>
#include <iostream>
#include <functional>
#include <vector>
#include <armadillo>

/* coded from scratch !!!
 * based on the paper
 * A LIMITED MEMORY ALGORITHM FOR BOUND CONSTRAINED OPTIMIZATION
 * (Byrd, Lu, Nocedal, Zhu)
 */

class LBFGSB {

  // contains options for optimization process
  Options Options_;

  // oracles for function value and gradient
  FunctionOracleType FunctionObjectiveOracle_;
  GradientOracleType FunctionGradientOracle_;

  arma::mat W, M;
  arma::vec lb, ub;
  double theta;
  int DIM;

  std::list<arma::vec> xHistory;

public:

  arma::vec XOpt;

  LBFGSB(const arma::vec & l, const arma::vec & u) :
      lb(l), ub(u), theta(1.0), DIM(l.n_rows) {
    lb = l;
    ub = u;
    theta = 1.0;
    DIM = l.n_rows;
    W = arma::zeros(DIM, 0);
    M = arma::zeros(0, 0);
  }

  LBFGSB(Options & Options, const arma::vec & l, const arma::vec & u) {
    Options_ = Options;
    lb = l;
    ub = u;
    theta = 1.0;
    DIM = l.n_rows;
    W = arma::zeros(DIM, 0);
    M = arma::zeros(0, 0);

  }

  /// <summary>
  /// find cauchy point in x
  /// </summary>
  /// <parameter name="x">start in x</parameter>
  void
  GetGeneralizedCauchyPoint(arma::vec & x, arma::vec & g, arma::vec & x_cauchy,
                            arma::vec & c) {
    const int DIM = x.n_rows;
    // PAGE 8
    // Algorithm CP: Computation of the generalized Cauchy point
    // Given x,l,u,g, and B = \theta I-WMW

    // {all t_i} = { (idx,value), ... }
    // TODO: use "std::set" ?
    std::vector<std::pair<int, double> > SetOfT;
    // the feasible set is implicitly given by "SetOfT - {t_i==0}"
    arma::vec d = arma::zeros(DIM);

    // n operations
    for (int j = 0; j < DIM; j++) {
      if (g(j) == 0) {
        SetOfT.push_back(std::make_pair(j, INF));
      } else {
        double tmp = 0;
        if (g(j) < 0) {
          tmp = (x(j) - ub(j)) / g(j);
        } else {
          tmp = (x(j) - lb(j)) / g(j);
        }
        d(j) = -g(j);
        SetOfT.push_back(std::make_pair(j, tmp));
      }

    }Debug(d.transpose());

    // paper: using heapsort
    // sortedindices [1,0,2] means the minimal element is on the 1th entry
    std::vector<int> SortedIndices = sort_indexes(SetOfT);

    x_cauchy = x;
    // Initialize
    // p := 	W^T*p
    arma::vec p = (W.t() * d);            // (2mn operations)
    // c := 	0
    c = arma::zeros(M.n_rows);
    // f' := 	g^T*d = -d^Td
    double f_prime = arma::dot(-d, d);              // (n operations)
    // f'' :=	\theta*d^T*d-d^T*W*M*W^T*d = -\theta*f' - p^T*M*p
    double f_doubleprime = (double) (-1.0 * theta) * f_prime -
                           arma::dot(p, M * p);// (O(m^2) operations)
    // \delta t_min :=	-f'/f''
    double dt_min = -f_prime / f_doubleprime;
    // t_old := 	0
    double t_old = 0;
    // b := 	argmin {t_i , t_i >0}
    int i = 0;
    for (int j = 0; j < DIM; j++) {
      i = j;
      if (SetOfT[SortedIndices[j]].second != 0)
        break;
    }
    int b = SortedIndices[i];
    // see below
    // t        			:= 	min{t_i : i in F}
    double t = SetOfT[b].second;
    // \delta t 			:= 	t - 0
    double dt = t - t_old;

    // examination of subsequent segments
    while ((dt_min >= dt) && (i < DIM)) {
      if (d(b) > 0)
        x_cauchy(b) = ub(b);
      else if (d(b) < 0)
        x_cauchy(b) = lb(b);

      // z_b = x_p^{cp} - x_b
      double zb = x_cauchy(b) - x(b);
      // c   :=  c +\delta t*p
      c += dt * p;
      // cache
      arma::rowvec wbt = W.row(b);

      f_prime += dt * f_doubleprime + g(b) * g(b)
                 + theta * g(b) * zb
                 - g(b) * arma::dot(wbt.t(), (M * c));
      f_doubleprime += -1.0 * theta * g(b) * g(b);
      f_doubleprime += 2.0 * g(b) * arma::dot(wbt.t(), M * p);
      // TODO Check M * wbt.t() is correct
      f_doubleprime += -g(b) * g(b) * arma::dot(wbt.t(), M * wbt.t());
      p += g(b) * wbt.t();
      d(b) = 0;
      dt_min = -f_prime / f_doubleprime;
      t_old = t;
      ++i;
      if (i < DIM) {
        b = SortedIndices[i];
        t = SetOfT[b].second;
        dt = t - t_old;
      }

    }

    dt_min = max(dt_min, 0);
    t_old += dt_min;

    Debug(SortedIndices[0] << " " << SortedIndices[1]);

    for (int ii = i; ii < x_cauchy.n_rows; ii++) {
      x_cauchy(SortedIndices[ii]) = x(SortedIndices[ii])
                                    + t_old * d(SortedIndices[ii]);
    }Debug(x_cauchy.transpose());

    c += dt_min * p;
    Debug(c.transpose());

  }

  /// <summary>
  /// find valid alpha for (8.5)
  /// </summary>
  /// <parameter name="x_cp">cauchy point</parameter>
  /// <parameter name="du">unconstrained solution of subspace minimization</parameter>
  /// <parameter name="FreeVariables">flag (1 if is free variable and 0 if is not free variable)</parameter>
  double FindAlpha(arma::vec & x_cp, arma::vec & du,
                   std::vector<int> & FreeVariables) {
    /* this returns
     * a* = max {a : a <= 1 and  l_i-xc_i <= a*d_i <= u_i-xc_i}
     */
    double alphastar = 1;
    const unsigned int n = FreeVariables.size();
    for (unsigned int i = 0; i < n; i++) {
      if (du(i) > 0) {
        alphastar = min(alphastar,
                        (ub(FreeVariables[i]) - x_cp(FreeVariables[i]))
                        / du(i));
      } else {
        alphastar = min(alphastar,
                        (lb(FreeVariables[i]) - x_cp(FreeVariables[i]))
                        / du(i));
      }
    }
    return alphastar;
  }

  /// <summary>
  /// using linesearch to determine step width
  /// </summary>
  /// <parameter name="x">start in x</parameter>
  /// <parameter name="dx">direction</parameter>
  /// <parameter name="f">current value of objective (will be changed)</parameter>
  /// <parameter name="g">current gradient of objective (will be changed)</parameter>
  /// <parameter name="t">step width (will be changed)</parameter>
  void LineSearch(arma::vec & x, arma::vec dx, double & f, arma::vec & g,
                  double & t) {

    const double alpha = 0.2;
    const double beta = 0.8;

    const double f_in = f;
    const arma::vec g_in = g;
    const double Cache = alpha * arma::dot(g_in, dx);

    t = 1.0;
    f = FunctionObjectiveOracle_(x + t * dx);
    while (f > f_in + t * Cache) {
      t *= beta;
      f = FunctionObjectiveOracle_(x + t * dx);
    }
    FunctionGradientOracle_(x + t * dx, g);
    x += t * dx;

  }

  /// <summary>
  /// direct primal approach
  /// </summary>
  /// <parameter name="x">start in x</parameter>
  void SubspaceMinimization(arma::vec & x_cauchy, arma::vec & x, arma::vec & c,
                            arma::vec & g,
                            arma::vec & SubspaceMin) {

    // cached value: ThetaInverse=1/theta;
    double theta_inverse = 1 / theta;

    // size of "t"
    std::vector<int> FreeVariablesIndex;
    Debug(x_cauchy.transpose());

    //std::cout << "free vars " << FreeVariables.rows() << std::endl;
    for (int i = 0; i < x_cauchy.n_rows; i++) {
      Debug(x_cauchy(i) << " " << ub(i) << " " << lb(i));
      if ((x_cauchy(i) != ub(i)) && (x_cauchy(i) != lb(i))) {
        FreeVariablesIndex.push_back(i);
      }
    }
    const int FreeVarCount = FreeVariablesIndex.size();

    arma::mat WZ = arma::zeros(W.n_cols, FreeVarCount);

    for (int i = 0; i < FreeVarCount; i++)
      WZ.col(i) = W.row(FreeVariablesIndex[i]).t();

    Debug(WZ);

    // r=(g+theta*(x_cauchy-x)-W*(M*c));
    Debug(g);Debug(x_cauchy);Debug(x);
    arma::vec rr = (g + theta * (x_cauchy - x) - W * (M * c));
    // r=r(FreeVariables);
    arma::vec r = arma::zeros(FreeVarCount);
    for (int i = 0; i < FreeVarCount; i++)
      r.row(i) = rr.row(FreeVariablesIndex[i]);

    Debug(r.transpose());

    // STEP 2: "v = w^T*Z*r" and STEP 3: "v = M*v"
    arma::vec v = M * (WZ * r);
    // STEP 4: N = 1/theta*W^T*Z*(W^T*Z)^T
    arma::mat N = theta_inverse * WZ * WZ.t();
    // N = I - MN
    N = arma::eye(N.n_rows, N.n_rows) - M * N;
    // STEP: 5
    // v = N^{-1}*v
    v = arma::inv(N) * v;
    // STEP: 6
    // HERE IS A MISTAKE IN THE ORIGINAL PAPER!
    arma::vec du = -theta_inverse * r
                   - theta_inverse * theta_inverse * WZ.t() * v;
    Debug(du.transpose());
    // STEP: 7
    double alpha_star = FindAlpha(x_cauchy, du, FreeVariablesIndex);

    // STEP: 8
    arma::vec dStar = alpha_star * du;

    SubspaceMin = x_cauchy;
    for (int i = 0; i < FreeVarCount; i++) {
      SubspaceMin(FreeVariablesIndex[i]) = SubspaceMin(
          FreeVariablesIndex[i]) + dStar(i);
    }
  }

  void Solve(arma::vec & x0, const FunctionOracleType & FunctionValue,
             const GradientOracleType & FunctionGradient) {
    FunctionObjectiveOracle_ = FunctionValue;
    FunctionGradientOracle_ = FunctionGradient;

    Assert(x0.n_rows == lb.n_rows, "lower bound size incorrect");
    Assert(x0.n_rows == ub.n_rows, "upper bound size incorrect");

    Assert(arma::all(x0 >= lb),
           "seed is not feasible (violates lower bound)");
    Assert(arma::all(x0 <= ub),
           "seed is not feasible (violates upper bound)");

    const int DIM = x0.n_rows;

    xHistory.push_back(x0);

    arma::mat yHistory = arma::zeros(DIM, 0);
    arma::mat sHistory = arma::zeros(DIM, 0);

    arma::vec x = x0, g;
    int k = 0;

    double f = FunctionObjectiveOracle_(x);
    FunctionGradientOracle_(x, g);

    theta = 1.0;

    W = arma::zeros(DIM, 0);
    M = arma::zeros(0, 0);

    auto noConvergence =
        [&](arma::vec & x, arma::vec & g) -> bool {
            arma::vec clamped = x - g;
            const auto too_big = arma::find(clamped > lb);
            clamped(too_big) = lb(too_big);
            const auto too_small = arma::find(clamped < ub);
            clamped(too_small) = ub(too_small);
            return (arma::norm(clamped - x, "inf") >= Options_.tol);
        };

    while (noConvergence(x, g) && (k < Options_.maxIter)) {
      Debug("iteration " << k)
      double f_old = f;
      arma::vec x_old = x;
      arma::vec g_old = g;

      // STEP 2: compute the cauchy point by algorithm CP
      arma::vec CauchyPoint = arma::zeros(DIM);
      arma::vec c = arma::zeros(DIM);
      GetGeneralizedCauchyPoint(x, g, CauchyPoint, c);
      // STEP 3: compute a search direction d_k by the primal method
      arma::vec SubspaceMin;
      SubspaceMinimization(CauchyPoint, x, c, g, SubspaceMin);

      arma::mat H;
      double Length = 0;

      // STEP 4: perform linesearch and STEP 5: compute gradient
      LineSearch(x, SubspaceMin - x, f, g, Length);

      xHistory.push_back(x);

      // prepare for next iteration
      arma::vec newY = g - g_old;
      arma::vec newS = x - x_old;

      // STEP 6:
      double test = arma::dot(newS, newY);
      test = (test < 0) ? -1.0 * test : test;

      if (test > EPS * arma::norm(newY, 2)) {
        if (k < Options_.m) {
          yHistory.resize(DIM, k + 1);
          sHistory.resize(DIM, k + 1);
        } else {

          yHistory.head_cols(Options_.m - 1) = yHistory.tail_cols(
              Options_.m - 1);
          sHistory.head_cols(Options_.m - 1) = sHistory.tail_cols(
              Options_.m - 1);
        }
        yHistory.tail_cols(1) = newY;
        sHistory.tail_cols(1) = newS;

        // STEP 7:
        theta = arma::dot(newY, newY)
                / arma::dot(newY, newS);

        W = arma::zeros(yHistory.n_rows,
                        yHistory.n_cols + sHistory.n_cols);

        W(0, 0, arma::size(yHistory)) = yHistory;

        W(0, yHistory.n_cols, arma::size(sHistory)) = theta * sHistory;

        arma::mat A = sHistory.t() * yHistory;
        arma::mat L = arma::trimatl(A);
        arma::mat MM(A.n_rows + L.n_rows, A.n_rows + L.n_cols);
        arma::mat D = -arma::diagmat(A);
        MM(0, 0, arma::size(D)) = D;
        MM(0, D.n_cols, arma::size(L.t())) = L.t();
        MM(D.n_rows, 0, arma::size(L)) = L;
        arma::mat bottom_right = (sHistory.t() * sHistory)
                                 * theta;
        MM(D.n_rows, D.n_cols, arma::size(bottom_right)) = bottom_right;

        M = arma::inv(MM);
      }

      arma::vec ttt = arma::zeros(1);
      ttt(0) = f_old - f;
      Debug("--> " << arma::norm(ttt));
      if (arma::norm(ttt) < Options_.tol) {
        // successive function values too similar
        break;
      }
      k++;

    }

    XOpt = x;
    x0 = x;

  }
};

#endif /* LBFGSB_H_ */


#pragma once

#include <boost/math/constants/constants.hpp>
#include <cmath>

#include "GNAObject.hh"
#include "ParametricLazy.hpp"
#include "TypesFunctions.hh"

class GaussianPeakWithBackground: public GNAObject,
                                  public TransformationBind<GaussianPeakWithBackground> {
public:
  GaussianPeakWithBackground(double n=1) {
    variable_(&m_b, "BackgroundRate");
    variable_(&m_mu, "Mu");
    variable_(&m_E0, "E0");
    variable_(&m_w, "Width");
    using namespace ParametricLazyOps;

    m_w_scaled = mkdep(m_w*std::sqrt( n ));
    m_E0_scaled = mkdep(m_E0*n);

    transformation_("rate")
      .input("E")
      .output("rate")
      .types(TypesFunctions::pass<0,0>)
      .func(&GaussianPeakWithBackground::calcRate)
      ;
  }

  void calcRate(FunctionArgs fargs) {
    const double pi = boost::math::constants::pi<double>();
    const auto &E = fargs.args[0].arr;
    fargs.rets[0].arr = m_b + m_mu*(1./std::sqrt(2*pi)/m_w_scaled)*(-(E-m_E0_scaled).square()/(2*m_w_scaled*m_w_scaled)).exp();
  }
protected:
  variable<double> m_b, m_mu, m_E0, m_w;
  dependant<double> m_w_scaled, m_E0_scaled;
};


#include <transwarp.h>
#include <iostream>
#include <boost/math/constants/constants.hpp>
#include <parallel/numeric>

namespace daq
{
    constexpr double f = 15.0;       // Hz
    constexpr double rate = 95000.0; // Hz

    int ricker_filter_size(double f_, double rate_, double width = 5.0) noexcept
    {
        constexpr double ricker_coefficient = 2.2508;
        return static_cast<int>(lround (width / ricker_coefficient / f_ * rate_ + 0.5));
    }

    // TODO: Improve make_odd(), align_by_ten(), ricker_filter_size() functions to do a simmetrical filter without sly tricks
    int align_by_ten(int sz) noexcept
    {
        return sz / 10 * 10 + 10;
    }

    int make_odd(int sz) noexcept
    {
        return (sz % 2) ? sz : (sz + 1);
    }

    constexpr size_t ricker_filter_max_size = 20000;
    using ricker_filter_data_type = std::array<double, ricker_filter_max_size + 1>;

    struct ricker_filter {
        ricker_filter_data_type& data;
        size_t& sz;
    };

    void create_filter(int dots, ricker_filter & rfd) noexcept
    {
        assert (rfd.data.size() > dots);

        auto const tricky_dots = (dots - 1) / 10;
        double const c = 2 / ((sqrt (3)) * pow (boost::math::constants::pi<double>(), 0.25));
        double const one_per_mh_dots = 1.0 / tricky_dots;
        double t = -5.0;

        for (int i = 0; i < dots; ++i)
        {
            const double pow_t_2 = pow (t, 2);
            rfd.data[i] = c * exp(-pow_t_2 / 2) * (1 - pow_t_2);
            t += one_per_mh_dots;
        }

        rfd.sz = dots;
    }

    using sample_type = int32_t;
    using signal_sequence_type = std::vector<sample_type>;
    using transform_result_type = double;

    using sample_quantity_type = int;
    using prepare_type = std::tuple<sample_quantity_type, const signal_sequence_type &>;
    using output_type = std::shared_ptr<std::vector<transform_result_type>>;

    output_type partial_task(prepare_type input, uint8_t part_index, uint8_t part_number, ricker_filter const & rfd)
    {
        auto const sample_quantity = std::get<0>(input);
        auto const & signal_sequence = std::get<1>(input);

        auto const default_iteration_number = sample_quantity / part_number;
        auto const begin_iter = std::begin(signal_sequence) + ((part_index - 1) * default_iteration_number);
        auto const end_iter = (part_index != part_number) ? (std::begin(signal_sequence) + ((part_index) * default_iteration_number + rfd.sz)) : std::end(signal_sequence);

        sample_type signal_subsequence [std::distance(begin_iter, end_iter)];
        ricker_filter_data_type::value_type filter [rfd.sz];

        std::copy (begin_iter, end_iter, signal_subsequence);
        std::copy (std::begin(rfd.data), std::begin(rfd.data) + rfd.sz, filter);

        auto const total_iterations = (part_index != part_number) ? (sample_quantity / part_number) : (sample_quantity / part_number + (sample_quantity % part_number));
        auto const data = std::make_shared<std::vector<transform_result_type>>(total_iterations);

        auto curr = std::begin(*data);
        auto const end = curr + total_iterations;

        auto pure_s_iter = signal_subsequence;

        while (curr != end)
        {
            *curr++ = static_cast<transform_result_type>(std::inner_product(filter, filter + rfd.sz, pure_s_iter++, 0.0) / rfd.sz);
        }

        return data;
    }

    namespace tw = transwarp;
    constexpr int hardware_threads = 8; /*std::thread::hardware_concurrency();*/
    tw::parallel & get_executor()
    {
        static tw::parallel executor(hardware_threads);
        return executor;
    }

    static std::shared_ptr<tw::task<output_type>> make_task_by_index (uint8_t index, signal_sequence_type const & ss, size_t from, size_t to, ricker_filter const & fd)
    {
        return tw::make_task(tw::consume, [&] (prepare_type input, uint8_t index)
                             {
                                 return partial_task(input, index, hardware_threads, fd);
                             },
                             tw::make_value_task (std::make_tuple(to - from, ss)), tw::make_value_task(index)
        );
    }

    static std::shared_ptr<tw::task<void>> gather_task(signal_sequence_type const & ss, size_t from, size_t to,
                                                       transform_result_type * const result, ricker_filter const & filter_data) {

        std::vector<std::shared_ptr<tw::task<output_type>>> vec (hardware_threads);
        uint8_t task_idx = 1;
        for (auto & elem : vec)
        {
            elem = make_task_by_index(static_cast<uint8_t>(task_idx++), ss, from, to, filter_data);
        }

        // capture by value
        return tw::make_task(tw::consume, [=](std::vector<output_type> const & parents) {
            size_t copy_pos = 0;
            for (auto & elem : parents)
            {
                std::copy (elem->begin(), elem->begin()+elem->size(), &result[copy_pos]);
                copy_pos += elem->size();
            }
        }, vec);

    }

    auto inner_product_lambda = [](auto&&... args){return std::inner_product(decltype(args)(args)...);};
    auto parallel_inner_product_lambda = [](auto&&... args){return __gnu_parallel::inner_product(decltype(args)(args)...);};

    template <typename L>
    inline void seq_transform(signal_sequence_type const & ss, transform_result_type * const result, ricker_filter const & filter_data, L const & lambda)
    {
        auto const iterations = static_cast<size_t>(rate);
        int curr_iteration = 0;
        while (curr_iteration < iterations)
        {
            result[curr_iteration] = static_cast<transform_result_type> (
                    lambda(std::begin(filter_data.data), std::begin(filter_data.data) + filter_data.sz, std::begin(ss) + curr_iteration, 0.0) / filter_data.sz
            );
            ++curr_iteration;
        }
    }

    void transform_uni(signal_sequence_type const & ss, transform_result_type * const result, ricker_filter const & filter_data)
    {
        seq_transform (ss, result, filter_data, inner_product_lambda);
    }

    void transform_omp (signal_sequence_type const & ss, transform_result_type * const result, ricker_filter const & filter_data)
    {
        seq_transform (ss, result, filter_data, parallel_inner_product_lambda);
    }

    void transform_tw(signal_sequence_type const & ss, transform_result_type * const result, ricker_filter const & filter_data)
    {
        auto final_task = gather_task(ss, 0, static_cast<size_t>(rate), result, filter_data);
        final_task->schedule_all(get_executor());
        final_task->get();
    }
}

template <typename F, typename ...T>
void measure_it(char const* f_name, F const & f, T &&... arguments)
{
    auto const time_point1 = std::chrono::high_resolution_clock::now();
    f (std::forward<T>(arguments)...);
    auto const time_point2 = std::chrono::high_resolution_clock::now();
    std::cout << f_name << std::chrono::duration_cast<std::chrono::milliseconds>(time_point2-time_point1).count() << std::endl;
}

std::array<daq::transform_result_type, static_cast<size_t>(daq::rate)> transform_uni_result{};
std::array<daq::transform_result_type, static_cast<size_t>(daq::rate)> transform_omp_result{};
std::array<daq::transform_result_type, static_cast<size_t>(daq::rate)> transform_tw_result{};

#ifndef MAIN_IS_ABSENT
int main()
{
    using namespace daq;

    ricker_filter_data_type filter_data {};
    size_t filter_size {};
    ricker_filter filter {filter_data, filter_size};

    create_filter (make_odd(align_by_ten(ricker_filter_size(f, rate))), filter);

    signal_sequence_type ss (static_cast<size_t>(rate) + filter.sz);
    std::iota(std::begin(ss), std::end(ss), -rate/2);

    measure_it("transform_uni: ", transform_uni, ss, std::addressof(transform_uni_result[0]), filter);
    measure_it("transform_omp: ", transform_omp, ss, std::addressof(transform_omp_result[0]), filter);
    measure_it("transform_tw:  ", transform_tw, ss, std::addressof(transform_tw_result[0]), filter);
    return 0;
}
#endif


#include "libsnark/common/data_structures/merkle_tree.hpp"  //for the merkle_authentication_node
#include "libsnark/common/default_types/r1cs_ppzksnark_pp.hpp"  //for the default_r1cs_ppzksnark_pp
#include "libsnark/gadgetlib1/gadgets/hashes/sha256/sha256_gadget.hpp"   // for the sha256_two_to_one_hash_gadget
#include "libsnark/common/utils.hpp"  //for the bit_vector
#include "libsnark/gadgetlib1/gadgets/merkle_tree/merkle_tree_check_read_gadget.hpp" // for the merkle_tree_check_read_gadget
#include "libsnark/gadgetlib1/gadgets/basic_gadgets.hpp"
#include "libsnark/zk_proof_systems/ppzksnark/r1cs_ppzksnark/r1cs_ppzksnark.hpp"
#include "algebra/fields/field_utils.hpp"
#include "libsnark/common/utils.hpp"
#include <boost/optional.hpp>
#include "gadget.hpp"

using namespace libsnark;
using namespace std;

template<typename HashT>
void generate_merkle_and_branch(bit_vector &prev_leaf, bit_vector &leaf, bit_vector &root,
                                size_t &address, bit_vector &address_bits,
                                std::vector<merkle_authentication_node> &path) {

    bit_vector prev_hash = leaf;
    path = std::vector<merkle_authentication_node> (tree_depth);

    for (long level = tree_depth - 1; level >= 0; --level) {
        const bool computed_is_right = (std::rand() % 2);
        address |= (computed_is_right ? 1ul << (tree_depth-1-level) : 0);
        address_bits.push_back(computed_is_right);
        bit_vector other(sha256_digest_len);
        std::generate(other.begin(), other.end(), [&](){return std::rand()%2;});
        bit_vector block = prev_hash;
        block.insert(computed_is_right ? block.begin() : block.end(), other.begin(), other.end());
        bit_vector h = HashT::get_hash(block);

        path[level] = other;
        prev_hash = h;
    }

    root = prev_hash;
}

template<typename ppzksnark_ppT, typename HashT>
r1cs_ppzksnark_keypair<ppzksnark_ppT> generate_keypair()
{
    typedef Fr<ppzksnark_ppT> FieldT;

    protoboard<FieldT> pb;
    toy_gadget<FieldT, HashT> g(pb);
    g.generate_r1cs_constraints();
    const r1cs_constraint_system<FieldT> constraint_system = pb.get_constraint_system();

    cout << "Number of R1CS constraints: " << constraint_system.num_constraints() << endl;
    cout << "auxiliary input size : " << constraint_system.auxiliary_input_size << endl;
    cout << "primary_input size : " << constraint_system.primary_input_size << endl;

    return r1cs_ppzksnark_generator<ppzksnark_ppT>(constraint_system);
}

template<typename ppzksnark_ppT, typename HashT>
boost::optional<r1cs_ppzksnark_proof<ppzksnark_ppT>> generate_proof(r1cs_ppzksnark_proving_key<ppzksnark_ppT> proving_key,
                                                                   const bit_vector &prev_leaf,
                                                                   const bit_vector &leaf,
                                                                   const bit_vector &root,
                                                                   const size_t address,
                                                                   const bit_vector &address_bits,
                                                                   const std::vector<merkle_authentication_node> &path
                                                                   )
{
    typedef Fr<ppzksnark_ppT> FieldT;

    protoboard<FieldT> pb;
    toy_gadget<FieldT, HashT> g(pb);
    g.generate_r1cs_constraints();
    g.generate_r1cs_witness(prev_leaf, leaf, root, address, address_bits, path);

    if (!pb.is_satisfied()) {
        return boost::none;
    }

    return r1cs_ppzksnark_prover<ppzksnark_ppT>(proving_key, pb.primary_input(), pb.auxiliary_input());
}

template<typename ppzksnark_ppT>
bool verify_proof(r1cs_ppzksnark_verification_key<ppzksnark_ppT> verification_key,
                  r1cs_ppzksnark_proof<ppzksnark_ppT> proof,
                  const bit_vector &prev_leaf,
                  const bit_vector &root
                 )
{
    typedef Fr<ppzksnark_ppT> FieldT;
    /*
    r1cs_primary_input<FieldT> input;
    input.insert(input.end(), prev_leaf.begin(), prev_leaf.end());
    input.insert(input.end(), root.begin(), root.end());
    */
    const r1cs_primary_input<FieldT> input = l_input_map<FieldT>(prev_leaf, root);
    return r1cs_ppzksnark_verifier_strong_IC<ppzksnark_ppT>(verification_key, input, proof);
}


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

  @copyright 2015 NumScale SAS
  @copyright 2015 J.T. Lapreste

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

#ifndef BOOST_SIMD_NO_INFINITIES
#include <boost/simd/function/simd/is_inf.hpp>
#endif
#include <boost/simd/constant/ratio.hpp>
#include <boost/simd/function/abs.hpp>
#include <boost/simd/function/atand.hpp>
#include <boost/simd/function/bitofsign.hpp>
#include <boost/simd/function/bitwise_or.hpp>
#include <boost/simd/function/if_else_zero.hpp>
#include <boost/simd/function/if_zero_else.hpp>
#include <boost/simd/function/is_nez.hpp>
#include <boost/simd/function/minus.hpp>
#include <boost/dispatch/function/overload.hpp>
#include <boost/config.hpp>

namespace boost { namespace simd { namespace ext
{
  namespace bd = boost::dispatch;
  namespace bs = boost::simd;
  BOOST_DISPATCH_OVERLOAD ( acotd_
                          , (typename A0)
                          , bd::cpu_
                          , bd::generic_<bd::floating_<A0> >
                          )
  {
    BOOST_FORCEINLINE A0 operator() ( A0 const& a0) const BOOST_NOEXCEPT
    {
      A0 z = Ratio<A0, 90>()-if_else_zero(is_nez(a0),atand(bs::abs(a0)));
      #ifndef BOOST_SIMD_NO_INFINITIES
      z = if_zero_else(is_inf(a0),z);
      #endif
      return bitwise_or(z, bitofsign(a0));
    }
  };
} } }

#endif


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

// EigenML includes
#include <eigenml/decision_tree/decision_tree.hpp>
#include <eigenml/core/eigenml.hpp>

using namespace std;
using namespace eigenml;

int main() {

    logging::init_cerr_log(logging::severity_level::info);

    logging::Logger logger("MAIN");

    std::srand((unsigned int) time(0));

    size_t N = 100000;
    size_t P = 1;
    Matrix X = Matrix::Random(N, P);
    Matrix c = Matrix::Random(P, 1);
    Vector Y(N);

    c << 1;
    Y = (X.array()).matrix();

    Vector I(X.rows());
    Matrix XY(X.rows(), I.cols() + X.cols()+Y.cols());
    std::iota(I.data(), I.data() + I.size(), 0);
    XY << I, X, Y;

    LOG_DEBUG << XY;

    // // Declare a tree
    LOG_INFO << "Fitting a tree";
    decision_tree::DecisionTreeParams params;

    params.max_depth = 6;
    params.criterion = decision_tree::SplitCriterion::kMSECriterion;

    decision_tree::DecisionTree<ModelType::kSupervisedRegressor> tree(params);
    tree.fit(X, Y);

    std::cout << (tree.transform(X)-Y).squaredNorm() << std::endl << "*****" << std::endl;
}


#include <cstdio>
#include <string>
#include <cstdlib>
#include <cmath>
#include <iostream>
#include <fstream>
#include <Eigen/Dense>
#include <vector>

#include "global.h"
#include "uDGP.h"
#include "SED.h"

using namespace std;
using namespace Eigen;

using std::ifstream;
using std::string;

void process_cmdline(char *argv[], char **data_file, char **option_file, char **output_file) {

	*data_file	    = argv[1];
	*option_file	= argv[2];
	*output_file	= argv[3];
}

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

	char *data_file, *option_file, *output_file;
	if (argc==4) {
		process_cmdline(argv, &data_file, &option_file, &output_file);
	} else {
		cout<<"Command line error!"<<endl;
		abort();
	}

	DataReader data_input(option_file);
	data_input.SetParameters();
    data_input.ReadData(data_file);

	uDGP *udgp_gd = new SED();
    udgp_gd->SetOutputFile(output_file);
    udgp_gd->SetData(&data_input);
    udgp_gd->SetMeasureMatrix();
    udgp_gd->Initialization();
    udgp_gd->GradientDescent();

    VectorXd smp_pos_out = udgp_gd->GetSamplePos();

    ofstream write_result(output_file, ios_base::trunc);
    for (int i=0; i<smp_pos_out.size(); i++) {
        write_result<<smp_pos_out(i)<<" ";
    }
    write_result<<"\n";
    write_result.close();

    delete udgp_gd;

	exit(0);
}


#include <algorithm>
#include <boost/optional.hpp>
#include <climits>
#include <cmath>
#include <cstdio>
#include <cstring>
#include <functional>
#include <iostream>
#include <limits>
#include <list>
#include <map>
#include <numeric>
#include <queue>
#include <set>
#include <sstream>
#include <string>
#include <tuple>
#include <type_traits>
#include <unordered_map>
#include <unordered_set>
#include <utility>
#include <vector>

#define REP(i, n) for(int i = 0, i##_MACRO = (n); i < i##_MACRO; i++)
#define RANGE(i, a, b) for(int i = (a), i##_MACRO = (b); i < i##_MACRO; i++)
#define EACH(e, a) for(auto&& e : a)
#define ALL(a) std::begin(a), std::end(a)
#define RALL(a) std::rbegin(a), std::rend(a)
#define FILL(a, n) memset((a), n, sizeof(a))
#define FILLZ(a) FILL(a, 0)
#define INT(x) (static_cast<int>(x))

using namespace std;

using ll = long long;
using VI = vector<int>;
using VI2D = vector<vector<int>>;

constexpr int INF = 2e9;
constexpr double EPS = 1e-10;
constexpr double PI = acos(-1.0);

constexpr int dx[] = {-1, 0, 1, 0};
constexpr int dy[] = {0, -1, 0, 1};

template <typename T>
constexpr int sign(T x) {
	return x < 0 ? -1 : x > 0 ? 1 : 0;
}

template <>
constexpr int sign(double x) {
	return x < -EPS ? -1 : x > EPS ? 1 : 0;
}

template <typename T, typename U>
constexpr void chmax(T& m, U x) {
	m = max(m, x);
}

template <typename T, typename U>
constexpr void chmin(T& m, U x) {
	m = min(m, x);
}

template <typename T>
constexpr T square(T x) {
	return x * x;
}

struct LoopInfo {
	ll dist_begin_loop;
	ll loop_begin_node;
	ll loop_size;
	LoopInfo(ll d, ll n, ll s)
		: dist_begin_loop(d), loop_begin_node(n), loop_size(s) {}
};

// loopの始点までの距離, loopの始点, loopのサイズ
LoopInfo find_loop(vector<ll>& a) {
	ll n = a.size();
	vector<ll> dist(n, 0);
	ll node = 0;
	ll prev_node = 0;
	ll i = 0;
	dist[0] = -1;
	while(true) {
		// cout << "node: " << node << endl;
		dist[node] = dist[prev_node] + 1;
		prev_node = node;
		node = a[node] - 1;
		if(dist[node] != 0 || (i != 0 && node == 0)) {
			// cout << "node: " << node << ", prev: " << prev_node
			//	 << ", dist[node]: " << dist[node] << endl;
			return LoopInfo(dist[node], node, dist[prev_node] - dist[node] + 1);
		}
		i++;
	}
}

ll find_node(vector<ll>& a, ll count) {
	ll node = 0;
	REP(i, count) { node = a[node] - 1; }
	return node;
}

int main() {
	ll n, k;
	cin >> n >> k;
	vector<ll> a(n);
	REP(i, n) { cin >> a[i]; }
	LoopInfo li = find_loop(a);
	// cout << "dist: " << li.dist_begin_loop << ", size: " << li.loop_size
	//	 << endl;
	if(k < li.dist_begin_loop) {
		cout << find_node(a, k) + 1 << endl;
	} else {
		cout << find_node(a,
						  li.dist_begin_loop +
							  (k - li.dist_begin_loop) % li.loop_size) +
					1
			 << endl;
	}
	return 0;
}


#define BOOST_TEST_MODULE "test_lennard_jones_potential"

#ifdef BOOST_TEST_DYN_LINK
#include <boost/test/unit_test.hpp>
#else
#include <boost/test/included/unit_test.hpp>
#endif

#include <mjolnir/forcefield/global/LennardJonesPotential.hpp>
#include <mjolnir/core/BoundaryCondition.hpp>
#include <mjolnir/core/SimulatorTraits.hpp>

BOOST_AUTO_TEST_CASE(LennardJones_double)
{
    using traits_type = mjolnir::SimulatorTraits<double, mjolnir::UnlimitedBoundary>;
    using real_type   = typename traits_type::real_type;
    using molecule_id_type = mjolnir::Topology::molecule_id_type;
    using group_id_type    = mjolnir::Topology::group_id_type;
    using potential_type   = mjolnir::LennardJonesPotential<traits_type>;
    using parameter_type   = potential_type::parameter_type;

    constexpr static std::size_t N = 10000;
    constexpr static real_type   h = 1e-6;

    const real_type sigma   = 3.0;
    const real_type epsilon = 1.0;
    const parameter_type param{sigma, epsilon};
    potential_type lj{
        potential_type::default_cutoff(),
        {{0, param}, {1, param}}, {},
        mjolnir::IgnoreMolecule<molecule_id_type>("Nothing"),
        mjolnir::IgnoreGroup   <group_id_type   >({})
    };

    const real_type x_min = 0.8 * sigma;
    const real_type x_max = lj.cutoff_ratio() * sigma;
    const real_type dx = (x_max - x_min) / N;

    for(std::size_t i=0; i<N; ++i)
    {
        const real_type x    = x_min + i * dx;
        const real_type pot1 = lj.potential(0, 1, x + h);
        const real_type pot2 = lj.potential(0, 1, x - h);
        const real_type dpot = (pot1 - pot2) / (2 * h);
        const real_type deri = lj.derivative(0, 1, x);

        BOOST_TEST(dpot == deri, boost::test_tools::tolerance(h));
    }
}

BOOST_AUTO_TEST_CASE(LennardJones_float)
{
    using traits_type = mjolnir::SimulatorTraits<float, mjolnir::UnlimitedBoundary>;
    using real_type   = typename traits_type::real_type;
    using molecule_id_type = mjolnir::Topology::molecule_id_type;
    using group_id_type    = mjolnir::Topology::group_id_type;
    using potential_type   = mjolnir::LennardJonesPotential<traits_type>;
    using parameter_type   = potential_type::parameter_type;
    constexpr std::size_t N = 1000;
    constexpr real_type   h = 0.002f;
    constexpr real_type tol = 0.005f;

    const real_type sigma   = 3.0f;
    const real_type epsilon = 1.0f;
    const parameter_type param{sigma, epsilon};
    potential_type lj{potential_type::default_cutoff(),
        {{0, param}, {1, param}}, {},
        mjolnir::IgnoreMolecule<molecule_id_type>("Nothing"),
        mjolnir::IgnoreGroup   <group_id_type   >({})
    };
    const real_type cutoff = lj.cutoff_ratio();

    const real_type x_min = 0.8f   * sigma;
    const real_type x_max = cutoff * sigma;
    const real_type dx = (x_max - x_min) / N;

    for(std::size_t i=0; i<N; ++i)
    {
        const real_type x    = x_min + i * dx;
        const real_type pot1 = lj.potential(0, 1, x + h);
        const real_type pot2 = lj.potential(0, 1, x - h);
        const real_type dpot = (pot1 - pot2) / (2 * h);
        const real_type deri = lj.derivative(0, 1, x);

        BOOST_TEST(dpot == deri, boost::test_tools::tolerance(tol));
    }
}


#include "house.hpp"
#include "ukf.hpp"
#include "dyn.hpp"
#include "filter_aux.hpp"
#include "pearsonator.hpp"
#include "timer.hpp"
#include "eigen_csv.hpp"

#include <Eigen/Dense>

#include <cmath>
#include <ctime>
#include <functional>
#include <iostream>
#include <random>
#include <vector>

using namespace Eigen;
using namespace std;

void run(bool gauss) {

    Matrix3d I;

    I << 500+400, 0, 0,
         0, 500+300, 0,
         0, 0, 400+300;

    DynamicModel::stf g = [&I] (double t, const Vector3d& w, const Vector3d& Td)
        -> VectorXd {
            return I.inverse() * (Td - w.cross(I * w));
        };

    DynamicModel f(g, 3, 1E-9, 1E-9);

    UKF::meas_model h = [] (double t, const Vector3d& w)
        -> VectorXd {
            return w.head(1);
        };

    HOUSE::meas_model hh = [] (double t, const VectorXd& w, const VectorXd& v)
        -> VectorXd {
            return w.head(1) + v;
        };

    double stdx0, stdw, stdn, dt;

    stdx0 = 0.01;
    stdw  = 0.001;
    stdn  = 0.001;

    dt = 0.1;

    Matrix3d Pxx0, Pww;
    MatrixXd Pnn(1,1);

    Pxx0 = Matrix3d::Identity() * stdx0 * stdx0;
    Pww  = Matrix3d::Identity() * stdw  * stdw;
    Pnn(0,0) = stdn * stdn;

    Vector3d wm0;
    wm0 << 0, 0, 0;

    Vector3d w0;

    int trials = 100;
    int steps = 6001;

    VectorXd t;
    t.setLinSpaced(steps, 0, (steps-1)*dt);

    vector<vector<VectorXd>> xtru, xest_ukf, xest_cut4, xest_cut6, xest_cut8,
        xest_house;

    double skew0, skeww, skewn, kurt0, kurtw, kurtn;

    if (gauss) {

        skew0 = 0;
        kurt0 = 3;

        skeww = 0;
        kurtw = 3;

        skewn = 0;
        kurtn = 3;

    } else {

        skew0 = -1;
        kurt0 = 30;

        skeww = -1;
        kurtw = 30;

        skewn = -1;
        kurtn = 30;

    }

    Pearsonator::TypeIV gen0_p(0, stdx0, skew0, kurt0), genw_p(0, stdw, skeww, kurtw),
        genn_p(0, stdn, skewn, kurtn);

    normal_distribution<double> gen0_g(0, stdx0), genw_g(0, stdw), genn_g(0, stdn);

    mt19937_64 mt(0);

    typedef function<double(void)> noisemaker;

    noisemaker gen0 = [&] () -> double {return gauss ? gen0_g(mt) : gen0_p(mt);};
    noisemaker genw = [&] () -> double {return gauss ? genw_g(mt) : genw_p(mt);};
    noisemaker genn = [&] () -> double {return gauss ? genn_g(mt) : genn_p(mt);};

    UKF::cut_dir = "../CUT/";

    UKF ukf (f, h, false, 0, wm0, Pxx0, Pww, Pnn, UKF::sig_type::JU,   1);
    UKF cut4(f, h, false, 0, wm0, Pxx0, Pww, Pnn, UKF::sig_type::CUT4, 1);
    UKF cut6(f, h, false, 0, wm0, Pxx0, Pww, Pnn, UKF::sig_type::CUT6, 1);
    UKF cut8(f, h, false, 0, wm0, Pxx0, Pww, Pnn, UKF::sig_type::CUT8, 1);

    HOUSE::Dist distx0(Pxx0), distw(Pww), distn(Pnn);

    distx0.mean = wm0;

    distx0.skew.setConstant(skew0);
    distx0.kurt.setConstant(kurt0);

    distw.skew.setConstant(skeww);
    distw.kurt.setConstant(kurtw);

    distn.skew.setConstant(skewn);
    distn.kurt.setConstant(kurtn);

    HOUSE house(f, hh, 1, 0, distx0, distw, distn, 0);

    MatrixXd run_times(trials,5);
    Timer timer;

    for (int j = 0; j < trials; j++) {

        ukf. reset(0, wm0, Pxx0);
        cut4.reset(0, wm0, Pxx0);
        cut6.reset(0, wm0, Pxx0);
        cut8.reset(0, wm0, Pxx0);
        house.reset(0, distx0);

        cout << "Running Trial " << j+1 << endl;

        MatrixXd T(3, steps), v(1, steps);
        for (int k = 0; k < steps; k++) {
            v(k) = genn();
            T(0,k) = genw();
            T(1,k) = genw();
            T(2,k) = genw();
        }

        w0 = wm0;
        w0(0) += gen0();
        w0(1) += gen0();
        w0(2) += gen0();

        vector<VectorXd> xtruk;
        VectorXd w = w0;
        for (int k = 0; k < steps; k++) {
            xtruk.push_back(w);
            if (k < steps-1)
                w = f(t(k), t(k+1), xtruk.back(), T.col(k));
        }
        xtru.push_back(xtruk);

        MatrixXd Z(1, steps);
        for (int k = 0; k < steps; k++)
            Z.col(k) = h(t(k), xtruk[k]) + v.col(k);

        cout << "   HOUSE" << endl;
        timer.tick();
        house.run(t, Z);
        run_times(j, 0) = timer.tock();
        vector<VectorXd> xest_house_trial;
        for (int k = 0; k < steps; k++)
            xest_house_trial.push_back(house.distx[k].mean);
        xest_house.push_back(xest_house_trial);

        cout << "   UKF" << endl;
        timer.tick();
        ukf.run(t, Z);
        run_times(j, 1) = timer.tock();
        xest_ukf.push_back(ukf.xest);

        cout << "   CUT4" << endl;
        timer.tick();
        cut4.run(t, Z);
        run_times(j, 2) = timer.tock();
        xest_cut4.push_back(cut4.xest);

        cout << "   CUT6" << endl;
        timer.tick();
        cut6.run(t, Z);
        run_times(j, 3) = timer.tock();
        xest_cut6.push_back(cut6.xest);

        cout << "   CUT8" << endl;
        timer.tick();
        cut8.run(t, Z);
        run_times(j, 4) = timer.tock();
        xest_cut8.push_back(cut8.xest);

    }

    string dist = gauss ? "gauss" : "pearson";

    save_rmse(t, xtru, xest_house, "out/house_rmse_" + dist + ".csv");
    save_rmse(t, xtru, xest_ukf,   "out/ukf_rmse_"   + dist + ".csv");
    save_rmse(t, xtru, xest_cut4,  "out/cut4_rmse_"  + dist + ".csv");
    save_rmse(t, xtru, xest_cut6,  "out/cut6_rmse_"  + dist + ".csv");
    save_rmse(t, xtru, xest_cut8,  "out/cut8_rmse_"  + dist + ".csv");

    save_abs_err_lump(t, xtru, xest_house, "out/house_err_" + dist + ".csv");
    save_abs_err_lump(t, xtru, xest_ukf,   "out/ukf_err_"   + dist + ".csv");
    save_abs_err_lump(t, xtru, xest_cut4,  "out/cut4_err_"  + dist + ".csv");
    save_abs_err_lump(t, xtru, xest_cut6,  "out/cut6_err_"  + dist + ".csv");
    save_abs_err_lump(t, xtru, xest_cut8,  "out/cut8_err_"  + dist + ".csv");

    vector<string> filters;
    filters.push_back("house");
    filters.push_back("ukf");
    filters.push_back("cut4");
    filters.push_back("cut6");
    filters.push_back("cut8");

    string time_file = "out/run_time_";
    time_file += dist;
    time_file += ".csv";

    EigenCSV::write(run_times, filters, time_file);

}

int main() {

    cout << "--- Gaussian Distributions ---" << endl;
    run(true);

    cout << "--- Pearson Type IV Distributions ---" << endl;
    run(false);

    return 0;

}


#include "exception.hh"
#include "network.hh"
#include "timer.hh"

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

#include <sys/resource.h>
#include <sys/time.h>

using namespace std;
using namespace Eigen;

constexpr size_t batch_size = 1;
constexpr size_t input_size = 1024;

void program_body( const unsigned int num_iterations )
{
  /* remove limit on stack size */
  const rlimit limits { RLIM_INFINITY, RLIM_INFINITY };
  CheckSystemCall( "setrlimit", setrlimit( RLIMIT_STACK, &limits ) );

  /* seed C RNG for Eigen random weight initialization */
  srand( Timer::timestamp_ns() );

  /* construct neural network on heap */
  // auto nn = make_unique<Network<float, batch_size, input_size, 4096, 1>>();
  // auto nn = make_unique<Network<float, batch_size, input_size, 2048, 2048, 1>>();
  auto nn = make_unique<Network<float, batch_size, input_size, 1024, 1024, 1024, 1024, 1024, 1024, 1024, 1>>();
  // auto nn = make_unique<Network<float, batch_size, input_size, 5, 3, 2, 1>>();
  nn->initializeWeightsRandomly();

  /* initialize inputs */
  vector<Matrix<float, batch_size, input_size>> inputs;
  for ( unsigned int i = 0; i < num_iterations; i++ ) {
    inputs.emplace_back( Matrix<float, batch_size, input_size>::Random() );
  }

  /* run forward prop benchmark */
  const uint64_t f_start = Timer::timestamp_ns();
  for ( unsigned int i = 0; i < num_iterations; i++ ) {
    /* forward prop */
    nn->apply( inputs[i] );
  }
  const uint64_t f_end = Timer::timestamp_ns();

  /* run backprop benchmark */
  const uint64_t b_start = Timer::timestamp_ns();
  for ( unsigned int i = 0; i < num_iterations; i++ ) {
    /* forward prop */
    nn->apply( inputs[i] );
    /* back prop*/
    nn->computeDeltas();
    nn->evaluateGradients( inputs[i] );
  }

  const uint64_t b_end = Timer::timestamp_ns();

  cout << "Average back propagation time (over " << num_iterations << " iterations, batch size=" << batch_size
       << "): ";
  Timer::pp_ns( cout, ( ( b_end - b_start ) - ( f_end - f_start ) ) / float( num_iterations ) );
  cout << " per iteration\n";
  cout << "Back Propagation time / forward propagation time: "
       << ( ( b_end - b_start ) * 1.0 / ( f_end - f_start ) - 1 ) << " x\n";
}

int main( int argc, char* argv[] )
{
  try {
    if ( argc <= 0 ) {
      abort();
    }

    if ( argc != 2 ) {
      cerr << "Usage: " << argv[0] << " NUM_ITERATIONS\n";
      return EXIT_FAILURE;
    }

    program_body( stoi( argv[1] ) );

    return EXIT_SUCCESS;
  } catch ( const exception& e ) {
    cerr << e.what() << "\n";
    return EXIT_FAILURE;
  }
}


/*
 * Copyright 2018 Autoware Foundation. All rights reserved.
 *
 * 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.
 *
 * v1.0 Yukihiro Saito
 */

#pragma once
#include <autoware_perception_msgs/DynamicObjectWithFeatureArray.h>
#include <list>
#include <unordered_map>
#include <vector>
#include "multi_object_tracker/tracker/tracker.hpp"
#define EIGEN_MPL2_ONLY
#include <Eigen/Core>
#include <Eigen/Geometry>
class DataAssociation
{
private:
  double getDistance(
    const geometry_msgs::Point & measurement, const geometry_msgs::Point & tracker);
  Eigen::MatrixXi can_assign_matrix_;
  Eigen::MatrixXd max_dist_matrix_;
  Eigen::MatrixXd max_area_matrix_;
  Eigen::MatrixXd min_area_matrix_;
  const double score_threshold_;

public:
  DataAssociation(
    std::vector<int> can_assign_vector, std::vector<double> max_dist_vector,
    std::vector<double> max_area_vector, std::vector<double> min_area_vector);
  bool assign(
    const Eigen::MatrixXd & src, std::unordered_map<int, int> & direct_assignment,
    std::unordered_map<int, int> & reverse_assignment);
  Eigen::MatrixXd calcScoreMatrix(
    const autoware_perception_msgs::DynamicObjectWithFeatureArray & measurements,
    const std::list<std::shared_ptr<Tracker>> & trackers);
  virtual ~DataAssociation(){};
};