#include "hausdorff.h"
#include "../ModelViewer/ColorMap.h"
#include <numeric>

extern CColorMap g_colorMap;
int numCandidates = 3;
void extractSets(cv::Mat img,vector<vector<int>> &rowset)
{
	
	for(int i=0;i<img.rows;i++)
	{
		vector<int> tmp;
		for(int j=0;j<img.cols;j++)
		{
			if(img.at<uchar>(i,j)==0)
				tmp.push_back(j);
		}
		rowset.push_back(tmp);
	}
	
}

void computeRCDists(vector<vector<int>> rowset,map<int,float> &mapdist)
{

	for(int i=1;i<rowset.size();i++)
	{
		if(rowset[i].size()!=0&&rowset[i-1].size()!=0)
		{
			float dist = hausdorff(rowset[i],rowset[i-1]);
			mapdist.insert(pair<int,float>(i,dist));	
		}
	}
	
	
}

vector<Node> findCriticalPoints(map<int,float> hddist)//vector<pair<int,float>> gradients
{
	vector<Node> nodes;
	vector<int> peaks;

	float fmax=0;
	for(map<int,float>::iterator it=hddist.begin();it!=hddist.end();it++)
	{
		if((*it).second>fmax)
			fmax = (*it).second;
	}
	Node tmp;
	for(map<int,float>::iterator it=hddist.begin();it!=hddist.end();it++)
	{
		if((*it).second>0.05*fmax)
		{
			tmp.end=(*it).first-1;
			if(tmp.end>=(*hddist.begin()).first)
				nodes.push_back(tmp);
		}
	}
	map<int,float>::reverse_iterator rit = hddist.rbegin();
	int lastrow = (*rit).first;
	if(tmp.end!=lastrow)
	{
		tmp.start = tmp.end+1;
		tmp.end = lastrow;
		nodes.push_back(tmp);
	}
	//printf("fmax is %f node size is %d\n",fmax,nodes.size());
	if(nodes.size()>0)
	{	
		nodes[0].start = (*(hddist.begin())).first-1;
		nodes[0].uppersistence = FLT_MAX;
		nodes[0].downpersistence = fabs(hddist[nodes[0].end]-hddist[nodes[0].end+1]);
	}
	if(nodes.size()>1)
	{

		for(int i=1;i<nodes.size()-1;i++)
		{
			nodes[i].start = nodes[i-1].end+1;
			nodes[i].uppersistence = fabs(hddist[nodes[i].start-1]-hddist[nodes[i].start]);;
			nodes[i].downpersistence = fabs(hddist[nodes[i].end]-hddist[nodes[i].end+1]);
		}
	}

	if(nodes.size()>=1)
	{			
		int i = nodes.size()-1;
		nodes[i].uppersistence = fabs(hddist[nodes[i].start-1]-hddist[nodes[i].start]);;
		nodes[i].downpersistence = FLT_MAX;
		
		//nodes[i].end = (*rit).first;
	}
	for(int i=0;i<nodes.size();i++)
		nodes[i].minpersistence = nodes[i].uppersistence>nodes[i].downpersistence?nodes[i].downpersistence :nodes[i].uppersistence;
	
	return nodes;	
}


vector<int> sortNodes(vector<Node>  nodes)
{
	vector<int> index;
	vector<float> persistences;
	for(int i=0;i<nodes.size();i++)
	{
		index.push_back(i);
		persistences.push_back(nodes[i].minpersistence);
		//persistences.push_back(nodes[i].end-nodes[i].start);
	}
	index= sortWithIndex(persistences);
	return index;



	return index;
}
vector<Node> mergeNodes(vector<Node> nodes)
{
	if(nodes.size()==0) return nodes;

	vector<int> sortIndex = sortNodes(nodes);
	for(int ii=0;ii<nodes.size();ii++)
		nodes[ii].merge =false;
	int minLines = 5;//(nodes[nodes.size()-1].end - nodes[0].start)*0.04;

	for(int i=0;i<nodes.size();i++)
		//for(int i=nodes.size()-1;i>=0;i--)
	{
		int ii= sortIndex[i];

		int len = nodes[ii].end - nodes[ii].start+1;
		if(len>minLines|| nodes[ii].merge ) continue;
		int maxindex,maxnum;

		int up=ii-1;

		while(up>=0&&nodes[up].merge)
			up--;

		if(up<0)
			up=ii;

		int below=ii+1;

		while(below<nodes.size()&&nodes[below].merge)
			below++;
		if(below==nodes.size())
			below=ii;



		if(nodes[ii].uppersistence<nodes[ii].downpersistence)
			maxindex=up;
		else
			maxindex=below;		

		if(maxindex==ii)continue;
		if(maxindex==up)
		{ 
		nodes[maxindex].merge = true;
		nodes[ii].start  = nodes[maxindex].start;
		nodes[ii].uppersistence = nodes[ii].uppersistence;			
		}
		else if(maxindex==below)
		{
		nodes[ii].merge = true;
		nodes[maxindex].start  = nodes[ii].start;
		nodes[maxindex].uppersistence = nodes[ii].uppersistence;
		}
		nodes[maxindex].minpersistence = nodes[maxindex].downpersistence>nodes[maxindex].uppersistence ?nodes[maxindex].uppersistence :nodes[maxindex].downpersistence;
		
	

	}
	vector<Node> fnodes;
	for(int i=0;i<nodes.size();i++)
	{
		if(!nodes[i].merge)
		{
			nodes[i].height = nodes[i].end-nodes[i].start+1;
			fnodes.push_back(nodes[i]);
		}
	}
	return fnodes;
}



