#include <ros/ros.h>
#include <iostream>
#include <fstream>
//#include <pcl/console/parse.h>
//#include <pcl/filters/extract_indices.h>
//#include <pcl/visualization/pcl_visualizer.h>
//#include <boost/thread/thread.hpp>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl/kdtree/kdtree_flann.h>
//#include <pcl/sample_consensus/ransac.h>
//#include <pcl/sample_consensus/sac_model_plane.h>
//#include <pcl/sample_consensus/sac_model_sphere.h>
#include <algorithm>
#include <vector>
#include <string>
#include <numeric>
#include <cmath>
#include <time.h>
#include <stdio.h>
#include <opencv2/opencv.hpp>
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/ml/ml.hpp>

using namespace cv;
using namespace std;

clock_t t[100];

//read 3D points from pcd file
pcl::PointCloud<pcl::PointXYZ>::Ptr ReadPCD(char* filePath)
{
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
	if (pcl::io::loadPCDFile<pcl::PointXYZ> (filePath, *cloud) == -1)
	{
		PCL_ERROR("Couldn't read file box_cloud.pcd \n");

	}
	return (cloud);
}

//k nearest neighbor search
void PointsKNN(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, int pointNum, int maxK, int** indexKNN, double** distKNN)
{
	pcl::KdTreeFLANN<pcl::PointXYZ> kdtree;
	kdtree.setInputCloud (cloud);
	pcl::PointXYZ searchPoint;
	std::vector<int> pointIdxNKNSearch(maxK);
  	std::vector<float> pointNKNSquaredDistance(maxK);
	for(int i=0; i<pointNum; i++)
	{
		searchPoint.x=cloud->points[i].x;
		searchPoint.y=cloud->points[i].y;
		searchPoint.z=cloud->points[i].z;
		if(kdtree.nearestKSearch (searchPoint, maxK, pointIdxNKNSearch, pointNKNSquaredDistance)>0)
		{
			for(int j=0; j<maxK; j++)
			{
				indexKNN[i][j]=pointIdxNKNSearch[j];
				distKNN[i][j]=pointNKNSquaredDistance[j];
			}

		}
	}

	/*for(int i=0; i<pointNum; i++)
	{
		for(int j=0; j<k; j++)
		{
			cout<<" "<<indexKNN[i][j];
		}
		cout<<endl;
	}
	cout<<endl;
	for(int i=0; i<pointNum; i++)
	{
		for(int j=0; j<k; j++)
		{
			cout<<" "<<distKNN[i][j];
		}
		cout<<endl;
	}*/
}

//compute sigma and NWR matrix, then get the normals of the points
cv::Mat NormalEstimation(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, int pointNum, int maxK, int k, int** indexKNN, double** distKNN, double randRate, std::vector<int> &randPoints)
{
	cv::Mat diffM(3, 1, CV_64FC1);
	cv::Mat referP(3, 1, CV_64FC1);
	cv::Mat neighP(3, 1, CV_64FC1);
	cv::Mat nwrSample(3, 3, CV_64FC1);
	cv::Mat nwrMat(3, 3, CV_64FC1);
	cv::Mat normals(pointNum*randRate, 3, CV_64FC1);
	double sigma=0.2;
	//compute sigma
	int nn;    //number of nonzero distance
	/*for(int i=0; i<pointNum; i++)
	{
		nn=0;
		for(int j=0; j<maxK; j++)
		{
			if(distKNN[i][j]!=0)
			{
				nn++;
			}
			if(nn==k)
			{
				sigma=distKNN[i][j]+sigma;
				break;
			}
		}
	}
	sigma=sigma/pointNum;*/
	//compute NWR Matrix
	t[10]=clock();
	//randomly pick some points
	//std::vector<int> randPoints;
	for(int i=0; i<pointNum; i++)
	{
		randPoints.push_back(i);
	}
	std::random_shuffle(randPoints.begin(), randPoints.end());
	pointNum=pointNum*randRate;
	//cout<<"randRate: "<<randRate<<endl;
	//cout<<"pointNum: "<<pointNum<<endl;
	for(int i=0; i<pointNum; i++)
	{
		referP.at<double>(0,0)=cloud->points[randPoints[i]].x;
		referP.at<double>(1,0)=cloud->points[randPoints[i]].y;
		referP.at<double>(2,0)=cloud->points[randPoints[i]].z;
		nwrMat=cv::Mat::zeros(cv::Size(3,3),CV_64FC1);
		nn=0;
		for(int j=0; j<maxK; j++)
		{
			if(distKNN[randPoints[i]][j]!=0)
			{
				neighP.at<double>(0,0)=cloud->points[indexKNN[randPoints[i]][j]].x;
				neighP.at<double>(1,0)=cloud->points[indexKNN[randPoints[i]][j]].y;
				neighP.at<double>(2,0)=cloud->points[indexKNN[randPoints[i]][j]].z;
				diffM=neighP-referP;
				nwrSample=diffM*diffM.t();
				nwrSample=nwrSample/(distKNN[randPoints[i]][j]*distKNN[randPoints[i]][j]);
				nwrSample=nwrSample*exp(-(distKNN[randPoints[i]][j]*distKNN[randPoints[i]][j])/(2*sigma*sigma));
				nwrMat=nwrMat+nwrSample;
				nn++;
			}
			if(nn==k)
			{
				break;
			}

		}
		//compute eigen value
		cv::Mat eigenvalue, eigenvector;
		cv::eigen(nwrMat, eigenvalue, eigenvector);
		//cout<<endl;
		//cout<<nwrMat<<endl;
		//cout<<endl;
		//cout<<eigenvalue<<endl;
		//cout<<endl;
		//cout<<eigenvector<<endl;
		normals.at<double>(i,0)=eigenvector.at<double>(2,0);
		normals.at<double>(i,1)=eigenvector.at<double>(2,1);
		normals.at<double>(i,2)=eigenvector.at<double>(2,2);
	}
	t[11]=clock();
	//printf("NormalEstimation: %lf s\n",(double)(t[11]-t[10])/CLOCKS_PER_SEC);
	return (normals);
}

/*//Open 3D viewer and add point cloud
boost::shared_ptr<pcl::visualization::PCLVisualizer>
simpleVis (pcl::PointCloud<pcl::PointXYZ>::ConstPtr cloud)
{
  boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer (new pcl::visualization::PCLVisualizer ("3D Viewer"));
  viewer->setBackgroundColor (0, 0, 0);
  viewer->addPointCloud<pcl::PointXYZ> (cloud, "sample cloud");
  viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3, "sample cloud");
  //viewer->addCoordinateSystem (1.0, "global");
  viewer->initCameraParameters ();
  return (viewer);
}*/

//implement one point ransac
cv::Mat OnePointRANSAC(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_hori, cv::Mat horiPoints, int maxIteration, double p, double disThreshold, int fitNum, std::vector<int> &best_inliers, std::vector<int> isExist, std::vector<int> randPoints)
{
	int iter=0;
	//plane which ransac find: planeModel is the index of the point on the plane and the plane normal
	cv::Mat planeModel = cv::Mat::zeros(2, 3, CV_64FC1); //The first row is the point on the plane, the second row is the normal of the plane
	int inNum=0;
	std::vector<int> inliers;
	int randInd;
	double e;
	while(iter<maxIteration)
	{
		//randomly select one point from horizontal points
		//t[12]=clock();
		randInd=rand()%cloud_hori->points.size();
		inliers.clear();
		for(int i=0; i<cloud->points.size(); i++)
		{
			if(i==randPoints[randInd] || isExist[i]==0)
			{
				continue;
			}
			//compute the distance between every point to the plane
			double x=cloud->points[i].x-cloud_hori->points[randInd].x;
			double y=cloud->points[i].y-cloud_hori->points[randInd].y;
			double z=cloud->points[i].z-cloud_hori->points[randInd].z;
			double xn=horiPoints.at<double>(randInd,0);
			double yn=horiPoints.at<double>(randInd,1);
			double zn=horiPoints.at<double>(randInd,2);
			double norm_n=sqrt(xn*xn+yn*yn+zn*zn);
			double dis=abs(x*xn+y*yn+z*zn)/norm_n;
			if (dis<disThreshold)
			{
				inliers.push_back(i);
			}
		}
		if (inliers.size()>fitNum)
		{
			inliers.push_back(randPoints[randInd]);
			//cout<<"inliers number: "<<inliers.size()<<endl;
			//e=1-(double(inliers.size())/cloud->points.size());
			//cout<<"e: "<<e<<endl;
			//maxIteration=log(1-p)/log(1-(1-e));
			if (inliers.size()>inNum)
			{
				inNum=inliers.size();
				best_inliers=inliers;
				cout<<"inliers number: "<<inliers.size()<<endl;
				e=1-(double(inliers.size())/cloud->points.size());
				cout<<"e: "<<e<<endl;
				maxIteration=log(1-p)/log(1-(1-e));
			}
		}
		//t[13]=clock();
		//printf("OnePointRANSAC one iteration time: %lf s\n",(double)(t[13]-t[12])/CLOCKS_PER_SEC);
		iter++;
	}
	cout<<"iteration times: "<<iter<<endl;
	cout<<"maxIteration: "<<maxIteration<<endl;
	//if do not find any plane
	if (inNum==0)
	{
		return planeModel;
	}
	//re-estimate the plane using all the inliers points
	cv::Mat pointsInlier(best_inliers.size(), 3,  CV_64FC1);
	for(int i=0; i<best_inliers.size(); i++)
	{
		pointsInlier.at<double>(i,0)=cloud->points[best_inliers[i]].x;
		pointsInlier.at<double>(i,1)=cloud->points[best_inliers[i]].y;
		pointsInlier.at<double>(i,2)=cloud->points[best_inliers[i]].z;
	}
	cv::Mat u,w,v;
	SVD::compute(pointsInlier, w, u, v);
	//cout<<"w: "<<endl<<w<<endl;
	//cout<<"u: "<<endl<<u<<endl;
	//cout<<"v: "<<endl<<v.row(2)<<endl;
	//cout<<"mean of v: "<<endl<<mean(v)[0]<<endl;
	planeModel.at<double>(1,0)=v.at<double>(2,0);
	planeModel.at<double>(1,1)=v.at<double>(2,1);
	planeModel.at<double>(1,2)=v.at<double>(2,2);

	double colMean;
	cv::Mat col(best_inliers.size(), 1,  CV_64FC1);
	for(int i=0; i<3; i++)
	{
		col=pointsInlier.col(i);
		colMean=mean(col)[0];
		planeModel.at<double>(0,i)=colMean;
	}
	return planeModel;
}

