#include <cv.h>
#include <cvaux.h>
#include <highgui.h>
#include <stdlib.h>
#include <stdio.h>
#include <vector>
#include <math.h>
#include <string.h>
#include "glut.h"
#include "main.h"
#include "transicao.h"
#include "calibra.h"
#include "nuvem_pontos.h"
#include "tsai.h"
#include "osgMeshViewer.h" 


int WIDTH = 3888, HEIGHT = 2592;

std::vector<Ponto> pontos3D;				// nuvem de pontos


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

void display(void)
{
	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);
	//gluPerspective(40,4/3,10,1000);
	glMatrixMode(GL_MODELVIEW);
	glLoadIdentity();
	
	//IplImage* img_aux = cvCreateImage(cvSize(WIDTH,HEIGHT), IPL_DEPTH_32F, 3);
	//cvResize(img_debug,img_aux);
	//glDrawPixels(WIDTH,HEIGHT,GL_BGR_EXT,GL_UNSIGNED_BYTE,img_pontos->imageData);
	//glDrawPixels(WIDTH,HEIGHT,GL_BGR_EXT,GL_UNSIGNED_BYTE,img_debug->imageData);
		
	//cvReleaseImage(&img_aux);

	/*glColor3d(1,1,1);
	glPointSize(10);
	glBegin(GL_POINTS);
		glVertex3f(-5.0448, 3.09562, 4.90728);
		glVertex3f(-4.9754, 2.7353, 4.6613);
	glEnd();*/
	
	glFlush();
	glutSwapBuffers(); 
}

void keyboard(unsigned char key, int x, int y)  
{
	switch ( key ) 
	{
		case 27: 	
			glutDestroyWindow(glutGetWindow());
			exit( 0 );   
			break;  
	 }
}

void zerarMatCtrl(MATRIZ_ID m[36][36])
{
	int i,j;

	for(i=0; i<36; i++)
	{
		for(j=0; j<36; j++)
		{
			m[i][j].posX = -1;
			m[i][j].posY = -1;
			m[i][j].flag = 0;
		}
	}
}

