/*
 * personBHMIP.cpp
 *
 *  Created on: Jul 9, 2013
 *      Author: gferrer
 */
#include "scene_elements/person_bhmip.h"
#include <Eigen/Dense>
#include <iostream>

Cperson_bhmip::Cperson_bhmip(unsigned int id, Cperson_abstract::target_type person_target_type,
		 Cperson_abstract::force_type person_force_type, double _time_window) :
    Cperson_abstract(id,person_target_type, person_force_type),
    time_window_( _time_window ) , phi_var_( 0.8*0.8 )
{
	destinations_.reserve(15);
}

Cperson_bhmip::~Cperson_bhmip()
{

}

void Cperson_bhmip::add_pointV( SpointV_cov point, Cperson_abstract::filtering_method filter)
{
	SpointV_cov filtered_pose;
	if( filter != Cperson_abstract::No_filtering )
	{
		SpointV_cov pointV = point;//only positions, position_covariances and time_stamp
		double dx, dy;
		if ( !trajectory_.empty() )
		{
			dx = point.x - trajectory_.back().x;
			dy = point.y - trajectory_.back().y;
			double dt =  point.time_stamp - trajectory_.back().time_stamp ;
			//just to avoid singularities, ideally time stamps are well separated in time
			if ( point.time_stamp - trajectory_.back().time_stamp  < 0.001 ) dt = 0.1;
			pointV.vx = dx / dt;
			pointV.vy = dy / dt;
		}
		else
		{
			pointV.vx = 0;
			pointV.vy = 0;
		}

		//slide temporal window for all trajectories
		trajectory_.push_back(pointV);
		trajectory_windowing();

		//filters velocities
		switch( filter )
		{
		  case Cperson_abstract::Linear_regression_filtering:
			filtered_pose = filter_current_state_linear_regression( );
			break;
		  case Cperson_abstract::Bayes_filtering:
			filtered_pose = filter_current_state_linear_regression_bayes( );
		  	break;
		  case Cperson_abstract::Low_pass_linear_regression_filtering:
		  default :
			filtered_pose = low_pass_filter_current_state_linear_regression( );
			break;
		}

	}
	else
	{
		//no filter is required
		trajectory_.push_back(point);
		//slide temporal window for all trajectories
		trajectory_windowing();
		filtered_pose = trajectory_.back();
	}

	//update current positions and velocities according to filtering results
	diff_pointV_ = filtered_pose - current_pointV_ ;
	current_pointV_ = filtered_pose;
	desired_velocity_ = filtered_pose.v();
	now_ = filtered_pose.time_stamp;

	//precalculates data required for the BHMIP
	intention_precalculation();

	observation_update_ = true;
}

SpointV_cov Cperson_bhmip::filter_current_state_linear_regression( )
{

	//Discard a single and double pose trajectory --------------------------------------------
	if (trajectory_.size() <= 2 )
	{
		return trajectory_.back();
	}
	// First order Regression: y(t) ~ beta * x(t) = beta_1 + beta_2 * x(t) ----------------------------
	//vector and matrices initialization
	unsigned int window_elements = trajectory_.size();
	double initial_ts = trajectory_.front().time_stamp;
	Eigen::VectorXd vx ( window_elements ) , vy ( window_elements );
	Eigen::MatrixXd R (window_elements , 2 ); //1st order regression -> 2 coeffs
	for (unsigned int i = 0 ; i < window_elements ; i++)
	{
		vx ( i ) = trajectory_.at(i).vx;
		vy ( i ) = trajectory_.at(i).vy;
		R.row ( i ) << 1 , trajectory_.at(i).time_stamp - initial_ts;
	}
	//pseudo inverse of the R matrix ---------------------------------------------------------------
	Eigen::MatrixXd invR ( 2 , window_elements ) , sqrR ( 2 , 2 );
	sqrR = R.transpose() * R;
	invR = sqrR.inverse() * R.transpose() ;

	//linear regression ----------------------------------------------------------------------------
	Eigen::Vector2d  r_pred (1 , trajectory_.back().time_stamp - initial_ts ),vx_coef,vy_coef;
	//invR * v = beta -> parameter estimation
	vx_coef = invR * vx;
	vy_coef = invR * vy;
	SpointV_cov filtered_pose(
			trajectory_.back().x, //no filtering at position
			trajectory_.back().y,
			trajectory_.back().time_stamp,
			r_pred.dot ( vx_coef ),
			r_pred.dot ( vy_coef ) );
    filtered_pose.cov[0] = trajectory_.back().cov_xx();
    filtered_pose.cov[1] = trajectory_.back().cov_xy();
    filtered_pose.cov[4] = trajectory_.back().cov_xy();
    filtered_pose.cov[5] = trajectory_.back().cov_yy();

	//covariance of velocity prediction, with respect to regression velocity
	Eigen::VectorXd vx_mean ( window_elements ) , vy_mean ( window_elements );
	vx_mean = R*vx_coef;
	vy_mean = R*vy_coef;
	double cov_vx = (vx - vx_mean).dot( vx - vx_mean );
	double cov_vy = (vy - vy_mean).dot( vy - vy_mean );
	double cov_vxy = (vy - vy_mean).dot( vx - vx_mean );

	//regression covariances + projected uncertainty from positions P_x / dt
	filtered_pose.cov[10] = cov_vx + 0.001;
	filtered_pose.cov[11] = cov_vxy;
	filtered_pose.cov[14] = filtered_pose.cov[11];
	filtered_pose.cov[15] = cov_vy + 0.001;

	return filtered_pose;
}

SpointV_cov Cperson_bhmip::filter_current_state_linear_regression_bayes( )
{

	//Discard a single and double pose trajectory --------------------------------------------
	if (trajectory_.size() <= 2 )
	{
		return trajectory_.back();
	}
	// First order Regression: y(t) ~ beta * x(t) = beta_1 + beta_2 * x(t) ----------------------------
	//vector and matrices initialization
	unsigned int window_elements = trajectory_.size();
	double initial_ts = trajectory_.front().time_stamp;
	Eigen::VectorXd vx ( window_elements ) , vy ( window_elements );
	Eigen::MatrixXd R (window_elements , 2 ); //1st order regression -> 2 coeffs
	for (unsigned int i = 0 ; i < window_elements ; i++)
	{
		vx ( i ) = trajectory_.at(i).vx;
		vy ( i ) = trajectory_.at(i).vy;
		R.row ( i ) << 1 , trajectory_.at(i).time_stamp - initial_ts;
	}
	//pseudo inverse of the R matrix ---------------------------------------------------------------
	Eigen::MatrixXd invR ( 2 , window_elements ) , sqrR ( 2 , 2 );
	sqrR = R.transpose() * R;
	invR = sqrR.inverse() * R.transpose() ;

	//linear regression ----------------------------------------------------------------------------
	Eigen::Vector2d  r_pred (1 , trajectory_.back().time_stamp - initial_ts ),vx_coef,vy_coef;
	//invR * v = beta -> parameter estimation
	vx_coef = invR * vx;
	vy_coef = invR * vy;
	SpointV_cov filtered_pose(
			trajectory_.back().x, //no filtering at position
			trajectory_.back().y,
			trajectory_.back().time_stamp,
			r_pred.dot ( vx_coef ),
			r_pred.dot ( vy_coef ) );
    filtered_pose.cov[0] = trajectory_.back().cov_xx();
    filtered_pose.cov[1] = trajectory_.back().cov_xy();
    filtered_pose.cov[4] = trajectory_.back().cov_xy();
    filtered_pose.cov[5] = trajectory_.back().cov_yy();

	//covariance of velocity prediction, average of uncertainty propagated from estimated positions, equidistribited
	double dt;
	Eigen::Matrix2d v_cov_joint = Eigen::Matrix2d::Zero();
	for( unsigned int i = 0; i < window_elements ; i++)
	{
		if( i == 0 )
			dt = R(1,1)-R(0,1);
		else
			dt = R(i,1)-R(i-1,1);
		Eigen::Matrix2d v_cov;
		v_cov << trajectory_.at(i).cov_xx() , trajectory_.at(i).cov_xy(),
				trajectory_.at(i).cov_xy() , trajectory_.at(i).cov_yy();
		v_cov_joint += v_cov / (dt*dt);
	}

	//regression covariances + projected uncertainty from positions P_x / dt
	double N = 2.0*(double)window_elements*(double)window_elements;
	filtered_pose.cov[10] = v_cov_joint(0,0)/N;
	filtered_pose.cov[11] = v_cov_joint(0,1)/N;
	filtered_pose.cov[14] = filtered_pose.cov[11];
	filtered_pose.cov[15] = v_cov_joint(1,1)/N;

	return filtered_pose;
}

SpointV_cov Cperson_bhmip::low_pass_filter_current_state_linear_regression( )
{

	//Discard a single and double pose trajectory --------------------------------------------
	if (trajectory_.size() <= 2 )
	{
		return trajectory_.back();
	}
	// First order Regression: y(t) ~ beta * x(t) = beta_1 + beta_2 * x(t) ----------------------------
	//vector and matrices initialization
	unsigned int window_elements = trajectory_.size();
//	double initial_ts = trajectory_.front().time_stamp;
	Eigen::VectorXd vx ( window_elements ) , vy ( window_elements );
	Eigen::VectorXd R (window_elements ); //0st order regression -> 1 coeffs
	Eigen::VectorXd r_pred (window_elements);
	for (unsigned int i = 0 ; i < window_elements ; i++)
	{
		vx ( i ) = trajectory_.at(i).vx;
		vy ( i ) = trajectory_.at(i).vy;
		R(i)=1;
		r_pred(i)=1;
	}
	//pseudo inverse of the R matrix ---------------------------------------------------------------
	Eigen::VectorXd invR (  window_elements );
	double sqrR =  R.dot( R);
	invR = (1/sqrR) * R.transpose() ;

	//linear regression ----------------------------------------------------------------------------

	//invR * v = beta -> parameter estimation
	double vx_coef =  invR.dot(vx.transpose());
	double vy_coef = invR.dot(vy.transpose());

	SpointV_cov filtered_pose(
			trajectory_.back().x, //no filtering at position
			trajectory_.back().y,
			trajectory_.back().time_stamp,
			vx_coef ,
			vy_coef );

    filtered_pose.cov[0] = trajectory_.back().cov_xx();
    filtered_pose.cov[1] = trajectory_.back().cov_xy();
    filtered_pose.cov[4] = trajectory_.back().cov_xy();
    filtered_pose.cov[5] = trajectory_.back().cov_yy();

	//covariance of velocity prediction, with respect to regression velocity
	Eigen::VectorXd vx_mean ( window_elements ) , vy_mean ( window_elements );
	vx_mean = R*vx_coef;
	vy_mean = R*vy_coef;
	double cov_vx = (vx - vx_mean).dot( vx - vx_mean );
	double cov_vy = (vy - vy_mean).dot( vy - vy_mean );
	double cov_vxy = (vy - vy_mean).dot( vx - vx_mean );

	//regression covariances + projected uncertainty from positions P_x / dt
	filtered_pose.cov[10] = cov_vx;// + 0.001;
	filtered_pose.cov[11] = cov_vxy;
	filtered_pose.cov[14] = filtered_pose.cov[11];
	filtered_pose.cov[15] = cov_vy;// + 0.001;

	return filtered_pose;
}

void  Cperson_bhmip::intention_precalculation()
{
	double phi , phi_prob;
	std::vector<double> s;
	phi_pose2dest_.push_back( s );
	phi_prob_.push_back( s );
	for(unsigned int i = 0; i < destinations_.size() ; ++i)
	{
		double dx = destinations_[i].x - current_pointV_.x;
		double dy = destinations_[i].y - current_pointV_.y;
		phi = diffangle( atan2(  dy , dx ) , current_pointV_.orientation());
		phi_pose2dest_.back().push_back( phi );
		//c * Pr ( phi | destination_i) calculations
		phi_prob = 100*exp( - phi*phi / phi_var_ );
		phi_prob_.back().push_back( phi_prob );
	}

}

void Cperson_bhmip::trajectory_windowing()
{
	while(  trajectory_.back().time_stamp  - trajectory_.front().time_stamp >  time_window_ )
	{
		trajectory_.pop_front();
		if(type_ == Person)//Robot and Virtual_Person do not require prediction methods
		{
			phi_pose2dest_.pop_front();
			phi_prob_.pop_front();
		}
	}
}

void Cperson_bhmip::refresh_person( double now )
{
	//this method is used when manual remove of persons is required and thus, we must
	//guarantee that the trajectory container and other variables are OK

	if (trajectory_.empty() )
	{
		while( trajectory_.back().time_stamp - now > time_window_)
		{
			trajectory_.pop_front();
			if (trajectory_.empty() ) break;
		}
	}
	now_ = now;
}

void Cperson_bhmip::prediction( double min_v_to_predict )
{
	//calculate if it is adequate to predict intentionality:
	//if target is almost stopped and decelerating
	if( current_pointV_.v() <  min_v_to_predict)
	{
		best_destination_ = Sdestination( 0, current_pointV_.x, current_pointV_.y,
				1.0, Sdestination::Stopping);
		desired_velocity_ = 0.0;//to avoid being too reactive to its environment, it remains in place v_d = 0.0
		return;
	}

	//calculate probabilities to destinations
	double norm_term(0.0),max_prob(-0.1);
	unsigned int max_prob_index;
	posterior_destinations_prob_.clear();
	for( unsigned int i = 0; i<destinations_.size() ; ++i)
	{
		posterior_destinations_prob_.push_back( destinations_[i].prob);
		for (unsigned int j = 0; j < phi_prob_.size() ; ++j)
		{
			//phi_probabilities [ time ] [ destination ]
			posterior_destinations_prob_[i] *= phi_prob_[j][i];
		}
		norm_term += posterior_destinations_prob_[i];
		if( max_prob < posterior_destinations_prob_[i])
		{
			max_prob_index = i;
			max_prob = posterior_destinations_prob_[i];
		}
	}

	//uncertain behavior of prediction depending on certain threshold
	if( 0) //max_prob < 1e16 )//hardcoded threshold corresponds aprox to a diff of 45º
	{
		best_destination_ = Sdestination( 0, current_pointV_.x, current_pointV_.y,
				1.0, Sdestination::Uncertain);
		return;
	}
	//normalization of probabilities
	norm_term = 1/norm_term;
	if ( !is_nan( norm_term ) && !destinations_.empty() )
	{
		for (unsigned int i = 0 ; i < destinations_.size() ; ++i)
			posterior_destinations_prob_[i] *= norm_term;
		best_destination_ = destinations_[max_prob_index];
		best_destination_.prob = posterior_destinations_prob_[max_prob_index];
	}
	else
	{
		best_destination_ = Sdestination( 0, current_pointV_.x, current_pointV_.y,
				1.0, Sdestination::Uncertain);
	}
}