int main(int argc, char **argv)
{
	srand((unsigned)time(NULL));
	t[1]=clock();
	ros::init(argc, argv, "read_points");
	ros::NodeHandle n;
	//read 3D points from pcd file
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud1 (new pcl::PointCloud<pcl::PointXYZ>);
	cloud1 = ReadPCD("/home/victor/project_ws/src/ground_plane/data/pcd_night/top1.pcd");
	/*//write all the points in cloud to ply file
	ofstream myfile2("/home/victor/project_ws/src/ground_plane/src/allPoints_origin.ply", ios::out);
	if(!myfile2)
	{
		cout<<"error!"<<endl;
	}
	else
	{
		myfile2<<"ply"<<endl;
		myfile2<<"format ascii 1.0"<<endl;
		myfile2<<"element vertex "<<cloud1->points.size()<<endl;
		myfile2<<"property double x"<<endl;
		myfile2<<"property double y"<<endl;
		myfile2<<"property double z"<<endl;
		myfile2<<"property uchar red"<<endl;
		myfile2<<"property uchar green"<<endl;
		myfile2<<"property uchar blue"<<endl;
		myfile2<<"element face 0"<<endl;
		myfile2<<"end_header"<<endl;
		for(int i=0; i<cloud1->points.size(); i++)
		{
			myfile2<<cloud1->points[i].x<<" "<<cloud1->points[i].y<<" "<<cloud1->points[i].z<<" "<<"255 0 0"<<endl;
		}
		myfile2.close();
	}*/

	//delete the nan points
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
	cloud = DeleteNAN(cloud1);
	t[2]=clock();
	printf("%lf s\n",(double)(t[2]-t[1])/CLOCKS_PER_SEC);
	int pointNum=cloud->points.size();
	//int pointNum=30;
	//cout the 3d points
	/*cout << "loaded " << cloud->width * cloud->height << " data points from box_cloud.pcd with the following fields: " << endl;
	for(int i=0; i<pointNum; i++)
	{
		cout<<" "<<cloud->points[i].x<<" "<<cloud->points[i].y<<" "<<cloud->points[i].z<<endl;
	}*/

	//write all the points in cloud to ply file
	ofstream myfile1("/home/victor/project_ws/src/ground_plane/src/allPoints.ply", ios::out);
	if(!myfile1)
	{
		cout<<"error!"<<endl;
	}
	else
	{
		myfile1<<"ply"<<endl;
		myfile1<<"format ascii 1.0"<<endl;
		myfile1<<"element vertex "<<pointNum<<endl;
		myfile1<<"property double x"<<endl;
		myfile1<<"property double y"<<endl;
		myfile1<<"property double z"<<endl;
		myfile1<<"property uchar red"<<endl;
		myfile1<<"property uchar green"<<endl;
		myfile1<<"property uchar blue"<<endl;
		myfile1<<"element face 0"<<endl;
		myfile1<<"end_header"<<endl;
		for(int i=0; i<pointNum; i++)
		{
			myfile1<<cloud->points[i].x<<" "<<cloud->points[i].y<<" "<<cloud->points[i].z<<" "<<"255 0 0"<<endl;
		}
		myfile1.close();
	}

	t[3]=clock();
	printf("%lf s\n",(double)(t[3]-t[2])/CLOCKS_PER_SEC);
	//k nearest neighbor search
	int maxK=30, k=30;
  	int** indexKNN;
  	indexKNN=new int*[pointNum];
  	for(int i=0; i<pointNum; i++)
  	{
  		indexKNN[i]=new int[maxK];
	}
	double** distKNN;
	distKNN=new double*[pointNum];
	for(int i=0; i<pointNum; i++)
  	{
  		distKNN[i]=new double[maxK];
	}
	PointsKNN(cloud, pointNum, maxK, indexKNN, distKNN);

	t[4]=clock();
	printf("%lf s\n",(double)(t[4]-t[3])/CLOCKS_PER_SEC);

	/*cout<<endl;
	cout<<" "<<cloud->points[0].x<<" "<<cloud->points[0].y<<" "<<cloud->points[0].z<<endl;
	cout<<" "<<cloud->points[30000].x<<" "<<cloud->points[30000].y<<" "<<cloud->points[30000].z<<endl;
	cout<<" "<<cloud->points[10000].x<<" "<<cloud->points[10000].y<<" "<<cloud->points[10000].z<<endl;*/

	//normal estimation
	//cv::Mat normals(pointNum, 3, CV_64FC1);
	cv::Mat normals;
	std::vector<int> randPoints;
	double randRate=0.1;
	normals=NormalEstimation(cloud, pointNum, maxK, k, indexKNN, distKNN, randRate, randPoints);
	/*for(int i=0; i<normals.rows; i++)
	{
		if(normals.at<double>(i,0)==0 && normals.at<double>(i,1)==0 && normals.at<double>(i,2)==0)
		{
			cout<<endl;
			cout<<"i="<<i<<endl;
			cout<<normals.at<double>(i,0)<<" "<<normals.at<double>(i,1)<<" "<<normals.at<double>(i,2)<<endl;
		}
	}*/
	cout<<endl;
	cout<<"Number of normals: "<<normals.rows<<endl;
	cout<<"Number of points: "<<cloud->points.size()<<endl;

	t[5]=clock();
	printf("%lf s\n",(double)(t[5]-t[4])/CLOCKS_PER_SEC);

	//cout<<normals<<endl;

	//use kmeans to find the planes
	/*cv::Mat labels, centers;
	cv::Mat normals_32;
	normals.convertTo(normals_32, CV_32FC1);
	kmeans(normals_32, 3, labels, TermCriteria(TermCriteria::EPS+TermCriteria::COUNT, 1000, 0.1), 3, KMEANS_PP_CENTERS, centers);
	cout<<endl;
	cout<<labels<<endl;
	cout<<endl;
	cout<<centers<<endl;*/

	//find the points in horizontal plane
	//int normals_size=1;
	double ab, an, bn, cosr, interAngle;
	cv::Mat horiPoints;  //(1, 3, CV_64FC1); //the normals of the horizontal plane points
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_hori (new pcl::PointCloud<pcl::PointXYZ>);
	//std::vector<int> indn;  //the index in normals
	for(int i=0; i<normals.rows; i++)
	{
		//compute the angle between normal and yAxis direction (0,1,0)
		ab=normals.at<double>(i,1);
		an=1;
		bn=sqrt(normals.at<double>(i,0)*normals.at<double>(i,0)+normals.at<double>(i,1)*normals.at<double>(i,1)+normals.at<double>(i,2)*normals.at<double>(i,2));
		cosr=ab/(an*bn);
		//cout<<M_PI<<endl;
		//cout<<acos(cosr)*180/M_PI<<endl;
		interAngle=acos(cosr)*180/M_PI;
		/*if(isnan(interAngle))
		{
			cout<<"i="<<i<<endl;
			cout<<normals.at<double>(i,0)<<" "<<normals.at<double>(i,1)<<" "<<normals.at<double>(i,2)<<endl;
		}*/
		if(interAngle<=7 || interAngle>=173)
		{
			cv::Mat hpRow(1, 3, CV_64FC1);
			hpRow.at<double>(0,0)=normals.at<double>(i,0);
			hpRow.at<double>(0,1)=normals.at<double>(i,1);
			hpRow.at<double>(0,2)=normals.at<double>(i,2);
			cloud_hori->push_back(cloud->points[randPoints[i]]);
			//indn.push_back(i);
			//cout<<endl;
			//cout<<hpRow<<endl;
			horiPoints.push_back(hpRow);
		}
	}
	cout<<endl;
	cout<<horiPoints.rows<<endl;
	//cout<<horiPoints.row(0)<<endl;
	//cout<<horiPoints.row(1)<<endl;
	cout<<cloud_hori->points.size()<<endl;
	//cout<<indn.size()<<endl;
	//cout<<horiPoints<<endl;

	t[6]=clock();
	printf("%lf s\n",(double)(t[6]-t[5])/CLOCKS_PER_SEC);

	//write the horizontal plane into ply file
	ofstream myfile("/home/victor/project_ws/src/ground_plane/src/horiPoints.ply", ios::out);
	if(!myfile)
	{
		cout<<"error!"<<endl;
	}
	else
	{
		myfile<<"ply"<<endl;
		myfile<<"format ascii 1.0"<<endl;
		myfile<<"element vertex "<<horiPoints.rows<<endl;
		myfile<<"property double x"<<endl;
		myfile<<"property double y"<<endl;
		myfile<<"property double z"<<endl;
		myfile<<"property uchar red"<<endl;
		myfile<<"property uchar green"<<endl;
		myfile<<"property uchar blue"<<endl;
		myfile<<"element face 0"<<endl;
		myfile<<"end_header"<<endl;
		for(int i=0; i<cloud_hori->points.size(); i++)
		{
			myfile<<cloud_hori->points[i].x<<" "<<cloud_hori->points[i].y<<" "<<cloud_hori->points[i].z<<" "<<"255 0 0"<<endl;
		}
		myfile.close();
	}

	t[7]=clock();
	printf("%lf s\n",(double)(t[7]-t[6])/CLOCKS_PER_SEC);

	//use one point ransac to find horizontal plane
	std::vector<std::vector<int> > inliers;
	std::vector<cv::Mat> planeModel;
	int minNum=10000;  //the minimum number of points in the plane
	//std::vector<int> isExist(cloud_hori->points.size(), 1);
	std::vector<int> isExist(cloud->points.size(), 1);

	while(accumulate(isExist.begin(), isExist.end(), 0)>minNum)
	{
		cv::Mat plane;  //first row is the point on the plane, second row is the normal
		std::vector<int> inl;
		//t[10]=clock();
		cout<<"isExist number: "<<endl<<accumulate(isExist.begin(), isExist.end(), 0)<<endl;
		//t[11]=clock();
		//printf("%lf s\n",(double)(t[11]-t[10])/CLOCKS_PER_SEC);
		t[10]=clock();
		plane=OnePointRANSAC(cloud, cloud_hori, horiPoints, horiPoints.rows, 0.99, 0.05, minNum, inl, isExist, randPoints);
		t[11]=clock();
		printf("OnePointRANSAC time: %lf s\n",(double)(t[11]-t[10])/CLOCKS_PER_SEC);
		if (countNonZero(plane)!=0)
		{
			planeModel.push_back(plane);
			inliers.push_back(inl);
			for(int i=0; i<inl.size(); i++)
			{
				isExist[inl[i]]=0;
			}
		}
		else
		{
			break;
		}
	}
	cout<<"Before aggregate planeModel size: "<<endl<<planeModel.size()<<endl;
	//aggregate the planes which should be the same one
	std::vector<std::vector<int> > inliers_fi;
	std::vector<cv::Mat> planeModel_fi;
	std::vector<int> indPlane;
	double disPlane;
	for(int i=0; i<planeModel.size(); i++)
	{
		indPlane.push_back(i);
	}
	for(int i=0; i<planeModel.size(); i++)
	{
		for(int j=i+1; j<planeModel.size(); j++)
		{
			disPlane=sqrt((planeModel[i].at<double>(0,0)-planeModel[j].at<double>(0,0))*(planeModel[i].at<double>(0,0)-planeModel[j].at<double>(0,0))+(planeModel[i].at<double>(0,1)-planeModel[j].at<double>(0,1))*(planeModel[i].at<double>(0,1)-planeModel[j].at<double>(0,1))+(planeModel[i].at<double>(0,2)-planeModel[j].at<double>(0,2))*(planeModel[i].at<double>(0,2)-planeModel[j].at<double>(0,2)));
			cout<<"i="<<i<<" j="<<j<<" distance: "<<disPlane<<endl;
			if (disPlane<0.01)
			{
				indPlane[j]=i;
			}
		}
	}
	for(int i=0; i<indPlane.size(); i++)
	{
		if(indPlane[i]==-1)
		{
			continue;
		}
		std::vector<int> vecfi;
		vecfi.insert(vecfi.end(), inliers[i].begin(), inliers[i].end());
		cv::Mat pm(2, 3, CV_64FC1);
		pm.at<double>(0,0)=planeModel[i].at<double>(0,0);
		pm.at<double>(0,1)=planeModel[i].at<double>(0,1);
		pm.at<double>(0,2)=planeModel[i].at<double>(0,2);
		int n=1;
		//pm.at<double>(1,0)=planeModel[i].at<double>(1,0);
		//pm.at<double>(1,1)=planeModel[i].at<double>(1,1);
		//pm.at<double>(1,2)=planeModel[i].at<double>(1,2);
		for(int j=i+1; j<indPlane.size(); j++)
		{
			if (indPlane[j]==indPlane[i])
			{
				pm.at<double>(0,0)=pm.at<double>(0,0)+planeModel[j].at<double>(0,0);
				pm.at<double>(0,1)=pm.at<double>(0,1)+planeModel[j].at<double>(0,1);
				pm.at<double>(0,2)=pm.at<double>(0,2)+planeModel[j].at<double>(0,2);
				n++;
				vecfi.insert(vecfi.end(), inliers[j].begin(), inliers[j].end());
				indPlane[j]=-1;
			}
		}
		pm.at<double>(0,0)=pm.at<double>(0,0)/n;
		pm.at<double>(0,1)=pm.at<double>(0,1)/n;
		pm.at<double>(0,2)=pm.at<double>(0,2)/n;
		cv::Mat pointsIn(vecfi.size(), 3, CV_64FC1);
		for(int j=0; j<vecfi.size(); j++)
		{
			pointsIn.at<double>(j,0)=cloud->points[vecfi[j]].x;
			pointsIn.at<double>(j,1)=cloud->points[vecfi[j]].y;
			pointsIn.at<double>(j,2)=cloud->points[vecfi[j]].z;
		}
		cv::Mat u0,w0,v0;
		SVD::compute(pointsIn, w0, u0, v0);
		pm.at<double>(1,0)=v0.at<double>(2,0);
		pm.at<double>(1,1)=v0.at<double>(2,1);
		pm.at<double>(1,2)=v0.at<double>(2,2);
		planeModel_fi.push_back(pm);
		inliers_fi.push_back(vecfi);
		indPlane[i]=-1;
	}
	cout<<"After aggregate planeModel size: "<<endl<<planeModel_fi.size()<<endl;
	t[8]=clock();
	printf("%lf s\n",(double)(t[8]-t[7])/CLOCKS_PER_SEC);

	//write the horizontal plane into ply file
	for(int i=0; i<inliers_fi.size(); i++)
	{
		cout<<"inliers size: "<<endl<<inliers_fi[i].size()<<endl;
		char outPath[200];
		sprintf(outPath, "/home/victor/project_ws/src/ground_plane/src/ransacPlane%d.ply", i);
		ofstream myfile0(outPath, ios::out);
		if(!myfile0)
		{
			cout<<"error!"<<endl;
		}
		else
		{
			myfile0<<"ply"<<endl;
			myfile0<<"format ascii 1.0"<<endl;
			myfile0<<"element vertex "<<inliers_fi[i].size()<<endl;
			myfile0<<"property double x"<<endl;
			myfile0<<"property double y"<<endl;
			myfile0<<"property double z"<<endl;
			myfile0<<"property uchar red"<<endl;
			myfile0<<"property uchar green"<<endl;
			myfile0<<"property uchar blue"<<endl;
			myfile0<<"element face 0"<<endl;
			myfile0<<"end_header"<<endl;
			for(int j=0; j<inliers_fi[i].size(); j++)
			{
				myfile0<<cloud->points[inliers_fi[i][j]].x<<" "<<cloud->points[inliers_fi[i][j]].y<<" "<<cloud->points[inliers_fi[i][j]].z<<" "<<"255 0 0"<<endl;
			}
			myfile0.close();
		}
	}

	t[9]=clock();
	printf("%lf s\n",(double)(t[9]-t[8])/CLOCKS_PER_SEC);

	/*//use ransac in pcl to find horizontal plane
	std::vector<int> inliers;
	pcl::PointCloud<pcl::PointXYZ>::Ptr final (new pcl::PointCloud<pcl::PointXYZ>);
	pcl::SampleConsensusModelPlane<pcl::PointXYZ>::Ptr model_p (new pcl::SampleConsensusModelPlane<pcl::PointXYZ> (cloud_hori));
	pcl::RandomSampleConsensus<pcl::PointXYZ> ransac (model_p);
	ransac.setDistanceThreshold (.01);
	ransac.computeModel();
	ransac.getInliers(inliers);
	//copies all inliers of the model computed to another PointCloud
  	pcl::copyPointCloud<pcl::PointXYZ>(*cloud_hori, inliers, *final);
	//visualize the inliers
	boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer;
	viewer = simpleVis(final);
	while (!viewer->wasStopped ())
  	{
    	viewer->spinOnce (100);
    	boost::this_thread::sleep (boost::posix_time::microseconds (100000));
  	}*/
	return 0;
}


/*
 * CurvatureFilter.hpp
 *
 *  Created on: Sep 23, 2017
 *      Author: Peter Fankhauser
 *   Institute: ETH Zurich, ANYbotics
 */

#pragma once

#include <Eigen/Core>
#include <string>

#include <filters/filter_base.hpp>
#include <grid_map_core/GridMap.hpp>

namespace grid_map {

/*!
 * Compute the curvature (second derivative) of a layer in the map.
 */
class CurvatureFilter : public filters::FilterBase<GridMap> {
 public:
  /*!
   * Constructor
   */
  CurvatureFilter();

  /*!
   * Destructor.
   */
  ~CurvatureFilter() override;

  /*!
   * Configures the filter from parameters on the Parameter Server
   */
  bool configure() override;

  /*!
   * Compute the curvature of a layer in a map and
   * saves it as additional grid map layer.
   * @param mapIn grid map containing the layer for which the curvature is computed for.
   * @param mapOut grid map containing mapIn and the new layer for the curvature.
   */
  bool update(const GridMap& mapIn, GridMap& mapOut) override;

 private:
  //! Input layer name.
  std::string inputLayer_;

  //! Output layer name.
  std::string outputLayer_;
};

}  // namespace grid_map


// test.cpp : Defines the entry point for the console application.
//

#include "stdafx.h"

#include <boost/gil/extension/io/bmp_io.hpp>

#include <boost/gil/extension/opencv/convert.hpp>

using namespace std;
using namespace boost;
using namespace gil;
using namespace opencv;

// Models a Unary Function
template <typename P>   // Models PixelValueConcept
struct mandelbrot_fn {
    typedef point2<ptrdiff_t>    point_t;

    typedef mandelbrot_fn        const_t;
    typedef P                    value_type;
    typedef value_type           reference;
    typedef value_type           const_reference;
    typedef point_t              argument_type;
    typedef reference            result_type;
    BOOST_STATIC_CONSTANT(bool, is_mutable=false);

    value_type                    _in_color,_out_color;
    point_t                       _img_size;
    static const int MAX_ITER=1000;        // max number of iterations

    mandelbrot_fn() {}
    mandelbrot_fn(const point_t& sz, const value_type& in_color, const value_type& out_color) : _in_color(in_color), _out_color(out_color), _img_size(sz) {}

    result_type operator()(const point_t& p) const {
        // normalize the coords to (-2..1, -1.5..1.5)
        // (actually make y -1.0..2 so it is asymmetric, so we can verify some view factory methods)
        double t=get_num_iter(point2<double>(p.x/(double)_img_size.x*3-2, p.y/(double)_img_size.y*3-1.0f));//1.5f));
        t=pow(t,0.2);

        value_type ret;
        for (int k=0; k<num_channels<P>::value; ++k)
            ret[k]=(typename channel_type<P>::type)(_in_color[k]*t + _out_color[k]*(1-t));
        return ret;
    }

private:
    double get_num_iter(const point2<double>& p) const {
        point2<double> Z(0,0);
        for (int i=0; i<MAX_ITER; ++i) {
            Z = point2<double>(Z.x*Z.x - Z.y*Z.y + p.x, 2*Z.x*Z.y + p.y);
            if (Z.x*Z.x + Z.y*Z.y > 4)
                return i/(double)MAX_ITER;
        }
        return 0;
    }
};

struct set_to_max
{
   template <typename CHANNEL > void operator()(CHANNEL& c) const
   {
      c = channel_traits< CHANNEL >::max_value();
   }
};

struct set_to_min
{
   template <typename CHANNEL > void operator()(CHANNEL& c) const
   {
      c = channel_traits< CHANNEL >::min_value();
   }
};

template< class PIXEL
        , class VIEW >
