#include <cv.h>
#include <cvaux.h>
#include <highgui.h>
#include <stdlib.h>
#include "main.h"
#include <stdio.h>
#include "GL/glut.h"
#include <math.h>
#include <string.h>
#include "tsai.h"

int WIDTH, HEIGHT;

bool calculouDistFocal = false;
float distanciaFocal;
float translacao[3], rotacao[9];
float mtsaiopengl_modelview[16], mtsaiopengl_projection[16];

IplImage* frame = 0;
CvCapture* capture = 0;

CvSize sizeChessboard = cvSize(8,6);
CvPoint2D32f* corners;
int corner_count;

float *xw, *yw, *xf, *yf;


void reshape( int w, int h )
{
	WIDTH = w;
	HEIGHT = h;
    display();	
}

void display(void)
{
	int i,j;


	glClearColor(0.0f, 0.0f, 0.0f, 1.0f);
	glClear( GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT );

	glViewport(0, 0, WIDTH, HEIGHT);
	glMatrixMode(GL_PROJECTION);
	glLoadIdentity();
	gluOrtho2D(0.0, WIDTH, 0.0, HEIGHT);
	glMatrixMode(GL_MODELVIEW);
	glLoadIdentity();

	if(!cvGrabFrame(capture)) 
	{
		free(corners);
		cvReleaseCapture(&capture);
		glutDestroyWindow(glutGetWindow());
		free(xw);
		free(yw);
		free(xf);
		free(yf);
		exit( 0 );   
	}
	
	// calibração da camera
	int flag = calibrarCamera();

	if (flag != 0)
	{
		// carrega matrizes de projecao e de transformacao
		glMatrixMode(GL_PROJECTION);
		glPushMatrix();
		glLoadIdentity();
		glLoadMatrixf(mtsaiopengl_projection);

		glMatrixMode(GL_MODELVIEW);
		glPushMatrix();
		glLoadIdentity();
		glLoadMatrixf(mtsaiopengl_modelview);

		// desenha os corners do tabuleiro 
		glColor3f(1,0,0);
		glPointSize(5);
		glBegin(GL_POINTS);
		for(i=0; i<corner_count; i++)
			glVertex3f(xw[i],yw[i],0.f);
		glEnd();

		// desenha os eixos
		glBegin(GL_LINES);
			glColor3f(1,0,0);
			glVertex3f(-1.0,-1.0,0.0);
			glVertex3f(8.0,-1.0,0.0);

			glColor3f(0,1,0);
			glVertex3f(-1.0,-1.0,0.0);
			glVertex3f(-1.0,6.0,0.0);

			glColor3f(0,0,1);
			glVertex3f(-1.0,-1.0,0.0);
			glVertex3f(-1.0,-1.0,8.0);
		glEnd();
		
		glColor3f(1,1,1);
		glTranslatef(2, 2, 0);
		glRotatef(90.0,1.0,0.0,0.0);
		glutSolidTeapot(0.8f);
		glColor3f(0,0,1);
		glutWireTeapot(0.8f);

		glMatrixMode(GL_PROJECTION);
		glPopMatrix();

		glMatrixMode(GL_MODELVIEW);
		glPopMatrix();
	}
		
	glFlush();
	glutSwapBuffers();
}

void keyboard(unsigned char key, int x, int y)  
{
	switch ( key ) 
	{
		case 27: 	
			free(corners);
			cvReleaseCapture(&capture);
			glutDestroyWindow(glutGetWindow());
			free(xw);
			free(yw);
			free(xf);
			free(yf);
			exit( 0 );   
			break;  
	 }
}

int calibrarCamera()
{
	int i,j;

	frame = cvRetrieveFrame(capture);
	IplImage* img_aux = cvCreateImage(cvSize(WIDTH,HEIGHT), 8, 3);
	cvResize(frame,img_aux);
	glDrawPixels(WIDTH,HEIGHT,GL_BGR_EXT,GL_UNSIGNED_BYTE,img_aux->imageData);
		
	// encontra os corners do tabuleiro
	int flag = cvFindChessboardCorners(img_aux,sizeChessboard,corners,&corner_count,CV_CALIB_CB_ADAPTIVE_THRESH);
	if (flag != 0)
	{
		int px = 0, py = 0;
		for(i=0; i<corner_count; i++)
		{
			xf[i] = corners[i].x - (WIDTH*0.5f);
			yf[i] = corners[i].y - (HEIGHT*0.5f);
			xw[i] = px;
			yw[i] = py;
			px++;
			if (px > 7)
			{
				py++;
				px = 0;
			}
		}
		
		if (!calculouDistFocal)
		{
			tsaiCalibration2D(corner_count,xw,yw,xf,yf,&distanciaFocal,translacao,rotacao);
			calculouDistFocal = true;
		}
		else
			tsaiCalibration2D_Tr(corner_count,xw,yw,xf,yf,distanciaFocal,translacao,rotacao);
		
		tsaiOGLModelViewMatrix(translacao,rotacao,mtsaiopengl_modelview);
		tsaiOGLProjectionMatrix(distanciaFocal, 10, 1000, WIDTH, HEIGHT, mtsaiopengl_projection);
	}

	cvReleaseImage(&img_aux);

	return flag;
}

void initVideo()
{
	FILE *arquivo;
	char nome[100];

	arquivo = fopen("inicio.txt","r");
	fscanf(arquivo,"%s",&nome);
	fscanf(arquivo,"%d",&WIDTH);
	fscanf(arquivo,"%d",&HEIGHT);
	fclose(arquivo);

	capture = cvCaptureFromAVI(nome);
	//capture = cvCaptureFromCAM(1);

	corners = (CvPoint2D32f*)malloc((sizeChessboard.width*sizeChessboard.height) * sizeof(CvPoint2D32f));	

	xw = (float *)malloc(48*sizeof(float));
	yw = (float *)malloc(48*sizeof(float));
	xf = (float *)malloc(48*sizeof(float));
	yf = (float *)malloc(48*sizeof(float));
}

int main(int argc, char* argv[])
{
	initVideo();

	glutInit(&argc,argv); 
    glutInitDisplayMode(GLUT_DEPTH | GLUT_DOUBLE | GLUT_RGBA); 
    glutInitWindowPosition(100,100);
	glutInitWindowSize(WIDTH,HEIGHT); 
	glutCreateWindow ("RA - Trabalho 2"); 

	glutDisplayFunc ( display ); 
    glutReshapeFunc ( reshape );
    glutKeyboardFunc ( keyboard );
    //glutSpecialFunc ();
    glutIdleFunc ( display );
    glutMainLoop ( );   

	return 0;
}