// Poster Program for the MLSS
// Weighted K-means & Cluster Merging for Image Segmentation
// Code by Manuel Arturo Deza

#include "cv.h"
#include "highgui.h"

#include <math.h>
#include <iostream>
#include <fstream>
#include <ctime>
#include <cstdlib>
#include <cstring>

using namespace std;

const int KK=3;
const int MAX_L=180000;
//const int MAX_G=6000;
const int MAX_Z=3000;
const int MAX_X=1000;//MAX Random cluster center generated
const int MAX_COL=1000;

void init_x_vector(short num_x,short *x);
void init_L_vector(IplImage Main,short *L);
void init_w_vector(float *w);
void init_col_vector(int num,short *col);

short get_I(short x, short y,IplImage Main);
short get_IX(short x, short y , IplImage Main);
short get_IY(short x, short y, IplImage Main);
short get_Imean(short x, short y, IplImage Main);

short get_score(short x1,short y1, short Cx,short Cy,IplImage Main, float *w);
short lowest_score(short Cx, short Cy, IplImage Main, float *w);

short assign_Label(short x1, short y1, IplImage Main, float *w, short num, short *z, short *L); 
void mass_assign_Label(IplImage Main, float *w, short num, short *z, short *L);

void recenter_clusters(IplImage Main, float *w, short num,short *z,short*L,short MT);
void merge_clusters(IplImage Main, short num, short *z, short *L, short MT);
void check_merges(short num, short *z);

//Generating cluster Functions
int gen_rand(int x);
float rand_dec(void);
void init_cluster_vector(short *z);
void gen_clusters(int n,short *z,int width, int height,IplImage Main,IplImage Main00);
void gen_grid_clusters(int &n,short *z,int width, int height,IplImage Main,IplImage Main00);


void Create_L_map(IplImage Main, short num, short*z,short *L);
void Create_L_map2(IplImage Main, short *col,short num, short*z,short *L);
void Create_L_map3(IplImage Main, short *col,short num, short*z,short *L);

void write_init_z(short num, short *z);
void copy_z(short num,short *z, short *z2);
void write_end_z(short num, short *z);
long double compute_L2_error(short num, short *z, short *z2);

short z_count(short num,short *z);

//Generate 0_Result.txt
void write_result(short num, short num2, short *z,short *z0,int iter, float *w,short MT, char *a);

