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

using namespace std;

//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);
void pintarCajaMinima(IplImage **imagen);
void fps_stuff(void);
void draw_text(unsigned int x, unsigned int y, char *text, GLfloat *color=NULL, void *font=GLUT_BITMAP_HELVETICA_18);



#define Z0 3.4641016151377545870548926830117
#define XY_PROP 0.813 
#define XY_PROP_90 0.3
#define AREA_0 9409
#define N_POS 5
#define N_ANG 5

/* Variables globales que pueden serte útiles */
unsigned int nFrames=0; // Contador del número de frames visualizados
float fps=60;    // Velocidad actual de visualización (frames por segundo)
#define FPS_CYCLE 15	// Actualizar el contador de fps cada N frames


//globales
GLdouble aspect_ratio = 320.0/240.0;
char buffer[200];
IplImage * img;
IplImage * gris;
IplImage * dist;
CvCapture* capture;
int area;
deque<posicion> position;
deque<double> angulos;
double ang;
posicion pos;
CvBox2D box ;

double xy_prop;
double giro;
static int updateTime;		// Soporte de animación
static int initTime;		// Soporte del cálculo de fps

int main(int argc, char** argv)
{
   glutInit(&argc, argv);
   glutInitDisplayMode (GLUT_RGB | GLUT_DOUBLE | GLUT_DEPTH );
   
   capture = cvCreateFileCapture( "Video.avi" );

   img = cvQueryFrame( capture );
   voltear_VERT_imagen( &img);
   
   if(img==NULL){
	   fprintf(stderr, "Error usando OpenCV: Fallo al acceder al flujo de imagenes\n");
	   exit(0);
   }
   gris = cvCreateImage(cvGetSize(img),8,1);
   dist = cvCreateImage(cvGetSize(img),8,1);

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

   init ();

   glutMainLoop();
   //cvReleaseCapture( &capture );
   return 0;   
}

void onIdle(){

	int updateNow;
	int elapsed;

	updateNow=glutGet(GLUT_ELAPSED_TIME);
	elapsed = updateNow-updateTime;
	
	if (elapsed > 30) {
		updateTime = updateNow;
		img = cvQueryFrame( capture );
		if(img)
			voltear_VERT_imagen( &img);
	}

	if(img){
		umbralizar(img, &gris);
		contornos(&gris,&dist);
		distancias(&dist);
		area=0;
		puntoCentral_area(gris,dist,&pos,&area);
		position.pop_front();
		position.push_back(pos);
		angulos.pop_front();
		angulos.push_back(box.angle);
		pos.x=0;
		pos.y=0;
		ang = 0;
		for(int i=0; i<N_POS; i++){
			pos.x+=position[i].x;
			pos.y+=position[i].y;
		}
		for(int i=0; i<N_ANG; i++){
			ang+=angulos[i];
		}
		ang/=N_ANG;
		pos.x/=N_POS;
		pos.y/=N_POS;

		xy_prop = box.size.width/box.size.height;
		pintarCajaMinima(&img);	
		
				

		glutPostRedisplay();
	}
	else{
		cvSetCaptureProperty(capture,CV_CAP_PROP_POS_FRAMES,0);
	}


}


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


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

	for( int i=0; i<N_POS; i++){
		position.push_back(pos);
	}
	for( int i=0; i<N_ANG; i++){
		angulos.push_back(box.angle);
	}

	initTime=glutGet(GLUT_ELAPSED_TIME);
   updateTime=glutGet(GLUT_ELAPSED_TIME);
   glutIdleFunc(onIdle);
}

void onDisplay(void)
{
	glClear( 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);
	GLdouble zFactor= 1/sqrt(aux);
	GLdouble Z = GLdouble(Z0) * zFactor;
	GLdouble despY =  2.0*Z*-tan(DEG2RAD(30.0f))/GLdouble(img->height);
	GLdouble despX = aspect_ratio*2.0*Z*-tan(DEG2RAD(30.0f))/GLdouble(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 ( (0.12*despX)*(GLdouble(pos.x)-(img->width)/2.0), (despY/11.0)*(pos.y-(img->height)/2.0), Z, (0.12*despX)*(pos.x-(img->width)/2.0), (despY/11.0)*(pos.y-(img->height)/2.0), 0.0, 0.0, 1.0, 0.0);
	gluLookAt ( (despX)*(GLdouble(pos.x)-(img->width)/2.0), (despY)*(pos.y-(img->height)/2.0), Z, (despX)*(pos.x-(img->width)/2.0), (despY)*(pos.y-(img->height)/2.0), 0.0, 0.0, 1.0, 0.0);
	//gluLookAt ( (0.12*despX)*(GLdouble(box.center.x)-(img->width)/2.0), (despY/11.0)*(box.center.y-(img->height)/2.0), Z, (0.12*despX)*(box.center.x-(img->width)/2.0), (despY/11.0)*(box.center.y-(img->height)/2.0), 0.0, 0.0, 1.0, 0.0);

	glRotatef(ang,0,0,-1);
	glRotatef( 90-100*xy_prop,1,0,0);
	
	glPushMatrix();

		
		//glTranslated( (pos.x-(img->width)/2)20, (pos.y-(img->height)/2),0);
		//glTranslatef( -2.3,1.73,0 );
		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 */

	//fps_stuff();

	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) );
	cvReleaseImage(&aux);

}

