#include <stdio.h>
#include <math.h>
#include <stdlib.h>

#include "algebra.h"
#include "stereo.h"

#include <im.h>  
#include <im_convert.h>
#include <im_image.h>
#include <im_util.h>

#include <iup.h>
#include <iupgl.h>
#include <Windows.h>
#include <glut.h>
//#include <GL/GL.h>
//#include <GL/glu.h>

Ihandle *canvas, *finale, *dg;

int num;
double maxi,mini;

typedef struct{
	float x,y,z;
}vec3;
vec3* pontos;

typedef struct {
	unsigned char* image;
	vec3 epiLine;
	float translation[3];
	float rotation[9];
	float focalLenght;
	vec3 cameraPosition;
	Matrix Ra;
	int  w;
	int  h;
	float F[9];
}viewData;
viewData leftView={NULL,{0,0,0}},rightView={NULL,{0,0,0}};

void ComputeFundamentalMatrix(float *Rl, float *Tl, float Fl,float *Rr, float *Tr, float Fr,float *F);
void MulMatrix3x3Vec3(const float *matrixA,const float *vec1,float *vec2);



void loadPhoto(char* name_in , unsigned char **img,int *h,int*w)
{
	unsigned char* idata = NULL;
	imImage* fotoAberta;
	int error;

	/* load src image */
	imFile* ifile = imFileOpen(name_in, &error);
	if (!ifile) return ; 
	fotoAberta = imFileLoadBitmap(ifile, 0, &error);
	imFileClose(ifile);
	if(*img) free(*img);		
	*img =(unsigned char*) malloc(fotoAberta->size);	
	*h   =	 fotoAberta->height;
	*w   =	 fotoAberta->width;		
	imConvertPacking(fotoAberta->data[0], *img, fotoAberta->width, fotoAberta->height, fotoAberta->depth, fotoAberta->data_type, 0);
}

void savePhoto(char* name_out , unsigned char *img,int h,int w)
{	
	int error;
	imFile* outFile=imFileNew(name_out,"TIFF", &error);
	if (!outFile) 
		printf("Error creating TIFF file. %d\n",error);
	else {
		imFileWriteImageInfo(outFile, w, h, IM_RGB|IM_PACKED, IM_BYTE);
		imFileWriteImageData(outFile,img);
		imFileClose(outFile);
	}
}


//Carrega o arquivo de calibração, se existir.
int loadCalibFile(viewData *view)
{
	FILE *calibFile;
	double a0,a1,a2,a3,a4,a5,a6,a7,a8,a9,a10,a11,a12,a13,a14,a15;
	int n;
	char calibFileName[40];
	if(view==&leftView)
	{
		sprintf(calibFileName,"../transV0/cam2.txt"); //esq
		//sprintf(calibFileName,"../transV0/padraoCam2.txt"); //esq
		printf("oi");
	}
	else
	{
		sprintf(calibFileName,"../transV0/cam1.txt"); //dir
		//sprintf(calibFileName,"../transV0/padraoCam1.txt"); //dir
		printf("oi2");
	}
	calibFile=fopen(calibFileName,"rt");
	if(!calibFile){
		printf("Calibration file not found. %s\n",calibFileName);
		return 0;
	}
	//carrega a matriz de rotação
	for(n=0;n<9;n++){
		fscanf(calibFile,"%f",&view->rotation[n]);
	}
	//carrega translação
	for(n=0;n<3;n++){
		fscanf(calibFile,"%f",&view->translation[n]);
	}
	//carrega a posição da câmera
	fscanf(calibFile,"%f",&view->cameraPosition.x);
	fscanf(calibFile,"%f",&view->cameraPosition.y);
	fscanf(calibFile,"%f",&view->cameraPosition.z);
	//carrega distância focal
	fscanf(calibFile,"%f",&view->focalLenght);

	fscanf( calibFile, " %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf",&a0,&a1,&a2,&a3,&a4,&a5,&a6,&a7,&a8,&a9,&a10,&a11,&a12,&a13,&a14,&a15);
	view->Ra = algExtractLT(algMatrix4x4(a0,a1,a2,a3,a4,a5,a6,a7,a8,a9,a10,a11,a12,a13,a14,a15));

	fscanf(calibFile,"%f %f",&view->w,&view->h);
	fclose(calibFile);
	return 1;
}

