#include <ros/ros.h>
#include <geometry_msgs/Pose.h>
#include <math.h>
#include <time.h>
#include <ros/ros.h>
#include <geometry_msgs/Pose.h>
#include <math.h>
#include <time.h>
#include <fstream>
#include <iostream>
#include <vector>
#include <Eigen/Eigen>
#include <linux/input-event-codes.h>

#include "origarm_ros/Command_Position.h"
#include "origarm_ros/Command_ABL.h"
#include "origarm_ros/keynumber.h"
#include "origarm_ros/transdiffConfigSeg.h"
#include "origarm_ros/transdiffConfig.h"
#include "origarm_ros/transdiffDataSeg.h"
#include "ros/package.h"
#include "myGlobalData.h"

#define ARRAY_SIZE(a) (sizeof(a) / sizeof((a)[0]))

using namespace std;

origarm_ros::transdiffConfigSeg transdiffConfigs;
origarm_ros::Command_ABL Command_ABL_demo;

ifstream inFile;
string GivenDataFilePath = ros::package::getPath("origarm_ros") + "/predefined_param/";
string GivenDataFileName = "traj_updated_1seg_3_2.txt";

const int ms = 1000;  //1ms
// float ts[] = {0.5*ms, 15*ms, 30*ms, 30*ms, 15*ms};
// float ts[] = {0.5*ms, 10*ms, 15*ms, 15*ms, 10*ms};
// float ts[] = {0.5*ms, 15*ms, 15*ms};

float ts[] = {0.5*ms, 10*ms,  5*ms, 10*ms,  5*ms, 10*ms,  5*ms, 10*ms};    //_3.txt
// float ts[] = {0.5*ms, 10*ms,  5*ms, 15*ms,  5*ms, 15*ms,  5*ms, 10*ms};    //_4.txt
// float ts[] = {0.5*ms, 15*ms,  5*ms, 50*ms,  5*ms, 50*ms,  5*ms, 15*ms}; //_beta.txt
// float ts[] = {0.5*ms, 30*ms,  10*ms, 30*ms,  10*ms, 30*ms,  10*ms, 30*ms};    //_length.txt

int mode_in;
float x_in, y_in, z_in;
float a_in[6], b_in[6], l_in[6];
int flag_pos = 0;          // flag_pos == 1, input xyz, otherwise, input abl, not applicable for 1seg. test for now

int flag_start = 0;
int period = 200;
int repeat = 1;            // whole repeat times
int repeat0 = 1;
int inner_repeat = 5;	   // for inner loop repeat times, finally repeat (inner_repeat+1) times
int inner_repeat0 = 5;
int inner_loopno[2] = {2, 6};    // inner loop repeat beteen inner_loopno[0] and inner_loopno[1] {2,4}

int startnum_low  = period*inner_loopno[0] + period/2;
int startnum_high = period*(inner_loopno[0]+2) + period/2;
int startnum = startnum_low;

int num = 0;     // smallest step number
int loop_no = 0; // to which step exactly

vector<vector<float>> s_A;
vector<vector<float>> s_B;
vector<vector<float>> s_L;

vector<vector<float>> state_A;
vector<vector<float>> state_B;
vector<vector<float>> state_L;

vector<float> alpha {0, 0, 0, 0, 0, 0};
vector<float> beta  {0, 0, 0, 0, 0, 0};
vector<float> length{g_length0, g_length0, g_length0, g_length0, g_length0, g_length0};

void keyCallback(const origarm_ros::keynumber &key)
{
	if (key.keycodePressed == KEY_J)
	{
		flag_start = 1;
	}
	else if (key.keycodePressed == KEY_B)
	{
		flag_start = 0;
	}
}

float constrain2PI(float s) // constrian angle to [-pi,pi)
{
	if (s >= M_PI)
	{
		s = s - 2 * M_PI;
	}
	else if (s < -M_PI)
	{
		s = s + 2 * M_PI;
	}

	return s;
}

//generate single point by defining starting and ending point
static float lineardiffgenetraj(float ps, float pe, int step, int tstep)
{
	float pm = ps + step*(pe-ps)/(tstep-1);
	return pm;
}

//IK of 2 symmetrical segments (xyz->abl), input x, y, z, output (a, b, l, -a, b, l)
static vector<float> TwoSegIK(float px, float py, float pz)
{
	vector<float> ABL_solution; // [a1,b1,l1,a2,b2,l2]
	float alpha1, beta1, length1, alpha2, beta2, length2;

	if (px == 0 && py == 0)
	{
		alpha1 = 0;
		alpha2 = 0;
		beta1 = 0;
		beta2 = 0;
		length1 = pz/3;
		length2 = pz/3;
	}
	else
	{
		if (py > 0 && px != 0)
		{
			alpha1 = 2*atan2(sqrt(px*px+py*py), abs(pz));
			beta1  = atan2(py, px);
		}
		else if (py > 0 && px == 0)
		{
			alpha1 = 2*atan2(sqrt(px*px+py*py), abs(pz));
			beta1  = M_PI/2;
		}
		else if (py < 0 && px != 0)
		{
			alpha1 = -2*atan2(sqrt(px*px+py*py), abs(pz));
			beta1  = atan2(py, px) + M_PI;
		}
		else if (py < 0 && px == 0)
		{
			alpha1 = -2*atan2(sqrt(px*px+py*py), abs(pz));
			beta1  = M_PI/2;
		}
		else if (py == 0 && px > 0)
		{
			alpha1 = 2*atan2(sqrt(px*px+py*py), abs(pz));
			beta1  = 0;
		}
		else if (py == 0 && px < 0)
		{
			alpha1 = -2*atan2(sqrt(px*px+py*py), abs(pz));
			beta1  = 0;
		}

		length1 = pz*alpha1/(2*sin(alpha1));
		alpha2  = -alpha1;
		beta2   = beta1;
		length2 = length1;
	}

	// printf("solution: %f, %f, %f\n", alpha1, beta1, length1);

	ABL_solution.push_back(alpha1);
	ABL_solution.push_back(beta1);
	ABL_solution.push_back(length1);
	ABL_solution.push_back(alpha2);
	ABL_solution.push_back(beta2);
	ABL_solution.push_back(length2);

	return ABL_solution;
}

//read state information from File, ABL state will be either read from file (flag_xyz == 1) or generate by xyz & TwoSegIK
static void readFromFile(int flag_xyz)
{
	vector<float> s_a;
	vector<float> s_b;
	vector<float> s_l;
	vector<float> s_a1;
	vector<float> s_b1;
	vector<float> s_l1;

	vector<float> abl_solution;

	inFile.open(GivenDataFilePath + GivenDataFileName, ios::in);

	if (!inFile)
	{
		printf("%s\n", "unable to open the file.");
		exit(1);
	}
	else
	{
		while (!inFile.eof())
		{
			inFile>>mode_in>>x_in>>y_in>>z_in>>a_in[0]>>b_in[0]>>l_in[0]>>a_in[1]>>b_in[1]>>l_in[1]>>a_in[2]>>b_in[2]>>l_in[2]>>a_in[3]>>b_in[3]>>l_in[3]>>a_in[4]>>b_in[4]>>l_in[4]>>a_in[5]>>b_in[5]>>l_in[5];

			// if ( inFile.eof() )
			// {
			// 	break;
			// }

			if (flag_xyz)
			{
				abl_solution = TwoSegIK(x_in, y_in, z_in);

				// printf("abl_solution:%f,  %f, %f\n", abl_solution[0], abl_solution[1], abl_solution[2]);

				for (int i = 0; i < 6; i++)
				{
					s_b.push_back(abl_solution[1]);
					s_l.push_back(abl_solution[2]/3);

					if (i < 3)
					{
						s_a.push_back(abl_solution[0]/3);
					}
					else
					{
						s_a.push_back(abl_solution[3]/3);
					}
				}

				if (s_a.size() < 6)
				{
					printf("%s\n", ".............");
					exit(1);
				}
				else
				{
					s_a1.assign(s_a.end()-6, s_a.end());
					s_b1.assign(s_b.end()-6, s_b.end());
					s_l1.assign(s_l.end()-6, s_l.end());

					s_A.push_back(s_a1);
					s_B.push_back(s_b1);
					s_L.push_back(s_l1);
				}
			}
			else
			{
				for (int i = 0; i < 6; i++)
				{
					s_a.push_back(a_in[i]);
					s_b.push_back(b_in[i]);
					s_l.push_back(l_in[i]);
				}

				if (s_a.size() < 6)
				{
					printf("%s\n", ".............");
					exit(1);
				}
				else
				{
					s_a1.assign(s_a.end()-6, s_a.end());
					s_b1.assign(s_b.end()-6, s_b.end());
					s_l1.assign(s_l.end()-6, s_l.end());

					s_A.push_back(s_a1);
					s_B.push_back(s_b1);
					s_L.push_back(s_l1);
				}
			}
		}
	}

	inFile.close();
}

//generate every data point by linearly differentiating each key point
static vector<vector<float>> TrajGeneration(vector<vector<float>> & vect1, int tstep)
{
	int vectsize = vect1.size(); // return row size of 2d vector, size of timestamp read from file

	vector< vector<float> > state;
	vector<float> state_1;
	vector<float> state1;

	int t = 0;

	for (int i = 0; i < 6; i++)
	{
		for (int j = 0; j < vectsize-1; j++)
		{
			float ps = vect1[j][i];
			float pe = vect1[j+1][i];

			// printf("ps: %f, pe: %f\n", ps, pe);

			for (int k = 0; k < tstep; k++)// include both starting & ending point
			{
				float pm = lineardiffgenetraj(ps, pe, k, tstep);
				state1.push_back(pm);
				// printf("state1[%d]: %f\n", t, state1[t]);
				t ++;
			}
		}

		int newvectorsize = tstep*(vectsize-1);

		if (state1.size() < newvectorsize)
		{
			printf("%s\n", "!.............");
			exit(1);
		}
		else
		{
			state_1.assign(state1.end() - newvectorsize, state1.end());
			state.push_back(state_1);
		}
	}

	return state;
}

static void writeABLCommand()
{
	if (flag_start && repeat > 0)
	{
		if (num < state_A[0].size())
		{
			for (int i = 0; i < 6; i++)
			{
				alpha[i]  = state_A[i][num];
				// beta[i]   = s_B[loop_no][i];
				beta[i]   = state_B[i][num]; // change beta only
				length[i] = state_L[i][num];
			}

			usleep(ts[loop_no]);
			num ++;
			if (num % period == 0)
			{
				loop_no ++;
				if ( loop_no == inner_loopno[1] && inner_repeat > 0)
				{
					num = num - period*(inner_loopno[1]-inner_loopno[0]);
					loop_no = inner_loopno[0];
					inner_repeat = inner_repeat - 1;
				}
			}
		}
		else
		{
			num = 0;
			loop_no = 0;
			repeat = repeat - 1;
			inner_repeat = inner_repeat0;
		}
	}
	else
	{
		num = 0;
		loop_no = 0;

		for (int i = 0; i < 6; i++)
		{
			alpha[i]  = 0;
			beta[i]   = 0;
			length[i] = g_length0;
		}
	}
}

void writetransConfig(int trans_mode, int segn)
{
	if (trans_mode == 0)
	{
		transdiffConfigs.transMode = 0;
		transdiffConfigs.transSegment = segn;
		transdiffConfigs.transActuatorNum = 6;
		transdiffConfigs.transPassiveNum = 0;
		transdiffConfigs.transActuatorIndex = {0,1,2,3,4,5};
	}
	else if (trans_mode == 1)
	{
		transdiffConfigs.transMode = 1;
		transdiffConfigs.transSegment = segn;
		transdiffConfigs.transActuatorNum = 6;
		transdiffConfigs.transPassiveNum = 0;
		transdiffConfigs.transActuatorIndex = {0,1,2,3,4,5};
	}
	else if (trans_mode == 2)
	{
		transdiffConfigs.transMode = 2;
		transdiffConfigs.transSegment = segn;
		transdiffConfigs.transActuatorNum = 3;
		transdiffConfigs.transPassiveNum = 3;
		transdiffConfigs.transActuatorIndex = {1,3,5,0,2,4};
	}
}