void Cperson_bhmip::reset(  )
{
	trajectory_.clear();
	current_pointV_ = SpointV_cov();
	diff_pointV_ = SpointV_cov();
	phi_pose2dest_.clear();
	phi_prob_.clear();
	force_to_goal_ = Sforce() ;
	force_int_person_ = Sforce() ;
	force_obstacle_ = Sforce() ;
	force_int_robot_ = Sforce() ;
}

void Cperson_bhmip::rotate_and_translate_trajectory(double R, double thetaZ, double linear_vx, double linear_vy, double v_rot_x, double v_rot_y, std::vector<double> vect_odom_eigen_tf, bool debug_odometry){
	// Function for local tracking. This function change the frame of the Current_pose and the window of poses for the person,
	// tacking into account the actual position of the robot.
	// Also compensate the rotational velocities applied to the person when the robot turns.
		// R -> Robot Translation ; thetaZ -> Robot rotation

	if(debug_odometry){
	std::cout << "!!!!!!!!!!!!!!!!!! (people prediction) rotate_and_translate_trajectory !!!!!!!!!!!!!" << std::endl;
	}
	hom2_ant.clear();

	Eigen::MatrixXd homT1; // homogeneous transfor para pasar los tracks al nuevo frame de la odometria.
	Eigen::MatrixXd homT1vis;
	Eigen::MatrixXd homT2;
	Eigen::MatrixXd J; // necesaria para introducir el aumento de la covarianza por culpa del error en R=traslacion y en thetaZ=rotación.
	Eigen::MatrixXd H_cov;
	Eigen::MatrixXd P;
	homT1.resize(3, 3);
	homT1vis.resize(3, 3);
	homT2.resize(3, 3);
	J.resize(4, 2);
	H_cov.resize(4, 4);
	P.resize(2, 2);

	/*homT1.row(0) << cos(thetaZ), -sin(thetaZ), R * cos(thetaZ/2);
	homT1.row(1) << sin(thetaZ), cos(thetaZ), R * sin(thetaZ/2);
	homT1.row(2) << 0, 0, 1;*/

	if(debug_odometry){
	std::cout << "(people prediction) homT1 tf=" << std::endl;
	std::cout << "["<<vect_odom_eigen_tf[0]<<","<<vect_odom_eigen_tf[1]<<","<< vect_odom_eigen_tf[2]<<"]" << std::endl;
	std::cout << "["<<vect_odom_eigen_tf[3]<<","<<vect_odom_eigen_tf[4]<<","<< vect_odom_eigen_tf[5]<<"]" << std::endl;
	std::cout << "["<<vect_odom_eigen_tf[6]<<","<<vect_odom_eigen_tf[7]<<","<< vect_odom_eigen_tf[8]<<"]"  << std::endl;
	}

	homT1.row(0) << vect_odom_eigen_tf[0], vect_odom_eigen_tf[1], vect_odom_eigen_tf[2];
	homT1.row(1) << vect_odom_eigen_tf[3], vect_odom_eigen_tf[4], vect_odom_eigen_tf[5];
	homT1.row(2) << vect_odom_eigen_tf[6], vect_odom_eigen_tf[7], vect_odom_eigen_tf[8];

	if(debug_odometry){
	homT1vis.row(0) << cos(thetaZ), -sin(thetaZ), R * cos(thetaZ/2);
	homT1vis.row(1) << sin(thetaZ), cos(thetaZ), R * sin(thetaZ/2);
	homT1vis.row(2) << 0, 0, 1;
		std::cout << "homT1vis _ prediction  = [ " << homT1(0, 0) << " , " << homT1(0, 1)
				<< " , " << homT1(0, 2) << std::endl << "               "
				<< homT1(1, 0) << " , " << homT1(1, 1) << " , " << homT1(1, 2)
				<< std::endl << "               " << homT1(2, 0) << " , "
				<< homT1(2, 1) << " , " << homT1(2, 2) << std::endl;
	}

	//homT2 = homT1.inverse();
	homT2 = homT1;//.inverse();

	if(debug_odometry){
		std::cout << "homT2 _ prediction = [ " << homT2(0, 0) << " , " << homT2(0, 1)
				<< " , " << homT2(0, 2) << std::endl << "               "
				<< homT2(1, 0) << " , " << homT2(1, 1) << " , " << homT2(1, 2)
				<< std::endl << "               " << homT2(2, 0) << " , "
				<< homT2(2, 1) << " , " << homT2(2, 2) << std::endl;
	}
		/*hom2_ant.push_back(homT2(0, 0));
		hom2_ant.push_back(homT2(0, 1));
		hom2_ant.push_back(homT2(0, 2));
		hom2_ant.push_back(homT2(1, 0));
		hom2_ant.push_back(homT2(1, 1));
		hom2_ant.push_back(homT2(1, 2));
		hom2_ant.push_back(homT2(2, 0));
		hom2_ant.push_back(homT2(2, 1));
		hom2_ant.push_back(homT2(2, 2));
		hom2_tf.push_front(hom2_ant);*/

	H_cov.row(0) << cos(thetaZ), sin(thetaZ), 0, 0;
	H_cov.row(1) << -sin(thetaZ), cos(thetaZ), 0, 0;
	H_cov.row(2) << 0, 0, cos(thetaZ), sin(thetaZ);
	H_cov.row(3) << 0, 0, -sin(thetaZ), cos(thetaZ);

	P.row(0) << 10 * R / 100, 0; //[G_R^2       0    ]
	P.row(1) << 0, 10 * thetaZ / 100; //[  0    G_thetaZ^2]

	// Robot local velocity
	trajectory_local_velocity_x_.push_back(linear_vx);
	trajectory_local_velocity_y_.push_back(linear_vy);

	// Change tracks trajectories to the actual robot frame.

	for(unsigned int i=0; i<trajectory_.size(); i++){

		/*Eigen::MatrixXd homT2_act;
		for(unsigned int j=0;j=<i;j++){
			homT2_act=homT2_act*hom2_tf
		}*/

		SpointV_cov track_ant=trajectory_[i];
		if(debug_odometry){
			std::cout << " Before (people prediction) trajectory_("<<i<<")"<< std::endl;
				trajectory_[i].print();
		}
		SpointV_cov track_act; //trajectory point transformed.

		Eigen::MatrixXd pos_ant(3,1);
		Eigen::MatrixXd pos_act(3,1);
		Eigen::MatrixXd vel_ant(3,1);
		Eigen::MatrixXd vel_act(3,1);
		Eigen::MatrixXd cov_ant(4,4);
		Eigen::MatrixXd cov_act(4,4);
		Eigen::MatrixXd robot_linear_vel(3,1);

		robot_linear_vel(0, 0) = linear_vx;
		robot_linear_vel(1, 0) = linear_vy;
		robot_linear_vel(2, 0) = 0;

		pos_ant(0, 0) = track_ant.x;
		pos_ant(1, 0) = track_ant.y;
		pos_ant(2, 0) = 1;

		pos_act = homT2 * pos_ant;

		vel_ant.row(0) << track_ant.vx;
		vel_ant.row(1) << track_ant.vy;
		vel_ant.row(2) << 0;

		vel_act = homT2 * vel_ant;

		//vel_act=vel_act-robot_linear_vel- w_robot x pos_act;

		J.row(0) << -cos(thetaZ), -sin(thetaZ) * track_ant.x
						+ cos(thetaZ) * track_ant.y + R * sin(thetaZ );
		J.row(1) << sin(thetaZ), -cos(thetaZ) * track_ant.x
						- sin(thetaZ) * track_ant.y + R * cos(thetaZ );
		J.row(2) << 0, -sin(thetaZ) * track_ant.vx + cos(thetaZ) * track_ant.vy;
		J.row(3) << 0, -cos(thetaZ) * track_ant.vx - sin(thetaZ) * track_ant.vy;

		cov_ant.row(0) << track_ant.cov[0], track_ant.cov[1], track_ant.cov[2], track_ant.cov[3];
		cov_ant.row(1) << track_ant.cov[4], track_ant.cov[5], track_ant.cov[6], track_ant.cov[7];
		cov_ant.row(2) << track_ant.cov[8], track_ant.cov[9], track_ant.cov[10], track_ant.cov[11];
		cov_ant.row(3) << track_ant.cov[12], track_ant.cov[13], track_ant.cov[14], track_ant.cov[15];

		//cov_act = H_cov * cov_ant * H_cov.transpose() + J * P * J.transpose();
		cov_act = H_cov * cov_ant * H_cov.transpose();

		std::vector<double> vect_cov_act;
		vect_cov_act.reserve(16);
		vect_cov_act.resize(16, 0.0);
		vect_cov_act[0] = (double) cov_act(0, 0); // cov_xx
		vect_cov_act[1] = (double) cov_act(0, 1); //cov_xy
		vect_cov_act[2] = (double) cov_act(0, 2); // cov_xvx
		vect_cov_act[3] = (double) cov_act(0, 3); //cov_xvy
		vect_cov_act[4] = (double) cov_act(1, 0); //cov_yx
		vect_cov_act[5] = (double) cov_act(1, 1); //cov_yy
		vect_cov_act[6] = (double) cov_act(1, 2); //cov_yvx
		vect_cov_act[7] = (double) cov_act(1, 3); //cov_yvy
		vect_cov_act[8] = (double) cov_act(2, 0); // cov_vxvx
		vect_cov_act[9] = (double) cov_act(2, 1); //cov_vxvy
		vect_cov_act[10] = (double) cov_act(2, 2); // cov_vxx
		vect_cov_act[11] = (double) cov_act(2, 3); //cov_vxy
		vect_cov_act[12] = (double) cov_act(3, 0); //cov_vyvx
		vect_cov_act[13] = (double) cov_act(3, 1); //cov_vyvy
		vect_cov_act[14] = (double) cov_act(3, 2); //cov_vyx
		vect_cov_act[15] = (double) cov_act(3, 3); //cov_vyy

		track_act = SpointV_cov(pos_act(0, 0), pos_act(1, 0),track_ant.time_stamp, vel_act(0, 0), vel_act(1, 0),vect_cov_act);

		trajectory_[i]=track_act;

		if(debug_odometry){
		std::cout << "(people prediction) track_ant("<<i<<")" << std::endl;
		track_ant.print();
		std::cout << " (people prediction) track_act("<<i<<")"<< std::endl;
		track_act.print();
		std::cout << " After (people prediction) trajectory_("<<i<<")"<< std::endl;
		trajectory_[i].print();
		}
	}

	// change current_pointV_ with odometry
	SpointV_cov ant_current_pointV=current_pointV_;
	if(debug_odometry){
	std::cout << " ANT current_pointV_="<< std::endl;
	current_pointV_.print();
	}
	//SpointV_cov act_current_pointV;
	Eigen::MatrixXd pos_ant_current_pointV(3,1);
	Eigen::MatrixXd pos_act_current_pointV(3,1);
	Eigen::MatrixXd vel_ant_current_pointV(3,1);
	Eigen::MatrixXd vel_act_current_pointV(3,1);
	Eigen::MatrixXd cov_ant_current_pointV(4,4);
	Eigen::MatrixXd cov_act_current_pointV(4,4);

	pos_ant_current_pointV(0, 0) = ant_current_pointV.x;
	pos_ant_current_pointV(1, 0) = ant_current_pointV.y;
	pos_ant_current_pointV(2, 0) = 1;

	pos_act_current_pointV = homT2 * pos_ant_current_pointV;

	vel_ant_current_pointV.row(0) << ant_current_pointV.vx;
	vel_ant_current_pointV.row(1) << ant_current_pointV.vy;
	vel_ant_current_pointV.row(2) << 0;

	vel_act_current_pointV = homT2 * vel_ant_current_pointV;

	J.row(0) << -cos(thetaZ), -sin(thetaZ) * ant_current_pointV.x + cos(thetaZ) * ant_current_pointV.y + R * sin(thetaZ / 2);
	J.row(1) << sin(thetaZ), -cos(thetaZ) * ant_current_pointV.x - sin(thetaZ) * ant_current_pointV.y + R * cos(thetaZ / 2);
	J.row(2) << 0, -sin(thetaZ) * ant_current_pointV.vx + cos(thetaZ) * ant_current_pointV.vy;
	J.row(3) << 0, -cos(thetaZ) * ant_current_pointV.vx - sin(thetaZ) * ant_current_pointV.vy;

	cov_ant_current_pointV.row(0) << ant_current_pointV.cov[0], ant_current_pointV.cov[1], ant_current_pointV.cov[2], ant_current_pointV.cov[3];
	cov_ant_current_pointV.row(1) << ant_current_pointV.cov[4], ant_current_pointV.cov[5], ant_current_pointV.cov[6], ant_current_pointV.cov[7];
	cov_ant_current_pointV.row(2) << ant_current_pointV.cov[8], ant_current_pointV.cov[9], ant_current_pointV.cov[10], ant_current_pointV.cov[11];
	cov_ant_current_pointV.row(3) << ant_current_pointV.cov[12], ant_current_pointV.cov[13], ant_current_pointV.cov[14], ant_current_pointV.cov[15];

	cov_act_current_pointV = H_cov * cov_ant_current_pointV * H_cov.transpose() + J * P * J.transpose();

	std::vector<double> vect_cov_act_current_pointV;
	vect_cov_act_current_pointV.reserve(16);
	vect_cov_act_current_pointV.resize(16, 0.0);
	vect_cov_act_current_pointV[0] = (double) cov_act_current_pointV(0, 0); // cov_xx
	vect_cov_act_current_pointV[1] = (double) cov_act_current_pointV(0, 1); //cov_xy
	vect_cov_act_current_pointV[2] = (double) cov_act_current_pointV(0, 2); // cov_xvx
	vect_cov_act_current_pointV[3] = (double) cov_act_current_pointV(0, 3); //cov_xvy
	vect_cov_act_current_pointV[4] = (double) cov_act_current_pointV(1, 0); //cov_yx
	vect_cov_act_current_pointV[5] = (double) cov_act_current_pointV(1, 1); //cov_yy
	vect_cov_act_current_pointV[6] = (double) cov_act_current_pointV(1, 2); //cov_yvx
	vect_cov_act_current_pointV[7] = (double) cov_act_current_pointV(1, 3); //cov_yvy
	vect_cov_act_current_pointV[8] = (double) cov_act_current_pointV(2, 0); // cov_vxvx
	vect_cov_act_current_pointV[9] = (double) cov_act_current_pointV(2, 1); //cov_vxvy
	vect_cov_act_current_pointV[10] = (double) cov_act_current_pointV(2, 2); // cov_vxx
	vect_cov_act_current_pointV[11] = (double) cov_act_current_pointV(2, 3); //cov_vxy
	vect_cov_act_current_pointV[12] = (double) cov_act_current_pointV(3, 0); //cov_vyvx
	vect_cov_act_current_pointV[13] = (double) cov_act_current_pointV(3, 1); //cov_vyvy
	vect_cov_act_current_pointV[14] = (double) cov_act_current_pointV(3, 2); //cov_vyx
	vect_cov_act_current_pointV[15] = (double) cov_act_current_pointV(3, 3); //cov_vyy

	//double track_vx=vel_act_current_pointV(0,0) - v_rot_x;
	//double track_vy=vel_act_current_pointV(1,0) - v_rot_y;

	//vel_act_current_pointV(0,0)=track_vx;
	//vel_act_current_pointV(1,0)=track_vy;

	current_pointV_ = SpointV_cov(pos_act_current_pointV(0, 0), pos_act_current_pointV(1, 0), ant_current_pointV.time_stamp, vel_act_current_pointV(0,0), vel_act_current_pointV(1,0),vect_cov_act_current_pointV);
	if(debug_odometry){
	std::cout << " AcT current_pointV_="<< std::endl;
	current_pointV_.print();
	}
}