//ponto com referencia ao centro da foto
vec3 procuraCorrespondente(float* F,int col,vec3 ponto,int largura,int altura,unsigned char *imagem)
{
	float epiLine1[3];
	float epiLine2[3];
	int i,ind;
	double a,b,c,yP;
	vec3 ret;

	Matrix F1;

	F1.m[0]=F[0];
	F1.m[1]=F[1];
	F1.m[2]=F[2];
	F1.m[3]=F[3];
	F1.m[4]=F[4];
	F1.m[5]=F[5];
	F1.m[6]=F[6];
	F1.m[7]=F[7];
	F1.m[8]=F[8];
	F1.m[9]=F[9];
	F1.m[10]=F[10];
	F1.m[11]=F[11];
	F1.m[12]=F[12];
	F1.m[13]=F[13];
	F1.m[14]=F[14];
	F1.m[15]=F[15];

	//printf("ponto :-> %lf,%lf\n",(double) ponto.x,(double) altura-ponto.y);
	stereoEpipolarLineEquation((double) ponto.x,(double) altura-ponto.y,F1,&a,&b,&c);

	epiLine1[0]=a;
	epiLine1[1]=b;
	epiLine1[2]=c;

	//Calcula percorrendo x

	for (i=0;i<largura;i++)
	{
		ret.x=(float)i; 
		ret.y= (float) floor(((epiLine1[0]*ret.x+epiLine1[2])/epiLine1[1]));
		yP = altura-ret.y;
		//printf("Y :-> %lf\n",yP);
		ind = 3*(i+(int)yP*largura)+2;

		//if ((altura > yP)&&(0 <= yP))
			//printf("codigos :-> %lf,%lf\n",col,imagem[ind]);

		//ind = 3*(i+(int)ret.y*largura)+2;

		//if ((altura > yP)&&(0 <= yP) && (yP != 767))
		//{
		//	printf("Y :-> %lf\n",yP);		
		//}
		//if (imagem[ind] == col)
		//{
		//	printf("codigo :-> %lf\n",col);
		//}
		if ((altura > yP)&&(0 <= yP) && (imagem[ind] == col))
		{
			ret.x=(float)i;
			ret.y=yP;
			ret.z=imagem[ind-1];
			//printf("reta mesmo :-> %lf,%lf,%lf\n",a,b,c);
			return ret;
		}
	}

	//Calcula percorrendo y

	//for (i=0;i<altura;i++)
	//{
	//	ret.y=(float)i; 
	//	ret.x= (float) floor(((epiLine1[1]*ret.y+epiLine1[2])/epiLine1[0]));
	//	yP = altura-ret.y;
	//	//printf("Y :-> %lf\n",yP);
	//	ind = 3*(ret.x+(int)yP*largura)+2;

	//	//if ((altura > yP)&&(0 <= yP))
	//		//printf("codigos :-> %lf,%lf\n",col,imagem[ind]);

	//	//ind = 3*(i+(int)ret.y*largura)+2;

	//	//if ((altura > yP)&&(0 <= yP) && (yP != 767))
	//	//{
	//	//	printf("Y :-> %lf\n",yP);		
	//	//}
	//	//if (imagem[ind] == col)
	//	//{
	//	//	printf("codigo :-> %lf\n",col);
	//	//}
	//	if ((largura > ret.x)&&(0 <= ret.x) && (imagem[ind] == col))
	//	{
	//		//ret.x=(float)i;
	//		ret.y=yP;
	//		ret.z=imagem[ind-1];
	//		printf("reta mesmo :-> %lf,%lf,%lf\n",a,b,c);
	//		return ret;
	//	}
	//}
	ret.x = -1;
	ret.y = -1;

	return ret;
}