inline
PIXEL min_channel_values( const VIEW& view )
{
   PIXEL min;

   // initialize the min pixel with the max values
   static_for_each( min, set_to_max() );

   for( int y=0; y < view.height(); ++y )
   {
      VIEW::x_iterator x_it = view.row_begin( y );

      for( int x = 0; x < view.width(); ++x )
      {
         typename PIXEL::const_reference p = x_it[x];

         for( int i = 0; i < num_channels<PIXEL>::type::value; ++i )
         {
            if( dynamic_at_c( p, i ) < dynamic_at_c( min, i ))
            {
               dynamic_at_c( min, i ) = dynamic_at_c( p, i );
            }
         }
      }
   }

   return min;
}

template <typename T>
struct add_vector
{
   typedef std::vector<T> type;
};

template< class PIXEL
        , class VIEW >
inline
PIXEL min_channel_values_( const VIEW&  view
                        , const size_t percent = 1 )
{
   typedef mpl::transform1< color_space_type<VIEW>::type, add_vector<mpl::_1> >::type channels_t;

   typedef kth_semantic_element_type< PIXEL, 0 >::type channel_t; //channel_0 = semantic_at_c<0>( PIXEL() );
   channel_t max = channel_traits<channel_t>::max_value();

   return PIXEL();
}

template< class PIXEL
        , class VIEW >
inline
PIXEL max_channel_values( const VIEW& view )
{
   PIXEL max;

   // initialize the max pixel with the min values
   static_for_each( max, set_to_min() );

   // find the max values
   for( VIEW::iterator it = view.begin()
      ; it != view.end()
      ; ++it )
   {
      for( int i = 0; i < num_channels<PIXEL>::type::value; ++i )
      {
         if( dynamic_at_c( *it, i ) > dynamic_at_c( max, i ))
         {
            dynamic_at_c( max, i ) = dynamic_at_c( *it, i );
         }
      }
   }

   return max;
}

template< class PIXEL
        , class VIEW >
inline
boost::fusion::vector<PIXEL, PIXEL>
minmax_channel_values( const VIEW& view )
{
   PIXEL min, max;

   static_for_each( min, set_to_max() );
   static_for_each( max, set_to_min() );

   // find the max values
   for( VIEW::iterator it = view.begin()
      ; it != view.end()
      ; ++it )
   {
      for( int i = 0; i < num_channels<PIXEL>::type::value; ++i )
      {
         if( dynamic_at_c( *it, i ) < dynamic_at_c( min, i ))
         {
            dynamic_at_c( min, i ) = dynamic_at_c( *it, i );
         }

         if( dynamic_at_c( *it, i ) > dynamic_at_c( max, i ))
         {
            dynamic_at_c( max, i ) = dynamic_at_c( *it, i );
         }
      }
   }

   return boost::fusion::vector<PIXEL, PIXEL>( min, max );
}

struct calc_diff
{
   template <typename CHANNEL > void operator()( CHANNEL&       diff
                                               , const CHANNEL& min
                                               , const CHANNEL& max ) const
   {
      diff = max - min;
   }
};

typedef channel_type<rgb16_pixel_t>::type channel_t;

template< class SRC
        , class DST
        >
void down_sample( const SRC& src_view
                , const DST& dst_view  )
{
   assert( src_view.dimensions() == dst_view.dimensions() );

   // @todo Take care of signed images. Bransform them into unsigned images
   // by adding half the value range to the channels.

   typedef SRC::value_type src_pixel_t;
   typedef DST::value_type dst_pixel_t;
   typedef channel_type<dst_pixel_t>::type dst_channel_t;

   boost::fusion::vector<src_pixel_t,src_pixel_t> minmax = minmax_channel_values<src_pixel_t>( src_view );

   const src_pixel_t& min = at_c<0>( minmax );
   const src_pixel_t& max = at_c<1>( minmax );

   // calculate the diff
   src_pixel_t diff;
   static_for_each( diff, min, max, calc_diff() );

   // sample down
   dst_channel_t dst_max = channel_traits< dst_channel_t >::max_value();

   for( int y=0; y < src_view.height(); ++y )
   {
      SRC::x_iterator src_it = src_view.row_begin( y );
      DST::x_iterator dst_it = dst_view.row_begin( y );

      for( int x = 0; x < src_view.width(); ++x )
      {
         typename src_pixel_t::const_reference src = src_it[x];
         typename dst_pixel_t::reference       dst = dst_it[x];

         for( int i = 0; i < num_channels<src_pixel_t>::type::value; ++i )
         {
            if( dynamic_at_c( diff, i ) == 0 )
            {
               dynamic_at_c( dst, i ) = 0;
            }
            else
            {
               dynamic_at_c( dst, i ) = static_cast<dst_channel_t>( dst_max * ( static_cast<float>( dynamic_at_c( src, i )
                                                                                - dynamic_at_c( min, i ))
                                                                              / static_cast<float>( dynamic_at_c( diff, i ))));
            } // else

         } //for

      } //for

   } // for
}

// channel_wise calculation.

// @todo need better name
template< typename DST_MAX >
struct foo
{
   foo( const DST_MAX& dst_max )
   : _dst_max( dst_max ){}

   template < typename DST_CHANNEL
            , typename SRC_CHANNEL >
   void operator()( DST_CHANNEL&  dst
                  , SRC_CHANNEL&  src
                  , SRC_CHANNEL&  min
                  , SRC_CHANNEL&  diff )
   const
   {
      if( diff == 0 )
      {
         dst = 0;

         return;
      }

      float d = ( static_cast<float>( _dst_max )
                * ( ( static_cast<float>( src ) - static_cast<float>( min ))
                  / static_cast<float>( diff ))));

      dst_channel = static_cast<DST_CHANNEL>( dst );
   }

   DST_MAX _dst_max;
};

// pixel_wise calculation.

// @todo need better name
template< typename SRC_VIEW
        , typename SRC_PIXEL
        , typename DST_VIEW
        , typename DST_MAX
        >
struct do_it
{
   do_it( const DST_VIEW&  dst_view
        , const DST_MAX&   dst_max
        , const SRC_PIXEL& min
        , const SRC_PIXEL& diff      )
   : _dst_view( dst_view )
   , _min( min )
   , _diff( diff )
   , _op( dst_max )
   {
      _dst_it = dst_view.begin();
   }

   void operator()( SRC_PIXEL& src )
   {
      static_for_each( *_dst_it
                     , src
                     , _min
                     , _diff
                     , _op    );
      ++_dst_it;
   }

   typename DST_VIEW::iterator _dst_it;

   foo<DST_MAX> _op;

   DST_VIEW _dst_view;

   SRC_PIXEL _min;
   SRC_PIXEL _diff;
};

int main(int argc, char* argv[])
{
   {
      rgb8_image_t dst( 10, 10 );

      min_channel_values<rgb8_image_t::value_type>( view( dst ));
   }

   {
/*
      bits8 max = 255;
      rgb16_pixel_t min;
      rgb16_pixel_t diff;

      rgb16_image_t src( 640, 480 );
      rgb8_image_t dst( 640, 480 );

      do_it< rgb16_view_t
           , rgb16_pixel_t
           , rgb8_view_t
           , bits8
           > d( view( dst ), max, min, diff );

      for_each_pixel( view( src ), d );

      bmp_write_view( ".\\red.bmp", view( dst ));
*/
   }

   {
      typedef rgb16_image_t src_image_t;
      typedef src_image_t::view_t src_view_t;
      typedef src_view_t::value_type src_pixel_t;
      typedef channel_type<src_pixel_t>::type src_channel_t;
      src_channel_t max_value = channel_traits<src_channel_t>::max_value();

      typedef mandelbrot_fn<src_pixel_t> deref_t;
      typedef deref_t::point_t           point_t;

      typedef virtual_2d_locator<deref_t,false> locator_t;
      typedef image_view<locator_t> my_virt_view_t;

      function_requires<PixelLocatorConcept<locator_t> >();
      gil_function_requires<StepIteratorConcept<locator_t::x_iterator> >();

      point_t dims( 640, 480 );
      my_virt_view_t mandel( dims
                           , locator_t( point_t( 0, 0 )
                           , point_t( 1 , 1)
                           , deref_t( dims
                                    , src_pixel_t( max_value,         0, 0 )
                                    , src_pixel_t(         0, max_value, 0 ))));

      src_image_t img( dims );
      copy_pixels( mandel, view( img ));

      // @todo How to compute the unsigned xxx8_image_t from src_image_t?
      rgb8_image_t eight_bit_img( dims );

      down_sample( view( img )
                 , view( eight_bit_img ));

      bmp_write_view( ".\\mandelbrot.bmp", view( eight_bit_img ));
   }

  	return 0;
}


/*  This file is part of libDAI - http://www.libdai.org/
 *
 *  Copyright (c) 2006-2011, The libDAI authors. All rights reserved.
 *
 *  Use of this source code is governed by a BSD-style license that can be found in the LICENSE file.
 */

#include <dai/util.h>
#include <dai/prob.h>
#include <strstream>

using namespace dai;

const Real tol = 1e-8;

#define BOOST_TEST_MODULE ProbTest

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

BOOST_AUTO_TEST_CASE( ConstructorsTest ) {
    // check constructors
    Prob x1;
    BOOST_CHECK_EQUAL( x1.size(), 0U );
    BOOST_CHECK( x1.p() == Prob::container_type() );

    Prob x2( 3 );
    BOOST_CHECK_EQUAL( x2.size(), 3 );
    BOOST_CHECK_CLOSE( x2[0], (Real)(1.0 / 3.0), tol );
    BOOST_CHECK_CLOSE( x2[1], (Real)(1.0 / 3.0), tol );
    BOOST_CHECK_CLOSE( x2[2], (Real)(1.0 / 3.0), tol );

    Prob x3( 4, (Real)1.0 );
    BOOST_CHECK_EQUAL( x3.size(), 4 );
    BOOST_CHECK( x3.p() == Prob::container_type( 4, (Real)1.0 ) );
    BOOST_CHECK_EQUAL( x3[0], (Real)1.0 );
    BOOST_CHECK_EQUAL( x3[1], (Real)1.0 );
    BOOST_CHECK_EQUAL( x3[2], (Real)1.0 );
    BOOST_CHECK_EQUAL( x3[3], (Real)1.0 );
    x3.set( 0, 0.5 );
    x3.set( 1, 1.0 );
    x3.set( 2, 2.0 );
    x3.set( 3, 4.0 );

    std::vector<Real> v;
    v.push_back( 0.5 );
    v.push_back( 1.0 );
    v.push_back( 2.0 );
    v.push_back( 4.0 );
    Prob x4( v.begin(), v.end(), 0 );
    BOOST_CHECK_EQUAL( x4.size(), 4 );
    BOOST_CHECK( x4.p() == x3.p() );
    BOOST_CHECK( x4 == x3 );
    BOOST_CHECK_EQUAL( x4[0], (Real)0.5 );
    BOOST_CHECK_EQUAL( x4[1], (Real)1.0 );
    BOOST_CHECK_EQUAL( x4[2], (Real)2.0 );
    BOOST_CHECK_EQUAL( x4[3], (Real)4.0 );

    Prob x5( v.begin(), v.end(), v.size() );
    BOOST_CHECK_EQUAL( x5.size(), 4 );
    BOOST_CHECK( x5.p() == x3.p() );
    BOOST_CHECK( x5 == x3 );
    BOOST_CHECK_EQUAL( x5[0], (Real)0.5 );
    BOOST_CHECK_EQUAL( x5[1], (Real)1.0 );
    BOOST_CHECK_EQUAL( x5[2], (Real)2.0 );
    BOOST_CHECK_EQUAL( x5[3], (Real)4.0 );

    std::vector<int> y( 3, 2 );
    Prob x6( y );
    BOOST_CHECK_EQUAL( x6.size(), 3 );
    BOOST_CHECK_EQUAL( x6[0], (Real)2.0 );
    BOOST_CHECK_EQUAL( x6[1], (Real)2.0 );
    BOOST_CHECK_EQUAL( x6[2], (Real)2.0 );

    Prob x7( x6 );
    BOOST_CHECK( x7 == x6 );

    Prob x8 = x6;
    BOOST_CHECK( x8 == x6 );

    x7.resize( 5 );
    BOOST_CHECK_EQUAL( x7.size(), 5 );
    BOOST_CHECK_EQUAL( x7[0], (Real)2.0 );
    BOOST_CHECK_EQUAL( x7[1], (Real)2.0 );
    BOOST_CHECK_EQUAL( x7[2], (Real)2.0 );
    BOOST_CHECK_EQUAL( x7[3], (Real)0.0 );
    BOOST_CHECK_EQUAL( x7[4], (Real)0.0 );

    x8.resize( 1 );
    BOOST_CHECK_EQUAL( x8.size(), 1 );
    BOOST_CHECK_EQUAL( x8[0], (Real)2.0 );
}

#ifndef DAI_SPARSE
BOOST_AUTO_TEST_CASE( IteratorTest ) {
    Prob x( 5, 0.0 );
    size_t i;
    for( i = 0; i < x.size(); i++ )
        x.set( i, (Real)i );

    i = 0;
    for( Prob::const_iterator cit = x.begin(); cit != x.end(); cit++, i++ )
        BOOST_CHECK_EQUAL( *cit, (Real)i );

    i = 0;
    for( Prob::iterator it = x.begin(); it != x.end(); it++, i++ )
        *it = (Real)(4 - i);

    i = 0;
    for( Prob::const_iterator it = x.begin(); it != x.end(); it++, i++ )
        BOOST_CHECK_EQUAL( *it, (Real)(4 - i) );

    i = 0;
    for( Prob::const_reverse_iterator crit = x.rbegin(); crit != static_cast<Prob::const_reverse_iterator>(x.rend()); crit++, i++ )
        BOOST_CHECK_EQUAL( *crit, (Real)i );

    i = 0;
    for( Prob::reverse_iterator rit = x.rbegin(); rit != x.rend(); rit++, i++ )
        *rit = (Real)(2 * i);

    i = 0;
    for( Prob::const_reverse_iterator crit = x.rbegin(); crit != static_cast<Prob::const_reverse_iterator>(x.rend()); crit++, i++ )
        BOOST_CHECK_EQUAL( *crit, (Real)2 * i );
}
#endif

