#include <stdlib.h>
#include <stdio.h>
#include <math.h>
#include <vector>
#include <list>
#include <cv.h>
#include <highgui.h>

#define INF 999999999
#define KERNEL 3
#define K 9
#define BACKGROUND 0
#define OBJECT 255


using namespace std;

typedef struct{
	int mano;
	int val0, val1, val2;
}pixel;
typedef struct{
	char name[100];
}imgName;
typedef struct{
	int x;
	int y;
}posicion;

int trackbar=0;

typedef struct{
	double dist;
	int valor;
}clasif;
void my_mouse_callback(int event, int x, int y, int flags, void* param){
	IplImage** imgs = (IplImage**) param;
	IplImage *image=imgs[trackbar];
	uchar *data=(uchar *)image->imageData;
	int height,width;
	int step,channels;	
	height    = image->height;
	width     = image->width;
	step	  = image->widthStep;
	channels  = image->nChannels;
	if(event==CV_EVENT_LBUTTONUP){
		int R,G,B;
		R=data[y*step+x*channels+2];
		G=data[y*step+x*channels+1];
		B=data[y*step+x*channels];

		printf("R=%d G=%d B=%d\n\tR-B=%d   R-G=%d   B-G=%d\n\n",R,G,B,R-B,R-G,B-G);
	}
}
void visor(int position){	
}




double eval_clasif(vector<pixel> entr , vector<pixel> test, double *v_error,int k=K, int kernel = KERNEL);
int clasif_vector(vector<pixel> entr, vector<pixel> test_pixel, int k=K, int kernel=KERNEL);
void dilate( IplImage  **img, int num);	
void erode(	IplImage  **img, int num);	
void cierre( IplImage  **img, int num);
void apertura (IplImage  **img, int num);
void distancia(IplImage **imagen);


