#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <GL/glut.h>
#include <cv.h>
#include <highgui.h>
#include "dibujo.h"


//void onIdle(void);
void init(void);
void onDisplay(void);
void reshape (int w, int h);
void voltear_VERT_imagen( IplImage **img);
void umbralizar(IplImage *img,IplImage **aux);
void contornos(IplImage **img, IplImage **dist);
void puntoCentral_area(IplImage *img,IplImage *dist,posicion *pos, int *area);
void distancias(IplImage **imagen);



#define Z0 3.75
#define XY_PROP 0.813 
#define XY_PROP_90 0.3
#define AREA_0 9409


//globales
char buffer[200];
IplImage * img;
IplImage * gris;
IplImage * dist;
int area;
posicion pos;
CvBox2D box ;
double xy_prop;
double giro;

int main(int argc, char** argv)
{
   glutInit(&argc, argv);
   glutInitDisplayMode (GLUT_RGB | GLUT_DOUBLE | GLUT_DEPTH );

   img = cvLoadImage("bscap004.jpg",1);
   
   if(img==NULL){
	   fprintf(stderr, "Error usando OpenCV: No se ha abierto la imagen\n");
	   exit(0);
   }
   gris = cvCreateImage(cvGetSize(img),8,1);
   dist = cvCreateImage(cvGetSize(img),8,1);

   voltear_VERT_imagen( &img);
   umbralizar(img, &gris);
   contornos(&gris,&dist);
   distancias(&dist);
   area=0;
   puntoCentral_area(gris,dist,&pos,&area);
   xy_prop = box.size.width/box.size.height;

   glutInitWindowSize (img->width,img->height); 
   glutInitWindowPosition (100, 100);
   glutCreateWindow ("Tracker");

   init ();

   glutMainLoop();
   return 0;   
}

void init(void) 
{
   glShadeModel (GL_SMOOTH);
   glEnable(GL_DEPTH_TEST);
 /*  glCullFace(GL_BACK);
   glEnable(GL_CULL_FACE);*/
   glutDisplayFunc(onDisplay); 
   glutReshapeFunc(reshape);

  /* glViewport (0, 0, img->width, img->height);
   glMatrixMode(GL_PROJECTION);
   glLoadIdentity();
   gluPerspective(60.0, (GLdouble)img->width/img->height, 0.0, 100); 
   glMatrixMode(GL_MODELVIEW);
   glLoadIdentity();
   gluLookAt (0.0, 0.0, 5.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0);*/

  
   //glutIdleFunc(onIdle);
}

void onDisplay(void)
{
	glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT );
	
	/*Pintamos imagen capturada*/
		glDisable(GL_DEPTH_TEST);
		cvCircle(img,cvPoint(pos.x,pos.y),2,cvScalar(150,150,150),2);
		//glDrawPixels(img->width,img->height, GL_BLUE, GL_UNSIGNED_BYTE, gris->imageData);
		glDrawPixels(img->width,img->height, GL_BGR_EXT, GL_UNSIGNED_BYTE, img->imageData);
		glEnable(GL_DEPTH_TEST);
	/*Fin de: Pintamos imagen capturada*/


	GLdouble zFactor =  xy_prop/XY_PROP;
	double aux= (double(area)/AREA_0);
	zFactor= zFactor/sqrt(aux);
	GLdouble Z = Z0 * zFactor;
	GLdouble despY = 2*Z*tan(30.0f)/img->height;
	GLdouble despX = 2*Z*tan(30.0f)/img->width;
	//
	//glMatrixMode (GL_PROJECTION);
	//glLoadIdentity();
	//glOrtho(-160,160,-120,120,-100,100);
	//
	//glMatrixMode(GL_MODELVIEW);
	//glLoadIdentity();
	/////* Situa la cámara: posición, hacia donde apunta, vector up */
	////gluLookAt (0.0, despY*(pos.y-(img->height)/2), 3.0, 0.0, despY*(pos.y-(img->height)/2), 0.0, 0.0, 1.0, 0.0);
	//gluLookAt (0.0, 0.0, 3.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0);

	//glPushMatrix();	
	//	glTranslatef((pos.x-(img->width)/2), (pos.y-(img->height)/2), 0.0);
	//	Flecha f1( Punto(50,0,0),Color(1,0,0) );
	//	Flecha f2( Punto(0,-50,0),Color(0,1,0) );
	//	Flecha f3( Punto(0,0,50),Color(0,0,1) );
	//	f1.dibujar( Punto(0,0,0) );
	//	f2.dibujar( Punto(0,0,0) );
	//	f3.dibujar( Punto(0,0,0) );
	//glPopMatrix();

	glLoadIdentity();
	/* Situa la cámara: posición, hacia donde apunta, vector up */
	gluLookAt ( (1.2*despX/10)*(pos.x-(img->width)/2), (despY/11)*(pos.y-(img->height)/2), Z, (1.2*despX/10)*(pos.x-(img->width)/2), (despY/11)*(pos.y-(img->height)/2), 0.0, 0.0, 1.0, 0.0);

	glRotatef(box.angle,0,0,-1);
	//glRotatef( 90-(100*xy_prop),1,0,0);


	glPushMatrix();
		Flecha f1( Punto(1,0,0),Color(1,0,0) );
		Flecha f2( Punto(0,-1,0),Color(0,1,0) );
		Flecha f3( Punto(0,0,1),Color(0,0,1) );
		f1.dibujar( Punto(0,0,0) );
		f2.dibujar( Punto(0,0,0) );
		f3.dibujar( Punto(0,0,0) );
	/*	glColor3f(0.5,0.5,0.5);
		glTranslatef(0,0,0.5);
		glRotatef(90,1,0,0);		
		glutWireTeapot(0.5);*/
	glPopMatrix();/* saca la matriz de la pila, para que */


	static GLenum glerr;
	if ((glerr=glGetError())!=GL_NO_ERROR) {
		fprintf(stderr, "Error OpenGL: %s (0x%X)\n", gluErrorString(glerr), glerr);
	}
	glutSwapBuffers();
}