BOOST_AUTO_TEST_CASE( QueriesTest ) {
    Prob x( 5, 0.0 );
    for( size_t i = 0; i < x.size(); i++ )
        x.set( i, 2.0 - i );

    // test accumulate, min, max, sum, sumAbs, maxAbs
    BOOST_CHECK_CLOSE( x.sum(), (Real)0.0, tol );
    BOOST_CHECK_CLOSE( x.accumulateSum( 0.0, fo_id<Real>() ), (Real)0.0, tol );
    BOOST_CHECK_CLOSE( x.accumulateSum( 1.0, fo_id<Real>() ), (Real)1.0, tol );
    BOOST_CHECK_CLOSE( x.accumulateSum( -1.0, fo_id<Real>() ), (Real)-1.0, tol );
    BOOST_CHECK_CLOSE( x.max(), (Real)2.0, tol );
    BOOST_CHECK_CLOSE( x.accumulateMax( -INFINITY, fo_id<Real>(), false ), (Real)2.0, tol );
    BOOST_CHECK_CLOSE( x.accumulateMax( 3.0, fo_id<Real>(), false ), (Real)3.0, tol );
    BOOST_CHECK_CLOSE( x.accumulateMax( -5.0, fo_id<Real>(), false ), (Real)2.0, tol );
    BOOST_CHECK_CLOSE( x.min(), (Real)-2.0, tol );
    BOOST_CHECK_CLOSE( x.accumulateMax( INFINITY, fo_id<Real>(), true ), (Real)-2.0, tol );
    BOOST_CHECK_CLOSE( x.accumulateMax( -3.0, fo_id<Real>(), true ), (Real)-3.0, tol );
    BOOST_CHECK_CLOSE( x.accumulateMax( 5.0, fo_id<Real>(), true ), (Real)-2.0, tol );
    BOOST_CHECK_CLOSE( x.sumAbs(), (Real)6.0, tol );
    BOOST_CHECK_CLOSE( x.accumulateSum( 0.0, fo_abs<Real>() ), (Real)6.0, tol );
    BOOST_CHECK_CLOSE( x.accumulateSum( 1.0, fo_abs<Real>() ), (Real)7.0, tol );
    BOOST_CHECK_CLOSE( x.accumulateSum( -1.0, fo_abs<Real>() ), (Real)7.0, tol );
    BOOST_CHECK_CLOSE( x.maxAbs(), (Real)2.0, tol );
    BOOST_CHECK_CLOSE( x.accumulateMax( 0.0, fo_abs<Real>(), false ), (Real)2.0, tol );
    BOOST_CHECK_CLOSE( x.accumulateMax( 1.0, fo_abs<Real>(), false ), (Real)2.0, tol );
    BOOST_CHECK_CLOSE( x.accumulateMax( -1.0, fo_abs<Real>(), false ), (Real)2.0, tol );
    BOOST_CHECK_CLOSE( x.accumulateMax( 3.0, fo_abs<Real>(), false ), (Real)3.0, tol );
    BOOST_CHECK_CLOSE( x.accumulateMax( -3.0, fo_abs<Real>(), false ), (Real)3.0, tol );
    x.set( 1, 1.0 );
    BOOST_CHECK_CLOSE( x.maxAbs(), (Real)2.0, tol );
    BOOST_CHECK_CLOSE( x.accumulateMax( 0.0, fo_abs<Real>(), false ), (Real)2.0, tol );
    BOOST_CHECK_CLOSE( x.accumulateMax( 1.0, fo_abs<Real>(), false ), (Real)2.0, tol );
    BOOST_CHECK_CLOSE( x.accumulateMax( -1.0, fo_abs<Real>(), false ), (Real)2.0, tol );
    BOOST_CHECK_CLOSE( x.accumulateMax( 3.0, fo_abs<Real>(), false ), (Real)3.0, tol );
    BOOST_CHECK_CLOSE( x.accumulateMax( -3.0, fo_abs<Real>(), false ), (Real)3.0, tol );
    for( size_t i = 0; i < x.size(); i++ )
        x.set( i, i ? (1.0 / i) : 0.0 );
    BOOST_CHECK_CLOSE( x.accumulateSum( 0.0, fo_inv0<Real>() ), (Real)10.0, tol );
    x /= x.sum();

    // test entropy
    BOOST_CHECK( x.entropy() < Prob(5).entropy() );
    for( size_t i = 1; i < 100; i++ )
        BOOST_CHECK_CLOSE( Prob(i).entropy(), dai::log((Real)i), tol );

    // test hasNaNs and hasNegatives
    BOOST_CHECK( !Prob( 3, 0.0 ).hasNaNs() );
    Real c = 0.0;
    BOOST_CHECK( Prob( 3, c / c ).hasNaNs() );
    BOOST_CHECK( !Prob( 3, 0.0 ).hasNegatives() );
    BOOST_CHECK( !Prob( 3, 1.0 ).hasNegatives() );
    BOOST_CHECK( Prob( 3, -1.0 ).hasNegatives() );
    x.set( 0, 0.0 ); x.set( 1, 0.0 ); x.set( 2, -1.0 ); x.set( 3, 1.0 ); x.set( 4, 100.0 );
    BOOST_CHECK( x.hasNegatives() );
    x.set( 2, -INFINITY );
    BOOST_CHECK( x.hasNegatives() );
    x.set( 2, INFINITY );
    BOOST_CHECK( !x.hasNegatives() );
    x.set( 2, -1.0 );

    // test argmax
    BOOST_CHECK( x.argmax() == std::make_pair( (size_t)4, (Real)100.0 ) );
    x.set( 4, 0.5 );
    BOOST_CHECK( x.argmax() == std::make_pair( (size_t)3, (Real)1.0 ) );
    x.set( 3, -2.0 );
    BOOST_CHECK( x.argmax() == std::make_pair( (size_t)4, (Real)0.5 ) );
    x.set( 4, -1.0 );
    BOOST_CHECK( x.argmax() == std::make_pair( (size_t)0, (Real)0.0 ) );
    x.set( 0, -2.0 );
    BOOST_CHECK( x.argmax() == std::make_pair( (size_t)1, (Real)0.0 ) );
    x.set( 1, -3.0 );
    BOOST_CHECK( x.argmax() == std::make_pair( (size_t)2, (Real)-1.0 ) );
    x.set( 2, -2.0 );
    BOOST_CHECK( x.argmax() == std::make_pair( (size_t)4, (Real)-1.0 ) );

    // test draw
    for( size_t i = 0; i < x.size(); i++ )
        x.set( i, i ? (1.0 / i) : 0.0 );
    for( size_t repeat = 0; repeat < 10000; repeat++ ) {
        BOOST_CHECK( x.draw() < x.size() );
        BOOST_CHECK( x.draw() != 0 );
    }
    x.set( 2, 0.0 );
    for( size_t repeat = 0; repeat < 10000; repeat++ ) {
        BOOST_CHECK( x.draw() < x.size() );
        BOOST_CHECK( x.draw() != 0 );
        BOOST_CHECK( x.draw() != 2 );
    }
    x.set( 4, 0.0 );
    for( size_t repeat = 0; repeat < 10000; repeat++ ) {
        BOOST_CHECK( x.draw() < x.size() );
        BOOST_CHECK( x.draw() != 0 );
        BOOST_CHECK( x.draw() != 2 );
        BOOST_CHECK( x.draw() != 4 );
    }
    x.set( 1, 0.0 );
    for( size_t repeat = 0; repeat < 10000; repeat++ )
        BOOST_CHECK( x.draw() == 3 );

    // test <, ==
    Prob a(3, 1.0), b(3, 1.0);
    BOOST_CHECK( !(a < b) );
    BOOST_CHECK( !(b < a) );
    BOOST_CHECK( a == b );
    a.set( 0, 0.0 );
    BOOST_CHECK( a < b );
    BOOST_CHECK( !(b < a) );
    BOOST_CHECK( !(a == b) );
    b.set( 2, 0.0 );
    BOOST_CHECK( a < b );
    BOOST_CHECK( !(b < a) );
    BOOST_CHECK( !(a == b) );
    b.set( 0, 0.0 );
    BOOST_CHECK( !(a < b) );
    BOOST_CHECK( b < a );
    BOOST_CHECK( !(a == b) );
    a.set( 1, 0.0 );
    BOOST_CHECK( a < b );
    BOOST_CHECK( !(b < a) );
    BOOST_CHECK( !(a == b) );
    b.set( 1, 0.0 );
    BOOST_CHECK( !(a < b) );
    BOOST_CHECK( b < a );
    BOOST_CHECK( !(a == b) );
    a.set( 2, 0.0 );
    BOOST_CHECK( !(a < b) );
    BOOST_CHECK( !(b < a) );
    BOOST_CHECK( a == b );
}

BOOST_AUTO_TEST_CASE( UnaryTransformationsTest ) {
    Prob x( 3 );
    x.set( 0, -2.0 );
    x.set( 1, 0.0 );
    x.set( 2, 2.0 );

    Prob y = -x;
    Prob z = x.pwUnaryTr( std::negate<Real>() );
    BOOST_CHECK_CLOSE( y[0], (Real)2.0, tol );
    BOOST_CHECK_CLOSE( y[1], (Real)0.0, tol );
    BOOST_CHECK_CLOSE( y[2], (Real)-2.0, tol );
    BOOST_CHECK( y == z );

    y = x.abs();
    z = x.pwUnaryTr( fo_abs<Real>() );
    BOOST_CHECK_CLOSE( y[0], (Real)2.0, tol );
    BOOST_CHECK_CLOSE( y[1], (Real)0.0, tol );
    BOOST_CHECK_CLOSE( y[2], (Real)2.0, tol );
    BOOST_CHECK( y == z );

    y = x.exp();
    z = x.pwUnaryTr( fo_exp<Real>() );
    BOOST_CHECK_CLOSE( y[0], dai::exp((Real)-2.0), tol );
    BOOST_CHECK_CLOSE( y[1], (Real)1.0, tol );
    BOOST_CHECK_CLOSE( y[2], (Real)1.0 / y[0], tol );
    BOOST_CHECK( y == z );

    y = x.log(false);
    z = x.pwUnaryTr( fo_log<Real>() );
    BOOST_CHECK( dai::isnan( y[0] ) );
    BOOST_CHECK_EQUAL( y[1], -INFINITY );
    BOOST_CHECK_CLOSE( y[2], dai::log((Real)2.0), tol );
    BOOST_CHECK( !(y == z) );
    y.set( 0, 0.0 );
    z.set( 0, 0.0 );
    BOOST_CHECK( y == z );

    y = x.log(true);
    z = x.pwUnaryTr( fo_log0<Real>() );
    BOOST_CHECK( dai::isnan( y[0] ) );
    BOOST_CHECK_CLOSE( y[1], (Real)0.0, tol );
    BOOST_CHECK_CLOSE( y[2], dai::log((Real)2.0), tol );
    BOOST_CHECK( !(y == z) );
    y.set( 0, 0.0 );
    z.set( 0, 0.0 );
    BOOST_CHECK( y == z );

    y = x.inverse(false);
    z = x.pwUnaryTr( fo_inv<Real>() );
    BOOST_CHECK_CLOSE( y[0], (Real)-0.5, tol );
    BOOST_CHECK_EQUAL( y[1], INFINITY );
    BOOST_CHECK_CLOSE( y[2], (Real)0.5, tol );
    BOOST_CHECK( y == z );

    y = x.inverse(true);
    z = x.pwUnaryTr( fo_inv0<Real>() );
    BOOST_CHECK_CLOSE( y[0], (Real)-0.5, tol );
    BOOST_CHECK_CLOSE( y[1], (Real)0.0, tol );
    BOOST_CHECK_CLOSE( y[2], (Real)0.5, tol );
    BOOST_CHECK( y == z );

    x.set( 0, 2.0 );
    y = x.normalized();
    BOOST_CHECK_CLOSE( y[0], (Real)0.5, tol );
    BOOST_CHECK_CLOSE( y[1], (Real)0.0, tol );
    BOOST_CHECK_CLOSE( y[2], (Real)0.5, tol );

    y = x.normalized( NORMPROB );
    BOOST_CHECK_CLOSE( y[0], (Real)0.5, tol );
    BOOST_CHECK_CLOSE( y[1], (Real)0.0, tol );
    BOOST_CHECK_CLOSE( y[2], (Real)0.5, tol );

    x.set( 0, -2.0 );
    y = x.normalized( NORMLINF );
    BOOST_CHECK_CLOSE( y[0], (Real)-1.0, tol );
    BOOST_CHECK_CLOSE( y[1], (Real)0.0, tol );
    BOOST_CHECK_CLOSE( y[2], (Real)1.0, tol );
}

BOOST_AUTO_TEST_CASE( UnaryOperationsTest ) {
    Prob xorg(3);
    xorg.set( 0, 2.0 );
    xorg.set( 1, 0.0 );
    xorg.set( 2, 1.0 );
    Prob y(3);

    Prob x = xorg;
    BOOST_CHECK( x.setUniform() == Prob(3) );
    BOOST_CHECK( x == Prob(3) );

    y.set( 0, dai::exp(2.0) );
    y.set( 1, 1.0 );
    y.set( 2, dai::exp(1.0) );
    x = xorg;
    BOOST_CHECK_SMALL( dist( x.takeExp(), y, DISTL1 ), tol );
    BOOST_CHECK_SMALL( dist( x, y, DISTL1 ), tol );
    x = xorg;
    BOOST_CHECK_SMALL( dist( x.pwUnaryOp( fo_exp<Real>() ), y, DISTL1 ), tol );
    BOOST_CHECK_SMALL( dist( x, y, DISTL1 ), tol );

    y.set( 0, dai::log(2.0) );
    y.set( 1, -INFINITY );
    y.set( 2, 0.0 );
    x = xorg;
    Prob z = x.takeLog();
    BOOST_CHECK_CLOSE( z[0], y[0], tol );
//    BOOST_CHECK_CLOSE( z[1], y[1], tol );
    BOOST_CHECK_CLOSE( z[2], y[2], tol );
    BOOST_CHECK( x == z );
    x = xorg;
    z = x.takeLog(false);
    BOOST_CHECK_CLOSE( z[0], y[0], tol );
//    BOOST_CHECK_CLOSE( z[1], y[1], tol );
    BOOST_CHECK_CLOSE( z[2], y[2], tol );
    BOOST_CHECK( x == z );
    x = xorg;
    z = x.pwUnaryOp( fo_log<Real>() );
    BOOST_CHECK_CLOSE( z[0], y[0], tol );
//    BOOST_CHECK_CLOSE( z[1], y[1], tol );
    BOOST_CHECK_CLOSE( z[2], y[2], tol );
    BOOST_CHECK( x == z );

    y.set( 1, 0.0 );
    x = xorg;
    BOOST_CHECK_SMALL( dist( x.takeLog(true), y, DISTL1 ), tol );
    BOOST_CHECK_SMALL( dist( x, y, DISTL1 ), tol );
    x = xorg;
    BOOST_CHECK_SMALL( dist( x.pwUnaryOp( fo_log0<Real>() ), y, DISTL1 ), tol );
    BOOST_CHECK_SMALL( dist( x, y, DISTL1 ), tol );

    y.set( 0, 2.0 / 3.0 );
    y.set( 1, 0.0 / 3.0 );
    y.set( 2, 1.0 / 3.0 );
    x = xorg;
    BOOST_CHECK_CLOSE( x.normalize(), (Real)3.0, tol );
    BOOST_CHECK_SMALL( dist( x, y, DISTL1 ), tol );

    x = xorg;
    BOOST_CHECK_CLOSE( x.normalize( NORMPROB ), (Real)3.0, tol );
    BOOST_CHECK_SMALL( dist( x, y, DISTL1 ), tol );

    y.set( 0, 2.0 / 2.0 );
    y.set( 1, 0.0 / 2.0 );
    y.set( 2, 1.0 / 2.0 );
    x = xorg;
    BOOST_CHECK_CLOSE( x.normalize( NORMLINF ), (Real)2.0, tol );
    BOOST_CHECK_SMALL( dist( x, y, DISTL1 ), tol );

    xorg.set( 0, -2.0 );
    y.set( 0, 2.0 );
    y.set( 1, 0.0 );
    y.set( 2, 1.0 );
    x = xorg;
    BOOST_CHECK( x.takeAbs() == y );
    BOOST_CHECK( x == y );

    for( size_t repeat = 0; repeat < 10000; repeat++ ) {
        x.randomize();
        for( size_t i = 0; i < x.size(); i++ ) {
            BOOST_CHECK( x[i] < (Real)1.0 );
            BOOST_CHECK( x[i] >= (Real)0.0 );
        }
    }
}