#include <kv/Heine.hpp>
#include <kv/qAiry.hpp>
#include <iostream>
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/io.hpp>
#include <cmath>
typedef kv::interval<double> itv;
typedef kv::complex< kv::interval<double> > cp;
using namespace std;
namespace ub = boost::numeric::ublas;
int main()
{
  cout.precision(17);
  ub::vector< itv > x(100);
  int n=40;
  itv q,xx,xy,omega;
  q="0.9";
  omega="0.3";
  x(0)=1.55;
  xy=mid(x(0));
  for(int i=0;i<=n;i++){
    xx=mid(x(i));
    x(i+1)=xx-kv::Ramanujan_qAiry(itv(q),itv(xx))*(xy-q*xy-omega)
      /(kv::Ramanujan_qAiry(itv(q),itv(xy))-kv::Ramanujan_qAiry(itv(q),itv(q*xy+omega)))
      +(1-(xy-q*xy-omega)*(kv::Ramanujan_qAiry(itv(q),itv(x(i)))-kv::Ramanujan_qAiry(itv(q),itv(q*x(i)+omega)))
	/(x(i)-q*x(i)-omega)/(kv::Ramanujan_qAiry(itv(q),itv(xy))-kv::Ramanujan_qAiry(itv(q),itv(q*xy+omega))))*(x(i)-xx);
    cout<<x(i+1)<<endl;
    cout<<"value of RqA inf"<<kv::Ramanujan_qAiry(itv(q),itv(x(i+1).lower()))<<endl;
    cout<<"value of RqA sup"<<kv::Ramanujan_qAiry(itv(q),itv(x(i+1).upper()))<<endl;
    cout<<"value of RqA mid"<<kv::Ramanujan_qAiry(itv(q),itv(mid(x(i+1))))<<endl;
    cout<<"value of contraction const:"<<abs(1-(xy-q*xy-omega)*(kv::Ramanujan_qAiry(itv(q),itv(x(i)))-kv::Ramanujan_qAiry(itv(q),itv(q*x(i)+omega)))
					     /(x(i)-q*x(i)-omega)/(kv::Ramanujan_qAiry(itv(q),itv(xy))-kv::Ramanujan_qAiry(itv(q),itv(q*xy+omega)))).upper()<<endl;
  }
}


#ifndef POINT_HPP_
#define POINT_HPP_

#include <boost/operators.hpp>
#include <ostream>

namespace gnssro
{
  template< class T , size_t Dim >
  class Point :
    boost::additive1< Point< T , Dim > ,
    boost::additive2< Point< T , Dim  > , T ,
    boost::multiplicative2< Point< T , Dim > , T> > >
    {
    public:

        const static size_t dim = Dim;

        //
        // Constructors
        //
        Point( void )
        {
            for( size_t i=0 ; i<dim ; ++i ) m_val[i] = 0.0;
        }

        Point( T val )
        {
            for( size_t i=0 ; i<dim ; ++i ) m_val[i] = val;
        }

        Point( T x , T y , T z = 0.0 )
        {
            if( dim > 0 ) m_val[0] = x;
            if( dim > 1 ) m_val[1] = y;
            if( dim > 2 ) m_val[2] = z;
        }

        Point( Point< T , Dim >& p )
        {
            for( size_t ii=0; ii<dim; ++ii )
                m_val[ii] = p[ii];
        }

        //  End Constructors

        //  Destructor
        ~Point(){};
        //  End Destructor

        //
        //  Operators
        //
        T operator[]( size_t i ) const { return m_val[i]; }
        T& operator[]( size_t i ) { return m_val[i]; }

        Point<T,dim>& operator= ( const Point<T,dim>& p )
        {
            for( size_t ii=0; ii<dim; ++ii )
                m_val[ii] = p[ii];
            return *this;
        }

        Point<T,dim>& operator+=( const Point<T,dim>& p )
        {
            for( size_t ii=0 ; ii<dim ; ++ii )
                m_val[ii] += p[ii];
            return *this;
        }

        Point<T,dim>& operator-=( const Point<T,dim>& p )
        {
            for( size_t ii=0; ii<dim; ++ii )
                m_val[ii] -= p[ii];
            return *this;
        }

        Point<T,dim>& operator+=( const T& val )
        {
            for( size_t ii=0; ii<dim; ++ii )
                m_val[ii] += val;
            return *this;
        }

        Point<T,dim>& operator-=( const T& val )
        {
            for( size_t ii=0; ii<dim; ++ii )
                m_val[ii] -= val;
            return *this;
        }

        Point<T,dim>& operator*=( const T &val )
        {
            for( size_t ii=0; ii<dim; ++ii )
                m_val[ii] *= val;
            return *this;
        }

        Point<T,dim>& operator/=( const T &val )
        {
            for( size_t i=0 ; i<dim ; ++i )
                m_val[i] /= val;
            return *this;
        }

        //  End Operators

    private:

        // Actual Point coordinates are private (access via [] operator):
        T m_val[dim];

    };

    //
    //  Additional vector operators
    //

    //
    //  the - operator
    //
    template< class T , size_t Dim >
    Point< T , Dim > operator-( const Point< T , Dim > &p )
    {
        Point< T , Dim > tmp;
        for( size_t i=0 ; i<Dim ; ++i ) tmp[i] = -p[i];
        return tmp;
    }

    //
    //  Inner product
    //
    template< class T , size_t Dim >
    T scalar_prod( const Point< T , Dim > &p1 , const Point< T , Dim > &p2 )
    {
        T tmp = 0.0;
        for( size_t i=0 ; i<Dim ; ++i ) tmp += p1[i] * p2[i];
        return tmp;
    }

    //
    //  L^1 norm
    //
    template< class T , size_t Dim >
    T norm( const Point< T , Dim > &p1 )
    {
        return scalar_prod( p1 , p1 );
    }

    //
    //  L^2 norm
    //
    template< class T , size_t Dim >
    T abs( const Point< T , Dim > &p1 )
    {
        return sqrt( norm( p1 ) );
    }

    //
    //  output stream operator
    //
    template< class T , size_t Dim >
    std::ostream& operator<<( std::ostream &out , const Point< T , Dim > &p )
    {
        if( Dim > 0 ) out << p[0];
        for( size_t i=1 ; i<Dim ; ++i ) out << " " << p[i];
        return out;
    }

}  //  end of namespace gnssro

#endif  //  POINT_HPP_


#include "fft.hpp"

#include "dynamic_grid.hpp"
#include "global.hpp"
#include "test_helpers.hpp"

#include <fftw3.h>
#include <vector>
#include <complex>
#include <cstdlib>

#include <boost/test/unit_test.hpp>

BOOST_AUTO_TEST_SUITE(fft_tests)

BOOST_AUTO_TEST_CASE(check_roundtrip)
{

	std::vector<complex_t> data(64);
	for(int i=0; i < 64; ++i)
	{
		data[i] = rand_value();
	}

	BOOST_TEST_CHECKPOINT("checking 1d fft");

	auto copy = data;

	fft(copy, std::vector<int>({64}));
	ifft(copy, std::vector<int>({64}));

	for(int i=0; i < 64; ++i)
	{
		BOOST_CHECK_SMALL( std::abs(copy[i] - data[i]), 1e3*std::numeric_limits<double>::epsilon() );
	}

	BOOST_TEST_CHECKPOINT("checking 2d fft");

	copy = data;

	fft(copy, std::vector<int>({8,8}));
	ifft(copy, std::vector<int>({8,8}));
	for(int i=0; i < 64; ++i)
	{
		BOOST_CHECK_SMALL( std::abs(copy[i] - data[i]), 1e3*std::numeric_limits<double>::epsilon() );
	}

	BOOST_TEST_CHECKPOINT("checking 3d fft");

	copy = data;

	fft(copy, std::vector<int>({4, 4, 4}));
	ifft(copy, std::vector<int>({4, 4, 4}));
	for(int i=0; i < 64; ++i)
	{
		BOOST_CHECK_SMALL( std::abs(copy[i] - data[i]), 1e3*std::numeric_limits<double>::epsilon() );
	}
}

BOOST_AUTO_TEST_CASE(check_error_handling)
{

    std::vector<complex_t> data(35);
    for(int i=0; i < 35; ++i)
	{
		data[i] = rand_value();
	}

	auto copy = data;

	// throw because copy.size does not match expected size
	BOOST_CHECK_THROW(fft(copy, std::vector<int>({64})), std::invalid_argument);
	BOOST_CHECK_THROW(fft(copy, std::vector<int>({5,5})), std::invalid_argument);
	BOOST_CHECK_THROW(fft(copy, std::vector<int>({5,5,5})), std::invalid_argument);

	// check that copy is left unchanged by the preceding calls
	for(unsigned int i=0; i < data.size(); ++i)
	{
		BOOST_CHECK_EQUAL( copy[i], data[i] );
	}

	// same thing for ifft
	BOOST_CHECK_THROW(ifft(copy, std::vector<int>({64})), std::invalid_argument);
	BOOST_CHECK_THROW(ifft(copy, std::vector<int>({5,5})), std::invalid_argument);
	BOOST_CHECK_THROW(ifft(copy, std::vector<int>({5,5,5})), std::invalid_argument);
	// check that copy is left unchanged
	for(unsigned int i=0; i < data.size(); ++i)
	{
		BOOST_CHECK_EQUAL( copy[i], data[i] );
	}

	// check size argument fitting into int
	BOOST_CHECK_THROW( fft(copy, std::vector<int>({-1})), std::overflow_error );
	BOOST_CHECK_THROW( ifft(copy, std::vector<int>({-1})), std::overflow_error );
}

/// \todo grid_interface_test is obsolete
/// \todo iterator interface and DynamicGrid interface tests missing.
/*
BOOST_AUTO_TEST_CASE(grid_interface_test)
{

    std::vector<std::complex<double>> data(64);
    for(int i=0; i < 64; ++i)
	{
		data[i] = rand_value();
	}

	// 1d

	auto grid_dat = data;
    grid<decltype(data), 1> grid1d(64, grid_dat );

	auto copy = data;

	fft(grid1d);
	fft(1, copy, 64);
	for(int i=0; i < 64; ++i)
	{
		BOOST_CHECK_EQUAL( copy[i], grid_dat[i] );
	}

	ifft(grid1d);
	ifft(1, copy, 64);
	for(int i=0; i < 64; ++i)
	{
		BOOST_CHECK_EQUAL( copy[i], grid_dat[i] );
	}

	// 3d
    grid<decltype(data), 3> grid3d(4, grid_dat );

	fft(grid3d);
	fft(3, copy, 4);
	for(int i=0; i < 64; ++i)
	{
		BOOST_CHECK_EQUAL( copy[i], grid_dat[i] );
	}

	ifft(grid3d);
	ifft(3, copy, 4);
	for(int i=0; i < 64; ++i)
	{
		BOOST_CHECK_EQUAL( copy[i], grid_dat[i] );
	}
}*/

BOOST_AUTO_TEST_CASE( fftw_wisdom_check )
{
	// try fft without wisdom
	fftw_forget_wisdom();
	fftw_cleanup();

	std::vector<std::complex<double>> data(64);
	for( auto& d : data )
		d = rand_value();

	auto copy = data;

	// fft without wisdom
	fft( data, std::vector<int>({64}) );
	// fft with wisdom
	fft( copy, std::vector<int>({64}) );

	for(unsigned i = 0; i < data.size(); ++i)
	{
		BOOST_CHECK_SMALL( std::abs(data[i] - copy[i]), 1e-10 );
	}

	// check inverse
	fftw_cleanup();
	ifft( data, std::vector<int>({64}) );
	ifft( copy, std::vector<int>({64}) );
	for(unsigned i = 0; i < data.size(); ++i)
		BOOST_CHECK_SMALL( std::abs(data[i] - copy[i]), 1e-10 );

}

/// \todo maybe add another check that actually checks values for a know case.

BOOST_AUTO_TEST_SUITE_END()


