/**
* Visão Computacional e Realidade Aumentada
*
* CornerDetector
* Aluno: Daniel Ribeiro Trindade
*/

#include <stdio.h>
#include <stdlib.h>
#include <GL/glut.h>

#include "cvcam.h"
#include "cv.h"
#include "highgui.h"
#include "tsai.h"

#define NEARPLANE  10
#define FARPLANE  1000

IplImage* in_frame = 0;
IplImage* gray_image = 0;
CvCapture* capture = 0;
CvPoint2D32f corners[49];
int corner_count =49;
double width, height;

#define NPTSX 8
#define NPTSY 6

static double xw[NPTSX*NPTSY], yw[NPTSX*NPTSY];
static double xf[NPTSX*NPTSY], yf[NPTSX*NPTSY];
static double mtsaiopengl_modelview[16];
static double mtsaiopengl_projection[16];

static double f, T[3], r[9];

static double xwn[NPTSX*NPTSY], ywn[NPTSX*NPTSY], xfn[NPTSX*NPTSY], yfn[NPTSX*NPTSY];
bool wasCalibrate = false;

/************************************************************************
 * 
 ************************************************************************/
static void Init( char* filename )
{
	int i;
	double cam_posx, cam_posy, cam_posz;

	capture = cvCaptureFromAVI( filename ); 
	in_frame = cvQueryFrame( capture );
	width = cvGetCaptureProperty( capture, CV_CAP_PROP_FRAME_WIDTH );
	height = cvGetCaptureProperty( capture, CV_CAP_PROP_FRAME_HEIGHT );
    
    int ind = 0;
    for (int h = 0; h <  NPTSY; h++) {
        for (int w = 0; w < NPTSX; w++) {
            xw[ind] = w+1;		
            yw[ind] = h+1;
            ind++;
        }
    }
    glEnable (GL_DEPTH_TEST);
    glShadeModel(GL_SMOOTH);  
    glClearColor(0.0, 0.0, 0.0, 0.0);
}

static void DrawOpencvImage( IplImage* image )
{
    glDrawPixels(width,height,GL_BGR_EXT,GL_UNSIGNED_BYTE,image->imageData);
}

static void Redraw ()
{
    glClearColor(0.0, 0.0, 0.0, 0.0);

	glMatrixMode(GL_PROJECTION);
	glLoadIdentity();
	gluOrtho2D(0.0,width, 0.0, height);
	glMatrixMode(GL_MODELVIEW);
	glLoadIdentity();
	DrawOpencvImage( in_frame );

    if(wasCalibrate)
    {
        float pos[4] = { 0.0f, 10.0f, 0.0f, 0.0f };
        glLightfv( GL_LIGHT0, GL_POSITION, pos );

        float white[4] = { 1.0f, 1.0f, 1.0f, 1.0f };
        float red[4] = { 1.0f, 0.0f, 0.0f, 1.0f };
        glMaterialfv( GL_FRONT_AND_BACK, GL_AMBIENT_AND_DIFFUSE, red );
        glMaterialfv( GL_FRONT_AND_BACK, GL_SPECULAR, white );
        glMaterialf( GL_FRONT_AND_BACK, GL_SHININESS, 50.0f );

        glMatrixMode(GL_PROJECTION);
        glLoadIdentity();
        glLoadMatrixd(mtsaiopengl_projection);

        glEnable(GL_LIGHTING);
        glEnable(GL_LIGHT0);
        glMatrixMode(GL_MODELVIEW);
        glLoadIdentity();
        glLoadMatrixd(mtsaiopengl_modelview);
        glRotatef(90, 1.0, 0.0, 0.0);
        glutSolidTeapot(1.0);
    }

	glutSwapBuffers();
}

static void End()
{
	cvReleaseCapture( &capture );
	cvReleaseImage( &gray_image );
	exit( 0 );
}

static void Idle()
{
	int i;
	double cam_posx, cam_posy, cam_posz;

	if(!cvGrabFrame(capture))
	{
		End();
	}

	in_frame = cvRetrieveFrame(capture);
    corner_count = 0;

    int result = cvFindChessboardCorners( in_frame, cvSize( NPTSX, NPTSY ), corners, &corner_count, CV_CALIB_CB_ADAPTIVE_THRESH );

	if( result )
    {
	    for( i = 0; i < NPTSX*NPTSY; i++ )
	    {
		    xf[i] = corners[i].x - width/2.0;
		    yf[i] = corners[i].y - height/2.0;
	    }

        if( !wasCalibrate )
        {
            if( tsaiCalibration2D(NPTSX*NPTSY, xw, yw, xf, yf, &f, T, r) )
            {
                tsaiOGLModelViewMatrix(T, r, mtsaiopengl_modelview);
                tsaiOGLProjectionMatrix(f, NEARPLANE, FARPLANE, width, height, mtsaiopengl_projection);
                wasCalibrate = true;
            }
        }
        else
        {
            tsaiCalibration2D_Tr(NPTSX*NPTSY, xw, yw, xf, yf, f, T, r);
            tsaiOGLModelViewMatrix(T, r, mtsaiopengl_modelview);
        }
        cvDrawChessboardCorners( in_frame, cvSize( NPTSX, NPTSY ), corners, corner_count, 1 );
    }
    glutPostRedisplay();

}

static void Keyboard (unsigned char key, int x, int y)
{
	switch (key) {
	case 'q':
		End();
		break;
	}
}

int main (int argc, char *argv[])
{
	Init( argv[1] );
	glutInit(&argc, argv);
	glutInitDisplayMode(GLUT_DOUBLE | GLUT_DEPTH | GLUT_RGB);
	glutInitWindowSize( width,height );
	glutCreateWindow("Realidade Aumentada");
	glutDisplayFunc(Redraw);
	glutKeyboardFunc(Keyboard);
	glutIdleFunc(Idle);
	glutMainLoop();
	return 0;
}