BOOST_AUTO_TEST_CASE( ScalarTransformationsTest ) {
    Prob x(3);
    x.set( 0, 2.0 );
    x.set( 1, 0.0 );
    x.set( 2, 1.0 );
    Prob y(3);

    y.set( 0, 3.0 ); y.set( 1, 1.0 ); y.set( 2, 2.0 );
    BOOST_CHECK_SMALL( dist( (x + 1.0), y, DISTL1 ), tol );
    y.set( 0, 0.0 ); y.set( 1, -2.0 ); y.set( 2, -1.0 );
    BOOST_CHECK_SMALL( dist( (x + (-2.0)), y, DISTL1 ), tol );

    y.set( 0, 1.0 ); y.set( 1, -1.0 ); y.set( 2, 0.0 );
    BOOST_CHECK_SMALL( dist( (x - 1.0), y, DISTL1 ), tol );
    y.set( 0, 4.0 ); y.set( 1, 2.0 ); y.set( 2, 3.0 );
    BOOST_CHECK_SMALL( dist( (x - (-2.0)), y, DISTL1 ), tol );

    BOOST_CHECK_SMALL( dist( (x * 1.0), x, DISTL1 ), tol );
    y.set( 0, 4.0 ); y.set( 1, 0.0 ); y.set( 2, 2.0 );
    BOOST_CHECK_SMALL( dist( (x * 2.0), y, DISTL1 ), tol );
    y.set( 0, -1.0 ); y.set( 1, 0.0 ); y.set( 2, -0.5 );
    BOOST_CHECK_SMALL( dist( (x * -0.5), y, DISTL1 ), tol );

    BOOST_CHECK_SMALL( dist( (x / 1.0), x, DISTL1 ), tol );
    y.set( 0, 1.0 ); y.set( 1, 0.0 ); y.set( 2, 0.5 );
    BOOST_CHECK_SMALL( dist( (x / 2.0), y, DISTL1 ), tol );
    y.set( 0, -4.0 ); y.set( 1, 0.0 ); y.set( 2, -2.0 );
    BOOST_CHECK_SMALL( dist( (x / -0.5), y, DISTL1 ), tol );
    BOOST_CHECK( (x / 0.0) == Prob(3, 0.0) );

    BOOST_CHECK( (x ^ 1.0) == x );
    BOOST_CHECK( (x ^ 0.0) == Prob(3, 1.0) );
    y.set( 0, 4.0 ); y.set( 1, 0.0 ); y.set( 2, 1.0 );
    BOOST_CHECK_SMALL( dist( (x ^ 2.0), y, DISTL1 ), tol );
    y.set( 0, std::sqrt(2.0) ); y.set( 1, 0.0 ); y.set( 2, 1.0 );
    BOOST_CHECK_SMALL( dist( (x ^ 0.5), y, DISTL1 ), tol );
}

BOOST_AUTO_TEST_CASE( ScalarOperationsTest ) {
    Prob xorg(3), x(3);
    xorg.set( 0, 2.0 );
    xorg.set( 1, 0.0 );
    xorg.set( 2, 1.0 );
    Prob y(3);

    x = xorg;
    BOOST_CHECK( x.fill( 1.0 ) == Prob(3, 1.0) );
    BOOST_CHECK( x == Prob(3, 1.0) );
    BOOST_CHECK( x.fill( 2.0 ) == Prob(3, 2.0) );
    BOOST_CHECK( x == Prob(3, 2.0) );
    BOOST_CHECK( x.fill( 0.0 ) == Prob(3, 0.0) );
    BOOST_CHECK( x == Prob(3, 0.0) );

    x = xorg;
    y.set( 0, 3.0 ); y.set( 1, 1.0 ); y.set( 2, 2.0 );
    BOOST_CHECK_SMALL( dist( (x += 1.0), y, DISTL1 ), tol );
    BOOST_CHECK_SMALL( dist( x, y, DISTL1 ), tol );
    y.set( 0, 1.0 ); y.set( 1, -1.0 ); y.set( 2, 0.0 );
    BOOST_CHECK_SMALL( dist( (x += -2.0), y, DISTL1 ), tol );
    BOOST_CHECK_SMALL( dist( x, y, DISTL1 ), tol );

    x = xorg;
    y.set( 0, 1.0 ); y.set( 1, -1.0 ); y.set( 2, 0.0 );
    BOOST_CHECK_SMALL( dist( (x -= 1.0), y, DISTL1 ), tol );
    BOOST_CHECK_SMALL( dist( x, y, DISTL1 ), tol );
    y.set( 0, 3.0 ); y.set( 1, 1.0 ); y.set( 2, 2.0 );
    BOOST_CHECK_SMALL( dist( (x -= -2.0), y, DISTL1 ), tol );
    BOOST_CHECK_SMALL( dist( x, y, DISTL1 ), tol );

    x = xorg;
    BOOST_CHECK_SMALL( dist( (x *= 1.0), x, DISTL1 ), tol );
    BOOST_CHECK_SMALL( dist( x, x, DISTL1 ), tol );
    y.set( 0, 4.0 ); y.set( 1, 0.0 ); y.set( 2, 2.0 );
    BOOST_CHECK_SMALL( dist( (x *= 2.0), y, DISTL1 ), tol );
    BOOST_CHECK_SMALL( dist( x, y, DISTL1 ), tol );
    y.set( 0, -1.0 ); y.set( 1, 0.0 ); y.set( 2, -0.5 );
    BOOST_CHECK_SMALL( dist( (x *= -0.25), y, DISTL1 ), tol );
    BOOST_CHECK_SMALL( dist( x, y, DISTL1 ), tol );

    x = xorg;
    BOOST_CHECK_SMALL( dist( (x /= 1.0), x, DISTL1 ), tol );
    BOOST_CHECK_SMALL( dist( x, x, DISTL1 ), tol );
    y.set( 0, 1.0 ); y.set( 1, 0.0 ); y.set( 2, 0.5 );
    BOOST_CHECK_SMALL( dist( (x /= 2.0), y, DISTL1 ), tol );
    BOOST_CHECK_SMALL( dist( x, y, DISTL1 ), tol );
    y.set( 0, -4.0 ); y.set( 1, 0.0 ); y.set( 2, -2.0 );
    BOOST_CHECK_SMALL( dist( (x /= -0.25), y, DISTL1 ), tol );
    BOOST_CHECK_SMALL( dist( x, y, DISTL1 ), tol );
    BOOST_CHECK( (x /= 0.0) == Prob(3, 0.0) );
    BOOST_CHECK( x == Prob(3, 0.0) );

    x = xorg;
    BOOST_CHECK( (x ^= 1.0) == x );
    BOOST_CHECK( x == x );
    BOOST_CHECK( (x ^= 0.0) == Prob(3, 1.0) );
    BOOST_CHECK( x == Prob(3, 1.0) );
    x = xorg;
    y.set( 0, 4.0 ); y.set( 1, 0.0 ); y.set( 2, 1.0 );
    BOOST_CHECK_SMALL( dist( (x ^= 2.0), y, DISTL1 ), tol );
    BOOST_CHECK_SMALL( dist( x, y, DISTL1 ), tol );
    y.set( 0, 2.0 ); y.set( 1, 0.0 ); y.set( 2, 1.0 );
    BOOST_CHECK_SMALL( dist( (x ^= 0.5), y, DISTL1 ), tol );
    BOOST_CHECK_SMALL( dist( x, y, DISTL1 ), tol );
}

BOOST_AUTO_TEST_CASE( VectorOperationsTest ) {
    size_t N = 6;
    Prob xorg(N), x(N);
    xorg.set( 0, 2.0 ); xorg.set( 1, 0.0 ); xorg.set( 2, 1.0 ); xorg.set( 3, 0.0 ); xorg.set( 4, 2.0 ); xorg.set( 5, 3.0 );
    Prob y(N);
    y.set( 0, 0.5 ); y.set( 1, 1.0 ); y.set( 2, 0.0 ); y.set( 3, 0.0 ); y.set( 4, -2.0 ); y.set( 5, 3.0 );
    Prob z(N), r(N);

    z.set( 0, 2.5 ); z.set( 1, 1.0 ); z.set( 2, 1.0 ); z.set( 3, 0.0 ); z.set( 4, 0.0 ); z.set( 5, 6.0 );
    x = xorg;
    r = (x += y);
    for( size_t i = 0; i < N; i++ )
        BOOST_CHECK_CLOSE( r[i], z[i], tol );
    BOOST_CHECK_SMALL( dist( x, z, DISTL1 ), tol );
    x = xorg;
    BOOST_CHECK( x.pwBinaryOp( y, std::plus<Real>() ) == r );
    BOOST_CHECK( x == r );

    z.set( 0, 1.5 ); z.set( 1, -1.0 ); z.set( 2, 1.0 ); z.set( 3, 0.0 ); z.set( 4, 4.0 ); z.set( 5, 0.0 );
    x = xorg;
    r = (x -= y);
    for( size_t i = 0; i < N; i++ )
        BOOST_CHECK_CLOSE( r[i], z[i], tol );
    BOOST_CHECK_SMALL( dist( x, z, DISTL1 ), tol );
    x = xorg;
    BOOST_CHECK( x.pwBinaryOp( y, std::minus<Real>() ) == r );
    BOOST_CHECK( x == r );

    z.set( 0, 1.0 ); z.set( 1, 0.0 ); z.set( 2, 0.0 ); z.set( 3, 0.0 ); z.set( 4, -4.0 ); z.set( 5, 9.0 );
    x = xorg;
    r = (x *= y);
    for( size_t i = 0; i < N; i++ )
        BOOST_CHECK_CLOSE( r[i], z[i], tol );
    BOOST_CHECK_SMALL( dist( x, z, DISTL1 ), tol );
    x = xorg;
    BOOST_CHECK( x.pwBinaryOp( y, std::multiplies<Real>() ) == r );
    BOOST_CHECK( x == r );

    z.set( 0, 4.0 ); z.set( 1, 0.0 ); z.set( 2, 0.0 ); z.set( 3, 0.0 ); z.set( 4, -1.0 ); z.set( 5, 1.0 );
    x = xorg;
    r = (x /= y);
    for( size_t i = 0; i < N; i++ )
        BOOST_CHECK_CLOSE( r[i], z[i], tol );
    BOOST_CHECK_SMALL( dist( x, z, DISTL1 ), tol );
    x = xorg;
    BOOST_CHECK( x.pwBinaryOp( y, fo_divides0<Real>() ) == r );
    BOOST_CHECK( x == r );

    z.set( 0, 4.0 ); z.set( 1, 0.0 ); z.set( 2, INFINITY ); /*z.set( 3, INFINITY );*/ z.set( 4, -1.0 ); z.set( 5, 1.0 );
    x = xorg;
    r = (x.divide( y ));
    BOOST_CHECK_CLOSE( r[0], z[0], tol );
    BOOST_CHECK_CLOSE( r[1], z[1], tol );
    BOOST_CHECK_EQUAL( r[2], z[2] );
    BOOST_CHECK( dai::isnan(r[3]) );
    BOOST_CHECK_CLOSE( r[4], z[4], tol );
    BOOST_CHECK_CLOSE( r[5], z[5], tol );
    x.set( 3, 0.0 ); r.set( 3, 0.0 );
    BOOST_CHECK( x == r );
    x = xorg;
    r = x.pwBinaryOp( y, std::divides<Real>() );
    BOOST_CHECK_CLOSE( r[0], z[0], tol );
    BOOST_CHECK_CLOSE( r[1], z[1], tol );
    BOOST_CHECK_EQUAL( r[2], z[2] );
    BOOST_CHECK( dai::isnan(r[3]) );
    BOOST_CHECK_CLOSE( r[4], z[4], tol );
    BOOST_CHECK_CLOSE( r[5], z[5], tol );
    x.set( 3, 0.0 ); r.set( 3, 0.0 );
    BOOST_CHECK( x == r );

    z.set( 0, std::sqrt(2.0) ); z.set( 1, 0.0 ); z.set( 2, 1.0 ); z.set( 3, 1.0 ); z.set( 4, 0.25 ); z.set( 5, 27.0 );
    x = xorg;
    r = (x ^= y);
    for( size_t i = 0; i < 6; i++ )
        BOOST_CHECK_CLOSE( r[i], z[i], tol );
    BOOST_CHECK( x == r );
    x = xorg;
    r = x.pwBinaryOp( y, fo_pow<Real>() );
    for( size_t i = 0; i < 6; i++ )
        BOOST_CHECK_CLOSE( r[i], z[i], tol );
    BOOST_CHECK( x == r );
}

BOOST_AUTO_TEST_CASE( VectorTransformationsTest ) {
    size_t N = 6;
    Prob x(N);
    x.set( 0, 2.0 ); x.set( 1, 0.0 ); x.set( 2, 1.0 ); x.set( 3, 0.0 ); x.set( 4, 2.0 ); x.set( 5, 3.0 );
    Prob y(N);
    y.set( 0, 0.5 ); y.set( 1, 1.0 ); y.set( 2, 0.0 ); y.set( 3, 0.0 ); y.set( 4, -2.0 ); y.set( 5, 3.0 );
    Prob z(N), r(N);

    z.set( 0, 2.5 ); z.set( 1, 1.0 ); z.set( 2, 1.0 ); z.set( 3, 0.0 ); z.set( 4, 0.0 ); z.set( 5, 6.0 );
    r = x + y;
    for( size_t i = 0; i < N; i++ )
        BOOST_CHECK_CLOSE( r[i], z[i], tol );
    z = x.pwBinaryTr( y, std::plus<Real>() );
    BOOST_CHECK( r == z );

    z.set( 0, 1.5 ); z.set( 1, -1.0 ); z.set( 2, 1.0 ); z.set( 3, 0.0 ); z.set( 4, 4.0 ); z.set( 5, 0.0 );
    r = x - y;
    for( size_t i = 0; i < N; i++ )
        BOOST_CHECK_CLOSE( r[i], z[i], tol );
    z = x.pwBinaryTr( y, std::minus<Real>() );
    BOOST_CHECK( r == z );

    z.set( 0, 1.0 ); z.set( 1, 0.0 ); z.set( 2, 0.0 ); z.set( 3, 0.0 ); z.set( 4, -4.0 ); z.set( 5, 9.0 );
    r = x * y;
    for( size_t i = 0; i < N; i++ )
        BOOST_CHECK_CLOSE( r[i], z[i], tol );
    z = x.pwBinaryTr( y, std::multiplies<Real>() );
    BOOST_CHECK( r == z );

    z.set( 0, 4.0 ); z.set( 1, 0.0 ); z.set( 2, 0.0 ); z.set( 3, 0.0 ); z.set( 4, -1.0 ); z.set( 5, 1.0 );
    r = x / y;
    for( size_t i = 0; i < N; i++ )
        BOOST_CHECK_CLOSE( r[i], z[i], tol );
    z = x.pwBinaryTr( y, fo_divides0<Real>() );
    BOOST_CHECK( r == z );

    z.set( 0, 4.0 ); z.set( 1, 0.0 ); z.set( 2, INFINITY ); /*z.set( 3, INFINITY );*/ z.set( 4, -1.0 ); z.set( 5, 1.0 );
    r = x.divided_by( y );
    BOOST_CHECK_CLOSE( r[0], z[0], tol );
    BOOST_CHECK_CLOSE( r[1], z[1], tol );
    BOOST_CHECK_EQUAL( r[2], z[2] );
    BOOST_CHECK( dai::isnan(r[3]) );
    BOOST_CHECK_CLOSE( r[4], z[4], tol );
    BOOST_CHECK_CLOSE( r[5], z[5], tol );
    z = x.pwBinaryTr( y, std::divides<Real>() );
    BOOST_CHECK_CLOSE( r[0], z[0], tol );
    BOOST_CHECK_CLOSE( r[1], z[1], tol );
    BOOST_CHECK_EQUAL( r[2], z[2] );
    BOOST_CHECK( dai::isnan(r[3]) );
    BOOST_CHECK_CLOSE( r[4], z[4], tol );
    BOOST_CHECK_CLOSE( r[5], z[5], tol );

    z.set( 0, std::sqrt(2.0) ); z.set( 1, 0.0 ); z.set( 2, 1.0 ); z.set( 3, 1.0 ); z.set( 4, 0.25 ); z.set( 5, 27.0 );
    r = x ^ y;
    BOOST_CHECK_CLOSE( r[0], z[0], tol );
    BOOST_CHECK_CLOSE( r[1], z[1], tol );
    BOOST_CHECK_CLOSE( r[2], z[2], tol );
    BOOST_CHECK_CLOSE( r[3], z[3], tol );
    BOOST_CHECK_CLOSE( r[4], z[4], tol );
    BOOST_CHECK_CLOSE( r[5], z[5], tol );
    z = x.pwBinaryTr( y, fo_pow<Real>() );
    BOOST_CHECK( r == z );
}