vector<Node> mergeNodes(vector<float> &distInRegions,vector<vector<int>> rowset,map<int,float> data,vector<Node> &nodes)
{
	if(nodes.size()==0) return nodes;

	vector<int> sortIndex = sortNodes(nodes);
	for(int ii=0;ii<nodes.size();ii++)
		nodes[ii].merge =false;
	int minLines =5;//(nodes[nodes.size()-1].end - nodes[0].start)*0.04;
	map<int,float>::iterator it = data.begin();
	int firstrow = (*it).first;

	map<int,float>::reverse_iterator rit = data.rbegin();
	int lastrow = (*rit).first;
	for(int i=0;i<nodes.size();i++)
	//for(int i=nodes.size()-1;i>=0;i--)
	{
		int ii= sortIndex[i];

		int len = nodes[ii].end - nodes[ii].start+1;
		if(len>minLines|| nodes[ii].merge ) continue;
		int maxindex,maxnum;
		
		int up=ii-1;
		
		while(up>=0&&nodes[up].merge)
			up--;

		if(up<0)
			up=ii;

		int below=ii+1;
	
		while(below<nodes.size()&&nodes[below].merge)
			below++;
		if(below==nodes.size())
			below=ii;

	
	
		

		//if(nodes[ii].uppersistence<nodes[ii].downpersistence)
		//	maxindex=up;
		//else
		//	maxindex=below;		


		/*if(maxindex==ii)continue;
		if(maxindex==up)
		{ 
			nodes[maxindex].merge = true;
			nodes[ii].start  = nodes[maxindex].start;
			nodes[ii].uppersistence = nodes[ii].uppersistence;			
		}
		else if(maxindex==below)
		{
			nodes[ii].merge = true;
			nodes[maxindex].start  = nodes[ii].start;
			nodes[maxindex].uppersistence = nodes[ii].uppersistence;
		}
		nodes[maxindex].minpersistence = nodes[maxindex].downpersistence>nodes[maxindex].uppersistence ?nodes[maxindex].uppersistence :nodes[maxindex].downpersistence;
		*/
		int index = ii;
		
		if(nodes[index].uppersistence<nodes[index].downpersistence)
		{				
			int minindex =  mergeNode(distInRegions,rowset,nodes,up,index);
			nodes[up].representiveIndex = minindex;
			if(nodes[up].start-1<firstrow)
				nodes[up].uppersistence = FLT_MAX;
			else
				nodes[up].uppersistence = hausdorff(rowset[minindex],rowset[nodes[up].start-1]);	
			if(nodes[index].end+1>lastrow)
				nodes[up].downpersistence = FLT_MAX;
			else
				nodes[up].downpersistence = hausdorff(rowset[minindex],rowset[nodes[index].end+1]);
			nodes[up].minpersistence = nodes[up].uppersistence>nodes[up].downpersistence?nodes[up].downpersistence :nodes[up].uppersistence;
			nodes[up].end = nodes[index].end;			
			nodes[index].merge=true;
			
		}	
		else if(nodes[index].uppersistence>nodes[index].downpersistence)
		{			
			nodes[index].merge=true;
			int minindex =  mergeNode(distInRegions,rowset,nodes,below,index);
			nodes[below].representiveIndex = minindex;
			if(nodes[index].start-1<firstrow)
				nodes[below].uppersistence = FLT_MAX;
			else
				nodes[below].uppersistence = hausdorff(rowset[minindex],rowset[nodes[index].start-1]);		
			if(nodes[below].end+1>lastrow)
				nodes[below].downpersistence = FLT_MAX;
			else
				nodes[below].downpersistence = hausdorff(rowset[minindex],rowset[nodes[below].end+1]);
			nodes[below].minpersistence = nodes[below].uppersistence>nodes[below].downpersistence?nodes[below].downpersistence :nodes[below].uppersistence;
			nodes[below].start = nodes[index].start;
			
		}	
		else if(nodes[index].uppersistence==nodes[index].downpersistence)
		{
			int upheight = nodes[up].end - nodes[up].start + 1;
			int belowheight = nodes[below].end - nodes[below].start + 1;
			if(upheight>belowheight)
			{
				int minindex =  mergeNode(distInRegions,rowset,nodes,up,index);
				nodes[up].representiveIndex = minindex;
				if(nodes[up].start-1<firstrow)
					nodes[up].uppersistence = FLT_MAX;
				else
					nodes[up].uppersistence = hausdorff(rowset[minindex],rowset[nodes[up].start-1]);	
				if(nodes[index].end+1>lastrow)
					nodes[up].downpersistence = FLT_MAX;
				else
					nodes[up].downpersistence = hausdorff(rowset[minindex],rowset[nodes[index].end+1]);
				nodes[up].minpersistence = nodes[up].uppersistence>nodes[up].downpersistence?nodes[up].downpersistence :nodes[up].uppersistence;
				nodes[up].end = nodes[index].end;			
				nodes[index].merge=true;
			}
			else
			{
				nodes[index].merge=true;
				int minindex =  mergeNode(distInRegions,rowset,nodes,below,index);
				nodes[below].representiveIndex = minindex;
				if(nodes[index].start-1<firstrow)
					nodes[below].uppersistence = FLT_MAX;
				else
					nodes[below].uppersistence = hausdorff(rowset[minindex],rowset[nodes[index].start-1]);		
				if(nodes[below].end+1>lastrow)
					nodes[below].downpersistence = FLT_MAX;
				else
					nodes[below].downpersistence = hausdorff(rowset[minindex],rowset[nodes[below].end+1]);
				nodes[below].minpersistence = nodes[below].uppersistence>nodes[below].downpersistence?nodes[below].downpersistence :nodes[below].uppersistence;
				nodes[below].start = nodes[index].start;
			}
					
		}

	}
	vector<Node> fnodes;
	for(int i=0;i<nodes.size();i++)
	{
		if(!nodes[i].merge)
		{	
			nodes[i].line = rowset[nodes[i].representiveIndex];
			nodes[i].height = nodes[i].end-nodes[i].start+1;
			fnodes.push_back(nodes[i]);
		}
	}
	return fnodes;
}