//=======================================================================
// Copyright (c)
//
// 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)
//=======================================================================
/**
 * @file expressions.hpp
 * @brief
 * @author Piotr Godlewski
 * @version 1.0
 * @date 2014-04-02
 */

#ifndef PAAL_EXPRESSIONS_HPP
#define PAAL_EXPRESSIONS_HPP

#include "paal/lp/ids.hpp"
#include "paal/utils/floating.hpp"
#include "paal/utils/functors.hpp"
#include "paal/utils/print_collection.hpp"
#include "paal/utils/pretty_stream.hpp"

#include <boost/range/adaptor/transformed.hpp>
#include <boost/range/algorithm/count_if.hpp>

#include <functional>
#include <unordered_map>

namespace paal {
namespace lp {

namespace {
struct linear_expression_traits {
    static const utils::compare<double> CMP;
};

const utils::compare<double> linear_expression_traits::CMP = utils::compare<double>();
}

/**
 * Expression class.
 */
class linear_expression {
    typedef std::unordered_map<col_id, double> Elements;
    typedef Elements::const_iterator ExprIter;

  public:
    /// Constructor.
    linear_expression() {}

    /// Constructor.
    linear_expression(col_id col, double coef = 1.) {
        m_coefs.emplace(col, coef);
    }

    /// Addition operator.
    linear_expression &operator+=(const linear_expression &expr) {
        join_expression(expr, utils::plus{});
        return *this;
    }

    /// Subtraction operator.
    linear_expression &operator-=(const linear_expression &expr) {
        join_expression(expr, utils::minus{});
        return *this;
    }

    /// Multiplication by a constant operator.
    linear_expression &operator*=(double val) {
        for (auto &elem : m_coefs) {
            elem.second *= val;
        }
        return *this;
    }

    /// Division by a constant operator.
    linear_expression &operator/=(double val) { return operator*=(1. / val); }

    /// Returns the coefficient for a given column.
    double get_coefficient(col_id col) const {
        auto elem = m_coefs.find(col);
        if (elem != m_coefs.end()) {
            return elem->second;
        } else {
            return 0.;
        }
    }

    /// Returns the iterator range of the elements in the expression.
    const Elements &get_elements() const { return m_coefs; }

    /// Returns the size (number of nonzero coefficients) of the expression.
    int non_zeros() const {
        return boost::count_if(m_coefs, [](std::pair<col_id, double> x) {
            return !linear_expression_traits::CMP.e(x.second, 0);
        });
    }

  private:
    template <typename Operation>
    void join_expression(const linear_expression &expr, Operation op) {
        for (auto new_elem : expr.m_coefs) {
            auto elem = m_coefs.find(new_elem.first);
            if (elem == m_coefs.end()) {
                new_elem.second = op(0., new_elem.second);
                m_coefs.insert(new_elem);
            } else {
                elem->second = op(elem->second, new_elem.second);
            }
        }
    }

    Elements m_coefs;
};

namespace detail {
inline std::string col_id_to_string(col_id col) {
    return " x_" + std::to_string(col.get());
}

template <typename Stream, typename PrintCol>
void print_expression(Stream &o, const linear_expression &expr,
                      PrintCol print_col) {
    print_collection(o, expr.get_elements() |
                            boost::adaptors::transformed(
                                [&](std::pair<col_id, double> col_and_val) {
        return pretty_to_string(col_and_val.second) +
               print_col(col_and_val.first);
    }),
                     " + ");
}
};

/// operator<< : printing expression
template <typename Stream>
Stream &operator<<(Stream &o, const linear_expression &expr) {
    detail::print_expression(o, expr, detail::col_id_to_string);
    return o;
}

/// linear_expression + linear_expression operator.
inline linear_expression operator+(linear_expression expr_left,
                                   const linear_expression &expr_right) {
    expr_left += expr_right;
    return expr_left;
}

/// linear_expression - linear_expression operator.
inline linear_expression operator-(linear_expression expr_left,
                                   const linear_expression &expr_right) {
    expr_left -= expr_right;
    return expr_left;
}

/// linear_expression * double operator.
inline linear_expression operator*(linear_expression expr, double val) {
    expr *= val;
    return expr;
}

/// double * linear_expression operator.
inline linear_expression operator*(double val, const linear_expression &expr) {
    return expr * val;
}

/// linear_expression / double operator.
inline linear_expression operator/(linear_expression expr, double val) {
    expr /= val;
    return expr;
}

/// Unary - operator.
inline linear_expression operator-(const linear_expression &expr) {
    return expr * (-1.);
}

} // lp
} // paal

#endif // PAAL_EXPRESSIONS_HPP


/*********************************************************************
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2021,
 *  Max Planck Institute for Intelligent Systems (MPI-IS).
 *  All rights reserved.
 *
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions
 *  are met:
 *
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above
 *     copyright notice, this list of conditions and the following
 *     disclaimer in the documentation and/or other materials provided
 *     with the distribution.
 *   * Neither the name of the MPI-IS nor the names
 *     of its contributors may be used to endorse or promote products
 *     derived from this software without specific prior written
 *     permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *  POSSIBILITY OF SUCH DAMAGE.
 *********************************************************************/

/* Author: Andreas Orthey */

#include <ompl/base/spaces/SphereStateSpace.h>
#include <ompl/tools/config/MagicConstants.h>
#include <cstring>
#include <boost/math/constants/constants.hpp>

using namespace boost::math::double_constants;  // pi
using namespace ompl::base;

SphereStateSampler::SphereStateSampler(const StateSpace *space) : StateSampler(space)
{
}

void SphereStateSampler::sampleUniform(State *state)
{
    // see for example http://corysimon.github.io/articles/uniformdistn-on-sphere/
    double theta = 2.0 * pi * rng_.uniformReal(0, 1) - pi; //uniform in [-pi,+pi]
    double phi = acos(1.0 - 2.0 * rng_.uniformReal(0, 1));//in [0,+pi]
    SphereStateSpace::StateType *S = state->as<SphereStateSpace::StateType>();
    S->setThetaPhi(theta, phi);
}

void SphereStateSampler::sampleUniformNear(State *state, const State *near, double distance)
{
    SphereStateSpace::StateType *S = state->as<SphereStateSpace::StateType>();
    const SphereStateSpace::StateType *Snear = near->as<SphereStateSpace::StateType>();
    S->setTheta(rng_.uniformReal(Snear->getTheta() - distance, Snear->getTheta() + distance));
    S->setPhi(rng_.uniformReal(Snear->getPhi() - distance, Snear->getPhi() + distance));
    space_->enforceBounds(state);
}

void SphereStateSampler::sampleGaussian(State *state, const State *mean, double stdDev)
{
    SphereStateSpace::StateType *S = state->as<SphereStateSpace::StateType>();
    const SphereStateSpace::StateType *Smean = mean->as<SphereStateSpace::StateType>();
    S->setTheta(rng_.gaussian(Smean->getTheta(), stdDev));
    S->setPhi(rng_.gaussian(Smean->getPhi(), stdDev));
    space_->enforceBounds(state);
}

StateSamplerPtr SphereStateSpace::allocDefaultStateSampler() const
{
    return std::make_shared<SphereStateSampler>(this);
}

SphereStateSpace::SphereStateSpace(double radius):
  radius_(radius)
{
    setName("Sphere" + getName());
    type_ = STATE_SPACE_SPHERE;

    StateSpacePtr SO2(std::make_shared<SO2StateSpace>());
    StateSpacePtr R1(std::make_shared<RealVectorStateSpace>(1));
    R1->as<RealVectorStateSpace>()->setBounds(0, pi);

    addSubspace(SO2, 1.0);
    addSubspace(R1, 1.0);
    lock();
}

double SphereStateSpace::distance(const State *state1, const State *state2) const
{
    // https://en.wikipedia.org/wiki/Great-circle_distance#Formulae

    const SphereStateSpace::StateType *S1 = state1->as<SphereStateSpace::StateType>();
    const SphereStateSpace::StateType *S2 = state2->as<SphereStateSpace::StateType>();

    // Note: Formula assumes phi in [-pi/2,+pi/2]
    float t1 = S1->getTheta();
    float phi1 = S1->getPhi() - pi/2.0;

    float t2 = S2->getTheta();
    float phi2 = S2->getPhi() - pi/2.0;

    // This is the Vincenty formula, but it is less numerically stable
    // double dt = t2 - t1;
    // double d1 = powf(cos(phi2) * sin(dt), 2);
    // double d2 = powf(cos(phi1) * sin(phi2) - sin(phi1) * cos(phi2) * cos(dt), 2);
    // double numerator = sqrtf(d1 + d2);
    // double denumerator = sin(phi1) * sin(phi2) + cos(phi1) * cos(phi2) * cos(dt);
    // return radius_ * atan2(numerator, denumerator);

    // Haversine formula
    float s = 0.5*(phi1 - phi2);
    float t = 0.5*(t1 - t2);
    float d = sqrtf(sin(s)*sin(s) + cos(phi1)*cos(phi2)*sin(t)*sin(t));
    return 2*radius_*asin(d);

}

double SphereStateSpace::getMeasure() const
{
    return 4 * pi * radius_ * radius_;
}

State *SphereStateSpace::allocState() const
{
    auto *state = new StateType();
    allocStateComponents(state);
    return state;
}

Eigen::Vector3f SphereStateSpace::toVector(const State *state) const
{
    Eigen::Vector3f v;

    const SphereStateSpace::StateType *S1 = state->as<SphereStateSpace::StateType>();
    float theta = S1->getTheta();
    float phi = S1->getPhi();

    v[0] = radius_*sin(phi)*cos(theta);
    v[1] = radius_*sin(phi)*sin(theta);
    v[2] = radius_*cos(phi);

    return v;
}


#include <pcl_ros_wrapper/common/surface.hpp>
#include <pcl/features/moment_of_inertia_estimation.h>
#include <pcl/surface/convex_hull.h>
#include <pcl/surface/concave_hull.h>
#include <pcl/features/normal_3d.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <boost/make_shared.hpp>

float pcl_ros_wrapper::common::get_cloud_volume(const PointCloudT::Ptr& input)
{
  pcl::MomentOfInertiaEstimation<pcl::PointXYZ> feature_extractor;
  feature_extractor.setInputCloud(input);
  feature_extractor.compute();

  pcl::PointXYZ   minPt;
  pcl::PointXYZ   maxPt;
  pcl::PointXYZ   position_OBB;
  Eigen::Matrix3f rotational_matrix_OBB;
  feature_extractor.getOBB(minPt, maxPt, position_OBB, rotational_matrix_OBB);

  return abs(maxPt.z - minPt.z) * abs(maxPt.x - minPt.x) * abs(maxPt.y - minPt.y);
}

pcl_ros_wrapper::PointCloudT::Ptr pcl_ros_wrapper::common::convex_hull(
  const PointCloudT::Ptr& cloud)
{
  auto                           hull_points = boost::make_shared<PointCloudT>();
  pcl::ConvexHull<pcl::PointXYZ> conv_hull;
  conv_hull.setInputCloud(cloud);
  conv_hull.reconstruct(*hull_points);
  return hull_points;
}

pcl_ros_wrapper::common::concave_hull_result pcl_ros_wrapper::common::concave_hull(
  const PointCloudT::Ptr& cloud,
  const float             alpha)
{
  auto                            hull_points  = boost::make_shared<PointCloudT>();
  auto                            hull_indices = boost::make_shared<pcl::PointIndices>();
  pcl::ConcaveHull<pcl::PointXYZ> conv_hull;
  conv_hull.setInputCloud(cloud);
  conv_hull.setAlpha(alpha);
  conv_hull.setKeepInformation(true);
  conv_hull.reconstruct(*hull_points);
  conv_hull.getHullPointIndices(*hull_indices);
  return concave_hull_result{ hull_points, hull_indices };
}

pcl_ros_wrapper::PointCloudNormalsT::Ptr pcl_ros_wrapper::common::do_normal_estimation(
  const PointCloudT::Ptr& cloud,
  float                   radius,
  bool                    visualize)
{
  auto cloud_normals = boost::make_shared<PointCloudNormalsT>();
  pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> ne;
  ne.setInputCloud(cloud);
  ne.setRadiusSearch(radius);
  ne.compute(*cloud_normals);

  if (visualize) {
    pcl::visualization::PCLVisualizer viewer("PCL Viewer");
    viewer.setBackgroundColor(0.0, 0.0, 0.0);
    viewer.addPointCloudNormals<pcl::PointXYZ, pcl::Normal>(cloud, cloud_normals, 1, 8);
    viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE,
                                            10);
    viewer.spin();
  }
  return cloud_normals;
}


// Copyright  (C)  2007  Ruben Smits <ruben dot smits at mech dot kuleuven dot be>

#ifndef KDL_CHAIN_IKSOLVERVEL_PINV_GIVENS_HPP
#define KDL_CHAIN_IKSOLVERVEL_PINV_GIVENS_HPP

#include "chainiksolver.hpp"
#include "chainjnttojacsolver.hpp"

#include <Eigen/Core>

using namespace Eigen;

namespace KDL
{
    /**
     * Implementation of a inverse velocity kinematics algorithm based
     * on the generalize pseudo inverse to calculate the velocity
     * transformation from Cartesian to joint space of a general
     * KDL::Chain. It uses a svd-calculation based on householders
     * rotations.
     *
     * @ingroup KinematicFamily
     */
    class ChainIkSolverVel_pinv_givens : public ChainIkSolverVel
    {
    public:

        /**
         * Constructor of the solver
         *
         * @param chain the chain to calculate the inverse velocity
         * kinematics for
         * @param eps if a singular value is below this value, its
         * inverse is set to zero, default: 0.00001
         * @param maxiter maximum iterations for the svd calculation,
         * default: 150
         *
         */
        explicit ChainIkSolverVel_pinv_givens(const Chain& chain);
        ~ChainIkSolverVel_pinv_givens();

        virtual int CartToJnt(const JntArray& q_in, const Twist& v_in, JntArray& qdot_out);
        /**
         * not (yet) implemented.
         *
         */
        virtual int CartToJnt(const JntArray& q_init, const FrameVel& v_in, JntArrayVel& q_out){return (error = E_NOT_IMPLEMENTED);};