BOOST_AUTO_TEST_CASE( RelatedFunctionsTest ) {
    Prob x(3), y(3), z(3);
    x.set( 0, 0.2 );
    x.set( 1, 0.8 );
    x.set( 2, 0.0 );
    y.set( 0, 0.0 );
    y.set( 1, 0.6 );
    y.set( 2, 0.4 );

    z = min( x, y );
    BOOST_CHECK_EQUAL( z[0], (Real)0.0 );
    BOOST_CHECK_EQUAL( z[1], (Real)0.6 );
    BOOST_CHECK_EQUAL( z[2], (Real)0.0 );
    z = max( x, y );
    BOOST_CHECK_EQUAL( z[0], (Real)0.2 );
    BOOST_CHECK_EQUAL( z[1], (Real)0.8 );
    BOOST_CHECK_EQUAL( z[2], (Real)0.4 );

    BOOST_CHECK_CLOSE( dist( x, x, DISTL1 ), (Real)0.0, tol );
    BOOST_CHECK_CLOSE( dist( y, y, DISTL1 ), (Real)0.0, tol );
    BOOST_CHECK_CLOSE( dist( x, y, DISTL1 ), (Real)(0.2 + 0.2 + 0.4), tol );
    BOOST_CHECK_CLOSE( dist( y, x, DISTL1 ), (Real)(0.2 + 0.2 + 0.4), tol );
    BOOST_CHECK_CLOSE( dist( x, y, DISTL1 ), x.innerProduct( y, 0.0, std::plus<Real>(), fo_absdiff<Real>() ), tol );
    BOOST_CHECK_CLOSE( dist( y, x, DISTL1 ), y.innerProduct( x, 0.0, std::plus<Real>(), fo_absdiff<Real>() ), tol );
    BOOST_CHECK_CLOSE( dist( x, x, DISTLINF ), (Real)0.0, tol );
    BOOST_CHECK_CLOSE( dist( y, y, DISTLINF ), (Real)0.0, tol );
    BOOST_CHECK_CLOSE( dist( x, y, DISTLINF ), (Real)0.4, tol );
    BOOST_CHECK_CLOSE( dist( y, x, DISTLINF ), (Real)0.4, tol );
    BOOST_CHECK_CLOSE( dist( x, y, DISTLINF ), x.innerProduct( y, 0.0, fo_max<Real>(), fo_absdiff<Real>() ), tol );
    BOOST_CHECK_CLOSE( dist( y, x, DISTLINF ), y.innerProduct( x, 0.0, fo_max<Real>(), fo_absdiff<Real>() ), tol );
    BOOST_CHECK_CLOSE( dist( x, x, DISTTV ), (Real)0.0, tol );
    BOOST_CHECK_CLOSE( dist( y, y, DISTTV ), (Real)0.0, tol );
    BOOST_CHECK_CLOSE( dist( x, y, DISTTV ), (Real)(0.5 * (0.2 + 0.2 + 0.4)), tol );
    BOOST_CHECK_CLOSE( dist( y, x, DISTTV ), (Real)(0.5 * (0.2 + 0.2 + 0.4)), tol );
    BOOST_CHECK_CLOSE( dist( x, y, DISTTV ), x.innerProduct( y, 0.0, std::plus<Real>(), fo_absdiff<Real>() ) / 2.0, tol );
    BOOST_CHECK_CLOSE( dist( y, x, DISTTV ), y.innerProduct( x, 0.0, std::plus<Real>(), fo_absdiff<Real>() ) / 2.0, tol );
    BOOST_CHECK_CLOSE( dist( x, x, DISTKL ), (Real)0.0, tol );
    BOOST_CHECK_CLOSE( dist( y, y, DISTKL ), (Real)0.0, tol );
    BOOST_CHECK_EQUAL( dist( x, y, DISTKL ), INFINITY );
    BOOST_CHECK_EQUAL( dist( y, x, DISTKL ), INFINITY );
    BOOST_CHECK_EQUAL( dist( x, y, DISTKL ), x.innerProduct( y, 0.0, std::plus<Real>(), fo_KL<Real>() ) );
    BOOST_CHECK_EQUAL( dist( y, x, DISTKL ), y.innerProduct( x, 0.0, std::plus<Real>(), fo_KL<Real>() ) );
    BOOST_CHECK_CLOSE( dist( x, x, DISTHEL ), (Real)0.0, tol );
    BOOST_CHECK_CLOSE( dist( y, y, DISTHEL ), (Real)0.0, tol );
    BOOST_CHECK_CLOSE( dist( x, y, DISTHEL ), (Real)(0.5 * (0.2 + dai::pow(std::sqrt(0.8) - std::sqrt(0.6), 2.0) + 0.4)), tol );
    BOOST_CHECK_CLOSE( dist( y, x, DISTHEL ), (Real)(0.5 * (0.2 + dai::pow(std::sqrt(0.8) - std::sqrt(0.6), 2.0) + 0.4)), tol );
    BOOST_CHECK_CLOSE( dist( x, y, DISTHEL ), x.innerProduct( y, 0.0, std::plus<Real>(), fo_Hellinger<Real>() ) / 2.0, tol );
    BOOST_CHECK_CLOSE( dist( y, x, DISTHEL ), y.innerProduct( x, 0.0, std::plus<Real>(), fo_Hellinger<Real>() ) / 2.0, tol );
    x.set( 1, 0.7 ); x.set( 2, 0.1 );
    y.set( 0, 0.1 ); y.set( 1, 0.5 );
    BOOST_CHECK_CLOSE( dist( x, y, DISTKL ), (Real)(0.2 * dai::log(0.2 / 0.1) + 0.7 * dai::log(0.7 / 0.5) + 0.1 * dai::log(0.1 / 0.4)), tol );
    BOOST_CHECK_CLOSE( dist( y, x, DISTKL ), (Real)(0.1 * dai::log(0.1 / 0.2) + 0.5 * dai::log(0.5 / 0.7) + 0.4 * dai::log(0.4 / 0.1)), tol );
    BOOST_CHECK_CLOSE( dist( x, y, DISTKL ), x.innerProduct( y, 0.0, std::plus<Real>(), fo_KL<Real>() ), tol );
    BOOST_CHECK_CLOSE( dist( y, x, DISTKL ), y.innerProduct( x, 0.0, std::plus<Real>(), fo_KL<Real>() ), tol );

    Prob xx(4), yy(4);
    for( size_t i = 0; i < 3; i++ ) {
        xx.set( i, x[i] );
        yy.set( i, y[i] );
    }
    std::stringstream ss;
    ss << xx;
    std::string s;
    std::getline( ss, s );
#ifdef DAI_SPARSE
    BOOST_CHECK_EQUAL( s, std::string("(size:4, def:0.25, 0:0.2, 1:0.7, 2:0.1)") );
#else
    BOOST_CHECK_EQUAL( s, std::string("(0.2, 0.7, 0.1, 0.25)") );
#endif
    std::stringstream ss2;
    ss2 << yy;
    std::getline( ss2, s );
#ifdef DAI_SPARSE
    BOOST_CHECK_EQUAL( s, std::string("(size:4, def:0.25, 0:0.1, 1:0.5, 2:0.4)") );
#else
    BOOST_CHECK_EQUAL( s, std::string("(0.1, 0.5, 0.4, 0.25)") );
#endif

    z = min( x, y );
    BOOST_CHECK_EQUAL( z[0], (Real)0.1 );
    BOOST_CHECK_EQUAL( z[1], (Real)0.5 );
    BOOST_CHECK_EQUAL( z[2], (Real)0.1 );
    z = max( x, y );
    BOOST_CHECK_EQUAL( z[0], (Real)0.2 );
    BOOST_CHECK_EQUAL( z[1], (Real)0.7 );
    BOOST_CHECK_EQUAL( z[2], (Real)0.4 );

    BOOST_CHECK_CLOSE( x.innerProduct( y, 0.0, std::plus<Real>(), std::multiplies<Real>() ), (Real)(0.2*0.1 + 0.7*0.5 + 0.1*0.4), tol );
    BOOST_CHECK_CLOSE( y.innerProduct( x, 0.0, std::plus<Real>(), std::multiplies<Real>() ), (Real)(0.2*0.1 + 0.7*0.5 + 0.1*0.4), tol );
    BOOST_CHECK_CLOSE( x.innerProduct( y, 1.0, std::plus<Real>(), std::multiplies<Real>() ), (Real)(1.0 + 0.2*0.1 + 0.7*0.5 + 0.1*0.4), tol );
    BOOST_CHECK_CLOSE( y.innerProduct( x, 1.0, std::plus<Real>(), std::multiplies<Real>() ), (Real)(1.0 + 0.2*0.1 + 0.7*0.5 + 0.1*0.4), tol );
    BOOST_CHECK_CLOSE( x.innerProduct( y, -1.0, std::plus<Real>(), std::multiplies<Real>() ), (Real)(-1.0 + 0.2*0.1 + 0.7*0.5 + 0.1*0.4), tol );
    BOOST_CHECK_CLOSE( y.innerProduct( x, -1.0, std::plus<Real>(), std::multiplies<Real>() ), (Real)(-1.0 + 0.2*0.1 + 0.7*0.5 + 0.1*0.4), tol );
}


// graph-tool -- a general graph modification and manipulation thingy
//
// Copyright (C) 2006-2018 Tiago de Paula Peixoto <tiago@skewed.de>
//
// This program is free software; you can redistribute it and/or
// modify it under the terms of the GNU General Public License
// as published by the Free Software Foundation; either version 3
// of the License, or (at your option) any later version.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.

#include "graph.hh"
#include "graph_filtering.hh"
#include "graph_properties.hh"
#include "graph_selectors.hh"

#include <boost/graph/breadth_first_search.hpp>
#include <boost/graph/dijkstra_shortest_paths.hpp>

#include <boost/python.hpp>

using namespace std;
using namespace boost;
using namespace graph_tool;

template <class DistMap>
class bfs_diam_visitor:
    public boost::bfs_visitor<null_visitor>
{
public:
    bfs_diam_visitor(DistMap dist_map, size_t& v)
        : _dist_map(dist_map), _v(v), _dist(0), _max_dist(0),
          _min_k(numeric_limits<size_t>::max()) {}

    template <class Graph>
    void tree_edge(typename graph_traits<Graph>::edge_descriptor e,
                   Graph& g)
    {
        typename graph_traits<Graph>::vertex_descriptor v = target(e,g);
        size_t dist = _dist_map[source(e,g)] + 1;
        if ((dist > _max_dist) ||
            (dist == _max_dist && total_degreeS()(v, g) <= _min_k))
        {
            _max_dist = dist;
            _min_k = total_degreeS()(v, g);
            _v = v;
        }
        _dist_map[v] = dist;
    }

private:
    DistMap _dist_map;
    size_t& _v;
    size_t _dist;
    size_t _max_dist;
    size_t _min_k;
};

template <class DistMap>
class djk_diam_visitor:
    public boost::dijkstra_visitor<null_visitor>
{
public:
    djk_diam_visitor(DistMap dist_map, size_t& v)
        : _dist_map(dist_map), _v(v), _max_dist(0),
          _min_k(numeric_limits<size_t>::max()) {}

    template <class Graph>
    void examine_vertex(typename graph_traits<Graph>::vertex_descriptor v,
                        Graph& g)
    {
        if ((_dist_map[v] > _max_dist) ||
            (_dist_map[v] == _max_dist && total_degreeS()(v, g) <= _min_k))
        {
            _max_dist = _dist_map[v];
            _min_k = total_degreeS()(v, g);
            _v = v;
        }
    }

private:
    DistMap _dist_map;
    size_t& _v;
    typename property_traits<DistMap>::value_type _max_dist;
    size_t _min_k;

};

struct do_bfs_search
{
    template <class Graph, class VertexIndexMap>
    void operator()(const Graph& g, size_t source, VertexIndexMap vertex_index,
                    size_t& target, long double& max_dist) const
    {
        typedef unchecked_vector_property_map<size_t, VertexIndexMap> dist_map_t;
        dist_map_t dist_map(vertex_index, num_vertices(g));
        parallel_vertex_loop
                (g,
                 [&](auto v)
                 {
                     dist_map[v] = numeric_limits<size_t>::max();
                 });
        dist_map[vertex(source,g)] = 0;

        unchecked_vector_property_map<boost::default_color_type, VertexIndexMap>
            color_map(vertex_index, num_vertices(g));
        target = source;
        breadth_first_search(g, vertex(source, g),
                             visitor(bfs_diam_visitor<dist_map_t>
                                      (dist_map, std::ref(target))).
                             vertex_index_map(vertex_index).
                             color_map(color_map));
        max_dist = dist_map[vertex(target, g)];
    }
};

struct do_djk_search
{
    template <class Graph, class VertexIndexMap, class WeightMap>
    void operator()(const Graph& g, size_t source, VertexIndexMap vertex_index,
                    WeightMap weight, size_t& target, long double& max_dist) const
    {
        typedef unchecked_vector_property_map<typename property_traits<WeightMap>::value_type,
                                              VertexIndexMap> dist_map_t;
        dist_map_t dist_map(vertex_index, num_vertices(g));
        target = source;
        dijkstra_shortest_paths(g, vertex(source, g),
                                weight_map(weight).
                                distance_map(dist_map).
                                vertex_index_map(vertex_index).
                                visitor(djk_diam_visitor<dist_map_t>
                                        (dist_map, std::ref(target))));
        max_dist = dist_map[vertex(target, g)];
    }
};

python::object get_diam(GraphInterface& gi, size_t source, boost::any weight)
{
    size_t target;
    long double max_dist;
    if (weight.empty())
    {
        run_action<>()
            (gi, std::bind(do_bfs_search(), std::placeholders::_1, source, gi.get_vertex_index(),
                           std::ref(target), std::ref(max_dist)))();
    }
    else
    {
        run_action<>()
            (gi, std::bind(do_djk_search(), std::placeholders::_1, source, gi.get_vertex_index(),
                           std::placeholders::_2, std::ref(target), std::ref(max_dist)),
             edge_scalar_properties())(weight);

    }
    return python::make_tuple(target, max_dist);
}

void export_diam()
{
    python::def("get_diam", &get_diam);
};


#include "mex.h"
#include <igl/matlab/parse_rhs.h>
#include <igl/matlab/prepare_lhs.h>
#include <igl/matlab/MexStream.h>
#include <igl/matlab/mexErrMsgTxt.h>
#include <Eigen/Core>
#include <Eigen/Sparse>