int main( int argc, char *argv[])
{srand((unsigned)time(NULL));

IplImage *frame=cvLoadImage(argv[1]);
IplImage *frame_G=cvCreateImage(cvGetSize(frame),IPL_DEPTH_8U,1);
IplImage *frame_GC=cvLoadImage(argv[1]);
IplImage *frame_GC2=cvCreateImage(cvSize(frame_G->width+KK-1,frame_G->height+KK-1),IPL_DEPTH_8U,3);

IplImage *frame_L=cvCreateImage(cvGetSize(frame),IPL_DEPTH_8U,3);

IplImage *frame_S=cvCreateImage(cvGetSize(frame),IPL_DEPTH_8U,3);

CvPoint offset=cvPoint((KK-1)/2,(KK-1)/2);
cvCopyMakeBorder(frame_GC,frame_GC2,offset,IPL_BORDER_REPLICATE,cvScalarAll(0));

IplImage *test=cvCreateImage(cvGetSize(frame),IPL_DEPTH_8U,1);
cvNamedWindow("test",CV_WINDOW_AUTOSIZE);
cvNamedWindow("frame_L",CV_WINDOW_AUTOSIZE);

ofstream outerr("0_err_evo.txt");
ofstream outerr2("1_err_evo.txt");
int num=0;
short numz=0;
short MT=5;//Leave on MT=5;
short iter=100;
float w[6];
short z[MAX_Z],z0[MAX_Z],z2[MAX_Z];
short L[MAX_L];
short col[MAX_COL];
init_w_vector(w);
init_cluster_vector(z);
init_L_vector(*frame,L);
init_col_vector(num,col);

cvSmooth(frame,frame_S,CV_GAUSSIAN,3,3);
cvCvtColor(frame_S,frame_G,CV_BGR2GRAY);

//cvCvtColor(frame,frame_G,CV_BGR2GRAY);
cvEqualizeHist(frame_G,test);


short a=0;
short b=0;
short c=0;
long double err=0.0;
//int num=10;
a=get_score(20,20,40,40,*test,w);
b=lowest_score(40,40,*test,w);
cout<<a<<endl<<b<<endl;


//gen_clusters(num,z,frame->width,frame->height,*frame_GC2,*frame_GC);
gen_grid_clusters(num,z,frame->width,frame->height,*frame_GC2,*frame_GC);
//c=assign_Label(20,20,*test,w,num,z,L);
check_merges(num,z);//added
write_init_z(num,z);
copy_z(num,z,z0);

for(int kiki=0;kiki<iter;kiki++)
	{for (int i=0;i<test->height;i++)
		{for(int j=0;j<test->width;j++)
			{c=assign_Label(j,i,*test,w,num,z,L);
			}
		}
	if(kiki==0)
		{Create_L_map3(*frame_L,col,num,z,L);
		cvSaveImage("0_before.png",frame_L);
		}
	//Introduce recalculate z cluster Centers here.
	copy_z(num,z,z2);	
	recenter_clusters(*test,w,num,z,L,MT);
	merge_clusters(*test,num,z,L,MT);
	check_merges(num,z);//added
	err=compute_L2_error(num,z,z2);
	//recenter_clusters(*test,w,num,z,L,MT);//added
	Create_L_map3(*frame_L,col,num,z,L);//added
	cvShowImage("frame_L",frame_L);//added
	//cvWaitKey(0);//added
	numz=z_count(num,z);
	cout<<"iteration : "<<kiki<<endl;
	cout<<"clusters: "<<numz<<endl;	
	cout<<"L2_error: "<<err<<endl;
	outerr<<kiki<<" "<<err<<endl;
	outerr2<<err<<endl;
	}
//added

for (int i=0;i<test->height;i++)
	{for(int j=0;j<test->width;j++)
		{c=assign_Label(j,i,*test,w,num,z,L);
		}
	}
// don't put recenter_clusters(*test,w,num,z,L,MT);
//added

write_end_z(num,z);
numz=z_count(num,z);
cout<<"started with: "<<num<<" clusters"<<endl;
cout<<"numz: "<<numz<<endl;

Create_L_map3(*frame_L,col,num,z,L);
cvSaveImage("0_after.png",frame_L);

char *res= "0_Result.txt";
write_result(num,numz,z,z0,iter,w,MT,res);


cvNamedWindow("frame",CV_WINDOW_AUTOSIZE);
//cvNamedWindow("frame_L",CV_WINDOW_AUTOSIZE);
cvNamedWindow("frame_S",CV_WINDOW_AUTOSIZE);
//cvNamedWindow("test",CV_WINDOW_AUTOSIZE);
cvNamedWindow("frame_G",CV_WINDOW_AUTOSIZE);
cvNamedWindow("frame_GC",CV_WINDOW_AUTOSIZE);
cvNamedWindow("frame_GC2",CV_WINDOW_AUTOSIZE);


cvShowImage("frame",frame);
cvShowImage("frame_L",frame_L);
cvShowImage("frame_G",frame_G);
cvShowImage("frame_S",frame_S);
//cvShowImage("test",test);
cvShowImage("frame_GC",frame_GC);
cvShowImage("frame_GC2",frame_GC2);

cvSaveImage("frame.jpg",frame);
cvSaveImage("frame_GC.jpg",frame_GC);
cvSaveImage("EQ_frame.jpg",test);
cvSaveImage("Result.jpg",frame_L);

cvWaitKey(0);

cvReleaseImage(&frame);
cvReleaseImage(&frame_G);
cvReleaseImage(&frame_S);
cvReleaseImage(&frame_L);
cvReleaseImage(&frame_GC);
cvReleaseImage(&frame_GC2);
cvReleaseImage(&test);

cvDestroyWindow("frame");
cvDestroyWindow("frame_G");
cvDestroyWindow("frame_S");
cvDestroyWindow("frame_L");
cvDestroyWindow("frame_GC");
cvDestroyWindow("frame_GC2");
cvDestroyWindow("test");
}

void init_x_vector(short num_x,short *x)
{
for (int i=0;i<num_x*5;i=i+5)
	{x[i+0]=-1;
	x[i+1]=-1;
	x[i+2]=-1;
	x[i+3]=-1;
	x[i+4]=-1;
	}
}