void reshape (int w, int h)
{
	glViewport (0, 0, (GLsizei) 320, (GLsizei) 240); 
	glMatrixMode (GL_PROJECTION);
	glLoadIdentity ();
	/* inicializa la proyección: */
	/* campo de visión, razón de aspecto, plano delantero y trasero */
	gluPerspective(60.0, (GLfloat) 320/(GLfloat) 240, 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;

	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> 25 && dif_v2_v0<125)  && (dif_v2_v1>-15 && dif_v2_v1<100) && (dif_v0_v1>-70 && dif_v0_v1<5)  ){
				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,3);
	erode(img,1);

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

	mem = cvCreateMemStorage(0);
	int contornos=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));
		}
	}
	if(contornos!=0)
		box = cvMinAreaRect2( ptr2);
	cvReleaseMemStorage(&mem);

}

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


		}
	}

	cvCopyImage(img,(*imagen));

	cvReleaseImage(&img);


}

void ejes( CvBox2D box, CvPoint2D32f pt[4] )
{
    double angle = box.angle*CV_PI/180.;
    float a = (float)cos(angle)*0.5f;
    float b = (float)sin(angle)*0.5f;
	CvPoint2D32f pt_aux;

    if( !pt )
        CV_Error( CV_StsNullPtr, "NULL vertex array pointer" );

	pt_aux.x=pt[0].x = box.center.x - a*box.size.height - b*box.size.width;
	pt_aux.y=pt[0].y = box.center.y + b*box.size.height - a*box.size.width;
    pt[1].x = box.center.x + a*box.size.height - b*box.size.width;
    pt[1].y = box.center.y - b*box.size.height - a*box.size.width;
    pt[2].x = 2*box.center.x - pt[0].x;
    pt[2].y = 2*box.center.y - pt[0].y;
    pt[3].x = 2*box.center.x - pt[1].x;
    pt[3].y = 2*box.center.y - pt[1].y;


	pt[0].x += (pt[1].x-pt[0].x)/2;
	pt[0].y += (pt[1].y-pt[0].y)/2;
	pt[1].x += (pt[2].x-pt[1].x)/2;
	pt[1].y += (pt[2].y-pt[1].y)/2;
	pt[2].x += (pt[3].x-pt[2].x)/2;
	pt[2].y += (pt[3].y-pt[2].y)/2;
	pt[3].x += (pt_aux.x-pt[3].x)/2;
	pt[3].y += (pt_aux.y-pt[3].y)/2;

}
void pintarCajaMinima(IplImage **imagen){
	//Pinta caja minima
			CvPoint2D32f points[4];
			CvPoint2D32f points2[4];
			CvPoint pt0, pt;
			CvPoint pt2;
			ejes(box, points2);
			cvBoxPoints(box, points);

			pt0.x = cvRound(points[3].x);
			pt0.y = cvRound(points[3].y);
			for(int i = 0; i < 4; i++ )
			{
				pt.x = cvRound(points[i].x);
				pt.y = cvRound(points[i].y);
				pt2.x = cvRound(points2[i].x);
				pt2.y = cvRound(points2[i].y);
				cvLine((*imagen), pt0, pt, cvScalar(175), 1, CV_AA, 0);
				cvCircle((*imagen),pt2,2,cvScalar(150,0,0),2);
				pt0 = pt;
			}
		//

}

// Función para calcular y mostrar por pantalla el conteo de frames por segundo
void fps_stuff(void) {
	static char msg[20];

	nFrames++;
	if (nFrames==FPS_CYCLE) {
		int taux=glutGet(GLUT_ELAPSED_TIME);
		if (taux==initTime) return;
		fps=(float)(nFrames*1000)/(taux-initTime);
		initTime=taux;
		sprintf_s(msg, sizeof(msg), "FPS: %.2f", fps); 
		nFrames=0;
	}
	draw_text(50, 50, msg);
}

// Muestra en la posición (x, y) la cadena de texto con la fuente y el color indicados
// Los dos últimos parámetros son opcionales. Ver en glut.h las posibles fuentes que se 
// pueden utilizar. 
// Por ejemplo: draw_text(30, 30, "Hola mundo");
void draw_text(unsigned int x, unsigned int y, char *text, GLfloat *color, void *font) {

	glPushAttrib(GL_DEPTH_BUFFER_BIT | GL_ENABLE_BIT);
	glDisable(GL_LIGHTING);
	glDisable(GL_DEPTH_TEST);
	glMatrixMode(GL_PROJECTION);
	glPushMatrix();
	glLoadIdentity();
	gluOrtho2D(0, 320, 0, 240);
	glMatrixMode(GL_MODELVIEW);
	glPushMatrix();
	glLoadIdentity();
	if (color==NULL) {
		GLfloat red[3]={1.0f, 0.0f, 0.0f};
		glColor3fv(red);
	} else
		glColor3fv(color);
	glRasterPos2f(x, 240-y);
	while (*text) 
		glutBitmapCharacter(GLUT_BITMAP_HELVETICA_18, *text++);
	glPopMatrix();
	glMatrixMode(GL_PROJECTION);
	glPopMatrix();
	glMatrixMode(GL_MODELVIEW);
	glPopAttrib();
}