        /// @copydoc KDL::SolverI::updateInternalDataStructures
        virtual void updateInternalDataStructures();

    private:
        const Chain& chain;
        unsigned int nj;
        ChainJntToJacSolver jnt2jac;
        Jacobian jac;
        bool transpose,toggle;
        unsigned int m,n;
        MatrixXd jac_eigen,U,V,B;
        VectorXd S,tempi,tempj,UY,SUY,qdot_eigen,v_in_eigen;
    };
}
#endif


#pragma once

#include <Eigen/Dense>

#include "spectral/basis/spectral_basis_factory_ks.hpp"
#include "spectral/basis/toolbox/spectral_basis.hpp"
#include "spectral/p2n_factory.hpp"

namespace boltzmann {

template <typename DERIVED, typename FUNC>
void
to_polar(Eigen::DenseBase<DERIVED>& dst,
         const FUNC& cart_fun,
         const SpectralBasisFactoryKS::basis_type basis)
{
  using basis_t = SpectralBasisFactoryKS::basis_type;

  int K = spectral::get_K(basis);
  const auto& P2N = P2NFactory<>::GetInstance(basis);

  dst.derived().resize(basis.size());

  QHermiteW quad(1., K);
  Eigen::MatrixXd Nd(K, K);
  for (int i = 0; i < K; ++i) {
    for (int j = 0; j < K; ++j) {
      double xi = quad.pts(i);
      double wi = quad.wts(i);
      double xj = quad.pts(j);
      double wj = quad.wts(j);
      Nd(i,j) = cart_fun(xi, xj) * std::sqrt(wi * wj);
    }
  }
  p2n.to_polar(dst, Nd);

}

}  // namespace boltzmann


/*============================================================================

The Medical Imaging Interaction Toolkit (MITK)

Copyright (c) German Cancer Research Center (DKFZ)
All rights reserved.

Use of this source code is governed by a 3-clause BSD license that can be
found in the LICENSE file.

============================================================================*/

#ifndef _mitkCLUtil_HXX
#define _mitkCLUtil_HXX

#include <mitkCLUtil.h>

#include <mitkImageAccessByItk.h>

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

// itk includes
#include <itkCheckerBoardImageFilter.h>
#include <itkShapedNeighborhoodIterator.h>
#include "itkHessianRecursiveGaussianImageFilter.h"
#include "itkUnaryFunctorImageFilter.h"
#include "vnl/algo/vnl_symmetric_eigensystem.h"
#include <itkLaplacianRecursiveGaussianImageFilter.h>
#include <itkMultiHistogramFilter.h>

// Morphologic Operations
#include <itkBinaryBallStructuringElement.h>
#include <itkBinaryDilateImageFilter.h>
#include <itkBinaryErodeImageFilter.h>
#include <itkBinaryFillholeImageFilter.h>
#include <itkBinaryMorphologicalClosingImageFilter.h>
#include <itkGrayscaleErodeImageFilter.h>
#include <itkGrayscaleDilateImageFilter.h>
#include <itkGrayscaleFillholeImageFilter.h>

// Image Filter
#include <itkDiscreteGaussianImageFilter.h>
#include <itkSubtractImageFilter.h>

void mitk::CLUtil::ProbabilityMap(const mitk::Image::Pointer & image , double mean, double stddev, mitk::Image::Pointer & outimage)
{
  AccessFixedDimensionByItk_3(image, mitk::CLUtil::itkProbabilityMap, 3, mean, stddev, outimage);
}

void mitk::CLUtil::ErodeGrayscale(mitk::Image::Pointer & image , unsigned int radius, mitk::CLUtil::MorphologicalDimensions d, mitk::Image::Pointer & outimage)
{
  AccessFixedDimensionByItk_3(image, mitk::CLUtil::itkErodeGrayscale, 3, outimage, radius, d);
}

void mitk::CLUtil::DilateGrayscale(mitk::Image::Pointer & image, unsigned int radius, mitk::CLUtil::MorphologicalDimensions d, mitk::Image::Pointer & outimage)
{
  AccessFixedDimensionByItk_3(image, mitk::CLUtil::itkDilateGrayscale, 3, outimage, radius, d);
}

void mitk::CLUtil::FillHoleGrayscale(mitk::Image::Pointer & image, mitk::Image::Pointer & outimage)
{
  AccessFixedDimensionByItk_1(image, mitk::CLUtil::itkFillHoleGrayscale, 3, outimage);
}

void mitk::CLUtil::InsertLabel(mitk::Image::Pointer & image, mitk::Image::Pointer & maskImage, unsigned int label)
{
  AccessByItk_2(image, mitk::CLUtil::itkInsertLabel, maskImage, label);
}

void mitk::CLUtil::GrabLabel(mitk::Image::Pointer & image, mitk::Image::Pointer & outimage, unsigned int label)
{
  AccessFixedDimensionByItk_2(image, mitk::CLUtil::itkGrabLabel, 3, outimage, label);
}

void mitk::CLUtil::ConnectedComponentsImage(mitk::Image::Pointer & image, mitk::Image::Pointer& mask, mitk::Image::Pointer &outimage, unsigned int& num_components)
{
  AccessFixedDimensionByItk_3(image, mitk::CLUtil::itkConnectedComponentsImage,3, mask, outimage, num_components);
}

void mitk::CLUtil::MergeLabels(mitk::Image::Pointer & img, const std::map<unsigned int, unsigned int> & map)
{
  AccessByItk_1(img, mitk::CLUtil::itkMergeLabels, map);
}

void mitk::CLUtil::CountVoxel(mitk::Image::Pointer image, std::map<unsigned int, unsigned int> & map)
{
  AccessByItk_1(image, mitk::CLUtil::itkCountVoxel, map);
}

void mitk::CLUtil::CountVoxel(mitk::Image::Pointer image, unsigned int label, unsigned int & count)
{
  AccessByItk_2(image, mitk::CLUtil::itkCountVoxel, label, count);
}

void mitk::CLUtil::CountVoxel(mitk::Image::Pointer image, unsigned int & count)
{
  AccessByItk_1(image, mitk::CLUtil::itkCountVoxel, count);
}

void mitk::CLUtil::CreateCheckerboardMask(mitk::Image::Pointer image, mitk::Image::Pointer & outimage)
{
  AccessFixedDimensionByItk_1(image, mitk::CLUtil::itkCreateCheckerboardMask,3, outimage);
}

void mitk::CLUtil::LogicalAndImages(const mitk::Image::Pointer & image1, const mitk::Image::Pointer & image2, mitk::Image::Pointer & outimage)
{
  AccessFixedDimensionByItk_2(image1,itkLogicalAndImages, 3, image2, outimage);

}

void mitk::CLUtil::InterpolateCheckerboardPrediction(mitk::Image::Pointer checkerboard_prediction, mitk::Image::Pointer & checkerboard_mask, mitk::Image::Pointer & outimage)
{
  AccessFixedDimensionByItk_2(checkerboard_prediction, mitk::CLUtil::itkInterpolateCheckerboardPrediction,3, checkerboard_mask, outimage);
}

void mitk::CLUtil::GaussianFilter(mitk::Image::Pointer image, mitk::Image::Pointer & smoothed ,double sigma)
{
  AccessFixedDimensionByItk_2(image, mitk::CLUtil::itkGaussianFilter,3, smoothed, sigma);
}

void mitk::CLUtil::DifferenceOfGaussianFilter(mitk::Image::Pointer image, mitk::Image::Pointer & smoothed, double sigma1, double sigma2)
{
  AccessFixedDimensionByItk_3(image, mitk::CLUtil::itkDifferenceOfGaussianFilter, 3, smoothed, sigma1, sigma2);
}

void mitk::CLUtil::LaplacianOfGaussianFilter(mitk::Image::Pointer image, mitk::Image::Pointer & smoothed, double sigma1)
{
  AccessByItk_2(image, mitk::CLUtil::itkLaplacianOfGaussianFilter, sigma1, smoothed);
}

void mitk::CLUtil::HessianOfGaussianFilter(mitk::Image::Pointer image, std::vector<mitk::Image::Pointer> &out, double sigma)
{
  AccessByItk_2(image, mitk::CLUtil::itkHessianOfGaussianFilter, sigma, out);
}

void mitk::CLUtil::LocalHistogram(mitk::Image::Pointer image, std::vector<mitk::Image::Pointer> &out, int Bins, int NeighbourhoodSize)
{
  AccessByItk_3(image, mitk::CLUtil::itkLocalHistograms, out, NeighbourhoodSize, Bins);
}

void mitk::CLUtil::DilateBinary(mitk::Image::Pointer & sourceImage, mitk::Image::Pointer& resultImage, int factor , MorphologicalDimensions d)
{
  AccessFixedDimensionByItk_3(sourceImage, mitk::CLUtil::itkDilateBinary, 3, resultImage, factor, d);
}

void mitk::CLUtil::ErodeBinary(mitk::Image::Pointer & sourceImage, mitk::Image::Pointer& resultImage, int factor, MorphologicalDimensions d)
{
  AccessFixedDimensionByItk_3(sourceImage, mitk::CLUtil::itkErodeBinary, 3, resultImage, factor, d);
}

void mitk::CLUtil::ClosingBinary(mitk::Image::Pointer & sourceImage, mitk::Image::Pointer& resultImage, int factor, MorphologicalDimensions d)
{
  AccessFixedDimensionByItk_3(sourceImage, mitk::CLUtil::itkClosingBinary, 3, resultImage, factor, d);
}

template<typename TImageType>
void mitk::CLUtil::itkProbabilityMap(const TImageType * sourceImage, double mean, double std_dev, mitk::Image::Pointer& resultImage)
{
  itk::Image<double, 3>::Pointer itk_img = itk::Image<double, 3>::New();
  itk_img->SetRegions(sourceImage->GetLargestPossibleRegion());
  itk_img->SetOrigin(sourceImage->GetOrigin());
  itk_img->SetSpacing(sourceImage->GetSpacing());
  itk_img->SetDirection(sourceImage->GetDirection());
  itk_img->Allocate();

  itk::ImageRegionConstIterator<TImageType> it(sourceImage,sourceImage->GetLargestPossibleRegion());
  itk::ImageRegionIterator<itk::Image<double, 3> > outit(itk_img,itk_img->GetLargestPossibleRegion());

  while(!it.IsAtEnd())
  {
    double x = it.Value();

    double prob = (1.0/(std_dev*std::sqrt(2.0*itk::Math::pi))) * std::exp(-(((x-mean)*(x-mean))/(2.0*std_dev*std_dev)));
    outit.Set(prob);
    ++it;
    ++outit;
  }

  mitk::CastToMitkImage(itk_img, resultImage);
}

template< typename TImageType >
void mitk::CLUtil::itkInterpolateCheckerboardPrediction(TImageType * checkerboard_prediction, Image::Pointer &checkerboard_mask, mitk::Image::Pointer & outimage)
{
  typename TImageType::Pointer itk_checkerboard_mask;
  mitk::CastToItkImage(checkerboard_mask,itk_checkerboard_mask);

  typename TImageType::Pointer itk_outimage = TImageType::New();
  itk_outimage->SetRegions(checkerboard_prediction->GetLargestPossibleRegion());
  itk_outimage->SetDirection(checkerboard_prediction->GetDirection());
  itk_outimage->SetOrigin(checkerboard_prediction->GetOrigin());
  itk_outimage->SetSpacing(checkerboard_prediction->GetSpacing());
  itk_outimage->Allocate();
  itk_outimage->FillBuffer(0);

  //typedef typename itk::ShapedNeighborhoodIterator<TImageType>::SizeType SizeType;
  typedef itk::Size<3> SizeType;
  SizeType size;
  size.Fill(1);
  itk::ShapedNeighborhoodIterator<TImageType> iit(size,checkerboard_prediction,checkerboard_prediction->GetLargestPossibleRegion());
  itk::ShapedNeighborhoodIterator<TImageType> mit(size,itk_checkerboard_mask,itk_checkerboard_mask->GetLargestPossibleRegion());
  itk::ImageRegionIterator<TImageType> oit(itk_outimage,itk_outimage->GetLargestPossibleRegion());

  typedef typename itk::ShapedNeighborhoodIterator<TImageType>::OffsetType OffsetType;
  OffsetType offset;
  offset.Fill(0);
  offset[0] = 1;       // {1,0,0}
  iit.ActivateOffset(offset);
  mit.ActivateOffset(offset);
  offset[0] = -1;      // {-1,0,0}
  iit.ActivateOffset(offset);
  mit.ActivateOffset(offset);
  offset[0] = 0; offset[1] = 1; //{0,1,0}
  iit.ActivateOffset(offset);
  mit.ActivateOffset(offset);
  offset[1] = -1;      //{0,-1,0}
  iit.ActivateOffset(offset);
  mit.ActivateOffset(offset);

  //    iit.ActivateOffset({{0,0,1}});
  //    iit.ActivateOffset({{0,0,-1}});
  //    mit.ActivateOffset({{0,0,1}});
  //    mit.ActivateOffset({{0,0,-1}});

  while(!iit.IsAtEnd())
  {
    if(mit.GetCenterPixel() == 0)
    {
      typename TImageType::PixelType mean = 0;
      for (auto i = iit.Begin(); ! i.IsAtEnd(); i++)
      { mean += i.Get(); }

      //std::sort(list.begin(),list.end(),[](const typename TImageType::PixelType x,const typename TImageType::PixelType y){return x<=y;});

      oit.Set((mean+0.5)/6.0);
    }
    else
    {
      oit.Set(iit.GetCenterPixel());
    }
    ++iit;
    ++mit;
    ++oit;
  }

  mitk::CastToMitkImage(itk_outimage,outimage);
}