int main(){
	FILE * f_Entrenamiento_RGB;
	FILE * f_Test_RGB;
	char buffer[200];
	char *p_buffer;
	vector<pixel> entrenamiento;
	vector<pixel> test;
	int v;
	pixel pix_aux;
	double error;
	IplImage * aux_img;
	int c;
	uchar *data3;

	FILE * fmano;
	fopen_s(&fmano,"MANO.txt","r");
	if(fmano==NULL){
		printf("Error en el fichero MANO.txt\n");
		exit(0);
	}

	int contador=0;
	vector<imgName> names;
	imgName auxname={0};
	//Leer cuadrados Mano
	while( fgets(buffer,200,fmano) != NULL ){
		if(buffer[0] != '\t'){
			int length=strlen(buffer);
			strncpy_s(auxname.name, buffer,length-1);
			names.push_back(auxname);
		}
	}

	IplImage** img=new IplImage* [names.size()];

	
	for(int i=0; i<names.size();i++){
		img[i] = cvLoadImage(names[i].name,1);		
	}
	if(img[0]==NULL) 
		return -1;

	
	
	
	cvNamedWindow( "Original", CV_WINDOW_AUTOSIZE );
	cvSetMouseCallback("Original",my_mouse_callback,(void*) img);
	cvCreateTrackbar("Foto","Original",&trackbar,names.size()-1, visor);	


	uchar *data;
	int height,width;
	int step,channels,step2;
	

	IplImage** img2=new IplImage* [names.size()];
	IplImage** img3=new IplImage* [names.size()];
	posicion* pos=new posicion[names.size()];
	posicion* pos2=new posicion[names.size()];

	for(int i=0; i<names.size();i++){
	
		img2[i] = cvCreateImage(cvGetSize(img[0]),8,1);

	}

	
	cvNamedWindow( "Clasificada", CV_WINDOW_AUTOSIZE );
	cvShowImage("Clasificada",img2[trackbar]);

	cvNamedWindow( "Distancia", CV_WINDOW_AUTOSIZE );
	
	cvSetMouseCallback("Clasificada",my_mouse_callback,(void*) img2);

	fopen_s(&f_Entrenamiento_RGB,"Entrenamiento.txt","r");
	if(f_Entrenamiento_RGB==NULL){
		printf("Error en el fichero Entrenamiento.txt\n");
		exit(0);
	}

	/*fopen_s(&f_Test_RGB,"Test.txt","r");
	if(f_Test_RGB==NULL){
		printf("Error en el fichero Test.txt\n");
		exit(0);
	}*/

	int e,t;
	fgets(buffer,200,f_Entrenamiento_RGB);
	sscanf_s(buffer,"%d",&e);

	/*fgets(buffer,200,f_Test_RGB);
	sscanf_s(buffer,"%d",&t);
	
	if(e!=t){
		printf("Error en los formatos de caracteristicas\n");
		exit(0);
	}

	

	//Leer entrenamiento

	
	while( fgets(buffer,200,f_Entrenamiento_RGB) != NULL ){
		//if(buffer[0] == '0' || buffer[0] == '1' ){ //Es información de un pixel
		v= entrenamiento.size();
		p_buffer=buffer;
		for( int i=0; i < e; i++){
			sscanf_s(p_buffer,"%d %d %d ",&pix_aux.val0, &pix_aux.val1, &pix_aux.val2);
			entrenamiento.push_back(pix_aux);
			
			for(int j=0; j<3; j++){
				while(*p_buffer != ' ')
					p_buffer++;
				p_buffer++;
			}
		}
		
		sscanf_s(p_buffer,"%d",&pix_aux.mano);
		while( v < entrenamiento.size()){
			entrenamiento[v].mano=pix_aux.mano;	
			v++;
		}
		
		//}
	}
	//Leer test
	while( fgets(buffer,200,f_Test_RGB) != NULL ){
		//if(buffer[0] == '0' || buffer[0] == '1' ){ //Es información de un pixel
		v= test.size();
		p_buffer=buffer;
		for( int i=0; i < e; i++){
			sscanf_s(p_buffer,"%d %d %d ",&pix_aux.val0, &pix_aux.val1, &pix_aux.val2);
			test.push_back(pix_aux);
			for(int j=0; j<3; j++){
				while(*p_buffer != ' ')
					p_buffer++;
				p_buffer++;
			}
		}
		
		sscanf_s(p_buffer,"%d",&pix_aux.mano);
		while( v < test.size() ) {
			test[v].mano=pix_aux.mano;
			v++;
		}
		//}
	}

	
	int valor=0;

	double * v_error= new double[K];
	for(int i=0; i<K; i++){
		v_error[i]=0;
	}

	//clasificador
	error= eval_clasif(entrenamiento,test,v_error,K,KERNEL);
	*/

	int dif_v2_v0; //Rojo_Azul
	int dif_v2_v1; //Rojo_Verde
	int dif_v0_v1; //Azul_Verde


	vector<pixel> test_pixel;
	pixel aux;
	int borde=int(  sqrt( double(e) )/2  );

	for(int n=0; n<names.size();n++){

		// get the image data
		height    = img[n]->height;
		width     = img[n]->width;
		step	  = img[n]->widthStep;
		channels  = img[n]->nChannels;
		data      = (uchar *)img[n]->imageData;
		uchar *data2    =  (uchar *)img2[n]->imageData;
		step2	  = img2[n]->widthStep;


		for(int i= borde; i < height; i++){
			for(int j= borde; j < width; j++){ 

				aux.val0 = data[(i)*step+(j)*channels+0];
				aux.val1 = data[(i)*step+(j)*channels+1];
				aux.val2 = data[(i)*step+(j)*channels+2];

				dif_v2_v0 = aux.val2-aux.val0;
				dif_v2_v1 = aux.val2-aux.val1;
				dif_v0_v1 = aux.val0-aux.val1;

				//if( (dif_v2_v0> 37 && dif_v2_v0<79)  && (dif_v2_v1>8 && dif_v2_v1<68) && (dif_v0_v1>-45 && dif_v0_v1<-10)  ){ //MuchoMas
				//if( (dif_v2_v0> 27 && dif_v2_v0<89)  && (dif_v2_v1>-2 && dif_v2_v1<78) && (dif_v0_v1>-55 && dif_v0_v1<0)  ){ //Mas
				if( (dif_v2_v0> 25 && dif_v2_v0<125)  && (dif_v2_v1>-15 && dif_v2_v1<100) && (dif_v0_v1>-70 && dif_v0_v1<5)  ){ //Tracker v.4 Menos
				//if( (dif_v2_v0> 15 && dif_v2_v0<135)  && (dif_v2_v1>-25 && dif_v2_v1<110) && (dif_v0_v1>-80 && dif_v0_v1<15)  ){ // MuchoMenos
					data2[i*step2+j]=255;
				}else {
					data2[i*step2+j]=0;
				}




				/*
				else{
						for(int m = -1*borde; m <= borde; m++){
							for(int n = -1*borde; n <= borde; n++){

								aux.val0 = data[(i+m)*step+(j+n)*channels+0];
								aux.val1 = data[(i+m)*step+(j+n)*channels+1];
								aux.val2 = data[(i+m)*step+(j+n)*channels+2];

							
								test_pixel.push_back(aux);
							}
						}
					data2[i*step2+j] = 255* clasif_vector( entrenamiento, test_pixel,3,1);
					test_pixel.clear();
				}*/
			}
		}

		//cierre(&img2[n],1);
		//apertura(&img2[n],1);
		dilate(&img2[n],1);
		erode(&img2[n],1);

		CvMemStorage *mem;
		CvSeq *contours, *ptr;

		mem = cvCreateMemStorage(0);
		cvFindContours(img2[n], mem, &contours, sizeof(CvContour), CV_RETR_EXTERNAL,CV_CHAIN_APPROX_SIMPLE, cvPoint(0,0));
		cvZero(img2[n]);
	
		int cont=1;
		int valor=0;
		img3[n]=cvCreateImage(cvGetSize(img2[n]),8,1);
		cvZero(img3[n]);

		for (ptr = contours ; ptr != NULL; ptr = ptr->h_next, cont++) {
			if(valor < ptr->total){
				valor=ptr->total;
			}
		}
		for (ptr = contours ; ptr != NULL; ptr = ptr->h_next) {
			CvScalar color = cvScalar(255);
			if(ptr->total==valor){
				cvDrawContours(img3[n], ptr, cvScalar(255), cvScalar(255), -1, CV_FILLED, 8, cvPoint(0,0));
				cvDrawContours(img2[n], ptr, cvScalar(255), cvScalar(255), -1, CV_FILLED, 8, cvPoint(0,0));
			}
			else{
				color = cvScalar(  rand()&200);
				cvDrawContours(img2[n], ptr, color, color, -1, CV_FILLED, 8, cvPoint(0,0));
			}
		}

		
		distancia(&img3[n]);

		data3    =  (uchar *)img3[n]->imageData;

		int index;
		int min=0;
		
		pos[n].x=0;
		pos[n].y=0;

		for(int i= 0; i < height; i++){
			for(int j= 0; j < width; j++){
				index = (i)*step2+(j);
				if( data3[index]>min){
					min=data3[index];
					pos2[n].x=pos[n].x;
					pos2[n].y=pos[n].y;

					pos[n].x=j;
					pos[n].y=i;
				}

			}
		}

		cvCircle(img[n],cvPoint(pos[n].x,pos[n].y),30,cvScalar(255,0,0),5);
		cvCircle(img3[n],cvPoint(pos[n].x,pos[n].y),5,cvScalar(255,0,0),5);
		cvCircle(img3[n],cvPoint(pos2[n].x,pos2[n].y),3,cvScalar(150,0,0),3);


	}

	char BINname[100]="BIN";
	for(int i=0; i<names.size();i++){
		strcat(BINname,names[i].name);		
		cvSaveImage(BINname,img2[i]);
		BINname[3]=0;
	}


	int key;
	while(1){
		key=(cvWaitKey(10) & 255);
		cvShowImage("Original",img[trackbar]);
		cvShowImage("Clasificada",img2[trackbar]);
		cvShowImage("Distancia",img3[trackbar]);

		if(key==27) break;
	}


	return 0;
}