Vector calculaZ(double xa, double ya,double xb, double yb, viewData b, viewData a)
{
	int erro;
	Vector eye_a = algVector(a.cameraPosition.x,a.cameraPosition.y,a.cameraPosition.z,1);
	Vector eye_b = algVector(b.cameraPosition.x,b.cameraPosition.y,b.cameraPosition.z,1);
	Vector pl = stereoReconstruct(xa,ya,a.focalLenght,a.Ra,eye_a,a.w/2,a.h/2,
		xb,yb,b.focalLenght,b.Ra,eye_b,b.w/2,b.h/2,&erro);
	//Vector Pw = algAdd(eye_a,algTransf(algTransp(a.Ra),pl));
	Vector Pw = pl;
	//printf("Camera_a :-> %f,%f,%f\n",a.cameraPosition.x,a.cameraPosition.y,a.cameraPosition.z);
	//printf("Camera_b :-> %f,%f,%f\n",b.cameraPosition.x,b.cameraPosition.y,b.cameraPosition.z);
	//printf("Eye_a :-> %f,%f,%f\n",eye_a.x,eye_a.y,eye_a.z);
	//printf("Eye_b :-> %f,%f,%f\n",eye_b.x,eye_b.y,eye_b.z);
	if (erro) Pw.z = -1.0f;
	return Pw;
}


int k_any(Ihandle *self, int c)
{
	if(c == K_q)
		return IUP_CLOSE;
	else
		return IUP_DEFAULT;
}

int redraw(Ihandle *self, float x, float y)
{
	int w, h,i;
	Vector a,b,c,d;
	char *size = IupGetAttribute(self, IUP_RASTERSIZE);
	sscanf(size, "%dx%d", &w, &h);

	IupGLMakeCurrent(self);
	glViewport(0, 0, w, h);
	glMatrixMode(GL_PROJECTION);
	glLoadIdentity();
	gluPerspective(70,w/h,1000,5000);

	glMatrixMode(GL_MODELVIEW);
	glLoadIdentity();
	a = algVector(leftView.cameraPosition.x,leftView.cameraPosition.y,leftView.cameraPosition.z,1);
	b = algVector(pontos[40].x,pontos[40].y,pontos[40].z,1);
	
	gluLookAt(leftView.cameraPosition.x,leftView.cameraPosition.y,leftView.cameraPosition.z,b.x,b.y,b.z,0,1.0,0);


	
	glClearColor(0.0, 0.0, 0.0, 1.f);
	glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
	


	{
	
	double mModel[16],mPrj[16];
	int    viewport[4],i;
	GLdouble wx,wy,wz,diff = maxi-mini;
	GLdouble ox,oy,oz;

	ox=b.x;
	oy=b.y;
	oz=b.z;
		
	glPointSize(1);
	glBegin(GL_POINTS);
	for (i=0;i<num;i++)
	{
		
		glColor3f(((pontos[i].z-mini)/diff),0.0,1-((pontos[i].z-mini)/diff));
		glVertex3f(pontos[i].x,pontos[i].y,pontos[i].z);
	}
	glEnd();
	}
	IupGLSwapBuffers(self); 

	return IUP_DEFAULT;
}