void mexFunction(
  int          nlhs,
  mxArray      *plhs[],
  int          nrhs,
  const mxArray *prhs[])
{
  //mexPrintf("Compiled at %s on %s\n",__TIME__,__DATE__);
  using namespace igl;
  using namespace igl::matlab;
  using namespace Eigen;
  igl::matlab::MexStream mout;
  std::streambuf *outbuf = std::cout.rdbuf(&mout);

  Eigen::MatrixXd V;
  Eigen::MatrixXi F;
  mexErrMsgTxt(nrhs>=2,"nrhs should be == 2");
  parse_rhs_double(prhs+0,V);
  parse_rhs_index(prhs+1,F);

  switch(nlhs)
  {
    case 2:
      prepare_lhs_index(F,plhs+1);
    case 1:
      prepare_lhs_double(V,plhs+0);
    default:break;
  }
}


/*
 * Copyright (c) 2018, Matheus Izvekov <mizvekov@gmail.com>
 *
 * Permission to use, copy, modify, and/or distribute this software for any
 * purpose with or without fee is hereby granted, provided that the above
 * copyright notice and this permission notice appear in all copies.
 *
 * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES WITH
 * REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF MERCHANTABILITY AND
 * FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY SPECIAL, DIRECT,
 * INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES WHATSOEVER RESULTING FROM
 * LOSS OF USE, DATA OR PROFITS, WHETHER IN AN ACTION OF CONTRACT, NEGLIGENCE
 * OR OTHER TORTIOUS ACTION, ARISING OUT OF OR IN CONNECTION WITH THE USE OR
 * PERFORMANCE OF THIS SOFTWARE.
 */

#pragma once

#include <boost/rational.hpp>

namespace boost {

template<typename T> static auto operator<<(const boost::rational<T> &a, int b) { return a * boost::rational<T>(T(1) << b, T(1)); }
template<typename T> static auto operator>>(const boost::rational<T> &a, int b) { return a * boost::rational<T>(T(1), T(1) << b); }

};

namespace std {

template<class T> struct numeric_limits<boost::rational<T>> {
	using type = boost::rational<T>;
	using base = numeric_limits<T>;

	static_assert(base::is_specialized, "no std::numeric_limits specialization for base type");

	static constexpr bool is_specialized = true;

	static constexpr type min()    noexcept { return { 1, base::max() }; }
	static constexpr type max()    noexcept { return { base::max() }; }
	static constexpr type lowest() noexcept { return { base::lowest() }; }

	static constexpr int  digits       = base::digits;
	static constexpr int  digits10     = base::digits10;
	static constexpr int  max_digits10 = base::max_digits10;
	static constexpr bool is_signed    = base::is_signed;
	static constexpr bool is_integer   = base::is_integer;
	static constexpr bool is_exact     = base::is_exact;
	static constexpr int  radix        = base::radix;

	static constexpr type epsilon()     noexcept { return min(); }
	static constexpr type round_error() noexcept { return { 1 }; }

	static constexpr int  min_exponent   = 0;
	static constexpr int  min_exponent10 = 0;
	static constexpr int  max_exponent   = 0;
	static constexpr int  max_exponent10 = 0;

	static constexpr bool has_infinity      = base::has_infinity;
	static constexpr bool has_quiet_NaN     = base::has_quiet_NaN;
	static constexpr bool has_signaling_NaN = base::has_signaling_NaN;
	static constexpr float_denorm_style has_denorm = base::has_denorm;
	static constexpr bool has_denorm_loss = base::has_denorm_loss;

	static constexpr type infinity()      noexcept { return { base::infinity() }; }
	static constexpr type quiet_NaN()     noexcept { return { base::quiet_NaN() }; }
	static constexpr type signaling_NaN() noexcept { return { base::signaling_NaN() }; }
	static constexpr type denorm_min()    noexcept { return { base::denorm_min(), base::max() }; }

	static constexpr bool is_iec559  = base::is_iec559;
	static constexpr bool is_bounded = base::is_bounded;
	static constexpr bool is_modulo  = base::is_modulo;

	static constexpr bool traps = base::traps;
	static constexpr bool tinyness_before = base::tinyness_before;
	static constexpr float_round_style round_style = base::round_style;
};

}


/*
@copyright Louis Dionne 2015
Distributed under the Boost Software License, Version 1.0.
(See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
 */

#include <boost/hana/assert.hpp>
#include <boost/hana/config.hpp>
#include <boost/hana/functional.hpp>
#include <boost/hana/integral_constant.hpp>
#include <boost/hana/pair.hpp>
#include <boost/hana/tuple.hpp>
#include <boost/hana/type.hpp>
using namespace boost::hana;

int main() {
    //! [main]
    BOOST_HANA_CONSTEXPR_LAMBDA auto sorted = sort_by(less ^on^ first, make<Tuple>(
        pair(int_<3>, 'x'),
        pair(int_<1>, type<void>),
        pair(int_<2>, 9876)
    ));

    BOOST_HANA_CONSTEXPR_CHECK(sorted == make<Tuple>(
        pair(int_<1>, type<void>),
        pair(int_<2>, 9876),
        pair(int_<3>, 'x')
    ));
    //! [main]
}


/* =========================================================================
   Copyright (c) 2010-2012, Institute for Microelectronics,
                            Institute for Analysis and Scientific Computing,
                            TU Wien.
   Portions of this software are copyright by UChicago Argonne, LLC.

                            -----------------
                  ViennaCL - The Vienna Computing Library
                            -----------------

   Project Head:    Karl Rupp                   rupp@iue.tuwien.ac.at

   (A list of authors and contributors can be found in the PDF manual)

   License:         MIT (X11), see file LICENSE in the base directory
============================================================================= */

//

// *** Boost
//
#include <boost/numeric/ublas/io.hpp>
#include <boost/numeric/ublas/triangular.hpp>
#include <boost/numeric/ublas/matrix_sparse.hpp>
#include <boost/numeric/ublas/matrix.hpp>

//
// *** ViennaCL
//

// #define VIENNACL_DEBUG_ALL
// #define VIENNACL_HAVE_UBLAS 1
// #define VIENNACL_DEBUG_CUSTOM_OPERATION
//#define VIENNACL_DEBUG_BUILD

#include "viennacl/scalar.hpp"
#include "viennacl/matrix.hpp"
#include "viennacl/vector.hpp"
#include "viennacl/linalg/prod.hpp"
#include "viennacl/linalg/norm_2.hpp"
#include "viennacl/linalg/direct_solve.hpp"
#include "examples/tutorial/Random.hpp"
#include "viennacl/generator/custom_operation.hpp"

//
// -------------------------------------------------------------
//
using namespace boost::numeric;
//
// -------------------------------------------------------------
//

template <typename ScalarType, unsigned int Alignment>
ScalarType diff ( ublas::vector<ScalarType> & v1, viennacl::vector<ScalarType,Alignment> & v2 ) {
    ublas::vector<ScalarType> v2_cpu ( v2.size() );
    viennacl::copy( v2.begin(), v2.end(), v2_cpu.begin() );
    for ( unsigned int i=0; i<v1.size(); ++i ) {
        if ( std::max ( fabs ( v2_cpu[i] ), fabs ( v1[i] ) ) > 0 )
            v2_cpu[i] = fabs ( v2_cpu[i] - v1[i] ) / std::max ( fabs ( v2_cpu[i] ), fabs ( v1[i] ) );
        else
            v2_cpu[i] = 0.0;
    }
    return norm_inf ( v2_cpu );
}

template< typename NumericT,  typename F,typename F2, unsigned int Alignment, typename Epsilon >
int test ( Epsilon const& epsilon ) {
    int retval = EXIT_SUCCESS;
    static const unsigned int SIZE = 10;
    // --------------------------------------------------------------------------
    ublas::vector<NumericT> rhs ( SIZE );
    for ( unsigned int i = 0; i < rhs.size(); ++i )
        rhs ( i ) = i;
    ublas::vector<NumericT> rhs2 = rhs;
    ublas::vector<NumericT> result = ublas::scalar_vector<NumericT> ( SIZE, 1 );
    ublas::vector<NumericT> result2 = result;
    ublas::vector<NumericT> rhs_trans = rhs;
    rhs_trans.resize ( result.size(), true );
    ublas::vector<NumericT> result_trans = ublas::zero_vector<NumericT> ( rhs.size() );

    ublas::matrix<NumericT,F2> matrix ( result.size(), rhs.size() );
    for ( unsigned int i = 0; i < matrix.size1(); ++i )
        for ( unsigned int j = 0; j < matrix.size2(); ++j )
            matrix ( i,j ) = i+j;

    std::cout << "----- Alignment " << Alignment << " -----" << std::endl;

    viennacl::vector<NumericT,Alignment> vcl_rhs ( rhs.size() );
    viennacl::vector<NumericT,Alignment> vcl_rhs_trans ( rhs_trans.size() );
    viennacl::vector<NumericT,Alignment> vcl_result_trans ( result_trans.size() );
    viennacl::vector<NumericT,Alignment> vcl_result ( result.size() );
    viennacl::matrix<NumericT, F, Alignment> vcl_matrix ( rhs.size(), rhs.size() );

    viennacl::copy ( rhs.begin(), rhs.end(), vcl_rhs.begin() );
    viennacl::copy ( result, vcl_result );
    viennacl::copy ( matrix, vcl_matrix );

    // --------------------------------------------------------------------------

    viennacl::generator::symbolic_matrix<1,NumericT,F,Alignment> symm2;

    viennacl::generator::symbolic_vector<0,NumericT,Alignment> symv;
    viennacl::generator::symbolic_vector<2,NumericT,Alignment> symv3;

    // --------------------------------------------------------------------------
    std::cout << "matrix-vector product" << std::endl;
    result     = ublas::prod ( matrix, rhs );
    viennacl::ocl::enqueue ( viennacl::generator::custom_operation ( symv = prod ( symm2,symv3 ), "prod" ) ( vcl_result,vcl_matrix,vcl_rhs ) );
    viennacl::ocl::get_queue().finish();
    if ( fabs ( diff ( result, vcl_result ) ) > epsilon ) {
        std::cout << "# Error at operation: matrix-vector product" << std::endl;
        std::cout << "  diff: " << fabs ( diff ( result, vcl_result ) ) << std::endl;
        retval = EXIT_FAILURE;
    }

    std::cout << "Prod times inprod" << std::endl;
    result     = ublas::inner_prod ( rhs,rhs ) *ublas::prod ( matrix, rhs );
    viennacl::ocl::enqueue ( viennacl::generator::custom_operation ( symv = inner_prod ( symv3,symv3 ) *prod ( symm2,symv3 ), "prod_times_inprod" ) ( vcl_result,vcl_matrix,vcl_rhs ) );
    viennacl::ocl::get_queue().finish();
    if ( fabs ( diff ( result, vcl_result ) ) > epsilon ) {
        std::cout << "# Error at operation: Prod times inprod" << std::endl;
        std::cout << "  diff: " << fabs ( diff ( result, vcl_result ) ) << std::endl;
        retval = EXIT_FAILURE;
    }
//
    //--------------------------------------------------------------------------
    std::cout << "prod minus v" << std::endl;
    result     = ublas::prod ( matrix, rhs ) -  rhs  ;
    viennacl::ocl::enqueue ( viennacl::generator::custom_operation ( symv = prod ( symm2,symv3 ) - symv3 , "prod_min_v" ) ( vcl_result,vcl_matrix,vcl_rhs ) );
    viennacl::ocl::get_queue().finish();
    if ( fabs ( diff ( result, vcl_result ) ) > epsilon ) {
        std::cout << "# Error at operation: prod minus ( v minus prod )" << std::endl;
        std::cout << "  diff: " << fabs ( diff ( result, vcl_result ) ) << std::endl;
        retval = EXIT_FAILURE;
    }

    //--------------------------------------------------------------------------
    std::cout << "prod + v" << std::endl;
    result     = ublas::prod ( matrix, rhs ) +  rhs  ;
    viennacl::ocl::enqueue ( viennacl::generator::custom_operation ( symv = prod ( symm2,symv3 ) + symv3 , "prod_plus_v" ) ( vcl_result,vcl_matrix,vcl_rhs ) );
    viennacl::ocl::get_queue().finish();
    if ( fabs ( diff ( result, vcl_result ) ) > epsilon ) {
        std::cout << "# prod + v" << std::endl;
        std::cout << "  diff: " << fabs ( diff ( result, vcl_result ) ) << std::endl;
        retval = EXIT_FAILURE;
    }

    //--------------------------------------------------------------------------
    std::cout << "prod plus inprod*v" << std::endl;
    result     = ublas::prod ( matrix, rhs ) +   ublas::inner_prod (rhs,rhs)*rhs  ;
    viennacl::generator::custom_operation op( symv = prod ( symm2,symv3 ) + symv3*inner_prod(symv3,symv3) , "prod_plus_inprod_v" );
    viennacl::ocl::enqueue (  op( vcl_result,vcl_matrix,vcl_rhs ) );
    viennacl::ocl::get_queue().finish();
    if ( fabs ( diff ( result, vcl_result ) ) > epsilon ) {
        std::cout << "# Error at operation: prod plus inprod*v" << std::endl;
        std::cout << "  diff: " << fabs ( diff ( result, vcl_result ) ) << std::endl;
        std::cout << op.kernels_source_code() << std::endl;
        retval = EXIT_FAILURE;
    }

    return retval;
}
int main() {
    std::cout << std::endl;
    std::cout << "----------------------------------------------" << std::endl;
    std::cout << "----------------------------------------------" << std::endl;
    std::cout << "## Test :: Matrix-Vector Product" << std::endl;
    std::cout << "----------------------------------------------" << std::endl;
    std::cout << "----------------------------------------------" << std::endl;
    std::cout << std::endl;

    int retval = EXIT_SUCCESS;

    std::cout << std::endl;
    std::cout << "----------------------------------------------" << std::endl;
    std::cout << std::endl;
    {
        typedef float NumericT;
        NumericT epsilon = 1.0E-3;
        std::cout << "# Testing setup:" << std::endl;
        std::cout << "  eps:     " << epsilon << std::endl;
        std::cout << "  numeric: float" << std::endl;

        std::cout << "---- Layout : Row Major" << std::endl;
        retval = test<NumericT, viennacl::row_major,ublas::row_major,1> ( epsilon );
//        retval = test<NumericT, viennacl::row_major,ublas::row_major,16> ( epsilon );

        std::cout << "---- Layout : Column Major" << std::endl;
        retval = test<NumericT, viennacl::column_major,ublas::column_major,1> ( epsilon );

        if ( retval == EXIT_SUCCESS )
            std::cout << "# Test passed" << std::endl;
        else
            return retval;
    }

    return retval;
}


//Copyright (c) 2008-2016 Emil Dotchevski and Reverge Studios, Inc.

//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/qvm/quat_operations.hpp>
#include <boost/qvm/mat_operations.hpp>
#include <boost/qvm/quat.hpp>
#include "test_qvm_quaternion.hpp"
#include "test_qvm_matrix.hpp"
#include "gold.hpp"

