#include <cv.h>
#include <cvaux.h>
#include <highgui.h>
#include <stdio.h>
#include <math.h>
#include "tsai.h"
#include "gl.h"
#include "glut.h"

  IplImage* Frame = NULL;
IplImage* imag = NULL;
  int corners_count;
  double pr[16];
  double mv[16];

  double pontos_mundo_x[48];
  double pontos_mundo_y[48];
  double pontos_mundo_xn[48];
  double pontos_mundo_yn[48];

  double pontos_tela_x[48];
  double pontos_tela_y[48];
  double pontos_tela_xn[48];
  double pontos_tela_yn[48];

  int sqrSize=1;
  int w,h;
  int primeira_passagem=0;
    CvCapture* capture = NULL;



/*************************************************************/
void display()
{
	int i=0;
	if (!imag) return;

	glClearColor( 0.0f, 0.0f, 0.0f, 0.0f );
	glClear( GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT );
    
	glMatrixMode(GL_PROJECTION);
	glLoadIdentity();
	gluOrtho2D (0.0, (GLdouble)w, 0.0, (GLdouble)h);
	glMatrixMode(GL_MODELVIEW);
	glLoadIdentity();
	glDepthMask(GL_FALSE);
	glDrawPixels(w, h, GL_BGR_EXT, GL_UNSIGNED_BYTE, imag->imageData);
	glDepthMask(GL_TRUE);

	glMatrixMode(GL_PROJECTION);	
	glLoadIdentity();
	glLoadMatrixd(pr);	
	glMatrixMode(GL_MODELVIEW);	
	glLoadIdentity();
	glLoadMatrixd(mv);

	glLineWidth(3.0) ;
	glPointSize(3.0) ;
	glDisable(GL_DEPTH_TEST);
	glDisable(GL_LIGHTING);

 //   glRotatef( 90.0, 1.0, 0.0, 0.0 ) ;
	//glScalef( 15.0, 15.0, -15.0 ) ;

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

	glBegin(GL_LINES);
		glColor3f(1.0,0.0,0.0);
		glVertex3f(0,0,0);
		glVertex3f(3,0,0);

		glColor3f(0.0,1.0,0.0);
		glVertex3f(0,0,0);
		glVertex3f(0,3,0);

		glColor3f(0.0,0.0,1.0);
		glVertex3f(0,0,0);
		glVertex3f(0,0,3);
	glEnd();

	glMatrixMode (GL_PROJECTION);
	glPopMatrix();
	glMatrixMode (GL_MODELVIEW);
	glPopMatrix();
	
	glEnable(GL_LIGHTING);
		
  glutSwapBuffers();
}

/*************************************************************/
void calibracao()
{
  int resp;
  CvPoint2D32f corners[48];
  int flags=CV_CALIB_CB_ADAPTIVE_THRESH;
  int pattern_was_found=0;
  unsigned int i,j,z,k;
  double f;
  double T[3];
  double r[9];
  double cam_posx, cam_posy, cam_posz;

  resp = cvFindChessboardCorners(Frame, cvSize(8,6),&corners[0], &corners_count,flags);

	if (resp!=0){
		for ( i=0; i<corners_count;++i)	{		
			pontos_tela_x[i]=corners[i].x - (Frame->width/2);
			pontos_tela_y[i]=corners[i].y - (Frame->height/2);
		}
		z=0;
		for ( i=0; i<6;i++){
			for ( j=0;j<8;j++){
				pontos_mundo_x[z] = j* sqrSize;
				pontos_mundo_y[z] = i* sqrSize;
				z++;
			}	
		}
		
	tsaiCalibration2D(corners_count,pontos_mundo_x, pontos_mundo_y, pontos_tela_x, pontos_tela_y, &f,T,r);

	
	tsaiCameraPosition(&cam_posx, &cam_posy, &cam_posz, T, r);
	tsaiOGLModelViewMatrix(T,r,mv);
	tsaiOGLProjectionMatrix(f, 10, 1000, Frame->width, Frame->height, pr);
	}
}

/*************************************************************/
void idle(void)
{
	Frame = cvQueryFrame(capture);
	if(Frame)
	{
		cvCopy( Frame, imag, 0 );
		calibracao();
	}
	display();
}

/*************************************************************/

int inic(int width, int height)
{
  capture = cvCaptureFromAVI("videosTeoricamenteMelhores1.avi");
  
  if( !capture )
  {
    fprintf(stderr,"Could not initialize capturing...\n");
    return 0;
  }
  Frame = cvQueryFrame( capture );

  	if(Frame)
	{
		w = Frame->width;
		h = Frame->height;
	}

  imag = cvCloneImage( Frame );
  return 1;
}

/*************************************************************/
/***********************   Main   ****************************/
/*************************************************************/

int main(  int argc, char** argv )
{
 	if(inic(w,h))
	{
		printf("w = %d   h = %d  ", w, h);
   	    glutInit(&argc, argv);
		glutInitDisplayMode(GLUT_RGB | GLUT_DOUBLE);
		glutInitWindowPosition(100, 100);
		glutInitWindowSize(w, h);
		glutCreateWindow("Trabalho 2");

		glutDisplayFunc(display);
		glutIdleFunc(idle);
		
		glutMainLoop();
	 
	}
	  //cvReleaseCapture( &capture );
	return 0;


}