int triggerTrans()
{
	int ret = 0;
	if (flag_start)
	{
		if (repeat == repeat0)
		{
			if (inner_repeat == 3 && num == startnum) // num == 400
			{
				writetransConfig(1,2);
				ret = 1;
			}
			else if (inner_repeat == 1 && num == startnum)
			{
				writetransConfig(2,2);
				ret = 1;
			}
			// else if (inner_repeat == 0 && num == startnum)
			// {
			// 	writetransConfig(0,2);
			// 	ret = 1;
			// }

		}
		else if (repeat == 0)
		{
			writetransConfig(0,2);
			ret = 1;
		}
		// else if (repeat == repeat0-1 && repeat != 0)
		// {
		// 	if (inner_repeat == 3 && num == 500)
		// 	{
		// 		writetransConfig(1,2);
		// 		ret = 1;
		// 	}
		// 	else if (inner_repeat == 2 && num == 500)
		// 	{
		// 		writetransConfig(2,2);
		// 		ret = 1;
		// 	}
		// 	else if (inner_repeat == 1 && num == 500)
		// 	{
		// 		writetransConfig(0,2);
		// 		ret = 1;
		// 	}
		// }
	}

	return ret;
}

void wrapAlphaAndBeta()
{
	for (int i = 0; i < SEGNUM; i++)
	{
		if (Command_ABL_demo.segment[i].A < 0)
		{
			Command_ABL_demo.segment[i].A = -Command_ABL_demo.segment[i].A;
			Command_ABL_demo.segment[i].B = constrain2PI(Command_ABL_demo.segment[i].B + M_PI);
		}
	}
}

int main(int argc, char **argv)
{
	ros::init(argc, argv, "demo_traj_updated");
	ros::NodeHandle nh;
	ros::Rate r(100);     //Hz

	ros::Publisher  pub1 = nh.advertise<origarm_ros::Command_ABL>("Cmd_ABL_joy", 100);
	ros::Publisher  pub2 = nh.advertise<origarm_ros::transdiffConfigSeg>("transdiffConfigSeg", 100);
	ros::Subscriber key_sub_ = nh.subscribe("key_number", 1, keyCallback);

	readFromFile(flag_pos);

	// for (int i = 0; i < s_B.size(); i++)
	// {
	// 	for (int j = 0; j < s_B[i].size(); j++)
	// 	{
	// 		printf("s_A[%d][%d]: %f, s_B[%d][%d]: %f, s_L[%d][%d]: %f\r\n",i,j,s_A[i][j], i,j,s_B[i][j], i,j,s_L[i][j]);
	// 	}
	// }

	state_A = TrajGeneration(s_A, period);
	state_B = TrajGeneration(s_B, period);
	state_L = TrajGeneration(s_L, period);

	while (ros::ok())
	{
		writeABLCommand();
		printf("flag_start: %d, num: %d, loop_no: %d, repeat: %d, inner_repeat: %d, transMode: %d, transSeg: %d\r\n", flag_start, num, loop_no, repeat, inner_repeat, transdiffConfigs.transMode, transdiffConfigs.transSegment);
		for (int i = 0; i < 6; i++)
		{
			printf("alpha[%d]: %.4f, beta[%d]: %.4f, length[%d]: %.4f\r\n", i, alpha[i], i, beta[i], i, length[i]);
		}

		for (int i = 0; i < 6; i++)
		{
			Command_ABL_demo.segment[i].A = alpha[i];
			Command_ABL_demo.segment[i].B = beta[i];
			Command_ABL_demo.segment[i].L = length[i];
		}
		for (int i = 6; i < SEGNUM; i++)
		{
			Command_ABL_demo.segment[i].A = 0;
			Command_ABL_demo.segment[i].B = 0;
			Command_ABL_demo.segment[i].L = g_length0;
		}

		wrapAlphaAndBeta();
		pub1.publish(Command_ABL_demo);

		if (triggerTrans())
		{
			pub2.publish(transdiffConfigs);
		}

		ros::spinOnce(); //necessary for subscribe
		r.sleep();
	}

	inFile.close();
	return 0;
}


#include "hoCuNDArray_utils.h"
#include "radial_utilities.h"
#include "hoNDArray_fileio.h"
#include "cuNDArray.h"
#include "imageOperator.h"
#include "identityOperator.h"
#include "hoPartialDerivativeOperator.h"
#include "hoCuConebeamProjectionOperator.h"
#include "cuConvolutionOperator.h"
#include "hoCuNDArray_blas.h"
#include "hoCuNDArray_elemwise.h"
#include "hoCuNDArray_blas.h"
#include "cgSolver.h"
#include "CBCT_acquisition.h"
#include "complext.h"
#include "encodingOperatorContainer.h"
#include "vector_td_io.h"
#include "hoPartialDerivativeOperator.h"
#include "hoCuGPBBSolver.h"
#include "hoCuTvOperator.h"
#include "hoCuTvPicsOperator.h"
#include "hoCuNCGSolver.h"
#include "hoCuCgDescentSolver.h"
#include "hoCuNDArray_utils.h"
#include "hoCuPartialDerivativeOperator.h"
#include "CBSubsetOperator.h"
#include "osMOMSolverD.h"
#include "osMOMSolverD3.h"
#include "cuSolverUtils.h"
#include <iostream>
#include <algorithm>
#include <sstream>
#include <math_constants.h>
#include <boost/program_options.hpp>
#include <boost/make_shared.hpp>
#include <solvers/osMOMSolverD3.h>
#include "hoLinearResampleOperator_eigen.h"
#include "multiplicationOperatorContainer.h"
#include "cuLinearResampleOperator.h"
#include "multiresRegistrationSolver.h"
#include "cuCGHSOFSolver.h"

#include "hdf5_utils.h"
#include "hoCuOFPartialDerivativeOperator.h"
#include "hoCuTVOFPartialDerivativeOperator.h"

using namespace std;
using namespace Gadgetron;

namespace po = boost::program_options;

boost::shared_ptr< hoCuNDArray<float> >
perform_registration( boost::shared_ptr< hoCuNDArray<float> > volume, float of_alpha, float of_beta, unsigned int num_multires_levels )
{
	std::vector<size_t> volume_dims_3d = *volume->get_dimensions();
	volume_dims_3d.pop_back();
	std::vector<size_t> volume_dims_3d_3 = volume_dims_3d;
	volume_dims_3d_3.push_back(3);

	size_t num_elements_3d = volume_dims_3d[0]* volume_dims_3d[1]* volume_dims_3d[2];
	size_t num_phases = volume->get_size(3);

	boost::shared_ptr< hoCuNDArray<float> > host_result_field( new hoCuNDArray<float> );
	{
		std::vector<size_t> volume_dims_4d = volume_dims_3d;
		volume_dims_4d.push_back(3);
		volume_dims_4d.push_back(volume->get_size(3));
		host_result_field->create( &volume_dims_4d );
	}

	// Upload host data to device
	//

	for( unsigned int i=0; i<num_phases; i++ ){

		hoCuNDArray<float> host_fixed( &volume_dims_3d, volume->get_data_ptr()+((i+1)%num_phases)*num_elements_3d );
		hoCuNDArray<float> host_moving( &volume_dims_3d, volume->get_data_ptr()+i*num_elements_3d );

		cuNDArray<float> fixed_image(&host_fixed);
		cuNDArray<float> moving_image(&host_moving);

		boost::shared_ptr< cuLinearResampleOperator<float,3> > R( new cuLinearResampleOperator<float,3>() );

		// Setup solver
		//
		cuCGHSOFSolver<float,3> OFs;
		//cuCKOpticalFlowSolver<float,3> OFs;
		OFs.set_interpolator( R );
		OFs.set_output_mode( cuCGHSOFSolver<float,3>::OUTPUT_VERBOSE );
		OFs.set_max_num_iterations_per_level( 500 );
		OFs.set_num_multires_levels( num_multires_levels );
		OFs.set_alpha(of_alpha);
		//OFs.set_beta(of_beta);
		//OFs.set_limit(0.01f);

		// Run registration
		//
		boost::shared_ptr< cuNDArray<float> > result = OFs.solve( &fixed_image, &moving_image );
        std::cout << "Dimensions of vector field ";
        auto dims = *result->get_dimensions();
        for (size_t d : dims) std::cout << d << " ";
        std::cout << std::endl;

		cuNDArray<float> dev_sub;
		dev_sub.create( &volume_dims_3d_3, result->get_data_ptr() );

		hoCuNDArray<float> host_sub( &volume_dims_3d_3, host_result_field->get_data_ptr()+i*num_elements_3d*3 );
		host_sub = dev_sub;

	}

	/*
 {
  std::cout << std::endl << "Writing out registration results for phase " << phase << "." << std::endl;
  char filename[256];
  sprintf(&(filename[0]), "def_moving_%i.real", phase);
  write_nd_array<float>(&host_result_image, (char*)filename);
 }
	 */

	return host_result_field;
}
class hoCuConvertOperator : public subsetOperator<hoCuNDArray<float>>{

public:
	hoCuConvertOperator(boost::shared_ptr<subsetOperator<hoCuNDArray<float>>> _op) : subsetOperator<hoCuNDArray<float>>(_op->get_number_of_subsets()),op(_op){};

	virtual void mult_M(hoCuNDArray<float> *in, hoCuNDArray<float> *out,int subset, bool accumulate) override { op->mult_M((hoCuNDArray<float>*)in,(hoCuNDArray<float>*) out, subset,accumulate);}
	virtual void mult_MH(hoCuNDArray<float> *in, hoCuNDArray<float> *out,int subset, bool accumulate) override { op->mult_MH((hoCuNDArray<float>*)in,(hoCuNDArray<float>*) out, subset, accumulate);}
	virtual void mult_MH_M(hoCuNDArray<float> *in, hoCuNDArray<float> *out,int subset, bool accumulate) override { op->mult_MH_M((hoCuNDArray<float>*)in,(hoCuNDArray<float>*) out, subset, accumulate);}

	virtual boost::shared_ptr< std::vector<size_t> > get_codomain_dimensions(int i) { return op->get_codomain_dimensions(i);}
	virtual boost::shared_ptr< std::vector<size_t> > get_domain_dimensions() { return op->get_domain_dimensions();}
	virtual void set_domain_dimensions( std::vector<size_t> * dims) { return op->set_domain_dimensions(dims);}
	virtual void set_codomain_dimensions( std::vector<size_t> * dims) { return op->set_codomain_dimensions(dims);}

protected:
	boost::shared_ptr<subsetOperator<hoCuNDArray<float>>> op;
};

boost::shared_ptr<hoCuNDArray<float> > calculate_prior(boost::shared_ptr<CBCT_binning>  binning,boost::shared_ptr<CBCT_acquisition> ps, hoCuNDArray<float>& projections, std::vector<size_t> is_dims, floatd3 imageDimensions){
	std::cout << "Calculating FDK prior" << std::endl;
	boost::shared_ptr<CBCT_binning> binning_pics=binning->get_3d_binning();
	std::vector<size_t> is_dims3d = is_dims;
	is_dims3d.pop_back();
	boost::shared_ptr< hoCuConebeamProjectionOperator >
	Ep( new hoCuConebeamProjectionOperator() );
	Ep->setup(ps,binning_pics,imageDimensions);
	Ep->set_codomain_dimensions(ps->get_projections()->get_dimensions().get());
	Ep->set_domain_dimensions(&is_dims3d);
	Ep->set_use_filtered_backprojection(true);
	boost::shared_ptr<hoCuNDArray<float> > prior3d(new hoCuNDArray<float>(&is_dims3d));
	Ep->mult_MH(&projections,prior3d.get());

	hoCuNDArray<float> tmp_proj(*ps->get_projections());
	Ep->mult_M(prior3d.get(),&tmp_proj);
	float s = dot(ps->get_projections().get(),&tmp_proj)/dot(&tmp_proj,&tmp_proj);
	*prior3d *= s;
	boost::shared_ptr<hoCuNDArray<float> > prior(new hoCuNDArray<float>(*expand( prior3d.get(), is_dims.back() )));
	std::cout << "Prior complete" << std::endl;
	return prior;
}