void voltear_VERT_imagen( IplImage **img){

	IplImage *aux = cvCreateImage(cvGetSize(*img),(*img)->depth,(*img)->nChannels);

	int height,width;
	int step,channels;	
	height    = aux->height;
	width     = aux->width;
	step	  = aux->widthStep;
	channels  = aux->nChannels;

	uchar *data      = (uchar *)(*img)->imageData;
	uchar *data2    =  (uchar *)aux->imageData;

	for(int i= 0; i < height; i++){
			for(int j= 0; j < width; j++){ 
				for(int k=0;k<channels;k++)
					data2[(height-i-1)*step+(j)*channels+k] = data[(i)*step+(j)*channels+k]; 
			}
	}

	cvCopyImage( aux,(*img) );

}

void reshape (int w, int h)
{
	glViewport (0, 0, (GLsizei) w, (GLsizei) h); 
	glMatrixMode (GL_PROJECTION);
	glLoadIdentity ();
	/* inicializa la proyección: */
	/* campo de visión, razón de aspecto, plano delantero y trasero */
	gluPerspective(60.0, (GLfloat) w/(GLfloat) h, 0.1, 200.0);
	glMatrixMode(GL_MODELVIEW);
	glLoadIdentity();
	/* Situa la cámara: posición, hacia donde apunta, vector up */
	gluLookAt (0.0, 0.0, 3.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0);
}

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+1;
			}


		}
	}

	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+1;
			}


		}
	}

	cvCopyImage(img,(*imagen));

	cvReleaseImage(&img);


}



void umbralizar(IplImage *img, IplImage **aux){

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

	height    = (img)->height;
	width     = (img)->width;
	step	  = (img)->widthStep;
	step2	  = (*aux)->widthStep;
	channels  = (img)->nChannels;
	data      = (uchar *)(img)->imageData;
	data2      = (uchar *)(*aux)->imageData;
	int val0, val1, val2;

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

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

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

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

			
			if( (dif_v2_v0> 27 && dif_v2_v0<100)  && (dif_v2_v1>-2 && dif_v2_v1<78) && (dif_v0_v1>-55 && dif_v0_v1<0)  ){
				data2[i*step2+j]=255;
			}else {
				data2[i*step2+j]=0;
			}

		}
	}

}


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 contornos(IplImage **img, IplImage **dist){

	dilate(img,1);
	erode(img,1);

	CvMemStorage *mem;
	CvSeq *contours, *ptr, *ptr2;

	mem = cvCreateMemStorage(0);
	cvFindContours((*img), mem, &contours, sizeof(CvContour), CV_RETR_EXTERNAL,CV_CHAIN_APPROX_SIMPLE, cvPoint(0,0));
	cvZero((*img));

	int cont=1;
	int valor=0;
	cvZero((*dist));

	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){
			ptr2=ptr;
			cvDrawContours((*dist), ptr, cvScalar(255), cvScalar(255), -1, CV_FILLED, 8, cvPoint(0,0));
			cvDrawContours((*img), ptr, cvScalar(255), cvScalar(255), -1, CV_FILLED, 8, cvPoint(0,0));
		}
		else{
			color = cvScalar(  rand()&200);
			cvDrawContours((*img), ptr, color, cvScalar(255), -1, CV_FILLED, 8, cvPoint(0,0));
		}
	}

	box = cvMinAreaRect2( ptr2);
}

void puntoCentral_area(IplImage *img,IplImage *dist,posicion *pos, int *area){
	int index;
	int min=0;
	(*pos).x=0;
	(*pos).y=0;
	int height = img->height;
	int width = img->width;
	int step = img->widthStep;
	uchar *data      = (uchar *)img->imageData;
	uchar *data2     = (uchar *)dist->imageData;

	for(int i= 0; i < height; i++){
		for(int j= 0; j < width; j++){
			index = (i)*step+(j);
			if( data2[index]>min){
				min=data2[index];
				(*pos).x=j;
				(*pos).y=i;
			}

			if( data[index] != 0){
				(*area)+=1;
			}

		}
	}

}

void distancias(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+1;
			}


		}
	}

	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+1;
			}


		}
	}

	*imagen=cvCloneImage(img);

	cvReleaseImage(&img);


}