void saveSegmentImg(cv::Mat img,vector<Node> nodes,const char*lightname)
{
	cv::Mat rimg(512,512,CV_8UC3);	
	for(int j=0;j<rimg.cols;j++)
		for(int k=0;k<rimg.rows;k++)
			rimg.at<cv::Vec3b>(j,k) =cv::Vec3b(255,255,255);

	for(int i=0;i<nodes.size();i++)
	{
		int start = nodes[i].start;
		int end=nodes[i].end;
		for(int j=start;j<=end;j++)
			for(int k=0;k<img.rows;k++)
			{
				if(img.at<UCHAR>(j,k)==0)
				{
					Color rgb = g_colorMap.GetClassColor((i+2)%92);
					UCHAR r = rgb[0]*255;
					UCHAR g = rgb[1]*255;
					UCHAR b = rgb[2]*255;
					rimg.at<cv::Vec3b>(j,k) =cv::Vec3b(r,g,b);


				}	
			}
	}

	char buf[255];
	itoa(nodes.size(),buf,10);
	string tmpname = lightname;
	tmpname.append("-");
	tmpname.append(buf);
	tmpname.append("-l.bmp");
	cv::imwrite(tmpname.c_str(),rimg);
	
}


vector<Node> convertNodeToRegion(vector<Node> nodes,vector<vector<int>> rowset,cv::Mat img)
{
	CvMoments moment; 
	CvMat cvMat = img;
	cvMoments( &cvMat, &moment, 0 ); 
	float m_00 = cvGetSpatialMoment( &moment, 0, 0 ); 
	float m_10 = cvGetSpatialMoment( &moment, 1, 0 ); 
	float m_01 = cvGetSpatialMoment( &moment, 0, 1 ); 
	
	cvReleaseData(&cvMat);

	double CenX = m_10 / m_00; 
	double CenY = m_01 / m_00;
	//vector<Region> regions;
	for(int i=0;i<nodes.size();i++)
	{
		//Region tmpReg;
		int mid = (nodes[i].start+nodes[i].end)/2;
		nodes[i].line = rowset[mid];
		nodes[i].height = nodes[i].end-nodes[i].start+1;
		nodes[i].start = nodes[i].start;
		nodes[i].end = nodes[i].end;
	/*	float sumY = 0;
		float sumX = 0;
		int numPt = 0;
		for(int j=nodes[i].start;j<=nodes[i].end;j++)
		{
			numPt += rowset[j].size();
			sumY +=j*rowset[j].size();
			sumX += std::accumulate(rowset[j].begin(),rowset[j].end(),0);
		}
		nodes[i].center = cv::Point2f(sumX/numPt-CenX,sumY/numPt-CenY);*/
		//regions.push_back(tmpReg);
	}
	return nodes;
}