int main(int argc, char** argv)
{
	string acquisition_filename;
	string outputFile;
	uintd3 imageSize;
	floatd3 voxelSize;
	int device;
	unsigned int iterations;
	floatd2 scale_factor;
	unsigned int subsets;
	int reg_iter;
	float rho;
	float tv_weight;
    float tv_weight4d;
    float tau;
	po::options_description desc("Allowed options");
	string tv_prior_filename;
	desc.add_options()
    		("help", "produce help message")
    		("acquisition,a", po::value<string>(&acquisition_filename)->default_value("acquisition.hdf5"), "Acquisition data")
    		("samples,n",po::value<unsigned int>(),"Number of samples per ray")
    		("output,f", po::value<string>(&outputFile)->default_value("reconstruction.hdf5"), "Output filename")
    		("size,s",po::value<uintd3>(&imageSize)->default_value(uintd3(512,512,1)),"Image size in pixels")
    		("binning,b",po::value<string>(),"Binning file for 4d reconstruction")
    		("SAG","Use exact SAG correction if present")
    		("voxelSize,v",po::value<floatd3>(&voxelSize)->default_value(floatd3(0.488f,0.488f,1.0f)),"Voxel size in mm")
    		("dimensions,d",po::value<floatd3>(),"Image dimensions in mm. Overwrites voxelSize.")
    		("iterations,i",po::value<unsigned int>(&iterations)->default_value(10),"Number of iterations")
    		("device",po::value<int>(&device)->default_value(0),"Number of the device to use (0 indexed)")
					("downsample,D",po::value<floatd2>(&scale_factor)->default_value(floatd2(1,1)),"Downsample projections this factor")
    		("subsets,u",po::value<unsigned int>(&subsets)->default_value(10),"Number of subsets to use")
    		("TV",po::value<float>(&tv_weight)->default_value(0),"Total variation weight")
            ("TV4D",po::value<float>(&tv_weight4d)->default_value(0),"Total variation in temporal direction")
                    ("tau",po::value<float>(&tau)->default_value(1e-5),"Solver tau")
					("reg_iter",po::value<int>(&reg_iter)->default_value(2),"Regularization iterations")
    		("use_prior","Use an FDK prior")
    		("TV-prior",po::value<string>(&tv_prior_filename)->default_value("reconstructionTV.real"),"TV prior for registration")
                    ("vector-field",po::value<string>(),"Stored vector field")
    		("3D","Only use binning data to determine wrong projections")
    		;

	po::variables_map vm;
	po::store(po::parse_command_line(argc, argv, desc), vm);
	po::notify(vm);

	if (vm.count("help")) {
		cout << desc << "\n";
		return 1;
	}

	std::cout << "Command line options:" << std::endl;
	for (po::variables_map::iterator it = vm.begin(); it != vm.end(); ++it){
		boost::any a = it->second.value();
		std::cout << it->first << ": ";
		if (a.type() == typeid(std::string)) std::cout << it->second.as<std::string>();
		else if (a.type() == typeid(int)) std::cout << it->second.as<int>();
		else if (a.type() == typeid(unsigned int)) std::cout << it->second.as<unsigned int>();
		else if (a.type() == typeid(float)) std::cout << it->second.as<float>();
		else if (a.type() == typeid(vector_td<float,3>)) std::cout << it->second.as<vector_td<float,3> >();
		else if (a.type() == typeid(vector_td<int,3>)) std::cout << it->second.as<vector_td<int,3> >();
		else if (a.type() == typeid(vector_td<unsigned int,3>)) std::cout << it->second.as<vector_td<unsigned int,3> >();
		else std::cout << "Unknown type" << std::endl;
		std::cout << std::endl;
	}

	cudaSetDevice(device);
	cudaDeviceReset();

	//Really weird stuff. Needed to initialize the device?? Should find real bug.
	cudaDeviceManager::Instance()->lockHandle();
	cudaDeviceManager::Instance()->unlockHandle();

	boost::shared_ptr<CBCT_acquisition> ps(new CBCT_acquisition());
	ps->load(acquisition_filename);
	ps->get_geometry()->print(std::cout);
    if (scale_factor[0] != 1 || scale_factor[1] != 1)
        ps->downsample(scale_factor[0],scale_factor[1]);

	float SDD = ps->get_geometry()->get_SDD();
	float SAD = ps->get_geometry()->get_SAD();

	boost::shared_ptr<CBCT_binning> binning(new CBCT_binning());
	if (vm.count("binning")){
		std::cout << "Loading binning data" << std::endl;
		binning->load(vm["binning"].as<string>());
		if (vm.count("3D"))
			binning = binning->get_3d_binning();
	} else binning->set_as_default_3d_bin(ps->get_projections()->get_size(2));
	binning->print(std::cout);

	floatd3 imageDimensions;
	if (vm.count("dimensions")){
		imageDimensions = vm["dimensions"].as<floatd3>();
		voxelSize = imageDimensions/imageSize;
	}
	else imageDimensions = voxelSize*imageSize;

	float lengthOfRay_in_mm = norm(imageDimensions);
	unsigned int numSamplesPerPixel = 3;
	float minSpacing = min(voxelSize)/numSamplesPerPixel;

	unsigned int numSamplesPerRay;
	if (vm.count("samples")) numSamplesPerRay = vm["samples"].as<unsigned int>();
	else numSamplesPerRay = ceil( lengthOfRay_in_mm / minSpacing );

	float step_size_in_mm = lengthOfRay_in_mm / numSamplesPerRay;
	size_t numProjs = ps->get_projections()->get_size(2);
	size_t needed_bytes = 2 * prod(imageSize) * sizeof(float);
	std::vector<size_t> is_dims = to_std_vector((uint64d3)imageSize);

	std::cout << "IS dimensions " << is_dims[0] << " " << is_dims[1] << " " << is_dims[2] << std::endl;
	std::cout << "Image size " << imageDimensions << std::endl;

	is_dims.push_back(binning->get_number_of_bins());

	// Define encoding matrix
	auto E = boost::make_shared<CBSubsetOperator<hoCuNDArray> >(subsets);

	//E->setup(ps,binning,imageDimensions);
	E->setup(ps,binning,imageDimensions);
	E->set_domain_dimensions(&is_dims);
	E->set_codomain_dimensions(ps->get_projections()->get_dimensions().get());

	auto E2 = boost::make_shared<hoCuConvertOperator>(E);

	//hoCuGPBBSolver<float> solver;
	//hoCuCgDescentSolver<float> solver;
//	osSPSSolver<hoCuNDArray<float>> solver;
	osMOMSolverD<hoCuNDArray<float>> solver;
	//osSPSSolver<hoCuNDArray<float>> solver;
	//hoCuNCGSolver<float> solver;
	solver.set_encoding_operator(E);
	//solver.set_domain_dimensions(&is_dims);
	solver.set_max_iterations(iterations);
	solver.set_output_mode(hoCuGPBBSolver<float>::OUTPUT_VERBOSE);
	solver.set_non_negativity_constraint(true);
    solver.set_tau(tau);
	solver.set_reg_steps(reg_iter);
	//solver.set_rho(rho);

	hoCuNDArray<float> projections = *ps->get_projections();
	E->offset_correct(&projections);

	boost::shared_ptr<hoCuNDArray<float> > prior;

	if (vm.count("use_prior")) {
		prior = calculate_prior(binning,ps,projections,is_dims,imageDimensions);
		solver.set_x0(prior);
	}
	/*
	if (tv_weight > 0){
		auto total_variation = boost::make_shared<hoCuTvOperator<float,4>>();
		total_variation->set_weight(tv_weight);
		//total_variation->set_weight_array(weight_array);
		solver.add_nonlinear_operator(total_variation);
		solver.set_kappa(tv_weight);
}
*/

    if (tv_weight4d > 0){
        boost::shared_ptr<hoNDArray<float>> displacements;
        if (vm.count("vector-field")){
            displacements = read_nd_array<float>(vm["vector-field"].as<string>().c_str());
            auto vdims = *displacements->get_dimensions();
            for (auto v : vdims) std::cout << v << " ";
            std::cout<<std::endl;
        } else {
            auto tv_recon = boost::make_shared<hoCuNDArray<float>>();
            *tv_recon = *read_nd_array<float>(tv_prior_filename.c_str());
            displacements = perform_registration(tv_recon, 0.01, 1, 3);
        }
        //clear(displacements.get());
        //auto DtOF = boost::make_shared<hoCuOFPartialDerivativeOperator<float>>();

		auto DtOF = boost::make_shared<hoCuTVOFPartialDerivativeOperator<float>>();
		//auto DtOF = boost::make_shared<hoCuPartialDerivativeOperator<float,4>>(3);
        DtOF->set_weight(tv_weight4d);
        DtOF->set_displacement_field(displacements);
        DtOF->set_domain_dimensions(&is_dims);
		auto is_dimsTV = is_dims;
		is_dimsTV.push_back(3);
        DtOF->set_codomain_dimensions(&is_dimsTV);
        solver.add_regularization_operator(DtOF);
    }

  if (tv_weight > 0){

  	auto Dx = boost::make_shared<hoCuPartialDerivativeOperator<float,4>>(0);
  	Dx->set_weight(tv_weight);
  	Dx->set_domain_dimensions(&is_dims);
  	Dx->set_codomain_dimensions(&is_dims);

  	auto Dy = boost::make_shared<hoCuPartialDerivativeOperator<float,4>>(1);
  	Dy->set_weight(tv_weight);
  	Dy->set_domain_dimensions(&is_dims);
  	Dy->set_codomain_dimensions(&is_dims);

  	auto Dz = boost::make_shared<hoCuPartialDerivativeOperator<float,4>>(2);
  	Dz->set_weight(tv_weight);
  	Dz->set_domain_dimensions(&is_dims);
  	Dz->set_codomain_dimensions(&is_dims);
	solver.add_regularization_group({Dx,Dy,Dz});
  }

	auto result = solver.solve(&projections);

	//write_nd_array<float>( result.get(), outputFile.c_str());
	saveNDArray2HDF5(result.get(),outputFile,imageDimensions,floatd3(0,0,0),"",iterations);
}


#include <albert/dht/routing_table/routing_table.hpp>

#include <random>
#include <memory>
#include <fstream>

#include <boost/asio/ip/address_v4.hpp>

#include <albert/log/log.hpp>
#include <albert/u160/u160.hpp>
#include <albert/utils/utils.hpp>

namespace albert::dht::routing_table {

void Bucket::split_if_required() {
  if (!(is_leaf() &&
      (fat_mode_ || self_in_bucket()) &&
      known_node_count() > BucketMaxGoodItems)){
    return;
  }
  // ref:
  //  In that case,
  //  the bucket is replaced by two new buckets,
  //  each with half the range of the old bucket,
  //  and the nodes from the old bucket are distributed among the two new ones

  left_ = std::make_unique<Bucket>(this, owner_);
  left_->prefix_ = prefix_;
  left_->prefix_length_ = prefix_length_ + 1;

  right_ = std::make_unique<Bucket>(this, owner_);
  right_->prefix_ = prefix_ | u160::U160::pow2(u160::U160Bits - prefix_length_ - 1);
  right_->prefix_length_ = prefix_length_ + 1;

  for (auto &item : known_nodes_) {
    if (left_->in_bucket(item.first)) {
      left_->known_nodes_.emplace(item.first, std::move(item.second));
    } else {
      assert(right_->in_bucket(item.first));
      right_->known_nodes_.emplace(item.first, std::move(item.second));
    }
  }
  known_nodes_.clear();

  left_->split_if_required();
  right_->split_if_required();
}

void Bucket::merge() {
  assert(!is_leaf());

  for (auto &item : left_->known_nodes_) {
    known_nodes_.emplace(item.first, std::move(item.second));
  }
  for (auto &item : right_->known_nodes_) {
    known_nodes_.emplace(item.first, std::move(item.second));
  }

  left_ = nullptr;
  right_ = nullptr;
}

bool Bucket::add_node(Entry entry) {
  assert(in_bucket(entry.id()));
  if (is_leaf()) {
    bool added = false;
//    if (self_in_bucket() || good_node_count() < BucketMaxGoodItems) {
      known_nodes_.insert(std::make_pair(entry.id(), std::move(entry)));
      added = true;
//    }
    split_if_required();
    return added;
  } else {
    if (left_->in_bucket(entry.id())) {
      return left_->add_node(std::move(entry));
    } else {
      assert(right_->in_bucket(entry.id()));
      return right_->add_node(std::move(entry));
    }
  }
}
void Bucket::encode_(std::ostream &os, int i) {
  if (is_leaf()) {
    os << indent(i) << "{" << std::endl;
    os << indent(i+1) << R"("prefix_length": )" << prefix_length_ << "," << std::endl;
    os << indent(i+1) << R"("prefix": ")" << prefix_.to_string() << "\"," << std::endl;
    os << indent(i+1) << R"("entry_count": )" << known_nodes_.size() << std::endl;
    os << indent(i) << "}" << std::endl;
  } else {
    left_->encode_(os, i);
    right_->encode_(os, i);
  }
}
size_t Bucket::leaf_count() const {
  if (is_leaf()) {
    return 1;
  } else {
    return left_->leaf_count() + right_->leaf_count();
  }
}
void Bucket::encode(std::ostream &os) {
  os << "[" << std::endl;
  encode_(os, 1);
  os << "]" << std::endl;

}
std::list<Entry> Bucket::k_nearest_good_nodes(const u160::U160 &id, size_t k) const {
  assert(k > 0);
  if (is_leaf()) {
    std::list<Entry> results;
    for (auto &item : known_nodes_) {
      results.push_back(item.second);
      if (results.size() >= k) {
        break;
      }
    }
    return results;
  } else {
    Bucket *primary_node{}, *secondary_node{};
    if (!id.bit(u160::U160Bits - prefix_length_ - 1)) {
      primary_node = left_.get();
      secondary_node = right_.get();
    } else {
      primary_node = right_.get();
      secondary_node = left_.get();
    }
    auto result = primary_node->k_nearest_good_nodes(id, k);
    if (result.size() < k) {
      result.merge(secondary_node->k_nearest_good_nodes(id, k - result.size()));
    }
    return result;
  }
}
bool Bucket::is_full() const {
  if (is_leaf()) {
    if (self_in_bucket()) {
      return prefix_length_ >= (u160::U160Bits - 1);
    } else {
      return good_node_count() >= owner_->max_bucket_size();
    }
  } else {
    return left_->is_full() && right_->is_full();
  }
}