void init_L_vector(IplImage Main,short *L)
{

int height=Main.height;
int width=Main.width;
int step=Main.widthStep;
int channel=Main.nChannels;
uchar *data;
data=(uchar *)Main.imageData;

for(int i=0;i<width*height;i++)
	{L[i]=-1;
	}
}

void init_w_vector(float *w)
{
w[0]=5.0;w[1]=5.0;w[2]=3.0;w[3]=2.0;w[4]=2.0;w[5]=3.0;
//w[0]=2;w[1]=2;w[2]=3;w[3]=5;w[4]=5;
float wm=0;
wm=w[0]*w[0]+w[1]*w[1]+w[2]*w[2]+w[3]*w[3]+w[4]*w[4]+w[5]*w[5];
wm=sqrt(wm);
w[0]=w[0]/wm;
w[1]=w[1]/wm;
w[2]=w[2]/wm;
w[3]=w[3]/wm;
w[4]=w[4]/wm;
w[5]=w[5]/wm;
}

void init_col_vector(int num, short *col)
{

//int L_red[20],L_blue[20],L_green[20];

for(int i=0;i<num;i++)
	{col[3*i]=gen_rand(255);
	col[3*i+1]=gen_rand(255);
	col[3*i+2]=gen_rand(255);
	cout<<col[3*i]<<" "<<col[3*i+1]<<" "<<col[3*i+2]<<" "<<endl;
	}

}

short get_I(short x, short y, IplImage Main)
{
int height=Main.height;
int width=Main.width;
int step=Main.widthStep;
int channel=Main.nChannels;
uchar *data;
data=(uchar *)Main.imageData;

return (short(data[y*step+x*channel]));

}

short get_IX(short x, short y, IplImage Main)
{

int height=Main.height;
int width=Main.width;
int step=Main.widthStep;
int channel=Main.nChannels;
uchar *data;
data=(uchar *)Main.imageData;

short Gx[9];
Gx[0]=-1;Gx[1]=-2;Gx[2]=-1;
Gx[3]=0;Gx[4]=0;Gx[5]=0;
Gx[6]=1;Gx[7]=2;Gx[8]=1;

short aux[9];
aux[0]=data[(y-1)*width+(x-1)*channel]; aux[1]=data[(y-1)*width+(x+0)*channel]; aux[2]=data[(y-1)*width+(x+1)*channel];
aux[3]=data[(y+0)*width+(x-1)*channel]; aux[4]=data[(y+0)*width+(x+0)*channel]; aux[5]=data[(y+0)*width+(x+1)*channel];
aux[6]=data[(y+1)*width+(x-1)*channel]; aux[7]=data[(y+1)*width+(x+0)*channel]; aux[8]=data[(y+1)*width+(x+1)*channel];

short score=0;

for (int i=0;i<9;i++)
	{score=score+aux[i]*Gx[i];
	}

return score;
}

short get_IY(short x, short y, IplImage Main)
{

int height=Main.height;
int width=Main.width;
int step=Main.widthStep;
int channel=Main.nChannels;
uchar *data;
data=(uchar *)Main.imageData;

short Gy[9];
Gy[0]=-1;Gy[1]=0;Gy[2]=1;
Gy[3]=-2;Gy[4]=0;Gy[5]=2;
Gy[6]=-1;Gy[7]=0;Gy[8]=1;

short aux[9];
aux[0]=data[(y-1)*width+(x-1)*channel]; aux[1]=data[(y-1)*width+(x+0)*channel]; aux[2]=data[(y-1)*width+(x+1)*channel];
aux[3]=data[(y+0)*width+(x-1)*channel]; aux[4]=data[(y+0)*width+(x+0)*channel]; aux[5]=data[(y+0)*width+(x+1)*channel];
aux[6]=data[(y+1)*width+(x-1)*channel]; aux[7]=data[(y+1)*width+(x+0)*channel]; aux[8]=data[(y+1)*width+(x+1)*channel];

short score=0;

for (int i=0;i<9;i++)
	{score=score+aux[i]*Gy[i];
	}

return score;
}