template< typename TImageType >
void mitk::CLUtil::itkCreateCheckerboardMask(TImageType * image, mitk::Image::Pointer & outimage)
{
  typename TImageType::Pointer zeroimg = TImageType::New();
  zeroimg->SetRegions(image->GetLargestPossibleRegion());
  zeroimg->SetDirection(image->GetDirection());
  zeroimg->SetOrigin(image->GetOrigin());
  zeroimg->SetSpacing(image->GetSpacing());

  zeroimg->Allocate();
  zeroimg->FillBuffer(0);

  typedef itk::CheckerBoardImageFilter<TImageType> FilterType;
  typename FilterType::Pointer filter = FilterType::New();
  filter->SetInput1(image);
  filter->SetInput2(zeroimg);
  typename FilterType::PatternArrayType pattern;
  pattern.SetElement(0,(image->GetLargestPossibleRegion().GetSize()[0]));
  pattern.SetElement(1,(image->GetLargestPossibleRegion().GetSize()[1]));
  pattern.SetElement(2,(image->GetLargestPossibleRegion().GetSize()[2]));
  filter->SetCheckerPattern(pattern);

  filter->Update();
  mitk::CastToMitkImage(filter->GetOutput(), outimage);
}

template <class TImageType>
void mitk::CLUtil::itkSumVoxelForLabel(TImageType* image, const mitk::Image::Pointer & source , typename TImageType::PixelType label, double & val )
{
  itk::Image<double,3>::Pointer itk_source;
  mitk::CastToItkImage(source,itk_source);

  itk::ImageRegionConstIterator<TImageType> inputIter(image, image->GetLargestPossibleRegion());
  itk::ImageRegionConstIterator< itk::Image<double,3> > sourceIter(itk_source, itk_source->GetLargestPossibleRegion());
  while(!inputIter.IsAtEnd())
  {
    if(inputIter.Value() == label) val += sourceIter.Value();
    ++inputIter;
    ++sourceIter;
  }
}

template <class TImageType>
void mitk::CLUtil::itkSqSumVoxelForLabel(TImageType* image, const mitk::Image::Pointer & source, typename TImageType::PixelType label, double & val )
{
  itk::Image<double,3>::Pointer itk_source;
  mitk::CastToItkImage(source,itk_source);

  itk::ImageRegionConstIterator<TImageType> inputIter(image, image->GetLargestPossibleRegion());
  itk::ImageRegionConstIterator< itk::Image<double,3> > sourceIter(itk_source, itk_source->GetLargestPossibleRegion());
  while(!inputIter.IsAtEnd())
  {
    if(inputIter.Value() == label) val += sourceIter.Value() * sourceIter.Value();
    ++inputIter;
    ++sourceIter;
  }
}

template<typename TStructuringElement>
void mitk::CLUtil::itkFitStructuringElement(TStructuringElement & se, MorphologicalDimensions d, int factor)
{
  typename TStructuringElement::SizeType size;
  size.Fill(factor);
  switch(d)
  {
  case(All):
  case(Axial):
    size.SetElement(2,0);
    break;
  case(Sagital):
    size.SetElement(0,0);
    break;
  case(Coronal):
    size.SetElement(1,0);
    break;
  }
  se.SetRadius(size);
  se.CreateStructuringElement();
}

template<typename TImageType>
void mitk::CLUtil::itkClosingBinary(TImageType * sourceImage, mitk::Image::Pointer& resultImage, int factor, MorphologicalDimensions d)
{
  typedef itk::BinaryBallStructuringElement<typename TImageType::PixelType, 3> BallType;
  typedef itk::BinaryMorphologicalClosingImageFilter<TImageType, TImageType, BallType> FilterType;

  BallType strElem;
  itkFitStructuringElement(strElem,d,factor);

  typename FilterType::Pointer erodeFilter = FilterType::New();
  erodeFilter->SetKernel(strElem);
  erodeFilter->SetInput(sourceImage);
  erodeFilter->SetForegroundValue(1);
  erodeFilter->Update();

  mitk::CastToMitkImage(erodeFilter->GetOutput(), resultImage);

}

template<typename TImageType>
void mitk::CLUtil::itkDilateBinary(TImageType * sourceImage, mitk::Image::Pointer& resultImage, int factor, MorphologicalDimensions d)
{
  typedef itk::BinaryBallStructuringElement<typename TImageType::PixelType, 3> BallType;
  typedef typename itk::BinaryDilateImageFilter<TImageType, TImageType, BallType> BallDilateFilterType;

  BallType strElem;
  itkFitStructuringElement(strElem,d,factor);

  typename BallDilateFilterType::Pointer erodeFilter = BallDilateFilterType::New();
  erodeFilter->SetKernel(strElem);
  erodeFilter->SetInput(sourceImage);
  erodeFilter->SetDilateValue(1);
  erodeFilter->Update();

  mitk::CastToMitkImage(erodeFilter->GetOutput(), resultImage);

}

template<typename TImageType>
void mitk::CLUtil::itkErodeBinary(TImageType * sourceImage, mitk::Image::Pointer& resultImage, int factor, MorphologicalDimensions d)
{
  typedef itk::BinaryBallStructuringElement<typename TImageType::PixelType, 3> BallType;
  typedef typename itk::BinaryErodeImageFilter<TImageType, TImageType, BallType> BallErodeFilterType;

  BallType strElem;
  itkFitStructuringElement(strElem,d,factor);

  typename BallErodeFilterType::Pointer erodeFilter = BallErodeFilterType::New();
  erodeFilter->SetKernel(strElem);
  erodeFilter->SetInput(sourceImage);
  erodeFilter->SetErodeValue(1);
//  erodeFilter->UpdateLargestPossibleRegion();
  erodeFilter->Update();

  mitk::CastToMitkImage(erodeFilter->GetOutput(), resultImage);

}

///
/// \brief itkFillHolesBinary
/// \param sourceImage
/// \param resultImage
///
template<typename TPixel, unsigned int VDimension>
void mitk::CLUtil::itkFillHolesBinary(itk::Image<TPixel, VDimension>* sourceImage, mitk::Image::Pointer& resultImage)
{
  typedef itk::Image<TPixel, VDimension> ImageType;
  typedef typename itk::BinaryFillholeImageFilter<ImageType> FillHoleFilterType;

  typename FillHoleFilterType::Pointer fillHoleFilter = FillHoleFilterType::New();
  fillHoleFilter->SetInput(sourceImage);
  fillHoleFilter->SetForegroundValue(1);
  fillHoleFilter->Update();

  mitk::CastToMitkImage(fillHoleFilter->GetOutput(), resultImage);
}

///
/// \brief itkLogicalAndImages
/// \param image1 keep the values of image 1
/// \param image2
///
template<typename TImageType>
void mitk::CLUtil::itkLogicalAndImages(const TImageType * image1, const mitk::Image::Pointer & image2, mitk::Image::Pointer & outimage)
{

  typename TImageType::Pointer itk_outimage = TImageType::New();
  itk_outimage->SetRegions(image1->GetLargestPossibleRegion());
  itk_outimage->SetDirection(image1->GetDirection());
  itk_outimage->SetOrigin(image1->GetOrigin());
  itk_outimage->SetSpacing(image1->GetSpacing());

  itk_outimage->Allocate();
  itk_outimage->FillBuffer(0);

  typename TImageType::Pointer itk_image2;
  mitk::CastToItkImage(image2,itk_image2);

  itk::ImageRegionConstIterator<TImageType> it1(image1, image1->GetLargestPossibleRegion());
  itk::ImageRegionConstIterator<TImageType> it2(itk_image2, itk_image2->GetLargestPossibleRegion());
  itk::ImageRegionIterator<TImageType> oit(itk_outimage,itk_outimage->GetLargestPossibleRegion());

  while(!it1.IsAtEnd())
  {
    if(it1.Value() == 0 || it2.Value() == 0)
    {
      oit.Set(0);
    }else
      oit.Set(it1.Value());
    ++it1;
    ++it2;
    ++oit;
  }

  mitk::CastToMitkImage(itk_outimage, outimage);
}

///
/// \brief GaussianFilter
/// \param image
/// \param smoothed
/// \param sigma
///
template<class TImageType>
void mitk::CLUtil::itkGaussianFilter(TImageType * image, mitk::Image::Pointer & smoothed ,double sigma)
{
  typedef itk::DiscreteGaussianImageFilter<TImageType,TImageType> FilterType;
  typename FilterType::Pointer filter = FilterType::New();
  filter->SetInput(image);
  filter->SetVariance(sigma);
  filter->Update();

  mitk::CastToMitkImage(filter->GetOutput(),smoothed);
}

template<class TImageType>
void mitk::CLUtil::itkDifferenceOfGaussianFilter(TImageType * image, mitk::Image::Pointer & smoothed, double sigma1, double sigma2)
{
  typedef itk::DiscreteGaussianImageFilter<TImageType, TImageType> FilterType;
  typedef itk::SubtractImageFilter <TImageType, TImageType> SubtractFilterType;
  typename FilterType::Pointer filter1 = FilterType::New();
  typename FilterType::Pointer filter2 = FilterType::New();
  typename SubtractFilterType::Pointer subFilter = SubtractFilterType::New();
  filter1->SetInput(image);
  filter1->SetVariance(sigma1);
  filter1->Update();
  filter2->SetInput(image);
  filter2->SetVariance(sigma2);
  filter2->Update();
  subFilter->SetInput1(filter1->GetOutput());
  subFilter->SetInput2(filter2->GetOutput());
  subFilter->Update();

  mitk::CastToMitkImage(subFilter->GetOutput(), smoothed);
}

template<typename TPixel, unsigned int VImageDimension>
void mitk::CLUtil::itkLaplacianOfGaussianFilter(itk::Image<TPixel, VImageDimension>* itkImage, double variance, mitk::Image::Pointer &output)
{
  typedef itk::Image<TPixel, VImageDimension> ImageType;
  typedef itk::DiscreteGaussianImageFilter< ImageType, ImageType >  GaussFilterType;
  typedef itk::LaplacianRecursiveGaussianImageFilter<ImageType, ImageType>    LaplacianFilter;

  typename GaussFilterType::Pointer gaussianFilter = GaussFilterType::New();
  gaussianFilter->SetInput(itkImage);
  gaussianFilter->SetVariance(variance);
  gaussianFilter->Update();
  typename LaplacianFilter::Pointer laplaceFilter = LaplacianFilter::New();
  laplaceFilter->SetInput(gaussianFilter->GetOutput());
  laplaceFilter->Update();
  mitk::CastToMitkImage(laplaceFilter->GetOutput(), output);
}

namespace Functor
{
  template <class TInput, class TOutput>
  class MatrixFirstEigenvalue
  {
  public:
    MatrixFirstEigenvalue() {}
    virtual ~MatrixFirstEigenvalue() {}

    int order;

    inline TOutput operator ()(const TInput& input)
    {
      double a, b, c;
      if (input[0] < 0.01 && input[1] < 0.01 &&input[2] < 0.01 &&input[3] < 0.01 &&input[4] < 0.01 &&input[5] < 0.01)
        return 0;
      vnl_symmetric_eigensystem_compute_eigenvals(input[0], input[1], input[2], input[3], input[4], input[5], a, b, c);
      switch (order)
      {
      case 0: return a;
      case 1: return b;
      case 2: return c;
      default: return a;
      }
    }
    bool operator !=(const MatrixFirstEigenvalue) const
    {
      return false;
    }
    bool operator ==(const MatrixFirstEigenvalue& other) const
    {
      return !(*this != other);
    }
  };
}

template<typename TPixel, unsigned int VImageDimension>
void mitk::CLUtil::itkHessianOfGaussianFilter(itk::Image<TPixel, VImageDimension>* itkImage, double variance, std::vector<mitk::Image::Pointer> &out)
{
  typedef itk::Image<TPixel, VImageDimension> ImageType;
  typedef itk::Image<double, VImageDimension> FloatImageType;
  typedef itk::HessianRecursiveGaussianImageFilter <ImageType> HessianFilterType;
  typedef typename HessianFilterType::OutputImageType VectorImageType;
  typedef Functor::MatrixFirstEigenvalue<typename VectorImageType::PixelType, double> DeterminantFunctorType;
  typedef itk::UnaryFunctorImageFilter<VectorImageType, FloatImageType, DeterminantFunctorType> DetFilterType;

  typename HessianFilterType::Pointer hessianFilter = HessianFilterType::New();
  hessianFilter->SetInput(itkImage);
  hessianFilter->SetSigma(std::sqrt(variance));
  for (unsigned int i = 0; i < VImageDimension; ++i)
  {
    mitk::Image::Pointer tmpImage = mitk::Image::New();
    typename DetFilterType::Pointer detFilter = DetFilterType::New();
    detFilter->SetInput(hessianFilter->GetOutput());
    detFilter->GetFunctor().order = i;
    detFilter->Update();
    mitk::CastToMitkImage(detFilter->GetOutput(), tmpImage);
    out.push_back(tmpImage);
  }
}

template<typename TPixel, unsigned int VImageDimension>
void mitk::CLUtil::itkLocalHistograms(itk::Image<TPixel, VImageDimension>* itkImage, std::vector<mitk::Image::Pointer> &out, int size, int bins)
{
  typedef itk::Image<TPixel, VImageDimension> ImageType;
  typedef itk::MultiHistogramFilter <ImageType, ImageType> MultiHistogramType;

  typename MultiHistogramType::Pointer filter = MultiHistogramType::New();
  filter->SetInput(itkImage);
  filter->SetUseImageIntensityRange(true);
  filter->SetSize(size);
  filter->SetBins(bins);
  filter->Update();
  for (int i = 0; i < bins; ++i)
  {
    mitk::Image::Pointer img = mitk::Image::New();
    mitk::CastToMitkImage(filter->GetOutput(i), img);
    out.push_back(img);
  }
}

template<class TImageType>
void mitk::CLUtil::itkErodeGrayscale(TImageType * image, mitk::Image::Pointer & outimage , unsigned int radius, mitk::CLUtil::MorphologicalDimensions d)
{
  typedef itk::BinaryBallStructuringElement<typename TImageType::PixelType, 3>          StructureElementType;
  typedef itk::GrayscaleErodeImageFilter<TImageType,TImageType,StructureElementType>    FilterType;

  StructureElementType ball;
  itkFitStructuringElement(ball,d, radius);

  typename FilterType::Pointer filter = FilterType::New();
  filter->SetKernel(ball);
  filter->SetInput(image);
  filter->Update();

  mitk::CastToMitkImage(filter->GetOutput(),outimage);
}