bool clasif_comp( clasif first, clasif second)
{
	if(first.dist < second.dist)
		return true;
	else return false;
}

double eval_clasif(vector<pixel> entr , vector<pixel> test, double *v_error,int k, int kernel){

	int dist = INF;
	double d;
	list<clasif> c;
	double error=0;
	int *valor=new int[k];

	int v0,v1,v2;

	clasif c_aux;
	
	int incr = kernel*kernel;
	int a0 = int(incr/2);
	
	int testSize=test.size();
	for(int i=0; i < testSize; i+=incr){

		c_aux.dist=INF;
		c_aux.valor=INF;

		for(int m=0; m<k; m++)//inicializar list con valores INF.
			c.push_back(c_aux);

		for(int j=0; j< entr.size(); j+=incr){
			d=0;
			for(int l=0; l<incr; l++){
				v0= (test[i+l].val0 - entr[j+l].val0);
				v1= (test[i+l].val1 - entr[j+l].val1);
				v2= (test[i+l].val2 - entr[j+l].val2);
				d+=double(v0*v0+v1*v1+v2*v2);
			}
			if(d<c.back().dist){
				c_aux.dist=d;
				c_aux.valor= entr[j+a0].mano;
				c.push_back(c_aux);
				c.sort(clasif_comp);
				c.pop_back();
			}
		}

		for( int z=0; z<k; z++){
			valor[z]=0;
		}
		for(int n = 0; n < k; n++){
			for( int z=n; z<k; z++){
				valor[z]+=c.front().valor;
			}
			c.pop_front();			
		}
		for( int z=0; z<k; z++){

			if( valor[z] >int( z/2.0 + 0.5) ){
				valor[z]=1;
			}
			else{
				valor[z]=0;
			}
			if(valor[z]!=test[i+a0].mano){
				v_error[z]+=1;
			}

		}
	}

	for( int z=0; z<k; z++){
		v_error[z]=v_error[z]/test.size();
	}
	return error/test.size();
}