std::list<std::tuple<Entry, u160::U160>> Bucket::find_some_node_for_filling_bucket(size_t k) const {
  std::list<Entry> selected_nodes;
  std::list<Entry> good_nodes;
  std::list<Entry> questionable_nodes;
  for (auto &item : known_nodes_) {
    if (item.second.is_good()) {
      good_nodes.push_back(item.second);
    } else if (!item.second.is_bad()) {
      questionable_nodes.push_back(item.second);
    }
  }

  // TODO: work around a possibilly glibc issue
  if (!good_nodes.empty()) {
    std::sample(
        good_nodes.begin(),
        good_nodes.end(),
        std::back_inserter(selected_nodes),
        k,
        std::mt19937{std::random_device{}()});
  }

  if (selected_nodes.size() < k && !questionable_nodes.empty()) {
    std::sample(
        questionable_nodes.begin(),
        questionable_nodes.end(),
        std::back_inserter(selected_nodes),
        k - selected_nodes.size(),
        std::mt19937{std::random_device{}()});
  }

  auto virtual_target_id =
      u160::U160::random_from_prefix(prefix_, prefix_length_);
  std::list<std::tuple<Entry, u160::U160>> results;
  for (auto &item : selected_nodes) {
    results.emplace_back(item, virtual_target_id);
  }
  return results;
}
size_t Bucket::total_good_node_count() const {
  if (is_leaf()) {
    return good_node_count();
  } else {
    return left_->total_good_node_count() + right_->total_good_node_count();
  }
}
void Bucket::dfs(const std::function<void(const Bucket &)> &cb) const {
  cb(*this);
  if (!is_leaf()) {
    left_->dfs(cb);
    right_->dfs(cb);
  }
}
bool Bucket::dfs_w(const std::function<bool (Bucket &)> &cb) {
  if (cb(*this)) {
    if (!is_leaf()) {
      if (left_->dfs_w(cb)) {
        return right_->dfs_w(cb);
      }
    } else {
      return true;
    }
  }
  return false;
}
void Bucket::bfs(const std::function<void(const Bucket &)> &cb) const {
  std::list<const Bucket*> queue;
  queue.push_back(this);

  while (!queue.empty()) {
    auto current_node = queue.front();
    queue.pop_front();
    cb(*current_node);

    if (!current_node->is_leaf()) {
      queue.push_back(current_node->left_.get());
      queue.push_back(current_node->right_.get());
    }
  }
}
bool Bucket::self_in_bucket() const {
  return min() <= owner_->self() && owner_->self() <= max();
}
bool Bucket::is_leaf() const {
  return left_ == nullptr && right_ == nullptr;
}
bool Bucket::in_bucket(u160::U160 id) const {
  return min() <= id && id <= max();
}
size_t Bucket::prefix_length() const { return prefix_length_; }

u160::U160 Bucket::min() const {
  return prefix_;
}
bool Bucket::make_good_now(const u160::U160 &id) {
  bool found = false;
  Bucket *bucket = nullptr;
  // TODO: optimize dfs to searching by id prefix
  dfs_w([&found, id, &bucket](Bucket &b) -> bool {
    if (!found && b.is_leaf()) {
      for (auto &item : b.known_nodes_) {
        if (item.first == id) {
          item.second.make_good_now();
          found = true;
          bucket = &b;
          return false;
        }
      }
    }
    return true;
  });
  if (bucket) {
    bucket->split_if_required();
  }
  return found;
}
bool Bucket::make_good_now(uint32_t ip, uint16_t port) {
  bool found = false;
  Bucket *bucket = nullptr;
  dfs_w([&found, &bucket, ip, port](Bucket &b) -> bool {
    if (!found && b.is_leaf()) {
      for (auto &item : b.known_nodes_) {
        if (item.second.ip() == ip && item.second.port() == port) {
          item.second.make_good_now();
          found = true;
          bucket = &b;
          return false;
        }
      }
    }
    return true;
  });
  if (bucket) {
    bucket->split_if_required();
  }
  return found;
}
std::string Bucket::indent(int n) {
  return std::string(n*2, ' ');
}
size_t Bucket::good_node_count() const {
  size_t ret = 0;
  for (auto &item : known_nodes_) {
    if (item.second.is_good()) {
      ret++;
    }
  }
  return ret;
}
size_t Bucket::total_known_node_count() const {
  if (is_leaf()) {
    return known_node_count();
  } else {
    return left_->total_known_node_count() + right_->total_known_node_count();
  }
}
size_t Bucket::known_node_count() const {
  return this->known_nodes_.size();
}
void Bucket::iterate_entries(const std::function<void(const Entry &)> &cb) const {
  for (auto &item : known_nodes_) {
    cb(item.second);
  }
}
std::optional<Entry> Bucket::remove(const u160::U160 &id) {
  std::optional<Entry> ret;
  dfs_w([id, &ret](Bucket &bucket) -> bool {
    if (bucket.known_nodes_.find(id) != bucket.known_nodes_.end()) {
      ret.emplace(std::move(bucket.known_nodes_.at(id)));
      bucket.known_nodes_.erase(id);
      return false;
    }
    return true;
  });
  return ret;
}
bool Bucket::require_response_now(const u160::U160 &target) {
  auto entry = search(target);
  if (entry) {
    entry->require_response_now();
    return true;
  } else {
    return false;
  }
}
Entry *Bucket::search(const u160::U160 &id) {
  Entry *ret = nullptr;
  bool found = false;
  dfs_w([&ret, &found, &id](Bucket &bucket) -> bool {
    if (!found && bucket.is_leaf()) {
      for (auto &item : bucket.known_nodes_) {
        if (item.first == id) {
          ret = &item.second;
          return false;
        }
      }
    }
    return true;
  });
  return ret;

}
std::tuple<size_t, size_t, size_t, std::list<krpc::NodeInfo>> Bucket::gc() {
  if (is_leaf()) {
    std::list<krpc::NodeInfo> nodes_to_delete;
    std::vector<krpc::NodeInfo> questionable_nodes, good_nodes;
    size_t n_good = 0, n_non_bad = 0, n_bad = 0;
    for (auto &node : known_nodes_) {
      if (node.second.is_bad()) {
        LOG(debug) << "Bucket::gc() prefix " << prefix_length_ << " delete bad node " << node.second.to_string();
        nodes_to_delete.push_back(node.second.node_info());
        owner_->black_list_node(node.second.ip(), node.second.port());
        n_bad++;
      } else if (node.second.is_good()) {
        n_good++;
        n_non_bad++;
        good_nodes.push_back(node.second.node_info());
      } else {
        n_non_bad++;
        questionable_nodes.push_back(node.second.node_info());
      }
    }
    size_t n_good_deleted = 0, n_questionable_deleted = 0;

    // If routing table is too big, we delete questionable nodes first, then good nodes.
    if (n_non_bad > owner_->max_bucket_size()) {
      LOG(debug) << "Bucket::gc() prefix " << prefix_length_ << " non_bad count " << n_non_bad << " delete extra nodes";
      for (size_t i = 0; i < std::min(n_non_bad - owner_->max_bucket_size(), questionable_nodes.size()); i++) {
        LOG(debug) << "Bucket::gc() prefix " << prefix_length_ << " delete questionable node " << questionable_nodes[i].to_string();
        nodes_to_delete.push_back(questionable_nodes[i]);
        n_questionable_deleted++;
      }
    }
    if (n_good > owner_->max_bucket_size()) {
      LOG(debug) << "Bucket::gc() prefix " << prefix_length_ << " good count " << n_good << " delete extra nodes";
      for (size_t i = 0; i < good_nodes.size() - owner_->max_bucket_size(); i++) {
        LOG(debug) << "Bucket::gc() prefix " << prefix_length_ << " delete good node " << good_nodes[i].to_string();
        nodes_to_delete.push_back(good_nodes[i]);
        n_good_deleted++;
      }
    }

    for (auto &node : nodes_to_delete) {
      known_nodes_.erase(node.id());
    }
    return {n_good_deleted, n_questionable_deleted, n_bad, nodes_to_delete};
  } else {
    size_t a1, a2, b1, b2, c1, c2;
    std::list<krpc::NodeInfo> l, r;
    std::tie(a1, b1, c1, l) = right_->gc();
    std::tie(a2, b2, c2, r) = left_->gc();
    l.merge(r);

    // merge buckets when all the sub-buckets are no bigger than max/2
    if (left_->is_leaf() && right_->is_leaf()) {
      if (left_->known_node_count() + right_->known_node_count() < owner_->max_bucket_size() / 2) {
        merge();
      }
    }

    return {a1 + a2, b1 + b2, c1 + c2, l};
  }
}

void Bucket::make_bad(uint32_t ip, uint16_t port) {
  dfs_w([ip, port](Bucket &b) -> bool {
    if (b.is_leaf()) {
      for (auto &item : b.known_nodes_) {
        if (item.second.ip() == ip && item.second.port() == port) {
          item.second.make_bad();
          return false;
        }
      }
    }
    return true;
  });
}
size_t Bucket::memory_size() const {
  size_t ret = sizeof(*this);
  ret += known_node_count() * (sizeof(u160::U160) + sizeof(Entry));
  if (left_) {
    ret += left_->memory_size();
  }
  if (right_) {
    ret += right_->memory_size();
  }
  return ret;
}
void Bucket::remove(uint32_t ip, uint16_t port) {
  dfs_w([ip, port](Bucket &bucket) -> bool {
    for (auto &item : bucket.known_nodes_) {
      if (item.second.ip() == ip && item.second.port() == port) {
        bucket.known_nodes_.erase(item.first);
        return false;
      }
    }
    return true;
  });
}

std::list<std::tuple<Entry, u160::U160>> RoutingTable::select_expand_route_targets() {
  std::list<std::tuple<Entry, u160::U160>> entries;
  if (fat_mode_) {
    // for fat routing tables, we make it shallow and fat
    root_.bfs([&entries](const Bucket &bucket) {
      if (bucket.is_leaf()) {
        entries.splice(entries.end(), bucket.find_some_node_for_filling_bucket(1));
      }
    });
  } else {
    // for non fat routing tables, we make it deep
    for (auto &entry : root_.k_nearest_good_nodes(self(), max_bucket_size())) {
      entries.emplace_back(entry, self());
    }
  }
  return entries;
}
void RoutingTable::encode(std::ostream &os) {
  os << "{"  << std::endl;
  os << R"("type": "routing_table",)" << std::endl;
  os << R"("self_id": ")" << self_id_.to_string() << "\"," << std::endl;
  os << R"("data": )" << std::endl;
  root_.encode(os);
  os << "}" << std::endl;
}

struct TrieLevelStat {
  size_t good = 0;
  size_t known = 0;
  size_t buckets = 0;
};