short get_score(short x1,short y1, short Cx,short Cy,IplImage Main, float *w)
{
short score[6];
score[0]=0;score[1]=0;score[2]=0;score[3]=0;score[4]=0;score[5]=0;

score[0]=abs(x1-Cx);
score[1]=abs(y1-Cy);
score[2]=abs(get_I(x1,y1,Main)-get_I(Cx,Cy,Main));
score[3]=abs(get_IX(x1,y1,Main)-get_IX(Cx,Cy,Main));
score[4]=abs(get_IY(x1,y1,Main)-get_IY(Cx,Cy,Main));
score[5]=abs(get_Imean(x1,y1,Main)-get_Imean(Cx,Cy,Main));

short total=0;
total=sqrt(w[0]*score[0]*score[0]+w[1]*score[1]*score[1]+w[2]*score[2]*score[2]+w[3]*score[3]*score[3]+w[4]*score[4]*score[4]+w[5]*score[5]*score[5]);

return total;
}

short lowest_score(short Cx, short Cy, IplImage Main, float *w)
{

int height=Main.height;
int width=Main.width;
int step=Main.widthStep;
int channel=Main.nChannels;
uchar *data;
data=(uchar *)Main.imageData;

ofstream outfile("Score_Lists_XY.txt");

short aux1=10000;short aux2=0;
short sw=0;

short xx=0,yy=0;

for (int i=0;i<height;i++)
	{for(int j=0;j<width;j++)
		{sw=0;
		//Check to see if point(j,i) is a Cluster.
		//Don't forget to check to see if a random point is generated twice!!
		//Glitches may occur!!
		if((j==Cx)&&(i==Cy))
			{sw=1;
			}		
		if(sw==0)
			{
			aux2=get_score(j,i,Cx,Cy,Main,w);
			outfile<<"("<<j<<","<<i<<"): "<<aux2<<"\n";
			if(aux2<aux1)
				{aux1=aux2;
				}
			}		
		}
	}

return aux1;
}


int gen_rand(int x)
{
	int random_integer;
		random_integer=(rand()%x);
return random_integer;
}


float rand_dec(void)
{
	float val = (double)rand()/(double)RAND_MAX;

return val;
}

void init_cluster_vector(short *z)
{
for(int i=0;i<MAX_Z;i++)
	{z[i]=-1;
	}
}

void gen_clusters(int n,short *z,int width, int height, IplImage Main,IplImage Main00)
{
ofstream outfile("Cluster_Points.txt");
short x1=0, y1=0;
for(int i=0;i<n;i++)
	{x1=gen_rand(width);
	y1=gen_rand(height);
	cout<<"x1: "<<x1<<"; y1: "<<y1<<";L1: "<<i<<endl;
	z[3*i]=x1;
	z[3*i+1]=y1;
	z[3*i+2]=i;
	outfile<<"x1: "<<x1<<"; y1: "<<y1<<"; L1: "<<i<<"\n";
	cvCircle(&Main,cvPoint(x1+(KK-1)/2,y1+(KK-1)/2),5,CV_RGB(255,0,0),1,8);
	cvCircle(&Main00,cvPoint(x1,y1),5,CV_RGB(255,0,0),1,8);
	}
}

short assign_Label(short x1, short y1, IplImage Main, float *w, short num, short *z, short *L)
{

int height=Main.height;
int width=Main.width;
int step=Main.widthStep;
int channel=Main.nChannels;
uchar *data;
data=(uchar *)Main.imageData;
// to test
// CORRECT ofstream outfile("Score_Clusters_Point.txt");

short aux1=10000;short aux2=0;
short sw=0;
short Lab=0;

short xx=0,yy=0;

for (int i=0;i<num;i++)
	{
		sw=0;
		//Check to see if point(j,i) is a Cluster.
		//Don't forget to check to see if a random point is generated twice!!
		//Glitches may occur!!
		if((x1==z[3*i])&&(y1==z[3*i+1]))
			{sw=1;//Lab=z[3*i+2];//Check!
			}		
		if(sw==0)
			{
			aux2=get_score(x1,y1,z[3*i],z[3*i+1],Main,w);
			// CORRECT outfile<<"("<<z[3*i]<<","<<z[3*i+1]<<"): "<<aux2<<"\n";
			if(aux2<=aux1)
				{aux1=aux2;
				Lab=z[3*i+2];
				}			
			}		
		
	}
L[y1*width+x1]=Lab;
// CORRECT cout<<"Label: "<<Lab<<endl;
return aux1;

}