namespace
    {
    void
    test()
        {
        using namespace boost::qvm;
        for( float r=0; r<6.28f; r+=0.5f )
            {
            test_qvm::quaternion<Q1> q1=roty_quat(r);
            test_qvm::matrix<M1,3,3> m1=convert_to< test_qvm::matrix<M1,3,3> >(q1);
            test_qvm::rotation_y(m1.b,r);
            BOOST_QVM_TEST_CLOSE(m1.a,m1.b,0.000001f);
            test_qvm::quaternion<Q2> q2(42,1);
            set_roty(q2,r);
            test_qvm::matrix<M1,3,3> m2=convert_to< test_qvm::matrix<M1,3,3> >(q2);
            test_qvm::rotation_y(m2.b,r);
            BOOST_QVM_TEST_CLOSE(m2.a,m2.b,0.000001f);
            test_qvm::quaternion<Q1> q3(42,1);
            test_qvm::quaternion<Q1> r1=q3*q1;
            rotate_y(q3,r);
            BOOST_QVM_TEST_EQ(q3.a,r1.a);
            }
        roty_quat(0.0f)+roty_quat(0.0f);
        -roty_quat(0.0f);
        }
    }

int
main()
    {
    test();
    return boost::report_errors();
    }


//Logging
#include <glog/logging.h>

//C++
#include <string>
#include <cmath>
#include <fstream>

//Boost
#include <boost/filesystem.hpp>

/** Default Values **/
static const int MOG_ID = 1;
static const int VIBE_ID = 2;
static const int PBAS_ID = 3;
static const int EVENT_ID = 32;

/** Function Headers */
void help();
double calcMean(std::vector<double> vals);
double calcVariance(std::vector<double> vals, const double mean);
void openTSVEventFile(std::string event_filename, int &video_id, std::vector<double> &vibe_vals, std::vector<double> &mog_vals);
std::vector<std::pair<size_t,size_t>> calculateEventTimes(std::vector<double> &vals, double dist, double fps);

// TODO Update the help info
void help() {
    LOG(INFO) << "--------------------------------------------------------------------------";
    LOG(INFO) << "This takes a TSV file. Each row should consist of the video ID,";
    LOG(INFO) << "a boolean indicating an expert observation, and the algorithm outputs.";
    LOG(INFO) << "Usage:";
    LOG(INFO) << "./event_data_parser <tsv time file>";
    LOG(INFO) << "for example: ./event_data_parser data.tsv";
    LOG(INFO) << "--------------------------------------------------------------------------";
}

double calcMean(std::vector<double> vals) {
    double val = 0;
    for (size_t i = 0; i < vals.size(); i++) {
        val += vals[i];
    }
    return val/vals.size();
}

double calcVariance(std::vector<double> vals, const double mean) {
    double var = 0;
    for (size_t i = 0; i < vals.size(); i++) {
        double diff = vals[i] - mean;
        var = diff * diff;
    }
    var = var/vals.size();
    return var;
}

void openTSVEventFile(std::string event_filename, int &video_id, std::vector<double> &vibe_vals, std::vector<double> &pbas_vals, std::vector<double> &mog_vals) {
    std::ifstream infile(event_filename);

    std::string vibe_val, pbas_val, mog_val;

    std::string line;
    while (std::getline(infile, line)) {
        std::istringstream iss(line);
        if (!(iss >> video_id >> vibe_val >> pbas_val >> mog_val)) {
            break;
        }
        vibe_vals.push_back(atof(vibe_val.c_str()));
        pbas_vals.push_back(atof(pbas_val.c_str()));
        mog_vals.push_back(atof(mog_val.c_str()));
    }
    infile.close();
}

std::vector<std::pair<size_t,size_t>> calculateEventTimes(std::vector<double> &vals, double dist, double fps) {
    std::vector<std::pair<size_t,size_t>> output;

    double mean = calcMean(vals);
    double stdev = sqrt(calcVariance(vals, mean));

    bool event_happening = false;
    size_t start_time = 0;
    LOG(INFO) << "Mean: " << mean;
    LOG(INFO) << "Stdev * Dist: " << dist * stdev;
    double bound = mean + (dist * stdev);
    for (size_t time = 0; time < vals.size(); time++) {
        if (vals[time] >= bound) {
            event_happening = true;
            start_time = time;
        } else if (event_happening && vals[time] < bound) {
            event_happening = false;
            output.push_back(std::pair<size_t,size_t>(start_time/fps, time/fps));
        }
    }
    return output;
}

/**
 * @function main
 */
int main(int argc, char* argv[])
{
    FLAGS_logtostderr = 1;
    google::InitGoogleLogging(argv[0]);

    //print help information
    help();

    //check for the input parameter correctness
    if(argc != 2) {
        LOG(ERROR) << "Incorret input list";
        return EXIT_FAILURE;
    }

    std::string tsv_event_filename(argv[1]);

    std::vector<size_t> event_times;
    std::vector<double> vibe_vals;
    std::vector<double> pbas_vals;
    std::vector<double> mog_vals;

    double fps = 10;

    int video_id;
    openTSVEventFile(tsv_event_filename, video_id, vibe_vals, pbas_vals, mog_vals);
    std::string video_id_str = std::to_string(static_cast<long long>(video_id));

    std::vector<std::pair<size_t,size_t>> vibe_events = calculateEventTimes(vibe_vals, 3, fps);
    std::vector<std::pair<size_t,size_t>> pbas_events = calculateEventTimes(pbas_vals, 3, fps);
    std::vector<std::pair<size_t,size_t>> mog_events = calculateEventTimes(mog_vals, 3, fps);

    // Open files
    boost::filesystem::path dir(video_id_str);
    boost::filesystem::create_directory(dir);
    std::ofstream event_file(video_id_str + "/significant_events.csv");

    for (size_t i = 0; i < vibe_events.size(); i++) {
        std::pair<size_t,size_t> pair = vibe_events[i];
        event_file << video_id << "," << EVENT_ID << "," << VIBE_ID << "," << pair.first << "," << pair.second << std::endl;
    }

    for (size_t i = 0; i < pbas_events.size(); i++) {
        std::pair<size_t,size_t> pair = pbas_events[i];
        event_file << video_id << "," << EVENT_ID << "," << PBAS_ID << "," << pair.first << "," << pair.second << std::endl;
    }

    for (size_t i = 0; i < mog_events.size(); i++) {
        std::pair<size_t,size_t> pair = mog_events[i];
        event_file << video_id << "," << EVENT_ID << "," << MOG_ID << "," << pair.first << "," << pair.second << std::endl;
    }

    // Close files
    event_file.close();

    return EXIT_SUCCESS;
}


//
// Created by yche on 3/22/18.
//

#include <fstream>

#include <boost/format.hpp>

#include "local_push_yche.h"

using boost::format;

Reduced_LocalPush::Reduced_LocalPush(GraphYche &g, string name, double c_, double epsilon, size_t n_) :
        LocalPush(g, name, c_, epsilon, n_) {
    string data_path = get_file_path_base() + ".P";
    init_PR();
}

string Reduced_LocalPush::get_file_path_base() {
    return LOCAL_PUSH_DIR + str(format("RLP_%s-%.3f-%.6f") % g_name % c % epsilon);
}

void Reduced_LocalPush::local_push(GraphYche &g) {
    auto start = std::chrono::high_resolution_clock::now();

    while (!Q.empty()) {
        max_q_size = max(max_q_size, Q.size());
        NodePair np = Q.front();
        Q.pop();
//        NodePair np = Q.back();
//        Q.pop_back();

        marker[np] = false;
        auto &R_np_ref = R[np];
        double residual_to_push = R_np_ref;
        if (np.first == np.second) { //singleton node
            residual_to_push -= r_max / (1 - c); // singleton nodes do not need to push all residual as 1
        }

        R_np_ref -= residual_to_push;
        P[np] += residual_to_push;

        // push residuals to neighbors of np
        auto a = np.first;
        auto b = np.second;
        if (a == b) {
            /* starting push for singleton nodes*/
            for (auto off_a = g.off_out[a]; off_a < g.off_out[a + 1]; off_a++) {
                auto out_nei_a = g.neighbors_out[off_a];
                for (auto off_b = g.off_out[b]; off_b < g.off_out[b + 1]; off_b++) {
                    auto out_nei_b = g.neighbors_out[off_b];
                    if (out_nei_a < out_nei_b) { // only push to partial pairs for a < b
                        NodePair pab(out_nei_a, out_nei_b); // the node-pair to be pushed to
                        double inc = c * residual_to_push / (g.in_degree(out_nei_a) * g.in_degree(out_nei_b));
                        n_push++;
                        auto &res_ref = R[pab];
                        res_ref += sqrt(2) * inc;
                        if (fabs(res_ref) / sqrt(2) > r_max) { // the criteria for reduced linear system
                            auto &is_in_q_flag_ref = marker[pab];
                            if (!is_in_q_flag_ref) {
                                Q.push(pab);
//            Q.push_back(pab);
                                is_in_q_flag_ref = true;
                            }
                        }
                    }
                }
            }
        } else {
            /* starting push for non-singleton nodes*/
            for (auto off_a = g.off_out[a]; off_a < g.off_out[a + 1]; off_a++) {
                for (auto off_b = g.off_out[b]; off_b < g.off_out[b + 1]; off_b++) {
                    // put out_nei_a here, since swap happens later
                    auto out_nei_a = g.neighbors_out[off_a];
                    auto out_nei_b = g.neighbors_out[off_b];

                    double inc = c * residual_to_push / (g.in_degree(out_nei_a) * g.in_degree(out_nei_b));
                    if (out_nei_a != out_nei_b) { //don't push to singleton nodes
                        //  a, b is normal in-neighbors of out_a and out_b
                        if (out_nei_a > out_nei_b) {
                            swap(out_nei_a, out_nei_b);
                        }
                        NodePair pab(out_nei_a, out_nei_b);
                        n_push++;
                        auto &res_ref = R[pab];
                        res_ref += inc;
                        if (fabs(res_ref) / sqrt(2) > r_max) { // the criteria for reduced linear system
                            auto &is_in_q_flag_ref = marker[pab];
                            if (!is_in_q_flag_ref) {
                                Q.push(pab);
//            Q.push_back(pab);
                                is_in_q_flag_ref = true;
                            }
                        }
                    }
                }
            }
        }
    }

    auto finish = std::chrono::high_resolution_clock::now();
    std::chrono::duration<double> elapsed = finish - start;
    if (cpu_time == -1) {
        cpu_time = elapsed.count();
        mem_size = getValue();
    }
}

void Reduced_LocalPush::push(NodePair &pab, double inc) {
    // only probing once
    n_push++;
    auto &res_ref = R[pab];
    res_ref += inc;
    if (fabs(res_ref) / sqrt(2) > r_max) { // the criteria for reduced linear system
        auto &is_in_q_flag_ref = marker[pab];
        if (!is_in_q_flag_ref) {
            Q.push(pab);
//            Q.push_back(pab);
            is_in_q_flag_ref = true;
        }
    }
}

double Reduced_LocalPush::how_much_residual_to_push(GraphYche &g, NodePair &np) {
    // determine the residual value for current pair to push
    double r = R[np];
    if (np.first == np.second) { //singleton node
        return r - r_max / (1 - c); // singleton nodes do not need to push all residual as 1
    }

#ifdef SELF_LOOP_MERGE
    /* check whether np forms a self-loop */
    if (g.exists_edge(np.first, np.second) && g.exists_edge(np.second, np.first)) { // check whether exists reverse edge
        auto in_deg_a = g.in_degree(np.first);
        auto in_deg_b = g.in_degree(np.second);
        double alpha = c / (in_deg_a * in_deg_b);
        int k = ceil(log(r_max / fabs(r)) / log(alpha));
        double residual_to_push = (1 - pow(alpha, k)) * r / (1 - alpha);
        return residual_to_push;
    }
#endif
    return r;
}

void Reduced_LocalPush::push_to_neighbors(GraphYche &g, NodePair &np, double current_residual) {
    // the push method using reduced linear system
    // out-neighbors of a,b
    auto a = np.first;
    auto b = np.second;
    if (a == b) {
        /* starting push for singleton nodes*/
        for (auto off_a = g.off_out[a]; off_a < g.off_out[a + 1]; off_a++) {
            auto out_nei_a = g.neighbors_out[off_a];
            for (auto off_b = g.off_out[b]; off_b < g.off_out[b + 1]; off_b++) {
                auto out_nei_b = g.neighbors_out[off_b];
                if (out_nei_a < out_nei_b) { // only push to partial pairs for a < b
                    NodePair pab(out_nei_a, out_nei_b); // the node-pair to be pushed to
                    double inc = c * current_residual / (g.in_degree(out_nei_a) * g.in_degree(out_nei_b));
                    push(pab, sqrt(2) * inc);
                }
            }
        }
    } else {
        /* starting push for non-singleton nodes*/
        for (auto off_a = g.off_out[a]; off_a < g.off_out[a + 1]; off_a++) {
            for (auto off_b = g.off_out[b]; off_b < g.off_out[b + 1]; off_b++) {
                // put out_nei_a here, since swap happens later
                auto out_nei_a = g.neighbors_out[off_a];
                auto out_nei_b = g.neighbors_out[off_b];

                double inc = c * current_residual / (g.in_degree(out_nei_a) * g.in_degree(out_nei_b));
                if (out_nei_a != out_nei_b) { //don't push to singleton nodes
                    //  a, b is normal in-neighbors of out_a and out_b
                    if (out_nei_a > out_nei_b) {
                        swap(out_nei_a, out_nei_b);
                    }
                    NodePair pab(out_nei_a, out_nei_b);
                    push(pab, 1 * inc);
                }
            }
        }
    }
}

double Reduced_LocalPush::query_P(int a, int b) {
    if (a == b) {
        return P.query(a, b);
    } else if (a > b) {
        return P.query(b, a) / sqrt(2);
    } else {
        return P.query(a, b) / sqrt(2);
    }
}


#include <Eigen/Dense>
#include <memory>

#include "HierarchicalClustering.h"
#include "ClusteringUtil.h"
#include "Clustering.h"

ExternalValidityEstimator::ExternalValidityEstimator(std::unique_ptr<ClusterPostProcessing> & cpp_, std::unique_ptr<ClusteringAlgorithm> & clustAlgo_, unsigned maxK_) : ClusterEstimator(cpp_), clustAlgo(std::move(clustAlgo_)), maxK(maxK_)
{

}

ExternalValidityEstimator::~ExternalValidityEstimator()
{

}

std::pair<unsigned, std::vector<ClusteringResult>> ExternalValidityEstimator::estimateK(const Eigen::MatrixXd & data)
{
    std::vector<ClusteringResult> results(maxK);

    // label trivial clustering for k == 1
    results[0].numClusters = 1;
    results[0].labels.resize(data.rows());
    std::fill(results[0].labels.begin(),results[0].labels.end(), 1);

    clustAlgo->initialize(data);

    // find minimum Davies Bouldin index
    double minDb = std::numeric_limits<double>::max();
    unsigned optK = 0;

    for (unsigned k = 2; k <= maxK; k++)
    {
        results[k-1].numClusters = k;
        results[k-1].labels = clustAlgo->cluster(k);
        cpp->run(results[k-1]);

        double db = ClusteringUtil::daviesBouldin(data, results[k-1].labels);

        if (db < minDb)
        {
            minDb = db;
            optK = results[k-1].numClusters;
        }
    }

    return std::make_pair(optK, results);
}

std::string ExternalValidityEstimator::name()
{
    return "ExternalValidityEstimator";
}

std::map<std::string, std::string> ExternalValidityEstimator::parameters()
{
    std::map<std::string, std::string> params = {{"max_k", std::to_string(maxK)}, {"cluster_validity", "davies_bouldin"}};
    params["clustering_algorithm"] = clustAlgo->name();
    for (auto & it : clustAlgo->parameters())
    {
        params["clustering_" + it.first] = it.second;
    }
    return params;
}