void RoutingTable::stat() const {
  LOG(info) << "Routing Table: ";
  if (fat_mode_) {
    std::map<size_t, TrieLevelStat> level_stat;
    root_.bfs([&level_stat](const Bucket &bucket) {
      if (bucket.is_leaf()) {
        auto p = bucket.prefix_length();
        level_stat[p].good += bucket.good_node_count();
        level_stat[p].known += bucket.known_node_count();
        level_stat[p].buckets += 1;
      }
    });
    for (auto &item : level_stat) {
      LOG(debug) << "  depth=" << item.first << ", buckets=" << item.second.buckets << " " << item.second.good << "/" << item.second.known;
    }
  } else {
    root_.bfs([](const Bucket &bucket) {
      if (bucket.is_leaf()) {
        LOG(debug) << "  len(p)=" << bucket.prefix_length()
                   << ", " << bucket.good_node_count() << "/" << bucket.known_node_count();
      }
    });
  }
  LOG(info) << "  total entries: " << root_.total_known_node_count();
  LOG(info) << "  total good entries: " << root_.total_good_node_count();
  LOG(info) << "  total node added: " << total_node_added_;
  LOG(info) << "  total good deleted: " << total_good_node_deleted_;
  LOG(info) << "  total questionable deleted: " << total_questionable_node_deleted_;
  LOG(info) << "  total bad deleted: " << total_bad_node_deleted_;
  LOG(info) << "  total bucket count: " << bucket_count();
}
bool RoutingTable::make_good_now(const u160::U160 &id) {
  return root_.make_good_now(id);
}
bool RoutingTable::add_node(Entry entry) {
  if (reverse_map_.find({entry.ip(), entry.port()}) == reverse_map_.end()) {
    reverse_map_[std::make_tuple(entry.ip(), entry.port())] = entry.id();
    if (is_full()) {
      LOG(debug) << "failed to add node because routing table is full";
      return false;
    } else {
      if (root_.add_node(std::move(entry))) {
        total_node_added_++;
        return true;
      } else {
        LOG(debug) << "failed to add node because routing table bucket is full";
        return false;
      }
    }
  } else {
    if (entry.id() == reverse_map_[{entry.ip(), entry.port()}]) {
      if (!root_.in_bucket(entry.node_info().id())) {
        LOG(error) << "!!!!!!!!!!!!!!!!!!! error, routing table inconsistent";
      }
    } else {
      black_list_node(entry.ip(), entry.port());
      reverse_map_.erase({entry.ip(), entry.port()});
      make_bad(entry.ip(), entry.port());
      LOG(debug) << "banned node " << entry.to_string() << " because it has multiple node IDs";
    }
    return false;
  }

}
bool RoutingTable::make_good_now(uint32_t ip, uint16_t port) {
  return root_.make_good_now(ip, port);
}
void RoutingTable::iterate_nodes(const std::function<void(const Entry &)> &callback) const {
  root_.dfs([&callback](const Bucket &bucket) {
    if (bucket.is_leaf()) {
      bucket.iterate_entries(callback);
    }
  });
}
std::optional<Entry> RoutingTable::remove_node(const u160::U160 &target) {
  auto node = root_.remove(target);
  if (node.has_value()) {
    reverse_map_.erase({node->ip(), node->port()});
  }
  return node;
}

void RoutingTable::gc() {
  auto t0 = std::chrono::high_resolution_clock::now();

  size_t bad{}, good{}, quest;
  std::list<krpc::NodeInfo> info;
  std::tie(good, quest, bad, info) = root_.gc();
  total_good_node_deleted_ += good;
  total_questionable_node_deleted_ += quest;
  total_bad_node_deleted_ += bad;
  for (auto &node : info) {
    reverse_map_.erase({node.ip(), node.port()});
  }
  auto t1 = std::chrono::high_resolution_clock::now();
  LOG(debug) << "RoutingTable::gc() good/bad/questionable = " << good << "/" << bad << "/" << quest << " in "
            << std::fixed << std::setprecision(2) << std::chrono::duration<double, std::milli>(t1 - t0).count() << "ms";
}
size_t RoutingTable::max_prefix_length() const {
  size_t length = 0;
  root_.dfs([&length](const Bucket &bucket) {
    if (bucket.prefix_length() > length) {
      length = bucket.prefix_length();
    }
  });
  return length;
}
size_t RoutingTable::known_node_count() const {
  return root_.total_known_node_count();
}
size_t RoutingTable::good_node_count() const {
  return root_.total_good_node_count();
}

bool RoutingTable::is_full() const {
  return root_.total_known_node_count() >= max_known_nodes_ || root_.is_full();
}

bool RoutingTable::require_response_now(const u160::U160 &target) {
  return root_.require_response_now(target);
}

std::list<Entry> RoutingTable::k_nearest_good_nodes(const u160::U160 &id, size_t k) const {
  return root_.k_nearest_good_nodes(id, k);
}

void RoutingTable::serialize(std::ostream &os) const {
  std::vector<std::shared_ptr<bencoding::Node>> nodes;
  auto list_node = std::make_shared<bencoding::ListNode>();
  iterate_nodes([&list_node](const Entry &entry) {
    if (entry.is_good()) {
      auto item = bencoding::make_list(
          entry.id().to_string(),
          boost::asio::ip::address_v4(entry.ip()).to_string(),
          entry.port(),
          entry.version()
      );
      list_node->append(item);
    }
  });
  bencoding::DictNode root_dict({{"nodes", list_node}});
  root_dict.encode(os, bencoding::EncodeMode::Bencoding);
}

std::unique_ptr<RoutingTable> RoutingTable::deserialize(
    std::istream &is, std::string name, std::string save_path, size_t max_bucket_size, size_t max_known_nodes,
    bool delete_good_nodes, bool fat_mode,
    std::function<void(uint32_t, uint16_t)> black_list_node) {
  auto ret = std::make_unique<RoutingTable>(u160::U160(), std::move(name), std::move(save_path), max_bucket_size, max_known_nodes,
                                            delete_good_nodes, fat_mode, std::move(black_list_node));
  auto root_dict = std::dynamic_pointer_cast<bencoding::DictNode>(bencoding::Node::decode(is));
  auto node_list = bencoding::get<bencoding::ListNode>(*root_dict, "nodes");
  for (size_t i = 0; i < node_list.size(); i++) {
    auto node = bencoding::get<bencoding::ListNode>(node_list, i);
    ret->add_node(Entry(
        u160::U160::from_hex(bencoding::get<std::string>(node, 0)),
        boost::asio::ip::address_v4::from_string(bencoding::get<std::string>(node, 1)).to_uint(),
        bencoding::get<uint16_t>(node, 2),
        bencoding::get<std::string>(node, 3)
    ));
  }
  return ret;
}

RoutingTable::~RoutingTable() {
  if (!save_path_.empty()) {
    LOG(info) << "Saving routing table to file '" << save_path_  << "'";
    std::ofstream save_file(save_path_);
    serialize(save_file);
  }
}
void RoutingTable::make_bad(uint32_t ip, uint16_t port) {
  root_.make_bad(ip, port);
}
size_t RoutingTable::bucket_count() const {
  size_t n = 0;
  root_.dfs([&n](const Bucket &b) {
    if (b.is_leaf() && b.good_node_count() > 0) {
      n++;
    }
  });
  return n;
}
void RoutingTable::black_list_node(uint32_t ip, uint16_t port) const {
  if (black_list_node_) {
    black_list_node_(ip, port);
  }
}
size_t RoutingTable::memory_size() const {
  size_t size = sizeof(*this);
  size += root_.memory_size() - sizeof(root_);
  size += name_.size();
  size += save_path_.size();
  size += (sizeof(reverse_map_.begin()->first) + sizeof(reverse_map_.begin()->second)) * reverse_map_.size();
  return size;
}

bool Entry::is_good() const noexcept {
  // A good node is a node has responded to one of our queries within the last 15 minutes,
  // A node is also good if it has ever responded to one of our queries and has sent us a query within the last 15 minutes

  return !is_bad() &&
      (std::chrono::high_resolution_clock::now() - last_seen_) < std::chrono::minutes(MaxGoodNodeAliveMinutes);
}

bool Entry::is_bad() const {
  if (!response_required)
    return false;

  return (std::chrono::high_resolution_clock::now() - last_require_response_) > krpc::KRPCTimeout || bad_;
}

bool Entry::require_response_now() {
  if (!response_required) {
    response_required = true;
    this->last_require_response_ = std::chrono::high_resolution_clock::now();
    LOG(trace) << "require response " << to_string();
    return true;
  } else {
    return false;
  }
}

void Entry::make_good_now() {
  this->last_seen_ = std::chrono::high_resolution_clock::now();
  this->response_required = false;
  this->bad_ = false;
}

void Entry::make_bad() {
  bad_ = true;
}
}


// Boost.Geometry (aka GGL, Generic Geometry Library)

// Copyright (c) 2009-2015 Mateusz Loskot, London, UK.
// Copyright (c) 2009-2015 Barend Gehrels, Amsterdam, the Netherlands.

// This file was modified by Oracle on 2015.
// Modifications copyright (c) 2015, Oracle and/or its affiliates.

// Contributed and/or modified by Adam Wulkiewicz, on behalf of Oracle

// Parts of Boost.Geometry are redesigned from Geodan's Geographic Library
// (geolib/GGL), copyright (c) 1995-2010 Geodan, Amsterdam, the Netherlands.

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

#ifndef BOOST_GEOMETRY_STRATEGIES_CARTESIAN_CENTROID_WEIGHTED_LENGTH_HPP
#define BOOST_GEOMETRY_STRATEGIES_CARTESIAN_CENTROID_WEIGHTED_LENGTH_HPP

#include <boost/math/special_functions/fpclassify.hpp>
#include <boost/numeric/conversion/cast.hpp>

#include <boost/geometry/algorithms/detail/distance/interface.hpp>
#include <boost/geometry/algorithms/detail/distance/point_to_geometry.hpp>
#include <boost/geometry/arithmetic/arithmetic.hpp>
#include <boost/geometry/util/for_each_coordinate.hpp>
#include <boost/geometry/util/select_most_precise.hpp>
#include <boost/geometry/strategies/centroid.hpp>
#include <boost/geometry/strategies/default_distance_result.hpp>

// Helper geometry
#include <boost/geometry/geometries/point.hpp>

namespace geofeatures_boost {} namespace boost = geofeatures_boost; namespace geofeatures_boost { namespace geometry
{

namespace strategy { namespace centroid
{

namespace detail
{

template <typename Type, std::size_t DimensionCount>
struct weighted_length_sums
{
    typedef typename geometry::model::point
        <
            Type, DimensionCount,
            cs::cartesian
        > work_point;

    Type length;
    work_point average_sum;

    inline weighted_length_sums()
        : length(Type())
    {
        geometry::assign_zero(average_sum);
    }
};
}

template
<
    typename Point,
    typename PointOfSegment = Point
>
class weighted_length
{
private :
    typedef typename select_most_precise
        <
            typename default_distance_result<Point>::type,
            typename default_distance_result<PointOfSegment>::type
        >::type distance_type;

public :
    typedef detail::weighted_length_sums
        <
            distance_type,
            geometry::dimension<Point>::type::value
        > state_type;

    static inline void apply(PointOfSegment const& p1,
            PointOfSegment const& p2, state_type& state)
    {
        distance_type const d = geometry::distance(p1, p2);
        state.length += d;

        typename state_type::work_point weighted_median;
        geometry::assign_zero(weighted_median);
        geometry::add_point(weighted_median, p1);
        geometry::add_point(weighted_median, p2);
        geometry::multiply_value(weighted_median, d/2);
        geometry::add_point(state.average_sum, weighted_median);
    }

    static inline bool result(state_type const& state, Point& centroid)
    {
        distance_type const zero = distance_type();
        if (! geometry::math::equals(state.length, zero)
            && geofeatures_boost::math::isfinite(state.length)) // Prevent NaN centroid coordinates
        {
            // NOTE: above distance_type is checked, not the centroid coordinate_type
            // which means that the centroid can still be filled with INF
            // if e.g. distance_type is double and centroid contains floats
            geometry::for_each_coordinate(centroid, set_sum_div_length(state));
            return true;
        }

        return false;
    }

    struct set_sum_div_length
    {
        state_type const& m_state;
        set_sum_div_length(state_type const& state)
            : m_state(state)
        {}
        template <typename Pt, std::size_t Dimension>
        void apply(Pt & centroid) const
        {
            typedef typename geometry::coordinate_type<Pt>::type coordinate_type;
            geometry::set<Dimension>(
                centroid,
                geofeatures_boost::numeric_cast<coordinate_type>(
                    geometry::get<Dimension>(m_state.average_sum) / m_state.length
                )
            );
        }
    };
};

#ifndef DOXYGEN_NO_STRATEGY_SPECIALIZATIONS

namespace services
{

// Register this strategy for linear geometries, in all dimensions

template <std::size_t N, typename Point, typename Geometry>
struct default_strategy
<
    cartesian_tag,
    linear_tag,
    N,
    Point,
    Geometry
>
{
    typedef weighted_length
        <
            Point,
            typename point_type<Geometry>::type
        > type;
};

} // namespace services

#endif // DOXYGEN_NO_STRATEGY_SPECIALIZATIONS

}} // namespace strategy::centroid

}} // namespace geofeatures_boost::geometry