vector<Node> loadImage(string filename,vector<vector<int>> &rowset,cv::Mat &oriimg,bool istest)
{
	cv::Mat img = loadImage(filename.c_str(),oriimg);

	int namepos = filename.find_last_of("/");
	string lightname = filename.substr(0,namepos);
	lightname.append("/light/");
	if(!fileExists(lightname))
		_mkdir(lightname.c_str());
	lightname.append(filename.substr(namepos,filename.length()-namepos));

	extractSets(img,rowset);

	map<int,float> rowdist;

	computeRCDists(rowset,rowdist);

	//FILE*	fp = fopen("d:/rowdist.txt","w");
	//for(map<int,float>::iterator it=rowdist.begin();it!=rowdist.end();it++)		
	//fprintf(fp,"%d \t%f\n",(*it).first,(*it).second);
	//fclose(fp);

	//vector<float> vdist = smoothDist(rowdist,kernel);
	/*	fp = fopen("d:/kernel.txt","w");
	for(map<int,float>::iterator it=rowdist.begin();it!=rowdist.end();it++)		
	fprintf(fp,"%d \t%f\n",(*it).first,(*it).second);
	fclose(fp);*/

	//vector<Node> nodes =  findCriticalPoints(rowdist);
	vector<float> distInRegions(rowset.size());
	time_t start ,end ; 
	time(&start);	

	vector<Node> nodes;
	if(istest)
		nodes = linkage(distInRegions,rowset,rowdist,50,istest);
	else
		nodes = linkage(distInRegions,rowset,rowdist,25,istest);
	//nodes = mergeNodes(distInRegions,rowset,rowdist,nodes);
 // 
	//vector<Node> nodes = linkage(rowdist,15);
	nodes = mergeNodes(nodes);
		time(&end);
	double cost=difftime(end,start); 
	printf("time %lf\n",cost);
	saveSegmentImg(img,nodes,lightname.c_str());
	//vector<Node> iregions = convertNodeToRegion(nodes,rowset,img);
	return nodes;
}
vector<vector<Node>> loadImages(char *imagePath,vector<string> imageNames,bool overseg)
{
	vector<vector<Node>> imgRegons;
	for(int ii=0;ii<imageNames.size();ii++)	
	{
		string filename(imagePath);
		string lightname(imagePath);
		lightname.append("light/");
		lightname.append(imageNames[ii].substr(0,imageNames[ii].size()-4));

		filename.append(imageNames[ii]);
		cout<<imageNames[ii]<<endl;
		vector<vector<int>> rowset;
		cv::Mat image;
		vector<Node> iregions = loadImage(filename,rowset,image,overseg);
		imgRegons.push_back(iregions);		
	}
	return imgRegons;
}

