#include <cv.h>
#include <cvaux.h>
#include <highgui.h>
#include "tsai.h"


extern int WIDTH, HEIGHT;


void calibrarCamera(IplImage* img, float *distanciaFocal, float translacao[3], float rotacao[9])
{
	int i,j;
	float *xw, *yw, *xf, *yf;
	int corner_count;
	CvPoint2D32f* corners;
	CvSize sizeChessboard = cvSize(8,6);
	int totalCorners = sizeChessboard.width * sizeChessboard.height;


	corners = (CvPoint2D32f*)malloc(totalCorners*sizeof(CvPoint2D32f));	
	xw = (float *)malloc(totalCorners*sizeof(float));
	yw = (float *)malloc(totalCorners*sizeof(float));
	xf = (float *)malloc(totalCorners*sizeof(float));
	yf = (float *)malloc(totalCorners*sizeof(float));

	// encontra os corners do tabuleiro
	int flag = cvFindChessboardCorners(img,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;
			}
		}
		
		tsaiCalibration2D(corner_count,xw,yw,xf,yf,distanciaFocal,translacao,rotacao);
		
		//tsaiOGLModelViewMatrix(translacao,rotacao,mtsaiopengl_modelview);
		//tsaiOGLProjectionMatrix(distanciaFocal, 10, 1000, WIDTH, HEIGHT, mtsaiopengl_projection);
	}
	else
		printf("ERRO - NÃO ENCONTROU TODOS OS CANTOS DO PADRÃO!");

	free(corners);
	free(xw);
	free(yw);
	free(xf);
	free(yf);
}