void Create_L_map(IplImage Main, short num, short *z,short *L)
{

int height=Main.height;
int width=Main.width;
int step=Main.widthStep;
int channel=Main.nChannels;
uchar *data;
data=(uchar *)Main.imageData;

//Vector value will depend on number of clusters!!
int L_red[20],L_blue[20],L_green[20];

for(int i=0;i<num;i++)
	{L_red[i]=gen_rand(255);
	L_blue[i]=gen_rand(255);
	L_green[i]=gen_rand(255);
	cout<<L_red[i]<<" "<<L_blue[i]<<" "<<L_green[i]<<" "<<endl;
	}

for (int i=0;i<height;i++)
	{for( int j=0; j<width;j++)
		{for (int k=0;k<num;k++)
			{if(L[i*width+j]==z[3*k+2])
				{data[i*step+j*channel]=L_blue[k];
				data[i*step+j*channel+1]=L_green[k];
				data[i*step+j*channel+2]=L_red[k];
				}
			}
			
		}
	}

}

void Create_L_map2(IplImage Main, short *col,short num, short*z,short *L)
{
int height=Main.height;
int width=Main.width;
int step=Main.widthStep;
int channel=Main.nChannels;
uchar *data;
data=(uchar *)Main.imageData;

//Vector value will depend on number of clusters!!
for (int i=0;i<height;i++)
	{for( int j=0; j<width;j++)
		{for (int k=0;k<num;k++)
			{if(L[i*width+j]==z[3*k+2])
				{data[i*step+j*channel]=col[3*k];
				data[i*step+j*channel+1]=col[3*k+1];
				data[i*step+j*channel+2]=col[3*k+2];
				}
			}
			
		}
	}

}

void mass_assign_Label(IplImage Main, float *w, short num, short *z, short *L)
{

int height=Main.height;
int width=Main.width;
int step=Main.widthStep;
int channel=Main.nChannels;
uchar *data;
data=(uchar *)Main.imageData;

ofstream outfile("ScoreAll_Clusters_Point.txt");
ofstream out2("Label_List.txt");

short aux1=10000;short aux2=0;
short sw=0;
short Lab=0;

//short xx=0,yy=0;
cvNamedWindow("frame",CV_WINDOW_AUTOSIZE);
for(int yy=0;yy<height;yy++)
	{for(int xx=0;xx<width;xx++) 
		{for (int i=0;i<num;i++)
			{
				sw=0;
				//Check to see if point(j,i) is a Cluster.
				//Don't forget to check to see if a random point is generated twice!!
				//Glitches may occur!!
				if((xx==z[3*i])&&(yy==z[3*i+1]))
					{sw=1;
					}		
				if(sw==0)
					{
					aux2=get_score(xx,yy,z[3*i],z[3*i+1],Main,w);
					outfile<<"("<<z[3*i]<<","<<z[3*i+1]<<"): "<<aux2<<"\n";
					if(aux2<aux1)
						{aux1=aux2;
						Lab=z[3*i+2];
						}			
					}		
		
			}
		L[yy*width+xx]=Lab;
		cout<<"Label:["<<xx<<"]["<<yy<<"]="<<Lab<<endl;
		out2<<"Label:["<<xx<<"]["<<yy<<"]="<<Lab<<endl;
//return aux1;	
		}
	}


}

void recenter_clusters(IplImage Main, float *w, short num,short *z,short*L, short MT)
{

int height=Main.height;
int width=Main.width;
int step=Main.widthStep;
int channel=Main.nChannels;
uchar *data;
data=(uchar *)Main.imageData;

int z_x=0,z_y=0,cont1=0;

for(int k=0;k<num;k++)
	{
	z_x=0;z_y=0;cont1=0;
	for (int i=0;i<height;i++)
		{for(int j=0;j<width;j++)
			{if(L[i*width+j]==k)
				{z_x=z_x+j;
				z_y=z_y+i;
				cont1=cont1+1;
				}
			}
		}
	if(cont1!=0)	
		{z_x=short(z_x/cont1);
		z_y=short(z_y/cont1);
		z[3*k]=z_x;
		z[3*k+1]=z_y;
		cout<<k<<"!=0"<<endl;
		cout<<"z_x: "<<z_x<<"; z_y: "<<z_y<<endl;
		}
	else
		{
		}	
	}

}