template<class TImageType>
void mitk::CLUtil::itkDilateGrayscale(TImageType * image, mitk::Image::Pointer & outimage , unsigned int radius, mitk::CLUtil::MorphologicalDimensions d)
{
  typedef itk::BinaryBallStructuringElement<typename TImageType::PixelType, 3>          StructureElementType;
  typedef itk::GrayscaleDilateImageFilter<TImageType,TImageType,StructureElementType>    FilterType;

  StructureElementType ball;
  itkFitStructuringElement(ball,d, radius);

  typename FilterType::Pointer filter = FilterType::New();
  filter->SetKernel(ball);
  filter->SetInput(image);
  filter->Update();

  mitk::CastToMitkImage(filter->GetOutput(),outimage);
}

template<class TImageType>
void mitk::CLUtil::itkFillHoleGrayscale(TImageType * image, mitk::Image::Pointer & outimage)
{
  typedef itk::GrayscaleFillholeImageFilter<TImageType,TImageType>    FilterType;

  typename FilterType::Pointer filter = FilterType::New();
  filter->SetInput(image);
  filter->Update();

  mitk::CastToMitkImage(filter->GetOutput(),outimage);
}

#endif


#include <math.h>
#include <uWS/uWS.h>
#include <iostream>
#include <string>
#include "json.hpp"
#include "PID.h"
#include <boost/algorithm/clamp.hpp>

// for convenience
using nlohmann::json;
using std::string;

// For converting back and forth between radians and degrees.
constexpr double pi() { return M_PI; }
double deg2rad(double x) { return x * pi() / 180; }
double rad2deg(double x) { return x * 180 / pi(); }

// Checks if the SocketIO event has JSON data.
// If there is data the JSON object in string format will be returned,
// else the empty string "" will be returned.
string hasData(string s) {
  auto found_null = s.find("null");
  auto b1 = s.find_first_of("[");
  auto b2 = s.find_last_of("]");
  if (found_null != string::npos) {
    return "";
  }
  else if (b1 != string::npos && b2 != string::npos) {
    return s.substr(b1, b2 - b1 + 1);
  }
  return "";
}

// for PID and twiddle
double MAX_ANGLE = 100.0;
double MAX_SPD_MOD = 1.0;
double MIN_SPD_MOD = 0.25;
double HI_ANGLE = 5.0;

int main() {
  uWS::Hub h;

  PID pid;
  PID spd_pid;
  /**
   * Initialize the pid variable.
   */

  double kd = 0.01;
  double kp = 0.1;
  double ki = 0.0001;
  pid.Init(kp, ki, kd);
  spd_pid.Init(0.3, 0.00, 0.001);
  int iter = 0;
  double tol = 0.0005  ;
  double best_cte = 100000;
  double throttle_set = 0.2;
  bool tune_steering = false;
  bool hs_tuning = false;
  bool tune_throttle = false;
  double target_speed = 20.0;
  double best_spd_err = 100000;
  bool spd_tuned = false;

  h.onMessage([&pid, &spd_pid, &iter, &tol, &best_cte, &best_spd_err, &hs_tuning,
                &throttle_set, &target_speed, &spd_tuned,
                &tune_steering, &tune_throttle](uWS::WebSocket<uWS::SERVER> ws, char *data, size_t length,
                     uWS::OpCode opCode) {
    // "42" at the start of the message means there's a websocket message event.
    // The 4 signifies a websocket message
    // The 2 signifies a websocket event
    if (length && length > 2 && data[0] == '4' && data[1] == '2') {
      auto s = hasData(string(data).substr(0, length));

      if (s != "") {
        auto j = json::parse(s);

        string event = j[0].get<string>();

        if (event == "telemetry") {
          // j[1] is the data JSON object
          double cte = std::stod(j[1]["cte"].get<string>());
          double speed = std::stod(j[1]["speed"].get<string>());
          double angle = std::stod(j[1]["steering_angle"].get<string>());
          double steer_value;
          /**
           * Calculate steering value--remember the steering value is
           *   [-1, 1]
           */

          double spd_err = speed - target_speed;

          if (iter == 20) {
            best_cte = fabs(cte);
            tune_steering = true;
            //spd_tuned = true;
            }

          pid.UpdateError(cte);
          steer_value = pid.TotalError();
          steer_value = boost::algorithm::clamp(steer_value, -1.0, 1.0);
          iter+=1;

          if (tune_steering == true) {
            std::cout << "cte/best: " << cte << " / " << best_cte << std::endl;
            pid.Twiddle(best_cte, cte);
            std::cout << "  --- tol: "<< pid.TolCheck() << std::endl;

            if (pid.TolCheck() < tol) {
              std::cout << "*************** steering tuned ***************" << std::endl;
              tune_steering = false;
              tune_throttle = true;
              best_spd_err = spd_err;
              hs_tuning = false;
            }
          }

          // if (tune_throttle) {
          //   std::cout << "speed err/best: " << spd_err << " / " << best_spd_err << std::endl;
          //   spd_pid.Twiddle(best_spd_err, spd_err);
          //   std::cout << "  --- tol: "<< spd_pid.TolCheck() << std::endl;

          //   if (spd_pid.TolCheck() < tol) {
          //     tune_throttle = false;
          //     spd_tuned = true;
          //   }
          // }

          if (iter % 3000 == 0 && hs_tuning == false) {
            tune_steering = true;
            hs_tuning = true;
            pid.RetrainPID();
          }

          if (tune_throttle) {
            spd_tuned = true;
            }

          if (spd_tuned) {
            spd_pid.UpdateError(spd_err);
            throttle_set = spd_pid.TotalError();
            throttle_set = boost::algorithm::clamp(throttle_set, 0.0, 0.5);

            double spd_mod = 1.0;
            if (fabs(angle) > HI_ANGLE) {
              spd_mod = 0.25;
            } else {
              double low_mod = 1.0 / (HI_ANGLE + 1);
              spd_mod = 1.0 / (fabs(angle) + 1);

              spd_mod =  (spd_mod - low_mod)/(1.0 - low_mod) * (MAX_SPD_MOD - MIN_SPD_MOD) + MIN_SPD_MOD;
              // spd_mod = boost::algorithm::clamp(spd_mod, 0.5, 1.0);
            }
            //std::cout << spd_mod << std::endl;
            throttle_set *= spd_mod;

          }

          // DEBUG

          // std::cout << iter << " ->  CTE: " << cte << " Steering Value: " << steer_value  << "   Throttle: " << throttle_set
          //           << std::endl;

          json msgJson;
          msgJson["steering_angle"] = steer_value;
          msgJson["throttle"] = throttle_set;
          auto msg = "42[\"steer\"," + msgJson.dump() + "]";
          // std::cout << msg << std::endl;
          ws.send(msg.data(), msg.length(), uWS::OpCode::TEXT);

        }  // end "telemetry" if
      } else {
        // Manual driving
        string msg = "42[\"manual\",{}]";
        ws.send(msg.data(), msg.length(), uWS::OpCode::TEXT);
      }
    }  // end websocket message if
  }); // end h.onMessage

  h.onConnection([&h](uWS::WebSocket<uWS::SERVER> ws, uWS::HttpRequest req) {
    std::cout << "Connected!!!" << std::endl;
  });

  h.onDisconnection([&h](uWS::WebSocket<uWS::SERVER> ws, int code,
                         char *message, size_t length) {
    ws.close();
    std::cout << "Disconnected" << std::endl;
  });

  int port = 4567;
  if (h.listen(port)) {
    std::cout << "Listening to port " << port << std::endl;
  } else {
    std::cerr << "Failed to listen to port" << std::endl;
    return -1;
  }

  h.run();

}


#include "sigma.h"
#include "../wavelet/wavelet.h"
#include <armadillo>
#include <iostream>

rpm::vector<double> SIGMA::analyse(const rpm::vector<double>& lx, double fs)
{
    constexpr double fmin = 50;
    constexpr double fmax = 400;

    constexpr double Tmin = 1.0 / fmax;
    constexpr double Tmax = 1.0 / fmin;

    constexpr double oqmin = 0.1;
    constexpr double oqmax = 0.9;

    constexpr double gwlen = Tmin;
    constexpr double fwlen = 0.000;

    constexpr int nclust = 2;

    // Calculate SWT
    constexpr int nlev = 5;
    Wt::DiscreteWavelet wavelet(Wt::BIOR, 15);
    rpm::vector<double> swc = Analysis::swt(lx, wavelet, nlev, 0, true);

    // Calculate multiscale product
    int dlen = (int) Wt::swt_buffer_length((int) lx.size());
    rpm::vector<double> mp(dlen, 1.0);
    for (int k = 1; k * dlen < (int) swc.size(); ++k) {
        for (int i = 0; i < dlen; ++i) {
            mp[i] *= swc[k * dlen + i];
        }
    }

    // Find third roots
    rpm::vector<double> nmp(mp);
    rpm::vector<double> pmp(mp);
    rpm::vector<double> crnmp(mp.size());
    rpm::vector<double> crpmp(mp.size());
    for (int i = 0; i < (int) mp.size(); ++i) {
        // Half-wave rectify on negative half of mp for GCI
        if (nmp[i] > 0)
            nmp[i] = 0;

        // Half-wave rectify on positive half of mp for GOI
        if (pmp[i] < 0)
            pmp[i] = 0;

        crnmp[i] = cbrt(nmp[i]);
        crpmp[i] = cbrt(pmp[i]);
    }

    // Group delay evaluation on mp
    auto [gcic, nsew, ngrdel, ntoff] = xewgrdel(nmp, fs, gwlen, fwlen);
    ngrdel.insert(ngrdel.begin(), ntoff, 0.0);
    ngrdel.erase(std::prev(ngrdel.end(), ntoff), ngrdel.end());

    auto [goic, psew, pgrdel, ptoff] = xewgrdel(pmp, fs, gwlen, fwlen);
    pgrdel.insert(pgrdel.begin(), ptoff, 0.0);
    pgrdel.erase(std::prev(pgrdel.end(), ptoff), pgrdel.end());

    // Set up other variables
    rpm::vector<double> gci(lx.size(), 0.0);
    rpm::vector<double> goi(lx.size(), 0.0);

    // --- GCI Detection ---

    // Model GD slope
    const int nr = ((int) std::round(gwlen * fs)) / 2 - 1;
    const int mngrdellen = 2 * nr + 1;
    rpm::vector<double> mngrdel(mngrdellen);
    for (int i = 0; i < mngrdellen; ++i) {
        mngrdel[i] = i - nr;
    }
    rpm::vector<double> cmngrdel(ngrdel.size(), 0.0);

    const int snfv = (int) gcic.size();
    arma::mat nfv(3, snfv, arma::fill::zeros);

    for (int i = 0; i < snfv; ++i) {
        int lbnd = (int) std::round(gcic[i] - nr);
        int ubnd = lbnd + mngrdellen - 1;

        if (lbnd >= 0 && ubnd < (int) ngrdel.size()) {
            double sum = 0.0;
            double min = HUGE_VAL;
            double mean = 0.0;

            for (int k = lbnd; k <= ubnd; ++k) {
                sum += crnmp[k];
                if (crnmp[k] < min)
                    min = crnmp[k];

                double v = mngrdel[k - lbnd] - ngrdel[k];
                mean += v * v;

                cmngrdel[k] = mngrdel[k - lbnd];
            }

            nfv(0, i) = sum; // Sum of crnmp over GD window
            nfv(1, i) = min; // Peak value of crnmp
            nfv(2, i) = sqrt(mean / mngrdellen); // Phase slope deviation
        }
    }

    // Determine clusters
    arma::gmm_diag ngmm;
    bool status = ngmm.learn(nfv, 3, arma::eucl_dist, arma::random_subset, 10, 5, 1e-8, false);

    // Find cluster with lowest crnmp sum
    int I = (int) arma::index_min(ngmm.means.row(0));

    // If the candidate belongs to the chosen cluster then keep
    for (int i = 0; i < snfv; ++i) {
        if (ngmm.assign(nfv.col(i), arma::eucl_dist) == (arma::uword) I) {
            int k = std::max(0, std::min((int) gci.size() - 1, (int) std::round(gcic[i])));
            gci[k] = 1.0;
        }
    }

    // --- Post-filter swallows (GCIs only) ---

    if (gci.size() > 2) {
        // If a gci is separated from all others by more than Tmax, delete
        rpm::list<int> fgci;
        for (int i = 0; i < (int) gci.size(); ++i) {
            if (gci[i] > 0) {
                fgci.push_back(i);
            }
        }

        // Check first one
        auto i0 = fgci.begin();
        auto i1 = std::next(i0);
        if (*i1 - *i0 > Tmax * fs) {
            fgci.pop_front();
        }

        // Check the middle
        auto i2 = std::next(fgci.begin(), 2);
        auto iend = std::prev(fgci.end());

        while (i2 != iend) {
            auto i2prev = std::prev(i2);
            auto i2next = std::next(i2);

            if ((*i2 - *i2prev > Tmax * fs)
                    && (*i2next - *i2 > Tmax * fs)) {
                fgci.erase(i2);
            }

            i2 = i2next;
        }

        // Check last one
        auto iendprev = std::prev(iend);
        if (*iend - *iendprev > Tmax * fs) {
            fgci.pop_back();
        }

        // Convert back
        std::fill(gci.begin(), gci.end(), 0.0);
        for (auto t : fgci) {
            gci[t] = 1.0;
        }
    }

    return gci;

    // --- GOI detection ---

}


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

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

 * See LICENSE for the license information

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

/*
 * testDiscreteBayesNet.cpp
 *
 *  @date Feb 27, 2011
 *  @author Frank Dellaert
 */

#include <gtsam/discrete/DiscreteBayesNet.h>
#include <gtsam/discrete/DiscreteFactorGraph.h>
#include <gtsam/discrete/DiscreteMarginals.h>
#include <gtsam/base/debug.h>
#include <gtsam/base/Testable.h>
#include <gtsam/base/Vector.h>

#include <CppUnitLite/TestHarness.h>

#include <boost/assign/list_inserter.hpp>
#include <boost/assign/std/map.hpp>

using namespace boost::assign;

#include <iostream>
#include <string>
#include <vector>

using namespace std;
using namespace gtsam;