int clasif_vector(vector<pixel> entr, vector<pixel> test_pixel, int k, int kernel){
	
	int dist = INF;
	double d;
	list<clasif> c;
	
	int v0,v1,v2;

	clasif c_aux;

	int incr = kernel*kernel;
	int a0 = int(incr/2);


	c_aux.dist=INF;
	c_aux.valor=INF;

	for(int m=0; m<k; m++)//inicializar lista con valores INF.
		c.push_back(c_aux);

	for(int j=0; j< entr.size(); j+=incr){
		d=0;
		for(int l=0; l<incr; l++){
			v0= (test_pixel[l].val0 - entr[j+l].val0);
			v1= (test_pixel[l].val1 - entr[j+l].val1);
			v2= (test_pixel[l].val2 - entr[j+l].val2);
			d+=double(v0*v0+v1*v1+v2*v2);
		}
		if(d<c.back().dist){
			c_aux.dist=d;
			c_aux.valor= entr[j+a0].mano;
			c.push_back(c_aux);
			c.sort(clasif_comp);
			c.pop_back();
		}
	}

	int valor=0;
	for(int n = 0; n < k; n++){

		if( c.front().valor == 1)
			valor++;
		c.pop_front();			
	}

	if( valor > int( k/2 + 0.5) ){
		valor=1;
	}
	else{
		valor=0;
	}
	return valor;
}