void write_init_z(short num, short *z)
{
ofstream outfile("z_cluster_init.txt");

for (int k=0;k<num;k++)
	{outfile<<z[3*k]<<" "<<z[3*k+1]<<" "<<z[3*k+2]<<"\n";
	}

}

void write_end_z(short num, short *z)
{
ofstream outfile("z_cluster_end.txt");

for (int k=0;k<num;k++)
	{outfile<<z[3*k]<<" "<<z[3*k+1]<<" "<<z[3*k+2]<<"\n";
	}

}

short z_count(short num,short *z)
{

ofstream outfile("Cluster_end.txt");
short zz=0;
for(int i=0;i<num;i++)
	{for(int k=0;k<num;k++)
		{if(z[3*k+2]==i)
			{zz++;
			break;
			}
		}
	}
outfile<<"Cluster starts with : "<<num<<" . Ends with : "<<zz<<" . "<<endl;
return zz;
}

void write_result(short num, short num2,short *z,short *z0, int iter, float *w, short MT,char *a)
{
ofstream outfile(a);
outfile<<"Segmentation Report : "<<endl;
outfile<<"----------------------"<<endl<<endl;
outfile<<"Initial Number of Clusters: "<<num<<endl;
outfile<<"Final Number of Clusters: "<<num2<<endl;
outfile<<"Number of iterations: "<<iter<<endl;
outfile<<"Weight vector values: "<<endl;
for(int k=0;k<5;k++)
	{outfile<<w[k]<<" ";
	}
outfile<<endl;
outfile<<"Merging Threshold: "<<MT<<endl<<endl;

outfile<<"Initial Clusters:"<<endl;
outfile<<"-----------------"<<endl;
for (int k=0;k<num;k++)
	{outfile<<z0[3*k]<<" "<<z0[3*k+1]<<" "<<z0[3*k+2]<<"\n";
	}


outfile<<endl<<endl<<"End Clusters:"<<endl;
            outfile<<"-------------"<<endl;
for (int k=0;k<num;k++)
	{outfile<<z[3*k]<<" "<<z[3*k+1]<<" "<<z[3*k+2]<<"\n";
	}

outfile<<endl<<endl;

}

void copy_z(short num,short *z, short *z2)
{
for (int k=0;k<num;k++)
	{z2[3*k]=z[3*k];
	z2[3*k+1]=z[3*k+1];
	z2[3*k+2]=z[3*k+2];
	//cout<<"z_copied!"<<endl;
	}
}

void Create_L_map3(IplImage Main, short *col,short num, short *z,short *L)
{
int height=Main.height;
int width=Main.width;
int step=Main.widthStep;
int channel=Main.nChannels;
uchar *data;
data=(uchar *)Main.imageData;

//Vector value will depend on number of clusters!!
for (int i=0;i<height;i++)
	{for( int j=0; j<width;j++)
		{for (int k=0;k<num;k++)
			{if(L[i*width+j]==z[3*k+2])
				{data[i*step+j*channel]=col[3*k];
				data[i*step+j*channel+1]=col[3*k+1];
				data[i*step+j*channel+2]=col[3*k+2];
				}
			}
			
		}
	}

for(int k=0;k<num;k++)
	{cvCircle(&Main,cvPoint(z[3*k],z[3*k+1]),5,CV_RGB(0,0,0),1,8);
	}
}