int main(void)
{   	
	int i,j,ind,ind1,ind2;
	float z;
	char nome[200];
	FILE* saida ;
	FILE* saida2 ;
    Vector ponto3d, eye1, eye2 ;   
    Matrix r1, r2,K1,K2,F1 ;
	int erro;
	int angulo;

	loadCalibFile(&leftView);
	loadCalibFile(&rightView);

	r1.m[0]=rightView.rotation[0];
	r1.m[4]=rightView.rotation[1];
	r1.m[8]=rightView.rotation[2];
	r1.m[12]=0.0;
	r1.m[1]=rightView.rotation[3];
	r1.m[5]=rightView.rotation[4];
	r1.m[9]=rightView.rotation[5];
	r1.m[13]=0.0;
	r1.m[2]=rightView.rotation[6];
	r1.m[6]=rightView.rotation[7];
	r1.m[10]=rightView.rotation[8];
	r1.m[14]=0.0;
	r1.m[3]=0.0;
	r1.m[7]=0.0;
	r1.m[11]=0.0;
	r1.m[15]=1.0;

	r2.m[0]=leftView.rotation[0];
	r2.m[4]=leftView.rotation[1];
	r2.m[8]=leftView.rotation[2];
	r2.m[12]=0.0;
	r2.m[1]=leftView.rotation[3];
	r2.m[5]=leftView.rotation[4];
	r2.m[9]=leftView.rotation[5];
	r2.m[13]=0.0;
	r2.m[2]=leftView.rotation[6];
	r2.m[6]=leftView.rotation[7];
	r2.m[10]=leftView.rotation[8];
	r2.m[14]=0.0;
	r2.m[3]=0.0;
	r2.m[7]=0.0;
	r2.m[11]=0.0;
	r2.m[15]=1.0;

	eye1.x = rightView.cameraPosition.x;
	eye1.y = rightView.cameraPosition.y;
	eye1.z = rightView.cameraPosition.z;
	eye1.w = 1.0;

	eye2.x = leftView.cameraPosition.x;
	eye2.y = leftView.cameraPosition.y;
	eye2.z = leftView.cameraPosition.z;
	eye2.w = 1.0;

	K1 = algMatrix4x4( leftView.focalLenght, 0.0, 0.0,0.0, 
					0.0, leftView.focalLenght, 0.0,0.0,//0.0, leftView.focalLenght, leftView.h,0.0,
                   0.0, 0.0, 1.0,0.0,
				   0.0, 0.0, 0.0,1.0);

	K2 = algMatrix4x4( rightView.focalLenght, 0.0, 0.0,0.0, 
					0.0, rightView.focalLenght, 0.0,0.0,//0.0, rightView.focalLenght, rightView.h,0.0,
                   0.0, 0.0, 1.0,0.0,
				   0.0, 0.0, 0.0,1.0);

	F1=stereoFundamentalMatrix(K1, r1, eye1, K2, r2, eye2);

	rightView.F[0]=F1.m[0];
	rightView.F[1]=F1.m[1];
	rightView.F[2]=F1.m[2];
	rightView.F[3]=F1.m[3];
	rightView.F[4]=F1.m[4];
	rightView.F[5]=F1.m[5];
	rightView.F[6]=F1.m[6];
	rightView.F[7]=F1.m[7];
	rightView.F[8]=F1.m[8];
	rightView.F[9]=F1.m[9];
	rightView.F[10]=F1.m[10];
	rightView.F[11]=F1.m[11];
	rightView.F[12]=F1.m[12];
	rightView.F[13]=F1.m[13];
	rightView.F[14]=F1.m[14];
	rightView.F[15]=F1.m[15];

	mini = 999999999;
	maxi = -1;

	for (angulo=0;angulo<=11;angulo++)
	{
	sprintf(nome, "correspondencia %03d.txt", angulo*30);
	saida = fopen(nome,"wt");
	saida2 = fopen("pontos_correspondentes.txt","wt");

	//ComputeFundamentalMatrix(rightView.rotation,rightView.translation,rightView.focalLenght,leftView.rotation,leftView.translation,leftView.focalLenght,rightView.F);
	sprintf(nome, "../transV0/Cavalo Camera 1 Rotacao %03d Vertical.bmp", angulo*30);
	loadPhoto(nome,&(rightView.image),&(rightView.h),&(rightView.w)); // dir.tiff
	//loadPhoto("../transV0/resultV1.BMP",&(rightView.image),&(rightView.h),&(rightView.w)); // dir.tiff
	//loadPhoto("../transV0/padraoCam1.BMP",&(rightView.image),&(rightView.h),&(rightView.w)); // dir.tiff
	printf("RW: %d \n",rightView.h);
	sprintf(nome, "../transV0/Cavalo Camera 2 Rotacao %03d Vertical.bmp", angulo*30);
	loadPhoto(nome,&(leftView.image),&(leftView.h),&(leftView.w)); // esq.tiff
	//loadPhoto("../transV0/resultV2.BMP",&(leftView.image),&(leftView.h),&(leftView.w)); // esq.tiff
	//loadPhoto("../transV0/padraoCam2.BMP",&(leftView.image),&(leftView.h),&(leftView.w)); // esq.tiff
	printf("LW: %d \n",leftView.w);

	for (j=0;j<rightView.h;j++)
		for (i=0;i<rightView.w;i++)
		{
			ind = 3*(i+j*rightView.w)+2;
			if (rightView.image[ind]!= 0 )
			{
				vec3 ponto = {(float)i,(float)j,0.0f} ;
				vec3 rep = procuraCorrespondente(rightView.F,rightView.image[ind],ponto,leftView.w,leftView.h,leftView.image);
				
				if (rep.x != -1)
				{
					Vector retor= calculaZ((rep.x+rep.z/100),rep.y,(i+rightView.image[ind-1]/100),j,leftView,	rightView);
					//printf("rep :-> %f,%f,%f\n",rep.x,rep.y,rep.z);
					if(retor.z != -1.0f)
					{
						ind1 = 3*((rep.x+rep.z/100)+rep.y*rightView.w)+2;
						ind2 = 3*((float)(i+rightView.image[ind-1]/100)+j*rightView.w)+2;
						//
						fprintf(saida2,"pontos :-> %f,%f,%f,%d\n",(rep.x+rep.z/100),rep.y,(float)(i+rightView.image[ind-1]/100),j);
						fprintf(saida2,"codigos faixa :-> %f,%f\n",(float)rightView.image[ind2],(float)leftView.image[ind1]);
						//printf("alguem :-> %f,%f,%d\n",rep.x,rep.y,rightView.image[ind]);
						//printf("rep :-> %f,%f,%f\n",rep.x,rep.y,rep.z);
						//printf("retor :-> %f,%f,%f\n",retor.x,retor.y,retor.z);
						fprintf(saida,"%f %f %f \n",retor.x, retor.y, retor.z);
						num++;                        						
					}
				}
			}
		}
	
	//loadPhoto("../transV0/resultH1.BMP",&(rightView.image),&(rightView.h),&(rightView.w)); // dir.tiff
	//loadPhoto("../transV0/resultH2.BMP",&(leftView.image),&(leftView.h),&(leftView.w)); // esq.tiff
	//for (j=0;j<rightView.h;j++)
	//	for (i=0;i<rightView.w;i++)
	//	{
	//		ind = 3*(i+j*rightView.w)+2;
	//		if (rightView.image[ind]!= 0 )
	//		{
	//			vec3 ponto = {(float)i,(float)j,0.0f} ;
	//			vec3 rep = procuraCorrespondente(rightView.F,rightView.image[ind],ponto,leftView.w,leftView.h,leftView.image);
	//			
	//			if (rep.x != -1)
	//			{
	//				Vector retor= calculaZ((rep.x+rep.z/100),rep.y,(i+rightView.image[ind-1]/100),j,leftView,	rightView);
	//				//printf("rep :-> %f,%f,%f\n",rep.x,rep.y,rep.z);
	//				if(retor.z != -1.0f)
	//				{
	//					ind1 = 3*((rep.x+rep.z/100)+rep.y*rightView.w)+2;
	//					ind2 = 3*((float)(i+rightView.image[ind-1]/100)+j*rightView.w)+2;
	//					//
	//					fprintf(saida2,"pontos :-> %f,%f,%f,%d\n",(rep.x+rep.z/100),rep.y,(float)(i+rightView.image[ind-1]/100),j);
	//					fprintf(saida2,"codigos faixa :-> %f,%f\n",(float)rightView.image[ind2],(float)leftView.image[ind1]);
	//					//printf("alguem :-> %f,%f,%d\n",rep.x,rep.y,rightView.image[ind]);
	//					//printf("rep :-> %f,%f,%f\n",rep.x,rep.y,rep.z);
	//					//printf("retor :-> %f,%f,%f\n",retor.x,retor.y,retor.z);
	//					fprintf(saida,"%f %f %f \n",retor.x, retor.y, retor.z);
	//					num++;                        						
	//				}
	//			}
	//		}
	//	}
	fclose(saida);
	fclose(saida2);

	}
	ponto3d = stereoReconstruct( 545.0, 458.0, 927.05801, r1, eye1, 512.0, 384.0, 567.0, 462.0, 927.05801, r2, eye2, 512.0, 384.0, &erro );
	printf( " X = %5g, Y = %5g, Z = %5g, erro = %d\n", algGetX( ponto3d ), algGetY( ponto3d ), algGetZ( ponto3d ), erro ) ;

return 1;
	 


}