#endif // BOOST_GEOMETRY_STRATEGIES_CARTESIAN_CENTROID_WEIGHTED_LENGTH_HPP


#include "rbfcore.h"
#include "utility.h"
#include "Solver.h"
#include <armadillo>
#include <fstream>
#include <limits>
#include <iomanip>
#include <ctime>
#include <chrono>
#include<algorithm>
#include "ImplicitedSurfacing.h"
typedef std::chrono::high_resolution_clock Clock;

void RBF_Core::BuildK(RBF_Paras para){

    isuse_sparse = para.isusesparse;
    sparse_para = para.sparse_para;
    Hermite_weight_smoothness = para.Hermite_weight_smoothness;
    Hermite_designcurve_weight = para.Hermite_designcurve_weight;
//    handcraft_sigma = para.handcraft_sigma;
//    wDir = para.wDir;
//    wOrt = para.wOrt;
//    wFlip = para.wFlip;
    curMethod = para.Method;

    Set_Actual_Hermite_LSCoef( para.Hermite_ls_weight );
    Set_Actual_User_LSCoef(  para.user_lamnbda  );
    isNewApprox = true;
    isnewformula = true;

    auto t1 = Clock::now();

    switch(curMethod){

    case Hermite_UnitNormal:
        Set_Hermite_PredictNormal(pts);
        break;
    }
    auto t2 = Clock::now();
    cout << "Build Time: " << (setup_time = std::chrono::nanoseconds(t2 - t1).count()/1e9) << endl<< endl;

    if(0)BuildCoherentGraph();
}

void RBF_Core::InitNormal(RBF_Paras para){

    auto t1 = Clock::now();
    curInitMethod = para.InitMethod;
    cout<<"Init Method: "<<mp_RBF_INITMETHOD[curInitMethod]<<endl;
    switch(curInitMethod){

    case Lamnbda_Search:
        Lamnbda_Search_GlobalEigen();
        break;

    }

    auto t2 = Clock::now();
    cout << "Init Time: " << (init_time = std::chrono::nanoseconds(t2 - t1).count()/1e9) << endl<< endl;

    mp_RBF_InitNormal[curMethod==HandCraft?0:1][curInitMethod] = initnormals;

}

void RBF_Core::OptNormal(int method){

    cout<<"OptNormal"<<endl;
    auto t1 = Clock::now();

    switch(curMethod){

    case Hermite_UnitNormal:
        Opt_Hermite_PredictNormal_UnitNormal();
        break;

    }
    auto t2 = Clock::now();
    cout << "Opt Time: " << (solve_time = std::chrono::nanoseconds(t2 - t1).count()/1e9) << endl<< endl;
    if(method==0)mp_RBF_OptNormal[curMethod==HandCraft?0:1][curInitMethod] = newnormals;
}

void RBF_Core::Surfacing(int method, int n_voxels_1d){

    n_evacalls = 0;
    Surfacer sf;

    surf_time = sf.Surfacing_Implicit(pts,n_voxels_1d,true,RBF_Core::Dist_Function);

    sf.WriteSurface(finalMesh_v,finalMesh_fv);

    cout<<"n_evacalls: "<<n_evacalls<<"   ave: "<<surf_time/n_evacalls<<endl;

}

int RBF_Core::InjectData(vector<double> &pts, RBF_Paras para){

    vector<int> labels;
    vector<double> normals,tangents;
    vector<uint> edges;

    return InjectData(pts,labels,normals,tangents,edges,para);
}

int RBF_Core::InjectData(vector<double> &pts, vector<int> &labels, vector<double> &normals, vector<double> &tangents, vector<uint> &edges, RBF_Paras para){

    isuse_sparse = para.isusesparse;
    sparse_para = para.sparse_para;
    //isuse_sparse = false;
    this->pts = pts;
    this->labels = labels;
    this->normals = normals;
    this->tangents = tangents;
    this->edges = edges;
    npt = this->pts.size()/3;
    curMethod = para.Method;
    curInitMethod = para.InitMethod;

    polyDeg = para.polyDeg;
    User_Lamnbda = para.user_lamnbda;
    rangevalue = para.rangevalue;
    maxvalue = 10000;

    cout<<"number of points: "<<pts.size()/3<<endl;
    cout<<"normals: "<<this->normals.size()<<endl;
    sol.Statue = 1;
    Init(para.Kernal);

    SetSigma(para.sigma);

    SetThis();

	return 1;
}

int RBF_Core::ThreeStep(vector<double>&pts, vector<int>&labels, vector<double>&normals, vector<double>&tangents,  vector<uint>&edges, RBF_Paras para){

    InjectData(pts, labels, normals, tangents, edges,  para);
    BuildK(para);
    InitNormal(para);
    OptNormal(0);

	return 1;
}

int RBF_Core::AllStep(vector<double> &pts, vector<int> &labels, vector<double> &normals, vector<double> &tangents, vector<uint> &edges, RBF_Paras para){

    InjectData(pts, labels, normals, tangents, edges,  para);
    BuildK(para);
    InitNormal(para);
    OptNormal(0);
    Surfacing(0,100);
	return 1;

}

void RBF_Core::BatchInitEnergyTest(vector<double> &pts, vector<int> &labels, vector<double> &normals, vector<double> &tangents, vector<uint> &edges, RBF_Paras para){

    InjectData(pts, labels, normals, tangents, edges,  para);
    BuildK(para);
    para.ClusterVisualMethod = 0;//RBF_Init_EMPTY
    for(int i=0;i<RBF_Init_EMPTY;++i){
        para.InitMethod = RBF_InitMethod(i);
        InitNormal(para);
        OptNormal(0);
        Record();
    }
    Print_Record_Init();
}

vector<double>* RBF_Core::ExportPts(){

    return &pts;

}

vector<double>* RBF_Core::ExportPtsNormal(int normal_type){

    if(normal_type==0)return &normals;
    else if(normal_type==1)return &initnormals;
    else if(normal_type==2)return &initnormals_uninorm;
    else if(normal_type==3)return &newnormals;

	return NULL;
}

vector<double>* RBF_Core::ExportInitNormal(int kmethod, RBF_InitMethod init_type){

    if(mp_RBF_InitNormal[kmethod].find(init_type)!=mp_RBF_InitNormal[kmethod].end())return &(mp_RBF_InitNormal[kmethod][init_type]);
    else return NULL;
}

vector<double>* RBF_Core::ExportOptNormal(int kmethod, RBF_InitMethod init_type){

    if(mp_RBF_OptNormal[kmethod].find(init_type)!=mp_RBF_OptNormal[kmethod].end())return &(mp_RBF_OptNormal[kmethod][init_type]);
    else return NULL;
}

void RBF_Core::Print_Record_Init(){

    cout<<"InitMethod"<<string(30-string("InitMethod").size(),' ')<<"InitEn\t\t FinalEn"<<endl;
    cout<<std::setprecision(8);
    {
        for(int i=0;i<record_initmethod.size();++i){
            cout<<record_initmethod[i]<<string(30-record_initmethod[i].size(),' ')<<record_initenergy[i]<<"\t\t"<<record_energy[i]<<endl;
        }
    }

}


#define BOOST_TEST_MODULE "Test LocalLengthGroupSpace class"

#include <boost/test/unit_test.hpp>
#include "GlobalGroupSpace.hpp"
#include "TimeSeriesSet.hpp"
#include "distance/Distance.hpp"
#include "Exception.hpp"
#include "Group.hpp"

#define TOLERANCE 1e-9

using namespace konex;

struct MockData
{
  data_t dat[7] = {110, 116, 118, 117, 16.5, 112, 112};
  std::string test_group_5_10_space = "datasets/test/test_group_5_10_space.txt";
  std::string test_group_5_10_different_space = "datasets/test/test_group_5_10_different_space.txt";
};

BOOST_AUTO_TEST_CASE( local_length_group_space, *boost::unit_test::tolerance(TOLERANCE) )
{
  MockData data;

  TimeSeriesSet tsSet;
  tsSet.loadData(data.test_group_5_10_different_space, 5, 0, " ");

  BOOST_CHECK_EQUAL(tsSet.getItemCount(), 5);
  BOOST_CHECK_EQUAL(tsSet.getItemLength(), 10);

  GlobalGroupSpace gSet(tsSet);
  gSet.group("euclidean", 0.5);
  candidate_time_series_t best = gSet.getBestMatch(tsSet.getTimeSeries(0, 0, 10));
  BOOST_TEST((best.dist) == 0);
  best = gSet.getBestMatch(tsSet.getTimeSeries(0,4,10));
  BOOST_TEST((best.dist) == 0);
  best = gSet.getBestMatch(tsSet.getTimeSeries(0,6,9));
  BOOST_TEST((best.dist) == 0);
  best = gSet.getBestMatch(tsSet.getTimeSeries(0,2,10));
  BOOST_TEST((best.dist) == 0);
  best = gSet.getBestMatch(tsSet.getTimeSeries(0,3,7));
  BOOST_TEST((best.dist) == 0);
  best = gSet.getBestMatch(tsSet.getTimeSeries(4,0,10));
  BOOST_TEST((best.dist) == 0);
  best = gSet.getBestMatch(tsSet.getTimeSeries(4,4,10));
  BOOST_TEST((best.dist) == 0);
  best = gSet.getBestMatch(tsSet.getTimeSeries(4,6,10));
  BOOST_TEST((best.dist) == 0);
  best = gSet.getBestMatch(tsSet.getTimeSeries(4,2,10));
  BOOST_TEST((best.dist) == 0);
  best = gSet.getBestMatch(tsSet.getTimeSeries(4,3,7));
  BOOST_TEST((best.dist) == 0);
  TimeSeries ts1(data.dat, 0,0,7);
  best = gSet.getBestMatch(ts1);
  BOOST_TEST((best.dist)> 0);
}

BOOST_AUTO_TEST_CASE( local_length_group_space_multi_threaded, *boost::unit_test::tolerance(TOLERANCE) )
{
  MockData data;

  TimeSeriesSet tsSet;
  tsSet.loadData(data.test_group_5_10_different_space, 5, 0, " ");

  BOOST_CHECK_EQUAL(tsSet.getItemCount(), 5);
  BOOST_CHECK_EQUAL(tsSet.getItemLength(), 10);

  GlobalGroupSpace gSet(tsSet);
  gSet.groupMultiThreaded("euclidean", 0.5, 4);
  candidate_time_series_t best = gSet.getBestMatch(tsSet.getTimeSeries(0, 0, 10));
  BOOST_TEST((best.dist) == 0);
  best = gSet.getBestMatch(tsSet.getTimeSeries(0,4,10));
  BOOST_TEST((best.dist) == 0);
  best = gSet.getBestMatch(tsSet.getTimeSeries(0,6,9));
  BOOST_TEST((best.dist) == 0);
  best = gSet.getBestMatch(tsSet.getTimeSeries(0,2,10));
  BOOST_TEST((best.dist) == 0);
  best = gSet.getBestMatch(tsSet.getTimeSeries(0,3,7));
  BOOST_TEST((best.dist) == 0);
  best = gSet.getBestMatch(tsSet.getTimeSeries(4,0,10));
  BOOST_TEST((best.dist) == 0);
  best = gSet.getBestMatch(tsSet.getTimeSeries(4,4,10));
  BOOST_TEST((best.dist) == 0);
  best = gSet.getBestMatch(tsSet.getTimeSeries(4,6,10));
  BOOST_TEST((best.dist) == 0);
  best = gSet.getBestMatch(tsSet.getTimeSeries(4,2,10));
  BOOST_TEST((best.dist) == 0);
  best = gSet.getBestMatch(tsSet.getTimeSeries(4,3,7));
  BOOST_TEST((best.dist) == 0);
  TimeSeries ts1(data.dat, 0,0,7);
  best = gSet.getBestMatch(ts1);
  BOOST_TEST((best.dist)> 0);
}

BOOST_AUTO_TEST_CASE( traverse_order )
{
  setWarpingBandRatio(0.4);
  vector<int> order = generateTraverseOrder(3, 7);
  vector<int> expected = { 3, 2, 4, 5 };
  BOOST_CHECK_EQUAL_COLLECTIONS(order.begin(), order.end(), expected.begin(), expected.end());
}


///////////////////////////////////////////////////////////////////////////////
//
// Copyright (C) 2014 MaidSafe.net Limited
//
// Distributed under the Boost Software License, Version 1.0.
//    (See accompanying file LICENSE_1_0.txt or copy at
//          http://www.boost.org/LICENSE_1_0.txt)
//
///////////////////////////////////////////////////////////////////////////////