void merge_clusters(IplImage Main, short num, short *z, short *L, short MT)
{
int height=Main.height;
int width=Main.width;
int step=Main.widthStep;
int channel=Main.nChannels;
uchar *data;
data=(uchar *)Main.imageData;

short aux1=0,aux2=0;
float aux0=0;


for(int k1=0;k1<num-1;k1++)
	{for(int k2=k1+1;k2<num;k2++)
		{aux0=pow(((z[3*k1]-z[3*k2])*(z[3*k1]-z[3*k2])+((z[3*k1+1]-z[3*k2+1])*(z[3*k1+1]-z[3*k2+1]))),0.5);
		if(aux0<=MT)
			{/*
				if(abs(z[3*k1]-z[3*k2])+abs(z[3*k1+1]-z[3*k2+1])>MT)				
					{aux1=round((z[3*k1]+z[3*k2])/2);
					aux2=round((z[3*k1+1]+z[3*k2+1])/2);
					z[3*k1]=aux1;z[3*k2]=aux1;
					z[3*k1+1]=aux2;z[3*k2+1]=aux2;
					}
				else	
					{z[3*k1]=z[3*k2];z[3*k1+1]=z[3*k2+1];
					}
				*/	
				//aux1=round((z[3*k1]+z[3*k2])/2);
				//aux2=round((z[3*k1+1]+z[3*k2+1])/2);
				//z[3*k1]=aux1;z[3*k2]=aux1;
				//z[3*k1+1]=aux2;z[3*k2+1]=aux2;
				cout<<"merge! : ("<<z[3*k2]<<","<<z[3*k2+1]<<") and ("<<z[3*k1]<<","<<z[3*k1+1]<<")"<<endl;				
				z[3*k2]=z[3*k1];z[3*k2+1]=z[3*k1+1];
				z[3*k2+2]=z[3*k1+2];
				//cout<<"merge!"<<endl;
			}
		}
	}


}

void check_merges(short num, short *z)
{
short aux1=0,aux2=0,aux3=0;

for(int k1=0;k1<num-1;k1++)
	{	aux1=z[3*k1];
		aux2=z[3*k1+1];
		aux3=z[3*k1+2];
	for(int k2=k1+1;k2<num;k2++)
		{if((aux1!=z[3*k2])&&(aux2!=z[3*k2+1])&&(aux3==z[3*k2+2]))
			{
			z[3*k2]=aux1;
			z[3*k2+1]=aux2;
			//z[k2+2]=aux3;
			cout<<"checked!"<<endl;
			}
		}
	}

}

void gen_grid_clusters(int &n,short *z,int width, int height,IplImage Main,IplImage Main00)
{

ofstream outfile("Cluster_Points_Grid.txt");
short x1=0, y1=0;
int k=0;
for(int i=0;i<height;i=i+height/10)
	{for(int j=0;j<width;j=j+width/10)	
		{//x1=gen_rand(10)*pow(-1,k)+j;
		//y1=gen_rand(10)*pow(-1,k)+i;
		x1=gen_rand(10)+j;
		y1=gen_rand(10)+i;
		cout<<"x1: "<<x1<<"; y1: "<<y1<<";L1: "<<k<<endl;
		z[3*k]=x1;
		z[3*k+1]=y1;
		z[3*k+2]=k;
		outfile<<"x1: "<<x1<<"; y1: "<<y1<<"; L1: "<<k<<"\n";
		cvCircle(&Main,cvPoint(x1+(KK-1)/2,y1+(KK-1)/2),5,CV_RGB(255,0,0),1,8);
		cvCircle(&Main00,cvPoint(x1,y1),5,CV_RGB(255,0,0),1,8);
		k++;		
		}
	}
n=k;
}

short get_Imean(short x, short y, IplImage Main)
{
int height=Main.height;
int width=Main.width;
int step=Main.widthStep;
int channel=Main.nChannels;
uchar *data;
data=(uchar *)Main.imageData;

short Gx[9];
Gx[0]=1;Gx[1]=2;Gx[2]=1;
Gx[3]=2;Gx[4]=2;Gx[5]=2;
Gx[6]=1;Gx[7]=2;Gx[8]=1;

short aux[9];
aux[0]=data[(y-1)*width+(x-1)*channel]; aux[1]=data[(y-1)*width+(x+0)*channel]; aux[2]=data[(y-1)*width+(x+1)*channel];
aux[3]=data[(y+0)*width+(x-1)*channel]; aux[4]=data[(y+0)*width+(x+0)*channel]; aux[5]=data[(y+0)*width+(x+1)*channel];
aux[6]=data[(y+1)*width+(x-1)*channel]; aux[7]=data[(y+1)*width+(x+0)*channel]; aux[8]=data[(y+1)*width+(x+1)*channel];

float score=0;

for (int i=0;i<9;i++)
	{score=score+aux[i]*Gx[i];
	}
score=score/14;

return short(score);

}

long double compute_L2_error(short num, short *z, short *z2)
{
long double error=0.0;
for(int i=0;i<num;i++)
	{error=error+(z2[i]-z[i])*(z2[i]-z[i]);
	}
return error;
}