void erode(	IplImage  **img, int num)
{
	int      i, j,k;
	int height,width,step,chan;
	uchar *data,*data_tmp;
	IplImage *tmp;

	height	= (*img)->height;
	width		= (*img)->width;
	step      = (*img)->widthStep;
	chan		= (*img)->nChannels;
	data      = (uchar *)(*img)->imageData;

	for (int n =0; n<num; n++){
		tmp = cvCloneImage((*img));
		data_tmp      = (uchar *)tmp->imageData;

		for(i=1;i<height-1;i++)
			for(j=1;j<width-1;j++) 
				for(k=0;k<chan;k++)
					if(data[i*step+j*chan+k]==OBJECT &&
						( 
						data_tmp[(i-1)*step+(j-1)*chan+k]==BACKGROUND ||
						data_tmp[(i-1)*step+(j)*chan+k]==BACKGROUND ||
						data_tmp[(i-1)*step+(j+1)*chan+k]==BACKGROUND ||
						data_tmp[(i)*step+(j-1)*chan+k]==BACKGROUND ||
						data_tmp[(i)*step+(j+1)*chan+k]==BACKGROUND ||
						data_tmp[(i+1)*step+(j-1)*chan+k]==BACKGROUND ||
						data_tmp[(i+1)*step+(j)*chan+k]==BACKGROUND ||
						data_tmp[(i+1)*step+(j+1)*chan+k]==BACKGROUND 
						)
						)data[i*step+j*chan+k]=BACKGROUND;

		cvReleaseImage(&tmp);
	}
} 

void dilate(	IplImage  **img, int num)
{
	int      i, j,k;
	int height,width,step,chan;
	uchar *data,*data_tmp;
	IplImage *tmp;

	height	= (*img)->height;
	width		= (*img)->width;
	step      = (*img)->widthStep;
	chan		= (*img)->nChannels;
	data      = (uchar *)(*img)->imageData;

	for (int n =0; n<num; n++){
		tmp = cvCloneImage((*img));
		data_tmp      = (uchar *)tmp->imageData;

		for(i=1;i<height-1;i++)
			for(j=1;j<width-1;j++) 
				for(k=0;k<chan;k++)
					if(data[i*step+j*chan+k]==BACKGROUND &&
						( 
						data_tmp[(i-1)*step+(j-1)*chan+k]==OBJECT ||
						data_tmp[(i-1)*step+(j)*chan+k]==OBJECT ||
						data_tmp[(i-1)*step+(j+1)*chan+k]==OBJECT ||
						data_tmp[(i)*step+(j-1)*chan+k]==OBJECT ||
						data_tmp[(i)*step+(j+1)*chan+k]==OBJECT ||
						data_tmp[(i+1)*step+(j-1)*chan+k]==OBJECT ||
						data_tmp[(i+1)*step+(j)*chan+k]==OBJECT ||
						data_tmp[(i+1)*step+(j+1)*chan+k]==OBJECT 
						)
						)data[i*step+j*chan+k]=OBJECT;

		cvReleaseImage(&tmp);
	}
} 
void cierre (IplImage  **img, int num){
	erode(img,num);
	dilate(img,num+1);
	erode(img,1);
}

void apertura (IplImage  **img, int num){
	
	dilate(img,num);
	erode(img,num+1);
	dilate(img,1);
}


void distancia(IplImage **imagen){
	
	uchar *data;
	int height,width;
	int step;
	IplImage *img= cvCloneImage(*imagen);


	height    = img->height;
	width     = img->width;
	step	  = img->widthStep;
	data      = (uchar *)img->imageData;
	int index;
	int min;

	//bool finish;

	for(int i= 1; i < height-1; i++){
		for(int j= 1; j < width-1; j++){

			index = (i)*step+(j);
			if( data[index] != 0){
				min=data[index];
				for(int m = -1; m <= 1; m+=1){
					for(int n = -1; n <= 1; n++){
						if( data[(i+m)*step+(j+n)]< min)
							min=data[(i+m)*step+(j+n)];
					}
				}

				data[index]=min+3;
			}


		}
	}

	for(int i= height-1; i > 0; i--){
		for(int j= width-1; j>0; j--){

			index = (i)*step+(j);
			if( data[index] != 0){
				min=data[index];
				for(int m = -1; m <= 1; m+=1){
					for(int n = -1; n <= 1; n++){
						if( data[(i+m)*step+(j+n)]< min)
							min=data[(i+m)*step+(j+n)];
					}
				}

				data[index]=min+3;
			}


		}
	}

	*imagen=cvCloneImage(img);

	cvReleaseImage(&img);


}