#include <boost/test/unit_test.hpp>
#include <boost/config.hpp>
#include <boost/integer_traits.hpp>
#include <cstdint>
#include <maidsafe/crux/detail/sequence_number.hpp>

using maidsafe::crux::detail::sequence_number;

template<class SN> void test_limits()
{
    BOOST_STATIC_CONSTEXPR auto max = SN::max_value;
    BOOST_STATIC_CONSTEXPR auto half = max/2;

    BOOST_REQUIRE(SN(0)       < SN(1));
    BOOST_REQUIRE(SN(0)       < SN(half-1));
    BOOST_REQUIRE(SN(0)       < SN(half));
    BOOST_REQUIRE(SN(0)       > SN(half+1));
    BOOST_REQUIRE(SN(0)       > SN(max-1));
    BOOST_REQUIRE(SN(0)       > SN(max));
    BOOST_REQUIRE(SN(half-1)  < SN(half));
    BOOST_REQUIRE(SN(half-1)  < SN(half+1));
    BOOST_REQUIRE(SN(half-1)  > SN(max-1));
    BOOST_REQUIRE(SN(half-1)  > SN(max));
    BOOST_REQUIRE(SN(half)    < SN(half+1));
    BOOST_REQUIRE(SN(half)    < SN(max-1));
    BOOST_REQUIRE(SN(half)    > SN(max));
    BOOST_REQUIRE(SN(half+1)  < SN(max-1));
    BOOST_REQUIRE(SN(half+1)  < SN(max));
}

BOOST_AUTO_TEST_SUITE(sequence_suite)

BOOST_AUTO_TEST_CASE(sequence_number_uint8)
{
    test_limits<sequence_number<std::uint8_t>>();
}

BOOST_AUTO_TEST_CASE(sequence_number_uint16)
{
    test_limits<sequence_number<std::uint16_t>>();
}

BOOST_AUTO_TEST_CASE(sequence_number_uint32)
{
    test_limits<sequence_number<std::uint32_t>>();
}

BOOST_AUTO_TEST_CASE(sequence_number_uint32_custom_max)
{
    using T = std::uint32_t;
    BOOST_STATIC_CONSTEXPR T max = boost::integer_traits<T>::const_max;

    test_limits<sequence_number<T, max/2>>();
    test_limits<sequence_number<T, max/4>>();
    test_limits<sequence_number<T, max/8>>();
    test_limits<sequence_number<T, max/16>>();
}

BOOST_AUTO_TEST_CASE(sequence_number_pre_increment)
{
    using T = std::uint8_t;
    BOOST_STATIC_CONSTEXPR T max = boost::integer_traits<T>::const_max/2;
    sequence_number<T, max> s;
    std::uint32_t counter = 0;
    while ((++s).value()) { ++counter; }
    BOOST_REQUIRE_EQUAL(counter, max);
}

BOOST_AUTO_TEST_CASE(sequence_number_post_increment)
{
    using T = std::uint8_t;
    BOOST_STATIC_CONSTEXPR T max = boost::integer_traits<T>::const_max/2;
    sequence_number<T, max> s;
    s++;
    std::uint32_t counter = 0;
    while ((s++).value()) { ++counter; }
    BOOST_REQUIRE_EQUAL(counter, max);
}

BOOST_AUTO_TEST_CASE(short_distances)
{
    using T = std::uint16_t;
    BOOST_STATIC_CONSTEXPR T max = sequence_number<T>::max_value;
    sequence_number<T> zero(0);
    sequence_number<T> one(1);
    sequence_number<T> half(max/2);
    sequence_number<T> second_largest(max-1);
    sequence_number<T> largest(max);

    BOOST_CHECK_EQUAL(zero.distance(one), 1);
    BOOST_CHECK_EQUAL(one.distance(zero), -1);
    BOOST_CHECK_EQUAL(second_largest.distance(largest), 1);
    BOOST_CHECK_EQUAL(largest.distance(second_largest), -1);
    BOOST_CHECK_EQUAL(largest.distance(zero), 1);
    BOOST_CHECK_EQUAL(zero.distance(largest), -1);
    BOOST_CHECK_EQUAL(zero.distance(half), half.value());
    BOOST_CHECK_EQUAL(half.distance(zero), -half.value());
    BOOST_CHECK_EQUAL(one.distance(half), half.value() - 1);
    BOOST_CHECK_EQUAL(half.distance(one), -(half.value() - 1));
    BOOST_CHECK_EQUAL(largest.distance(half), -(half.value() + 1));
    BOOST_CHECK_EQUAL(half.distance(largest), largest.distance(half));
    BOOST_CHECK_EQUAL(second_largest.distance(half), -half.value());
    BOOST_CHECK_EQUAL(half.distance(second_largest), half.value());
    BOOST_CHECK_EQUAL(largest.distance(one), 2);
    BOOST_CHECK_EQUAL(one.distance(largest), -2);
    BOOST_CHECK_EQUAL(second_largest.distance(one), 3);
    BOOST_CHECK_EQUAL(one.distance(second_largest), -3);
}

BOOST_AUTO_TEST_CASE(medium_distances)
{
    using T = std::uint32_t;
    using difference_type = sequence_number<T>::difference_type;
    BOOST_STATIC_CONSTEXPR T max = sequence_number<T>::max_value;
    sequence_number<T> zero(0);
    sequence_number<T> one(1);
    sequence_number<T> half(max/2);
    sequence_number<T> second_largest(max-1);
    sequence_number<T> largest(max);

    BOOST_CHECK_EQUAL(zero.distance(one), 1);
    BOOST_CHECK_EQUAL(one.distance(zero), -1);
    BOOST_CHECK_EQUAL(second_largest.distance(largest), 1);
    BOOST_CHECK_EQUAL(largest.distance(second_largest), -1);
    BOOST_CHECK_EQUAL(largest.distance(zero), 1);
    BOOST_CHECK_EQUAL(zero.distance(largest), -1);
    BOOST_CHECK_EQUAL(zero.distance(half), static_cast<difference_type>(half.value()));
    BOOST_CHECK_EQUAL(half.distance(zero), -static_cast<difference_type>(half.value()));
    BOOST_CHECK_EQUAL(one.distance(half), static_cast<difference_type>(half.value() - 1));
    BOOST_CHECK_EQUAL(half.distance(one), -static_cast<difference_type>(half.value() - 1));
    BOOST_CHECK_EQUAL(largest.distance(half), -static_cast<difference_type>(half.value() + 1));
    BOOST_CHECK_EQUAL(half.distance(largest), largest.distance(half));
    BOOST_CHECK_EQUAL(second_largest.distance(half), -static_cast<difference_type>(half.value()));
    BOOST_CHECK_EQUAL(half.distance(second_largest), static_cast<difference_type>(half.value()));
    BOOST_CHECK_EQUAL(largest.distance(one), 2);
    BOOST_CHECK_EQUAL(one.distance(largest), -2);
    BOOST_CHECK_EQUAL(second_largest.distance(one), 3);
    BOOST_CHECK_EQUAL(one.distance(second_largest), -3);
}

BOOST_AUTO_TEST_CASE(long_distances)
{
    using T = std::uint64_t;
    using difference_type = sequence_number<T>::difference_type;
    BOOST_STATIC_CONSTEXPR T max = sequence_number<T>::max_value;
    sequence_number<T> zero(0);
    sequence_number<T> one(1);
    sequence_number<T> half(max/2);
    sequence_number<T> second_largest(max-1);
    sequence_number<T> largest(max);

    BOOST_CHECK_EQUAL(zero.distance(one), 1);
    BOOST_CHECK_EQUAL(one.distance(zero), -1);
    BOOST_CHECK_EQUAL(second_largest.distance(largest), 1);
    BOOST_CHECK_EQUAL(largest.distance(second_largest), -1);
    BOOST_CHECK_EQUAL(largest.distance(zero), 1);
    BOOST_CHECK_EQUAL(zero.distance(largest), -1);
    BOOST_CHECK_EQUAL(zero.distance(half), static_cast<difference_type>(half.value()));
    BOOST_CHECK_EQUAL(half.distance(zero), -static_cast<difference_type>(half.value()));
    BOOST_CHECK_EQUAL(one.distance(half), static_cast<difference_type>(half.value() - 1));
    BOOST_CHECK_EQUAL(half.distance(one), -static_cast<difference_type>(half.value() - 1));
    BOOST_CHECK_EQUAL(largest.distance(half), -static_cast<difference_type>(half.value() + 1));
    BOOST_CHECK_EQUAL(half.distance(largest), largest.distance(half));
    BOOST_CHECK_EQUAL(second_largest.distance(half), -static_cast<difference_type>(half.value()));
    BOOST_CHECK_EQUAL(half.distance(second_largest), static_cast<difference_type>(half.value()));
    BOOST_CHECK_EQUAL(largest.distance(one), 2);
    BOOST_CHECK_EQUAL(one.distance(largest), -2);
    BOOST_CHECK_EQUAL(second_largest.distance(one), 3);
    BOOST_CHECK_EQUAL(one.distance(second_largest), -3);
}

BOOST_AUTO_TEST_CASE(custom_short_distances)
{
    using T = std::uint16_t;
    BOOST_STATIC_CONSTEXPR T max = boost::integer_traits<T>::const_max / (1 << 1);
    sequence_number<T, max> zero(0);
    sequence_number<T, max> one(1);
    sequence_number<T, max> half(max/2);
    sequence_number<T, max> second_largest(max-1);
    sequence_number<T, max> largest(max);

    BOOST_CHECK_EQUAL(zero.distance(one), 1);
    BOOST_CHECK_EQUAL(one.distance(zero), -1);
    BOOST_CHECK_EQUAL(second_largest.distance(largest), 1);
    BOOST_CHECK_EQUAL(largest.distance(second_largest), -1);
    BOOST_CHECK_EQUAL(largest.distance(zero), 1);
    BOOST_CHECK_EQUAL(zero.distance(largest), -1);
    BOOST_CHECK_EQUAL(zero.distance(half), half.value());
    BOOST_CHECK_EQUAL(half.distance(zero), -half.value());
    BOOST_CHECK_EQUAL(one.distance(half), half.value() - 1);
    BOOST_CHECK_EQUAL(half.distance(one), -(half.value() - 1));
    BOOST_CHECK_EQUAL(largest.distance(half), -(half.value() + 1));
    BOOST_CHECK_EQUAL(half.distance(largest), largest.distance(half));
    BOOST_CHECK_EQUAL(second_largest.distance(half), -half.value());
    BOOST_CHECK_EQUAL(half.distance(second_largest), half.value());
    BOOST_CHECK_EQUAL(largest.distance(one), 2);
    BOOST_CHECK_EQUAL(one.distance(largest), -2);
    BOOST_CHECK_EQUAL(second_largest.distance(one), 3);
    BOOST_CHECK_EQUAL(one.distance(second_largest), -3);
}

BOOST_AUTO_TEST_CASE(custom_medium_distances)
{
    using T = std::uint32_t;
    using difference_type = sequence_number<T>::difference_type;
    BOOST_STATIC_CONSTEXPR T max = boost::integer_traits<T>::const_max / (1 << 1);
    sequence_number<T, max> zero(0);
    sequence_number<T, max> one(1);
    sequence_number<T, max> half(max/2);
    sequence_number<T, max> second_largest(max-1);
    sequence_number<T, max> largest(max);

    BOOST_CHECK_EQUAL(zero.distance(one), 1);
    BOOST_CHECK_EQUAL(one.distance(zero), -1);
    BOOST_CHECK_EQUAL(second_largest.distance(largest), 1);
    BOOST_CHECK_EQUAL(largest.distance(second_largest), -1);
    BOOST_CHECK_EQUAL(largest.distance(zero), 1);
    BOOST_CHECK_EQUAL(zero.distance(largest), -1);
    BOOST_CHECK_EQUAL(zero.distance(half), static_cast<difference_type>(half.value()));
    BOOST_CHECK_EQUAL(half.distance(zero), -static_cast<difference_type>(half.value()));
    BOOST_CHECK_EQUAL(one.distance(half), static_cast<difference_type>(half.value() - 1));
    BOOST_CHECK_EQUAL(half.distance(one), -static_cast<difference_type>(half.value() - 1));
    BOOST_CHECK_EQUAL(largest.distance(half), -static_cast<difference_type>(half.value() + 1));
    BOOST_CHECK_EQUAL(half.distance(largest), largest.distance(half));
    BOOST_CHECK_EQUAL(second_largest.distance(half), -static_cast<difference_type>(half.value()));
    BOOST_CHECK_EQUAL(half.distance(second_largest), static_cast<difference_type>(half.value()));
    BOOST_CHECK_EQUAL(largest.distance(one), 2);
    BOOST_CHECK_EQUAL(one.distance(largest), -2);
    BOOST_CHECK_EQUAL(second_largest.distance(one), 3);
    BOOST_CHECK_EQUAL(one.distance(second_largest), -3);
}