void batchImageSimplify()
{
	char labeledpath[255]= "E:/data/Large-Chairs/test/labeldesc/";
	vector<string> labeledImg = getFileNames(labeledpath);
	char testedpath[255]= "E:/data/Large-Chairs/test/testdesc/";	
	vector<string> testedImg = getFileNames(testedpath);


	vector<vector<Node>> labeledImgRegions= loadImages(labeledpath,labeledImg,false);
	vector<vector<Node>> testImgRegons= loadImages(testedpath,testedImg,true);

	vector<pair<int,float>> matchparis;
	for(int i=0;i<testImgRegons.size();i++)
	{
		float mincost = FLT_MAX;
		int mindex = -1;
		vector<pair<int,float>> imatchparis;
		for(int j=0;j<labeledImgRegions.size();j++)
		{
			vector<pair<int,int>> pairs;
			float cost = matchRegions(testImgRegons[i],labeledImgRegions[j],pairs);
			imatchparis.push_back(pair<int,float>(j,cost));
			for(int ii=pairs.size()-1;ii>=0;ii--)
			{
				printf("%d --> %d\n",pairs[ii].first,pairs[ii].second);
				//matchLines(testImgRegons[pairs[ii].first],labeledImgRegions[pairs[ii].second]);
			}

		}
		sortPairwithIndex(imatchparis);

		string logname(testedpath);
		//logname.append("light/");
		logname.append(testedImg[i].substr(0,testedImg[i].size()-4));
		logname.append(".txt");
		fp = fopen(logname.c_str(),"w");
		for(int ii=0;ii<imatchparis.size();ii++)
			fprintf(fp,"%s \t%f\n",labeledImg[imatchparis[ii].first].c_str(),imatchparis[ii].second);
		fclose(fp);

		matchparis.push_back(imatchparis[0]);

	}
	/*
	vector<pair<int,float>> matchparis;
	for(int i=0;i<testImgRegons.size();i++)
	{
		float mincost = FLT_MAX;
		int mindex = -1;
		vector<pair<int,float>> imatchparis;
		for(int j=0;j<labeledImgRegions.size();j++)
		{
			int num1 = testImgRegons[i].size();
			int num2 = labeledImgRegions[j].size();
			vector<vector<float>> distmatrix(num1);
			for(int jj=0;jj<num1;jj++)
				distmatrix[jj].resize(num2,0);
			for(int jj=0;jj<num1;jj++)
				for(int kk=0;kk<num2;kk++)
				{
					float dist = hausdorff(testImgRegons[i][jj].line,labeledImgRegions[j][kk].line);
					distmatrix[jj][kk] = dist;
				}

			//printf("\n");
		
			float mcost = -1;
			vector<pair<int,int>> pairs = minCost(distmatrix, num1-1, num2-1, mcost);
			vector<pair<int,float>> costLabelToTest(labeledImgRegions[j].size());
			
			for(int ii=0;ii<costLabelToTest.size();ii++)
			{
				costLabelToTest[ii].first = -1;
				costLabelToTest[ii].second = 0;
			}
			for(int ii=0;ii<pairs.size();ii++)
			{
				int src = pairs[ii].first;//test
				int dst = pairs[ii].second;//label
				costLabelToTest[dst].second += distmatrix[src][dst]*testImgRegons[i][src].height;
				costLabelToTest[dst].first += testImgRegons[i][src].height;			
			}
			float sumcost = 0;
			for(int ii=0;ii<costLabelToTest.size();ii++)
			{
				pair<int,float> ipair = costLabelToTest[ii];
				if(ipair.first!=-1)
					sumcost += (ipair.second/ipair.first)*labeledImgRegions[j][ii].height;
			}
			//return sumcost;
			//cout<<testedImg[i]<<'\t'<<labeledImg[j]<<'\t'<<sumcost<<endl;
			if(sumcost<mincost)
			{
				mincost = sumcost;
				mindex = j;
			}
			imatchparis.push_back(pair<int,float>(j,sumcost));
			//for(int ii=pairs.size()-1;ii>=0;ii--)
			//	printf("%d --> %d\n",pairs[ii].first,pairs[ii].second);
		
		}
		sortPairwithIndex(imatchparis);
	
		string logname(testedpath);
		//logname.append("light/");
		logname.append(testedImg[i].substr(0,testedImg[i].size()-4));
		logname.append(".txt");
		FILE *fp = fopen(logname.c_str(),"w");
		for(int ii=0;ii<imatchparis.size();ii++)
			fprintf(fp,"%s \t%f\n",labeledImg[imatchparis[ii].first].c_str(),imatchparis[ii].second);
		fclose(fp);
		
		matchparis.push_back(imatchparis[0]);
		
	}*/
	for(int i=0;i<matchparis.size();i++)
	{
		cout<<testedImg[i]<<" "<<"is matched to "<<labeledImg[matchparis[i].first]<<" with the cost "<<matchparis[i].second<<endl;
	}


}