/* ************************************************************************* */
TEST(DiscreteBayesNet, bayesNet) {
  DiscreteBayesNet bayesNet;
  DiscreteKey Parent(0, 2), Child(1, 2);

  auto prior = boost::make_shared<DiscreteConditional>(Parent % "6/4");
  CHECK(assert_equal(Potentials::ADT({Parent}, "0.6 0.4"),
                     (Potentials::ADT)*prior));
  bayesNet.push_back(prior);

  auto conditional =
      boost::make_shared<DiscreteConditional>(Child | Parent = "7/3 8/2");
  EXPECT_LONGS_EQUAL(1, *(conditional->beginFrontals()));
  Potentials::ADT expected(Child & Parent, "0.7 0.8 0.3 0.2");
  CHECK(assert_equal(expected, (Potentials::ADT)*conditional));
  bayesNet.push_back(conditional);

  DiscreteFactorGraph fg(bayesNet);
  LONGS_EQUAL(2, fg.back()->size());

  // Check the marginals
  const double expectedMarginal[2]{0.4, 0.6 * 0.3 + 0.4 * 0.2};
  DiscreteMarginals marginals(fg);
  for (size_t j = 0; j < 2; j++) {
    Vector FT = marginals.marginalProbabilities(DiscreteKey(j, 2));
    EXPECT_DOUBLES_EQUAL(expectedMarginal[j], FT[1], 1e-3);
    EXPECT_DOUBLES_EQUAL(FT[0], 1.0 - FT[1], 1e-9);
  }
}

/* ************************************************************************* */
TEST(DiscreteBayesNet, Asia) {
  DiscreteBayesNet asia;
  DiscreteKey Asia(0, 2), Smoking(4, 2), Tuberculosis(3, 2), LungCancer(6, 2),
      Bronchitis(7, 2), Either(5, 2), XRay(2, 2), Dyspnea(1, 2);

  asia.add(Asia % "99/1");
  asia.add(Smoking % "50/50");

  asia.add(Tuberculosis | Asia = "99/1 95/5");
  asia.add(LungCancer | Smoking = "99/1 90/10");
  asia.add(Bronchitis | Smoking = "70/30 40/60");

  asia.add((Either | Tuberculosis, LungCancer) = "F T T T");

  asia.add(XRay | Either = "95/5 2/98");
  asia.add((Dyspnea | Either, Bronchitis) = "9/1 2/8 3/7 1/9");

  // Convert to factor graph
  DiscreteFactorGraph fg(asia);
  LONGS_EQUAL(3, fg.back()->size());

  // Check the marginals we know (of the parent-less nodes)
  DiscreteMarginals marginals(fg);
  Vector2 va(0.99, 0.01), vs(0.5, 0.5);
  EXPECT(assert_equal(va, marginals.marginalProbabilities(Asia)));
  EXPECT(assert_equal(vs, marginals.marginalProbabilities(Smoking)));

  // Create solver and eliminate
  Ordering ordering;
  ordering += Key(0), Key(1), Key(2), Key(3), Key(4), Key(5), Key(6), Key(7);
  DiscreteBayesNet::shared_ptr chordal = fg.eliminateSequential(ordering);
  DiscreteConditional expected2(Bronchitis % "11/9");
  EXPECT(assert_equal(expected2, *chordal->back()));

  // solve
  auto actualMPE = chordal->optimize();
  DiscreteValues expectedMPE;
  insert(expectedMPE)(Asia.first, 0)(Dyspnea.first, 0)(XRay.first, 0)(
      Tuberculosis.first, 0)(Smoking.first, 0)(Either.first, 0)(
      LungCancer.first, 0)(Bronchitis.first, 0);
  EXPECT(assert_equal(expectedMPE, actualMPE));

  // add evidence, we were in Asia and we have dyspnea
  fg.add(Asia, "0 1");
  fg.add(Dyspnea, "0 1");

  // solve again, now with evidence
  DiscreteBayesNet::shared_ptr chordal2 = fg.eliminateSequential(ordering);
  auto actualMPE2 = chordal2->optimize();
  DiscreteValues expectedMPE2;
  insert(expectedMPE2)(Asia.first, 1)(Dyspnea.first, 1)(XRay.first, 0)(
      Tuberculosis.first, 0)(Smoking.first, 1)(Either.first, 0)(
      LungCancer.first, 0)(Bronchitis.first, 1);
  EXPECT(assert_equal(expectedMPE2, actualMPE2));

  // now sample from it
  DiscreteValues expectedSample;
  SETDEBUG("DiscreteConditional::sample", false);
  insert(expectedSample)(Asia.first, 1)(Dyspnea.first, 1)(XRay.first, 1)(
      Tuberculosis.first, 0)(Smoking.first, 1)(Either.first, 1)(
      LungCancer.first, 1)(Bronchitis.first, 0);
  auto actualSample = chordal2->sample();
  EXPECT(assert_equal(expectedSample, actualSample));
}

/* ************************************************************************* */
TEST_UNSAFE(DiscreteBayesNet, Sugar) {
  DiscreteKey T(0, 2), L(1, 2), E(2, 2), C(8, 3), S(7, 2);

  DiscreteBayesNet bn;

  // try logic
  bn.add((E | T, L) = "OR");
  bn.add((E | T, L) = "AND");

  // try multivalued
  bn.add(C % "1/1/2");
  bn.add(C | S = "1/1/2 5/2/3");
}

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


#ifdef COMPILATION// -*-indent-tabs-mode:t;c-basic-offset:4;tab-width:4;autowrap:nil;-*-
$CXX $0 -o $0x -lcudart  -lcufft `pkg-config --libs fftw3` -lboost_timer -lboost_unit_test_framework&&$0x&&rm $0x;exit
#endif
// © Alfredo A. Correa 2020

#ifndef MULTI_ADAPTORS_FFT_HPP
#define MULTI_ADAPTORS_FFT_HPP

#include "../adaptors/fftw.hpp"
#include "../adaptors/cufft.hpp"

namespace boost{
namespace multi{
namespace fft{

	static constexpr int forward = fftw::forward;//FFTW_FORWARD;
	static constexpr int none = 0;
	static constexpr int backward = fftw::backward;//FFTW_BACKWARD;

	static_assert( forward != none and none != backward and backward != forward, "!");

	template<std::size_t I> struct priority : std::conditional_t<I==0, std::true_type, struct priority<I-1>>{};

	template<class... Args> auto dft_aux_(priority<0>, Args&&... args) DECLRETURN(  fftw::dft(std::forward<Args>(args)...))
	template<class... Args> auto dft_aux_(priority<1>, Args&&... args) DECLRETURN(cufft ::dft(std::forward<Args>(args)...))
	template<class... Args> auto dft(Args&&... args) DECLRETURN(dft_aux_(priority<1>{}, std::forward<Args>(args)...))

	template<class In, class... Args> auto dft(std::array<bool, std::decay_t<In>::dimensionality> which, In&& in, Args&&... args) DECLRETURN(dft_aux_(priority<1>{}, which, std::forward<In>(in), std::forward<Args>(args)...))

	template<class... Args> auto many_dft_aux_(priority<0>, Args&&... args) DECLRETURN(  fftw::many_dft(std::forward<Args>(args)...))
	template<class... Args> auto many_dft_aux_(priority<1>, Args&&... args) DECLRETURN(cufft ::many_dft(std::forward<Args>(args)...))
	template<class... Args> auto many_dft(Args&&... args) DECLRETURN(many_dft_aux_(priority<1>{}, std::forward<Args>(args)...))

	template<class... Args> auto dft_forward_aux_(priority<0>, Args&&... args) DECLRETURN(  fftw::dft_forward(std::forward<Args>(args)...))
	template<class... Args> auto dft_forward_aux_(priority<1>, Args&&... args) DECLRETURN(cufft ::dft_forward(std::forward<Args>(args)...))
	template<class... Args> auto dft_forward(Args&&... args) DECLRETURN(dft_forward_aux_(priority<1>{}, std::forward<Args>(args)...))
	template<class In, class... Args> auto dft_forward(std::array<bool, std::decay_t<In>::dimensionality> which, In&& in, Args&&... args) DECLRETURN(dft_forward_aux_(priority<1>{}, which, std::forward<In>(in), std::forward<Args>(args)...))

	template<class... Args> auto dft_backward_aux_(priority<0>, Args&&... args) DECLRETURN(  fftw::dft_backward(std::forward<Args>(args)...))
	template<class... Args> auto dft_backward_aux_(priority<1>, Args&&... args) DECLRETURN(cufft ::dft_backward(std::forward<Args>(args)...))
	template<class... Args> auto dft_backward(Args&&... args) DECLRETURN(dft_backward_aux_(priority<1>{}, std::forward<Args>(args)...))
	template<class In, class... Args> auto dft_backward(std::array<bool, std::decay_t<In>::dimensionality> which, In&& in, Args&&... args) DECLRETURN(dft_backward_aux_(priority<1>{}, which, std::forward<In>(in), std::forward<Args>(args)...))

}}}

#if not __INCLUDE_LEVEL__

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

#include <boost/timer/timer.hpp>
#include <boost/config.hpp>

namespace utf = boost::unit_test;

using complex = std::complex<double>;
namespace multi = boost::multi;

using std::cout;

BOOST_AUTO_TEST_CASE(fft_combinations, *utf::tolerance(0.00001)){
	cout<< "# threads is " << multi::fftw::plan::with_nthreads() <<"\n";
	cout<<"=========================================================\n";
	cout<< BOOST_PLATFORM <<' '<< BOOST_COMPILER <<' '<< __DATE__<<'\n';

	auto const in = []{
		multi::array<complex, 4> ret({32, 90, 98, 96});
		std::generate(ret.data_elements(), ret.data_elements() + ret.num_elements(),
			[](){return complex{std::rand()/1./RAND_MAX, std::rand()/1./RAND_MAX};}
		);
		return ret;
	}();
	std::cout<<"memory size "<< in.num_elements()*sizeof(complex)/1e6 <<" MB\n";

	multi::cuda::array<complex, 4> const in_gpu = in;
	multi::cuda::managed::array<complex, 4> const in_mng = in;

	std::vector<std::array<bool, 4>> cases = {
		{false, true , true , true },
		{false, true , true , false},
		{true , false, false, false},
		{true , true , false, false},
		{false, false, true , false},
		{false, false, false, false},
	};

	for(auto c : cases){
		cout<<"case: "<<std::boolalpha;
		copy(begin(c), end(c), std::ostream_iterator<bool>{cout,", "}); cout<<"\n";

		multi::array<complex, 4> out(extensions(in));
		{
			cout<<"flops "<< multi::fftw::plan(c, in, out, multi::fft::forward).flops() <<"\n";
			boost::timer::auto_cpu_timer t{"cpu____ %ws wall, CPU (%p%)\n"};
			multi::fft::dft(c, in, out, multi::fft::forward);
		}
		{
			boost::timer::auto_cpu_timer t{"cpu_hot %ws wall, CPU (%p%)\n"};
			multi::fft::dft(c, in, out, multi::fft::forward);
		}
		multi::cuda::array<complex, 4> out_gpu(extensions(in_gpu));
		{
			boost::timer::auto_cpu_timer t{"gpu_cld %ws wall, CPU (%p%)\n"};
			multi::fft::dft(c, in_gpu   , out_gpu   , multi::fft::forward);
			BOOST_TEST( abs( static_cast<complex>(out_gpu[5][4][3][1]) - out[5][4][3][1] ) == 0. );
		}
		{
			boost::timer::auto_cpu_timer t{"gpu_hot %ws wall, CPU (%p%)\n"};
			multi::fft::dft(c, in_gpu   , out_gpu   , multi::fft::forward);
//			BOOST_TEST( abs( static_cast<complex>(out_gpu[5][4][3][1]) - out[5][4][3][1] ) == 0. );
		}
		multi::cuda::managed::array<complex, 4> out_mng(extensions(in_mng));
		{
			boost::timer::auto_cpu_timer t{"mng_cld %ws wall, CPU (%p%)\n"};
			multi::fft::dft(c, in_mng   , out_mng   , multi::fft::forward);
			cudaDeviceSynchronize();
			BOOST_TEST( abs( out_mng[5][4][3][1] - out[5][4][3][1] ) == 0. );
		}
		{
		///	boost::timer::auto_cpu_timer t{"mng_hot %ws wall, CPU (%p%)\n"};
			multi::fft::dft(c, in_mng()   , out_mng()   , multi::fft::forward);
			cudaDeviceSynchronize();
			BOOST_TEST( abs( out_mng[5][4][3][1] - out[5][4][3][1] ) == 0. );
		}
	}

}
#endif
#endif


#include <fstream>
#include <iostream>
#include <string>
#include <unordered_map>
#include <boost/algorithm/string.hpp>
#include <xtensor/xarray.hpp>
#include <xtensor/xview.hpp>

using namespace std;

class Distributions {
	vector<string> keys;
	unordered_map<string, size_t> lookup;
	xt::xarray<double> values;

	public:
		Distributions(istream& is) :keys{}, lookup{} {
			string line;
			int i = 0;
			vector<xt::xarray<double>> tempVector;
			while (getline(is, line)) {
				if (i == 0 || line.empty()) {
					++i;
					continue;
				}
				vector<string> elements;
				boost::split(elements, line, boost::is_any_of(","));
				if (elements.size() < 2) {
					throw runtime_error("Invalid distribution data");
				}
				string key = elements[0];
				keys.push_back(key);
				lookup[key] = i-1;

				xt::xarray<double> distribution = xt::zeros<double>({elements.size() - 1});
				for (int k = 0; k < elements.size() - 1; ++k) {
					distribution[k] = stod(elements[k+1]);
				}
				tempVector.push_back(distribution);
				++i;
			}
			if (tempVector.empty()) {
				return;
			}

			size_t innerDim = tempVector[0].size();
			values = xt::zeros<double>({tempVector.size(), innerDim});
			for (int j = 0; j < tempVector.size(); ++j) {
				for (int l = 0; l < innerDim; ++l) {
					values(j, l) = tempVector[j][l];
				}
			}
		}

		xt::xarray<double> operator[](const string& id) {
			return xt::view(values, lookup[id]);
		}

		vector<string>& Keys() {
			return keys;
		}

		xt::xarray<double>& Values() {
			return values;
		}

		size_t size() {
			return lookup.size();
		}
};