void criarNuvemPontos()
{
	FILE *arquivo;
	char nome[100];
	float distFocal_cam1, distFocal_cam2;		// distancia focal das cameras
	float T_cam1[3], R_cam1[9], T_cam2[3], R_cam2[9];
	float pos_cam1[3], pos_cam2[3];		// posição das cameras
	Ponto c_cam1, c_cam2;				// centro de projeção das cameras
	IplImage* img_positiva;
	IplImage* img_negativa;
	IplImage* img_calibracao;
	IplImage* img_pos1;
	IplImage* img_pos2;
	IplImage* img_laplace;		// caso use laplace para ajudar na busca das interseções
	noMat **pontos;					// pontos referente aos cruzamentos dos padroes projetados de uma camera
	int i, qtdeCameras = 2;
	MATRIZ_ID pontosCam1[36][36];		

	
	zerarMatCtrl(pontosCam1);

	img_pos1 = cvCreateImage(cvSize(WIDTH,HEIGHT), IPL_DEPTH_32F, 3);
	img_pos2 = cvCreateImage(cvSize(WIDTH,HEIGHT), IPL_DEPTH_32F, 3);
	img_laplace = cvCreateImage(cvSize(WIDTH,HEIGHT), IPL_DEPTH_32F, 3);

	arquivo = fopen("inicio.txt","r");

	for(i=1; i<=qtdeCameras; i++)
	{
		printf("\nANALISE CAM%d",i);
		
		pontos = initMatriz();

		//lê o padrão de calibração
		fscanf(arquivo,"%s",&nome);
		img_calibracao = cvLoadImage(nome);
		
		// processa para o padrão com faixas verticais
		fscanf(arquivo,"%s",&nome);
		img_positiva = cvLoadImage(nome);
		fscanf(arquivo,"%s",&nome);
		img_negativa = cvLoadImage(nome);
		recuperarPadrao(img_positiva, img_negativa, img_pos1, HEIGHT, WIDTH, 1);
		printf(".");
		
		fscanf(arquivo,"%s",&nome);
		img_positiva = cvLoadImage(nome);
		fscanf(arquivo,"%s",&nome);
		img_negativa = cvLoadImage(nome);
		recuperarPadrao(img_positiva, img_negativa, img_pos2, HEIGHT, WIDTH, 1);
		printf(".");
		
		cvLaplace(img_pos1,img_laplace);
		encontarTransicaoLaplace(pontos,img_laplace,img_pos1,img_pos2,HEIGHT,WIDTH,1);
		printf(".");
		
	
		// processa para o padrão com faixas horizontais
		fscanf(arquivo,"%s",&nome);
		img_positiva = cvLoadImage(nome);
		fscanf(arquivo,"%s",&nome);
		img_negativa = cvLoadImage(nome);
		recuperarPadrao(img_positiva, img_negativa, img_pos1, WIDTH, HEIGHT, 2);
		printf(".");
		
		fscanf(arquivo,"%s",&nome);
		img_positiva = cvLoadImage(nome);
		fscanf(arquivo,"%s",&nome);
		img_negativa = cvLoadImage(nome);
		recuperarPadrao(img_positiva, img_negativa, img_pos2, WIDTH, HEIGHT, 2);
		printf(".");
		
		cvLaplace(img_pos1,img_laplace);
		encontarTransicaoLaplace(pontos,img_laplace,img_pos1,img_pos2,WIDTH,HEIGHT,2);
		printf(".");


		if (i==1)	// cam1
		{
			printf("\nSET VP");

			calibrarCamera(img_calibracao,&distFocal_cam1,T_cam1,R_cam1);
			tsaiCameraPosition(&(pos_cam1[0]), &(pos_cam1[1]), &(pos_cam1[2]), T_cam1, R_cam1);
			c_cam1.x = WIDTH/2;
			c_cam1.y = HEIGHT/2;
			
			setVP(pontosCam1,pontos);
		}
		else		// cam2
		{
			printf("\nCALCULANDO NUVEM DE PONTOS\n");

			calibrarCamera(img_calibracao,&distFocal_cam2,T_cam2,R_cam2);
			tsaiCameraPosition(&(pos_cam2[0]), &(pos_cam2[1]), &(pos_cam2[2]), T_cam2, R_cam2);
			c_cam2.x = WIDTH/2;
			c_cam2.y = HEIGHT/2;

			encontrarNuvemPontos(pontos3D, pontosCam1, pontos, 
								distFocal_cam1, pos_cam1, R_cam1, c_cam1,
								distFocal_cam2, pos_cam2, R_cam2, c_cam2);
		}

		deleteMatriz(pontos);
	}

	fclose(arquivo);

	cvReleaseImage(&img_positiva);
	cvReleaseImage(&img_negativa);
	cvReleaseImage(&img_laplace);
	cvReleaseImage(&img_calibracao);
	cvReleaseImage(&img_pos1);
	cvReleaseImage(&img_pos2);
}

int main(int argc, char* argv[])
{
	FILE *arquivo;
	int i;

	criarNuvemPontos();
	
	/*printf("\nREGISTRANDO NUVEM DE PONTOS\n\n");
	arquivo = fopen("nuvem_pontos_0.txt","w");
	for(i=0; i<pontos3D.size(); i++)
		fprintf(arquivo,"%0.5f %0.5f %0.5f\n",pontos3D[i].x,pontos3D[i].y,pontos3D[i].z);
	fclose(arquivo);*/

	osg::Node* scene = createOSGScene(pontos3D);
    viewOSGScene(scene);

	/*glutInit(&argc,argv); 
    glutInitDisplayMode(GLUT_DEPTH | GLUT_DOUBLE | GLUT_RGBA); 
    glutInitWindowPosition(0,0);
	glutInitWindowSize(WIDTH,HEIGHT); 
	glutCreateWindow ("RA - Trabalho 3"); 

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

	return 0;
}