BOOST_AUTO_TEST_CASE(custom_long_distances)
{
    using T = std::uint64_t;
    using difference_type = sequence_number<T>::difference_type;
    BOOST_STATIC_CONSTEXPR T max = boost::integer_traits<T>::const_max / (1 << 1);
    sequence_number<T, max> zero(0);
    sequence_number<T, max> one(1);
    sequence_number<T, max> half(max/2);
    sequence_number<T, max> second_largest(max-1);
    sequence_number<T, max> largest(max);

    BOOST_CHECK_EQUAL(zero.distance(one), 1);
    BOOST_CHECK_EQUAL(one.distance(zero), -1);
    BOOST_CHECK_EQUAL(second_largest.distance(largest), 1);
    BOOST_CHECK_EQUAL(largest.distance(second_largest), -1);
    BOOST_CHECK_EQUAL(largest.distance(zero), 1);
    BOOST_CHECK_EQUAL(zero.distance(largest), -1);
    BOOST_CHECK_EQUAL(zero.distance(half), static_cast<difference_type>(half.value()));
    BOOST_CHECK_EQUAL(half.distance(zero), -static_cast<difference_type>(half.value()));
    BOOST_CHECK_EQUAL(one.distance(half), static_cast<difference_type>(half.value() - 1));
    BOOST_CHECK_EQUAL(half.distance(one), -static_cast<difference_type>(half.value() - 1));
    BOOST_CHECK_EQUAL(largest.distance(half), -static_cast<difference_type>(half.value() + 1));
    BOOST_CHECK_EQUAL(half.distance(largest), largest.distance(half));
    BOOST_CHECK_EQUAL(second_largest.distance(half), -static_cast<difference_type>(half.value()));
    BOOST_CHECK_EQUAL(half.distance(second_largest), static_cast<difference_type>(half.value()));
    BOOST_CHECK_EQUAL(largest.distance(one), 2);
    BOOST_CHECK_EQUAL(one.distance(largest), -2);
    BOOST_CHECK_EQUAL(second_largest.distance(one), 3);
    BOOST_CHECK_EQUAL(one.distance(second_largest), -3);
}

BOOST_AUTO_TEST_CASE(custom4_short_distances)
{
    using T = std::uint16_t;
    BOOST_STATIC_CONSTEXPR T max = boost::integer_traits<T>::const_max / (1 << 4);
    sequence_number<T, max> zero(0);
    sequence_number<T, max> one(1);
    sequence_number<T, max> half(max/2);
    sequence_number<T, max> second_largest(max-1);
    sequence_number<T, max> largest(max);

    BOOST_CHECK_EQUAL(zero.distance(one), 1);
    BOOST_CHECK_EQUAL(one.distance(zero), -1);
    BOOST_CHECK_EQUAL(second_largest.distance(largest), 1);
    BOOST_CHECK_EQUAL(largest.distance(second_largest), -1);
    BOOST_CHECK_EQUAL(largest.distance(zero), 1);
    BOOST_CHECK_EQUAL(zero.distance(largest), -1);
    BOOST_CHECK_EQUAL(zero.distance(half), half.value());
    BOOST_CHECK_EQUAL(half.distance(zero), -half.value());
    BOOST_CHECK_EQUAL(one.distance(half), half.value() - 1);
    BOOST_CHECK_EQUAL(half.distance(one), -(half.value() - 1));
    BOOST_CHECK_EQUAL(largest.distance(half), -(half.value() + 1));
    BOOST_CHECK_EQUAL(half.distance(largest), largest.distance(half));
    BOOST_CHECK_EQUAL(second_largest.distance(half), -half.value());
    BOOST_CHECK_EQUAL(half.distance(second_largest), half.value());
    BOOST_CHECK_EQUAL(largest.distance(one), 2);
    BOOST_CHECK_EQUAL(one.distance(largest), -2);
    BOOST_CHECK_EQUAL(second_largest.distance(one), 3);
    BOOST_CHECK_EQUAL(one.distance(second_largest), -3);
}

BOOST_AUTO_TEST_SUITE_END()


/*
    Copyright (C) 2010,2011 Wei Dong <wdong.pku@gmail.com>. All Rights Reserved.

    DISTRIBUTION OF THIS PROGRAM IN EITHER BINARY OR SOURCE CODE FORM MUST BE
    PERMITTED BY THE AUTHOR.
*/
#ifndef KGRAPH_VALUE_TYPE
#define KGRAPH_VALUE_TYPE float
#endif

#include <cctype>
#include <type_traits>
#include <iostream>
#include <boost/timer/timer.hpp>
#include <boost/program_options.hpp>
#include <kgraph.h>
#include <kgraph-data.h>

using namespace std;
using namespace boost;
using namespace kgraph;
namespace po = boost::program_options;

typedef KGRAPH_VALUE_TYPE value_type;

int main (int argc, char *argv[]) {
    string input_path;
    string query_path;
    string output_path;
    string eval_path;
    unsigned K, P;

    po::options_description desc_visible("General options");
    desc_visible.add_options()
    ("help,h", "produce help message.")
    ("data", po::value(&input_path), "input path")
    ("query", po::value(&query_path), "query path")
    ("eval", po::value(&eval_path), "eval path")
    (",K", po::value(&K)->default_value(default_K), "")
    (",P", po::value(&P)->default_value(default_P), "")
    ;

    po::options_description desc("Allowed options");
    desc.add(desc_visible);

    po::positional_options_description p;
    p.add("data", 1);
    p.add("query", 1);

    po::variables_map vm;
    po::store(po::command_line_parser(argc, argv).options(desc).positional(p).run(), vm);
    po::notify(vm);

    if (vm.count("help") || vm.count("data") == 0 || vm.count("query") == 0) {
        cout << "search <data> <index> <query> [output]" << endl;
        cout << desc_visible << endl;
        return 0;
    }

    if (P < K) {
        P = K;
    }

    Matrix<value_type> data;
    Matrix<value_type> query;
    Matrix<unsigned> result;

    data.load_lshkit(input_path);
    query.load_lshkit(query_path);
    unsigned dim = data.dim();
    VectorOracle<Matrix<value_type>, value_type const*> oracle(data,
            [dim](value_type const *a, value_type const *b)
            {
                float r = 0;
                for (unsigned i = 0; i < dim; ++i) {
                    float v = float(a[i]) - (b[i]);
                    r += v * v;
                }
                return r;
            });
    float recall = 0;
    float cost = 0;
    float time = 0;

    result.resize(query.size(), K);
    KGraph::SearchParams params;
    params.K = K;
    params.P = P;
    KGraph *kgraph = KGraph::create();
    {
        KGraph::IndexParams params;
        kgraph->build(oracle, params, NULL);
    }

    boost::timer::auto_cpu_timer timer;
    cerr << "Searching..." << endl;

#pragma omp parallel for reduction(+:cost)
    for (unsigned i = 0; i < query.size(); ++i) {
        KGraph::SearchInfo info;
        kgraph->search(oracle.query(query[i]), params, result[i], &info);
        cost += info.cost;
    }
    cost /= query.size();
    time = timer.elapsed().wall / 1e9;
    delete kgraph;
    if (eval_path.size()) {
        Matrix<unsigned> gs;
        gs.load_lshkit(eval_path);
        BOOST_VERIFY(gs.dim() >= K);
        BOOST_VERIFY(gs.size() >= query.size());
        kgraph::Matrix<float> gs_dist(query.size(), K);
        kgraph::Matrix<float> result_dist(query.size(), K);
#pragma omp parallel for
        for (unsigned i = 0; i < query.size(); ++i) {
            auto const Q = oracle.query(query[i]);
            float *gs_dist_row = gs_dist[i];
            float *result_dist_row = result_dist[i];
            unsigned const *gs_row = gs[i];
            unsigned const *result_row = result[i];
            for (unsigned k = 0; k < K; ++k) {
                gs_dist_row[k] = Q(gs_row[k]);
                result_dist_row[k] = Q(result_row[k]);
            }
            sort(gs_dist_row, gs_dist_row + K);
            sort(result_dist_row, result_dist_row + K);
        }
        recall = AverageRecall(gs_dist, result_dist, K);
    }
    cout << "Time: " << time << " Recall: " << recall << " Cost: " << cost << endl;
    return 0;
}


#include "tensorflow/core/public/session.h"
#include "tensorflow/core/platform/env.h"
#include <utility>
#include "tf_session.hpp"
#include <Eigen/Dense>
#include <iostream>
#include <algorithm>
#include <cassert>

using namespace tensorflow;

namespace hotbox {

namespace {

// Use shared_ptr to support copy constructor.
std::shared_ptr<tensorflow::Session> session_;

void CheckStatus(Status status) {
  if (!status.ok()) {
    std::cerr << status.ToString() << "\n";
    assert(false);
  }
}

Status TfRestore(const string& path, Session* session) {
  TensorProto path_proto;
  path_proto.set_dtype(DataType::DT_STRING);
  TensorShape({1, 1}).AsProto(path_proto.mutable_tensor_shape());
  path_proto.add_string_val(path);
  Tensor path_tensor;

  if (!path_tensor.FromProto(path_proto)) {
    return errors::Internal("Tensor::FromProto failed.");
  }
  return session->Run(
      {{"save/Const:0", path_tensor}},
        {}, {"save/restore_all"}, nullptr);
}

}  // anonymous namespace

TfSession::TfSession(const TfSessionConfig& config) :
  input_dim_(config.input_dim), output_vars_(config.output_vars) {
  // Initialize a tensorflow session
  tensorflow::SessionOptions options;
  session_.reset(tensorflow::NewSession(options));

  // Read in the protobuf graph we exported
  GraphDef graph_def;
  Status status = ReadBinaryProto(Env::Default(),
    config.graph_path, &graph_def);
  CheckStatus(status);

  // Add the graph to the session
  status = session_->Create(graph_def);
  CheckStatus(status);
  status = TfRestore(config.weight_path, session_.get());
  CheckStatus(status);
  std::cout << "Variable loaded\n";

  // Figure out output dim.
  Tensor X(tensorflow::DT_FLOAT, TensorShape({1, input_dim_}));
  // Sum the dim over all outputs
  std::vector<tensorflow::Tensor> outputs;

  // Run the session, evaluating our "c" operation from the graph
  status = session_->Run({{"x", X}}, output_vars_, {}, &outputs);
  CheckStatus(status);
  for (int i = 0; i < outputs.size(); ++i) {
    auto mat = outputs[i].matrix<float>();
    output_dim_ += mat.dimensions()[1];
    std::cout << "output " << output_vars_[i] << " has dim: " <<
      mat.dimensions()[1] << "\n";
  }
}

TfSession::~TfSession() {
  session_->Close();
}

std::vector<float> TfSession::Transform(const std::vector<float>& v) {
  Tensor X(tensorflow::DT_FLOAT, TensorShape({1, input_dim_}));
  auto dst = X.flat<float>().data();
  std::copy_n(v.cbegin(), input_dim_, dst);
  std::vector<tensorflow::Tensor> outputs;

  // Run the session, evaluating our "c" operation from the graph
  Status status = session_->Run({{"x", X}}, output_vars_, {}, &outputs);
  CheckStatus(status);

  std::vector<float> out(output_dim_);
  int curr = 0;
  for (const auto& t : outputs) {
    auto mat = t.matrix<float>();
    std::copy_n(mat.data(), mat.dimensions()[1], out.begin() + curr);
    curr += mat.dimensions()[1];
  }
  return out;
}
}  // namespace hotbox


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

#include <iostream>
#include <map>
#include "../src/camera.hpp"

using namespace std;

BOOST_AUTO_TEST_SUITE(test_camera_sute)

BOOST_AUTO_TEST_SUITE(test_pinhole_sute)

BOOST_AUTO_TEST_CASE(test_correct_ray_direction) {
    Vec eye(0.0, 0.0, 0.0), lookat(1.0, 0.0, 0.0);
    float d = 0.5;

    Pinhole camera(eye, lookat, d);
    Vec dir(camera.ray_direction(Point2D(0.0, 0.0)));
    BOOST_CHECK(dir == Vec(1.0, 0.0, 0.0));
}

BOOST_AUTO_TEST_SUITE_END()

BOOST_AUTO_TEST_SUITE_END()