#ifdef WIN32            
#include <windows.h>    /* necessary because of the Microsoft OpenGL headers dependency */
#endif

#include <GL/gl.h>
#include <GL/glu.h>
#include <GL/glut.h>

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

#include <iup.h>
#include <iupcontrols.h>
#include <iupgl.h>

#include <im.h>
#include <im_capture.h>
#include <im_format_avi.h>
#include <im_image.h>
#include <im_util.h>

#include "util.h"
#include "jv.h"
#include "tsai.h"
#include "fundamental.h"
#include "table_mau.h"
#include "stereo.h"
#include "algebra.h"
#include "bresline.h"

#define PI 3.1415926535897
#define MAX_LINES 500
#define MAX_INTERSECTION 500
#define NEW_POINT_TSAI 8
#define LINES_IN_MODEL 7
#define TOLERANCIA 30

typedef struct{
	float x,y;
	float x_proj,y_proj;
	float epiLine[3];
	float epiLinePROJ[3];
	float x1,y1,x2,y2;
	float x1_proj,y1_proj,x2_proj,y2_proj;
	float faixa;
	float xe;
	float ye;
	float ze;
}Epipolar;

Epipolar epi[6291456];
int qtdEpi = 0;

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

int modelo; // 1 (cano_amassado_vista_1), 2 (cano_amassado_vista_2), 3 (cano_amassado_vista_3)
int padrao_calibracao; // 1 final , 2 inicio 1 , 3 inicio 2


/* inteseção do modelo */
static jvPoint2D iMOD[MAX_INTERSECTION];
int qtdIM;

/* novos pontos do modelo e da imagem retornados no New Data */
jvPoint2D p1n[NEW_POINT_TSAI * NEW_POINT_TSAI];
jvPoint2D p2n[NEW_POINT_TSAI * NEW_POINT_TSAI];

/* matrizes de transformação do opengl */
float *mtsaiopengl_projection_direita;
float *mtsaiopengl_modelview_direita;

float *mtsaiopengl_projection_esquerda;
float *mtsaiopengl_modelview_esquerda;

/* valores maximos e minimos do modelo */
float maxX , maxY, minX , minY;

/* homografia */
float H[9];

/* variáveis para o tsai */
float *xwn = NULL;
float *ywn = NULL;
float *xfn = NULL;
float *yfn = NULL;
float *h = NULL;

/* foco, translação e rotação */
static float f_direita, T_direita[3], r_direita[9], MF_direita[9];
static float f_esquerda, T_esquerda[3], r_esquerda[9], MF_esquerda[9];

/* posicao da camera */
float cam_posx_direita, cam_posy_direita, cam_posz_direita;
float cam_posx_esquerda, cam_posy_esquerda, cam_posz_esquerda;


	unsigned char* imgTransicaoDireita = NULL;
	unsigned char* imgTransicaoEsquerda = NULL;
	imImage* imgPositiva1_direita = NULL;
	imImage* imgPositiva2_direita = NULL;
	imImage* imgPositiva1_esquerda = NULL;
	imImage* imgPositiva2_esquerda = NULL;
	int **pixelFaixasDireita = NULL;
	int *qtdPixelFaixasDireita = NULL;
	int **pixelFaixasEsquerda = NULL;
	int *qtdPixelFaixasEsquerda = NULL;


int intersecao( int nLines , jvLine *lines , jvPoint2D *iIMG, int width , int height )
{
	int n, j, qtd;

	float *a, *b, *c;
   
	a  = malloc( nLines * sizeof(float) );
	b  = malloc( nLines * sizeof(float) );
	c  = malloc( nLines * sizeof(float) );

	qtd = 0;

   // calcula os indices a,b,c das equacoes da reta
   for( n = 0 ; n < nLines ; n++ )
   {
		a[n] = lines[n].v2y - lines[n].v1y;
		b[n] = lines[n].v1x - lines[n].v2x;
		c[n] = (lines[n].v2x*lines[n].v1y)-(lines[n].v1x*lines[n].v2y);
   }


   // para cada linha verifica as intersecao
   for( n = 0 ; n < nLines ; n++ )
   {
        // verifica se as proximas linhas tem intersecao
		for( j = n + 1 ; j < nLines ; j++ )
		{
  			// calcula o X
			//b1c2      //b2c1        //a1b2      //a2b1
			iIMG[qtd].x = ((b[n]*c[j])-(b[j]*c[n]))/((a[n]*b[j])-(a[j]*b[n]));

			// calcula o Y
			//a2c1      //a1c2        //a1b2      //a2b1
			iIMG[qtd].y = ((a[j]*c[n])-(a[n]*c[j]))/((a[n]*b[j])-(a[j]*b[n]));

			// se estiver no campo de visao
			if ( iIMG[qtd].x <= width && iIMG[qtd].x >= 0 && iIMG[qtd].y >= 0 && iIMG[qtd].y <= height )
				qtd++;
		}
   }

   free(a);
   free(b);
   free(c); 

   return qtd;
}



int carregaLinhas( char* model_file_name , jvLine *linhasModelo )
{
	int i, qtd;
	FILE *fd;
   
	fd = fopen(model_file_name, "r");

	if (!fd)
		return 0;
	
	if (fscanf(fd, "%d", &qtd) != 1)
		return 0;
 
	for ( i = 0 ; i < qtd ; i++ )
	{
		if (fscanf(fd, "%f %f %f %f", &(linhasModelo[i].v1x), &(linhasModelo[i].v1y), &(linhasModelo[i].v2x), &(linhasModelo[i].v2y)) != 4)
		{
			return 0;
		}
	}

	fclose(fd);
	
	return qtd;
}

void carregaMaxMin ( int qtd , jvLine *linhas)
{
	int y;

	maxX = minX = linhas[0].v1x;
	maxY = minY = linhas[0].v1y;

	// encontra os máximos e mínimos
	for ( y = 0 ; y < qtd ; y++ )
	{
		if ( linhas[y].v1x < minX )
			minX = linhas[y].v1x;
		if ( linhas[y].v2x < minX )
			minX = linhas[y].v2x;

		if ( linhas[y].v1x > maxX )
			maxX = linhas[y].v1x;
		if ( linhas[y].v2x > maxX )
			maxX = linhas[y].v2x;

		if ( linhas[y].v1y < minY )
			minY = linhas[y].v1y;
		if ( linhas[y].v2y < minY )
			minY = linhas[y].v2y;
		
		if ( linhas[y].v1y > maxY )
			maxY = linhas[y].v1y;
		if ( linhas[y].v2y > maxY )
			maxY = linhas[y].v2y;
	}
}




void multiplicaHomografia(int qtdLinhas , jvLine *linhasCalculadas , jvLine *linhasOriginais , float *homografia )
{
	int y;
	float w;

	// calcula as linhas usando a homografia encontrada no tsai
	for ( y = 0 ; y < qtdLinhas ; y++ )
	{
		linhasCalculadas[y].v1x	=	(float)(homografia[0] * linhasOriginais[y].v1x +
											homografia[1] * linhasOriginais[y].v1y +
											homografia[2]);

		linhasCalculadas[y].v1y	=	(float)(homografia[3] * linhasOriginais[y].v1x +
											homografia[4] * linhasOriginais[y].v1y +
											homografia[5]);

		w						=	(float)(homografia[6] * linhasOriginais[y].v1x +
											homografia[7] * linhasOriginais[y].v1y +
											homografia[8]);
		
		linhasCalculadas[y].v1x = linhasCalculadas[y].v1x / w ;
		linhasCalculadas[y].v1y = linhasCalculadas[y].v1y / w ;

		linhasCalculadas[y].v2x	=	(float)(homografia[0] * linhasOriginais[y].v2x +
											homografia[1] * linhasOriginais[y].v2y +
											homografia[2]);

		linhasCalculadas[y].v2y	=	(float)(homografia[3] * linhasOriginais[y].v2x +
											homografia[4] * linhasOriginais[y].v2y +
											homografia[5]);

		w						=	(float)(homografia[6] * linhasOriginais[y].v2x +
											homografia[7] * linhasOriginais[y].v2y +
											homografia[8]);
		linhasCalculadas[y].v2x = linhasCalculadas[y].v2x / w ;
		linhasCalculadas[y].v2y = linhasCalculadas[y].v2y / w ;
	}
}

void alocaRecursos()
{
	// novos pontos que serão gerados
	xwn		= malloc( NEW_POINT_TSAI * NEW_POINT_TSAI * sizeof(float) );
	ywn		= malloc( NEW_POINT_TSAI * NEW_POINT_TSAI * sizeof(float) );
	xfn		= malloc( NEW_POINT_TSAI * NEW_POINT_TSAI * sizeof(float) );
	yfn		= malloc( NEW_POINT_TSAI * NEW_POINT_TSAI * sizeof(float) );
	
	// criação da homografia
	h		= malloc( 9 * sizeof(float)	 );
}

void liberaRecursos()
{
	// libera os recursos
	if ( h != NULL )
	{
		free(xwn);
		free(ywn);
		free(xfn);
		free(yfn);
		free(h);
	}
}

void desenhaPontos( unsigned char* buffer  , int width , int height , jvPoint2D* pontos )
{
    int i;
	int ponto;
	int x, y;

	for ( i = 0 ; i < NEW_POINT_TSAI * NEW_POINT_TSAI ; i++ )
	{
		x = pontos[i].x;
		y = pontos[i].y;

		ponto = (y * width) + x;

		buffer[ ponto ] = 255;
		buffer[ ponto+1 ] = 255;
		buffer[ ponto+2 ] = 255;
		buffer[ ponto+3 ] = 255;
		buffer[ ponto+4 ] = 255;
		buffer[ ponto+5 ] = 255;
		buffer[ ponto+6 ] = 255;
		buffer[ ponto+7 ] = 255;
		buffer[ ponto+8 ] = 255;
		buffer[ ponto+9 ] = 255;
		buffer[ ponto+10 ] = 255;
	}

}

void inverteImagem(imImage* imagemOriginal , unsigned char *tmpBuffer)
{
	int i, j, k;
	imbyte *tmpDataR,*tmpDataG,*tmpDataB;

	tmpDataR = (imbyte*)imagemOriginal->data[0];
	tmpDataG = (imbyte*)imagemOriginal->data[1];
	tmpDataB = (imbyte*)imagemOriginal->data[2];

	// para cada pixel (inverte a imagem)
	for( i = 0 , k = 0 ; i < imagemOriginal->height ; i++ )
		for( j = 0 ; j  < imagemOriginal->width ; j++ , k++ )
			tmpBuffer[i*imagemOriginal->width+j] = (unsigned char) (0.2126f*(255 - tmpDataR[k])+0.7152f*(255 - tmpDataG[k])+0.0722f*(255 - tmpDataB[k]));
}



int carregaLinhasManualmente( jvLine *linhas , int pos_cam )
{
	// camera da direita
	if ( pos_cam == 1)
	{
		linhas[0].v1x = 625;
		linhas[0].v1y = 458;
		linhas[0].v2x = 1820;
		linhas[0].v2y = 493;

		linhas[1].v1x = 1000;
		linhas[1].v1y = 833;
		linhas[1].v2x = 1815;
		linhas[1].v2y = 893;

		linhas[2].v1x = 648;
		linhas[2].v1y = 1394;
		linhas[2].v2x = 1812;
		linhas[2].v2y = 1530;

		linhas[3].v1x = 648;
		linhas[3].v1y = 1394;
		linhas[3].v2x = 625;
		linhas[3].v2y = 458;

		linhas[4].v1x = 1000;
		linhas[4].v1y = 833;
		linhas[4].v2x = 995;
		linhas[4].v2y = 473;

		linhas[5].v1x = 1390;
		linhas[5].v1y = 863;
		linhas[5].v2x = 1390;
		linhas[5].v2y = 483;

		linhas[6].v1x = 1812;
		linhas[6].v1y = 1530;
		linhas[6].v2x = 1815;
		linhas[6].v2y = 893;
	}
	else
	{
		linhas[0].v1x = 1220;
		linhas[0].v1y = 392;
		linhas[0].v2x = 2970;
		linhas[0].v2y = 654;

		linhas[1].v1x = 1755;
		linhas[1].v1y = 939;
		linhas[1].v2x = 2920;
		linhas[1].v2y = 1111;

		linhas[2].v1x = 1047;
		linhas[2].v1y = 1607;
		linhas[2].v2x = 2804;
		linhas[2].v2y = 1841;

		linhas[3].v1x = 1047;
		linhas[3].v1y = 1607;
		linhas[3].v2x = 1220;
		linhas[3].v2y = 392;

		linhas[4].v1x = 1755;
		linhas[4].v1y = 939;
		linhas[4].v2x = 1815;
		linhas[4].v2y = 484;

		linhas[5].v1x = 2340;
		linhas[5].v1y = 1024;
		linhas[5].v2x = 2408;
		linhas[5].v2y = 572;

		linhas[6].v1x = 2804;
		linhas[6].v1y = 1841;
		linhas[6].v2x = 2920;
		linhas[6].v2y = 1111;
	}

	return 7;
}


int calibraCameraManual( char *arquivo_saida , char *nome_arquivo , int pos_cam , float *cam_posx , float *cam_posy , float *cam_posz , float *mtsaiopengl_projection , float *mtsaiopengl_modelview , float *f , float *T,  float *r )
{
	jvPoint2D iIMG[MAX_INTERSECTION];
	int qtdII;
	imImage* img;
	int i, erro, nLinesInLeftImage;
	jvLine linesInLeftImage[MAX_LINES];
	unsigned char *tmpBufferA;

	img = imFileImageLoad( nome_arquivo , 0 , &erro );

	if (erro == 1)
		return 0;
	
	tmpBufferA = (unsigned char *) malloc(img->width*img->height);

	// inverte e guarda em tmpBufferA
	inverteImagem( img , tmpBufferA );

	// tem que fazer aki pq varia para cada imagem
	jvInit( img->width , img->height , 2 , 2 , "modelo.mdl" );

	// carrega manualmente as posicoes da linha na imagem
	nLinesInLeftImage = carregaLinhasManualmente( linesInLeftImage , pos_cam );

	// calcula as interseções das linhas encontradas (apenas as reconhecidas - TRUE)
	qtdII = intersecao( nLinesInLeftImage , linesInLeftImage , iIMG , img->width , img->height );

	// se encontrar todas as interseções
	// calcula a homografia
	if ( qtdII == qtdIM )
		jvFindTransf( H , qtdII, iMOD, iIMG ) ;
	else
		return 0;


	// cria os novos pontos
	jvBuildNewData( minX , minY , maxX , maxY , H , NEW_POINT_TSAI , NEW_POINT_TSAI , p1n , p2n );

	desenhaPontos( tmpBufferA ,  img->width , img->height , p2n );

	// salva uma imagem
	salvaImagem( arquivo_saida , img->width , img->height , tmpBufferA );

	// aloca os recursos
	alocaRecursos();

	// arruma os pontos para enviar ao tsai
	for ( i = 0 ; i < NEW_POINT_TSAI * NEW_POINT_TSAI ; i++ )
	{
		xwn[i] = p1n[i].x;
		ywn[i] = p1n[i].y;

		// converte para coordenada (0,0) no centro da imagem
		xfn[i] = p2n[i].x - (img->width  / 2);
		yfn[i] = p2n[i].y - (img->height / 2);
	}

	// calibra a camera
	// f já é ponteiro, nao precisa passar com &
	//tsaiCalibration2D( NEW_POINT_TSAI * NEW_POINT_TSAI, xwn, ywn, xfn, yfn, f, T, r);
	tsaiCalibration2D_Tr( NEW_POINT_TSAI * NEW_POINT_TSAI, xwn, ywn, xfn, yfn, *f, T, r);

	// calcula a posição da camera
	tsaiCameraPosition(cam_posx, cam_posy, cam_posz, T, r);

	tsaiOGLProjectionMatrix( *f,  10,  1000,  img->width,  img->height,  mtsaiopengl_projection );

	// calcula a matrix Model View
	tsaiOGLModelViewMatrix(T, r, mtsaiopengl_modelview);

	// se ja houver alocado, limpa (tsai)
	liberaRecursos();

	imImageDestroy(img);
	free(tmpBufferA);

	jvClose();

	return 1;
}


int calibracao()
{
	int nLinhasModelo;
	jvLine linhasModelo[MAX_LINES];

	FILE *arq;
	int i, resultado;
	char* nome_arquivo_direita;
	char* nome_arquivo_esquerda;
	char* aux;
	char* arquivo_saida_cam;
	char* imagem_saida_direita;
	char* imagem_saida_esquerda;

	nome_arquivo_direita = (char*) malloc(sizeof(char) * 500);
	nome_arquivo_esquerda = (char*) malloc(sizeof(char) * 500);
	aux = (char*) malloc(sizeof(char) * 500);
	arquivo_saida_cam = (char*) malloc(sizeof(char) * 500);
	imagem_saida_direita = (char*) malloc(sizeof(char) * 500);
	imagem_saida_esquerda = (char*) malloc(sizeof(char) * 500);

	mtsaiopengl_projection_direita	= malloc ( sizeof(float) * 16 );
	mtsaiopengl_modelview_direita	= malloc ( sizeof(float) * 16 );

	mtsaiopengl_projection_esquerda = malloc ( sizeof(float) * 16 );
	mtsaiopengl_modelview_esquerda	= malloc ( sizeof(float) * 16 );

	switch(padrao_calibracao)
	{
		case(1):
			nome_arquivo_direita = "imagens\\calibracao\\mau\\direita\\final\\01.jpg";
			nome_arquivo_esquerda = "imagens\\calibracao\\mau\\esquerda\\final\\01.jpg";
			arquivo_saida_cam = "out\\calib_cam_final_01.txt";
			imagem_saida_direita = "out\\calib_direita_final_01.jpg";
			imagem_saida_esquerda = "out\\calib_esquerda_final_01.jpg";
		break;
	}


	// carrega as linhas
	nLinhasModelo = carregaLinhas( "modelo.mdl" , linhasModelo );

	// carrega os máximos e minimos do modelo
	carregaMaxMin( nLinhasModelo , linhasModelo );

	// calcula as interseções das linhas encontradas (DE TODAS - FALSE)
	qtdIM = intersecao( nLinhasModelo , linhasModelo , iMOD , maxX , maxY );

	f_direita = 7509.333;
	// calibra a camera da direita
	// armazenando os valores em cam_posx_direita, cam_posy_direita e cam_posz_direita
	resultado = calibraCameraManual( imagem_saida_direita , nome_arquivo_direita , 1 , &cam_posx_direita , &cam_posy_direita , &cam_posz_direita , mtsaiopengl_projection_direita ,  mtsaiopengl_modelview_direita , &f_direita , T_direita , r_direita );

	if ( resultado == 0 )
	{
		printf("\n\nErro ao calibrar a camera da direita.... !!!");
		return 0;
	}

	f_esquerda = 8448.00;
	// calibra a camera da direita
	// armazenando os valores em cam_posx_esquerda, cam_posy_esquerda e cam_posz_esquerda
	resultado = calibraCameraManual( imagem_saida_esquerda , nome_arquivo_esquerda , 2 , &cam_posx_esquerda, &cam_posy_esquerda , &cam_posz_esquerda , mtsaiopengl_projection_esquerda , mtsaiopengl_modelview_esquerda , &f_esquerda , T_esquerda , r_esquerda );

	if ( resultado == 0 )
	{
		printf("\n\nErro ao calibrar a camera da esquerda.... !!!");
		return 0;
	}

	ComputeFundamentalMatrix(r_esquerda,T_esquerda,f_esquerda,r_direita,T_direita,f_direita,MF_esquerda);
	ComputeFundamentalMatrix(r_direita,T_direita,f_direita,r_esquerda,T_esquerda,f_esquerda,MF_direita);

	arq = fopen(arquivo_saida_cam,"w");

	fprintf(arq, "Camera da direita:");
	fprintf(arq, "\n\tArquivo de calibracao: '%s'", nome_arquivo_direita );
	fprintf(arq, "\n\tPosicao da camera: x (%f) / y (%f) / z (%f)", cam_posx_direita , cam_posy_direita , cam_posz_direita );
	fprintf(arq, "\n\tf: %f", f_direita );
	fprintf(arq, "\n\tT:");
	for ( i = 0 ; i < 3 ; i++ )
		fprintf(arq, "\n\t\t[%d] '%f'", i , T_direita[i] );
	fprintf(arq, "\n\tR:");
	for ( i = 0 ; i < 9 ; i++ )
		fprintf(arq, "\n\t\t[%d] '%f'", i , r_direita[i] );
	fprintf(arq, "\n\tMatrix projecao:");
	for ( i = 0 ; i < 16 ; i++ )
		fprintf(arq, "\n\t\t[%d] '%f'", i , mtsaiopengl_projection_direita[i] );
	fprintf(arq, "\n\tMatrix model view:");
	for ( i = 0 ; i < 16 ; i++ )
		fprintf(arq, "\n\t\t[%d] '%f'", i , mtsaiopengl_modelview_direita[i] );

	fprintf(arq, "\n\nCamera da esquerda:");
	fprintf(arq, "\n\tArquivo de calibracao: '%s'", nome_arquivo_esquerda );
	fprintf(arq, "\n\tPosicao da camera: x (%f) / y (%f) / z (%f)", cam_posx_esquerda , cam_posy_esquerda , cam_posz_esquerda );
	fprintf(arq, "\n\tf: %f", f_esquerda );
	fprintf(arq, "\n\tT:");
	for ( i = 0 ; i < 3 ; i++ )
		fprintf(arq, "\n\t\t[%d] '%f'", i , T_esquerda[i] );
	fprintf(arq, "\n\tR:");
	for ( i = 0 ; i < 9 ; i++ )
		fprintf(arq, "\n\t\t[%d] '%f'", i , r_esquerda[i] );
	fprintf(arq, "\n\tMatrix projecao:");
	for ( i = 0 ; i < 16 ; i++ )
		fprintf(arq, "\n\t\t[%d] '%f'", i , mtsaiopengl_projection_esquerda[i] );
	fprintf(arq, "\n\tMatrix model view:");
	for ( i = 0 ; i < 16 ; i++ )
		fprintf(arq, "\n\t\t[%d] '%f'", i , mtsaiopengl_modelview_esquerda[i] );
	
	fclose(arq);

	return 1;
}

unsigned char* encontraTransicao(imImage* imgPositiva , imImage* imgNegativa , char *saida_faixas, char *saida_faixas_negativa)
{
	int i, j, k;
	unsigned char rP, gP, bP, rN, gN, bN;
	int pixelPositivo; // 0 - r , 1 - g , 2 - b
	unsigned char valorPositivo , valorNegativo ;
	imbyte *imgPositivaR,*imgPositivaG,*imgPositivaB;
	imbyte *imgNegativaR,*imgNegativaG,*imgNegativaB;
	unsigned char* resultado;
	unsigned char* resultadoCoresR;
	unsigned char* resultadoCoresG;
	unsigned char* resultadoCoresB;
	unsigned char* resultadoCoresNR;
	unsigned char* resultadoCoresNG;
	unsigned char* resultadoCoresNB;

	resultado = (unsigned char *) malloc(imgPositiva->width*imgPositiva->height);
	resultadoCoresR = (unsigned char *) malloc( imgPositiva->width * imgPositiva->height);
	resultadoCoresG = (unsigned char *) malloc( imgPositiva->width * imgPositiva->height);
	resultadoCoresB = (unsigned char *) malloc( imgPositiva->width * imgPositiva->height);

	resultadoCoresNR = (unsigned char *) malloc( imgPositiva->width * imgPositiva->height);
	resultadoCoresNG = (unsigned char *) malloc( imgPositiva->width * imgPositiva->height);
	resultadoCoresNB = (unsigned char *) malloc( imgPositiva->width * imgPositiva->height);
    
	imgNegativaR = (imbyte*)imgNegativa->data[0];
	imgNegativaG = (imbyte*)imgNegativa->data[1];
	imgNegativaB = (imbyte*)imgNegativa->data[2];   

	imgPositivaR = (imbyte*)imgPositiva->data[0];
	imgPositivaG = (imbyte*)imgPositiva->data[1];
	imgPositivaB = (imbyte*)imgPositiva->data[2];   

	// para cada pixel
	for ( i = 0 , k = 0 , pixelPositivo = -1 ; i < imgPositiva->height ; i++ )
	{
		// para cada pixel
		for ( j = 0 ; j < imgPositiva->width ; j++ , k++ )
		{
			// pega r, g, b positivo da imagem
			rP = imgPositivaR[k];
			gP = imgPositivaG[k];
			bP = imgPositivaB[k];
			// pega r, g b negativo da imagem
			rN = imgNegativaR[k];
			gN = imgNegativaG[k];
			bN = imgNegativaB[k];

			// se tiver alguma coisa
			if ( rP > TOLERANCIA || gP > TOLERANCIA || bP > TOLERANCIA )
			{
				// para o primeiro
				if ( pixelPositivo == -1 )
				{
					if ( rP >= gP && rP >= bP )
						pixelPositivo = 0;
					else if ( gP >= rP && gP >= bP )
						pixelPositivo = 1;
					else if ( bP >= rP && bP >= gP )
						pixelPositivo = 2;
				}

				// se for R
				if ( pixelPositivo == 0 )
				{
					valorPositivo = rP;
					valorNegativo = rN;

					resultadoCoresR[k] = 255;
					resultadoCoresG[k] = 0;
					resultadoCoresB[k] = 0;

					resultadoCoresNR[k] = 0;
					resultadoCoresNG[k] = 255;
					resultadoCoresNB[k] = 255;
				}
				else if ( pixelPositivo == 1 )
				{
					valorPositivo = gP;
					valorNegativo = gN;

					resultadoCoresR[k] = 0;
					resultadoCoresG[k] = 255;
					resultadoCoresB[k] = 0;

					resultadoCoresNR[k] = 255;
					resultadoCoresNG[k] = 0;
					resultadoCoresNB[k] = 255;
				}
				else if ( pixelPositivo == 2 )
				{
					valorPositivo = bP;
					valorNegativo = bN;

					resultadoCoresR[k] = 0;
					resultadoCoresG[k] = 0;
					resultadoCoresB[k] = 255;

					resultadoCoresNR[k] = 255;
					resultadoCoresNG[k] = 255;
					resultadoCoresNB[k] = 0;
				}

				if ( valorNegativo >= valorPositivo )
				{
					resultado[k] = 255;
					pixelPositivo = -1;
				}
				else
					resultado[k] = 0;
			}
			else
			{
				resultadoCoresR[k] = 0;
				resultadoCoresG[k] = 0;
				resultadoCoresB[k] = 0;
				resultadoCoresNR[k] = 0;
				resultadoCoresNG[k] = 0;
				resultadoCoresNB[k] = 0;
				resultado[k] = 0;
			}
		}
	}

	imSaveRGB(imgPositiva->width, imgPositiva->height, IM_RGB, resultadoCoresR , resultadoCoresG , resultadoCoresB, saida_faixas );

	imSaveRGB(imgPositiva->width, imgPositiva->height, IM_RGB, resultadoCoresNR , resultadoCoresNG , resultadoCoresNB, saida_faixas_negativa );

	free(resultadoCoresR);
	free(resultadoCoresG);
	free(resultadoCoresB);

	free(resultadoCoresNR);
	free(resultadoCoresNG);
	free(resultadoCoresNB);
	
	return resultado;
}

int encontraCor( imImage* imagem , int pos )
{
	unsigned char r, g, b;

	r = ((unsigned char*)imagem->data[0])[pos];
	g = ((unsigned char*)imagem->data[1])[pos];
	b = ((unsigned char*)imagem->data[2])[pos];

	if ( r >= g && r >= b )
		return 0;
	if ( g >= r && g >= b )
		return 1;
	if ( b >= r && b >= g )
		return 2;

	return -1;
}


int **criaArquivoFaixasLinha(char* arquivo_saida, char* arquivo_saida2, imImage* img1 , imImage* img2 , unsigned char* imgTransicao )
{
	FILE *arq;
	int i , j , k , val;
	int coresq1, cordir1, coresq2, cordir2;
	int contPixel[MAX_FAIXAS32];
	int **pixelFaixas = NULL;
	unsigned char* resultadoCoresR;
	unsigned char* resultadoCoresG;
	unsigned char* resultadoCoresB;
	resultadoCoresR = (unsigned char *) malloc( img1->width * img1->height);
	resultadoCoresG = (unsigned char *) malloc( img1->width * img1->height);
	resultadoCoresB = (unsigned char *) malloc( img1->width * img1->height);

	pixelFaixas = (int **)malloc(MAX_FAIXAS32 * sizeof(int *));

	for ( i = 0 ; i < MAX_FAIXAS32 ; i++ )
	{
		pixelFaixas[i] = (int *)malloc (2000 * sizeof(int));
		for ( j = 0 ; j < 2000 ; j++ )
			pixelFaixas[i][j] = -1;
	}


	for ( i = 0 ; i < MAX_FAIXAS32 ; i++ )
		contPixel[i] = 0;
	
	arq = fopen( arquivo_saida , "w" );

	for ( i = 0 , k = 0; i < img1->height ; i++ )
	{
		fprintf(arq, "%d", i);

		for ( j = 0 ; j < img1->width ; j++ , k++ )
		{
			if ( imgTransicao[k] == 255 )
			{
				coresq1 = encontraCor( img1  , k-3 );
				cordir1 = encontraCor( img1  , k+3 );
				coresq2 = encontraCor( img2  , k-3 );
				cordir2 = encontraCor( img2  , k+3 );

				val = getBcslStripeCode ( coresq1, cordir1 , coresq2 , cordir2 );

				if ( val >= 0 && val <= MAX_FAIXAS32 )
				{
					fprintf(arq, "   %d,%d", j , val);
					
					pixelFaixas[val][contPixel[val]] = k;
					contPixel[val]++;

					resultadoCoresR[k] = 255;
					resultadoCoresG[k] = 255;
					resultadoCoresB[k] = val;
				}
			}
			else
			{
				resultadoCoresR[k] = 0;
				resultadoCoresG[k] = 0;
				resultadoCoresB[k] = 0;
				fprintf(arq, "   %d,%d", j , 0);
			}
			
		}
		fprintf(arq, "\n");
	}

	fclose(arq);

	imSaveRGB(img1->width, img1->height, IM_RGB, resultadoCoresR , resultadoCoresG , resultadoCoresB, arquivo_saida2 );

	free(resultadoCoresR);
	free(resultadoCoresG);
	free(resultadoCoresB);

	return pixelFaixas;
}

int* criarArquivoPixelFaixas(char *nome , int **pixelFaixas)
{
	FILE *arq;
	int i , j;
	int *qtdPixel;

	qtdPixel = (int*) malloc(sizeof(int) * MAX_FAIXAS32);

	arq = fopen( nome , "w" );

	for ( i = 0 ; i < MAX_FAIXAS32 ; i++ )
	{
		fprintf( arq , "%d\t", i );
		for ( j = 0 ; pixelFaixas[i][j] != -1 ; j++ )
		{
			fprintf( arq , "%d\t", pixelFaixas[i][j] );
		}
        fprintf( arq , "qtd(%d)\n", j );
		qtdPixel[i] = j;
	}

	fclose(arq);

	return qtdPixel;
}

void menuCalibracao()
{
	padrao_calibracao = 1;
}

void menu()
{
	modelo = 0;
	
	while ( modelo <= 0 || modelo >= 4 )
	{
		printf("\n\nEscolha o modelo:");
		printf("\n(1) cano amassado vista 1");
		printf("\n(2) cano amassado vista 2");
		printf("\n\n(3) sair\n\n-> ");
		scanf("%d", &modelo);

		if ( modelo == 3 )
			exit(0);
	}
}

int encontraTransicaoFaixas()
{
	char* nome_arquivo_direita_positiva1 = (char*) malloc(sizeof(char) * 500);
	char* nome_arquivo_direita_negativa1 = (char*) malloc(sizeof(char) * 500);
	char* nome_arquivo_direita_positiva2 = (char*) malloc(sizeof(char) * 500);
	char* nome_arquivo_direita_negativa2 = (char*) malloc(sizeof(char) * 500);
	
	char* nome_arquivo_esquerda_positiva1 = (char*) malloc(sizeof(char) * 500);
	char* nome_arquivo_esquerda_negativa1 = (char*) malloc(sizeof(char) * 500);
	char* nome_arquivo_esquerda_positiva2 = (char*) malloc(sizeof(char) * 500);
	char* nome_arquivo_esquerda_negativa2 = (char*) malloc(sizeof(char) * 500);

	char* nome_arquivo_saida_direita1 = (char*) malloc(sizeof(char) * 500);
	char* nome_arquivo_saida_direita2 = (char*) malloc(sizeof(char) * 500);
	char* nome_arquivo_saida_esquerda1 = (char*) malloc(sizeof(char) * 500);
	char* nome_arquivo_saida_esquerda2 = (char*) malloc(sizeof(char) * 500);

	char* nome_arquivo_saida_direita1_f = (char*) malloc(sizeof(char) * 500);
	char* nome_arquivo_saida_direita2_f = (char*) malloc(sizeof(char) * 500);
	char* nome_arquivo_saida_esquerda1_f = (char*) malloc(sizeof(char) * 500);
	char* nome_arquivo_saida_esquerda2_f = (char*) malloc(sizeof(char) * 500);

	char* nome_arquivo_saida_direita1_fn = (char*) malloc(sizeof(char) * 500);
	char* nome_arquivo_saida_direita2_fn = (char*) malloc(sizeof(char) * 500);
	char* nome_arquivo_saida_esquerda1_fn = (char*) malloc(sizeof(char) * 500);
	char* nome_arquivo_saida_esquerda2_fn = (char*) malloc(sizeof(char) * 500);

	unsigned char* imgTransicao;

	int erro;

	imImage* imgNegativa;

	switch(modelo)
	{
		// cano amassado 1
		case (1):
			nome_arquivo_direita_negativa1 = "imagens\\cano_amassado_vista_1\\3_2\\direita\\01.jpg";
			nome_arquivo_direita_negativa2 = "imagens\\cano_amassado_vista_1\\3_2\\direita\\02.jpg";
			nome_arquivo_direita_positiva1 = "imagens\\cano_amassado_vista_1\\3_2\\direita\\03.jpg";
			nome_arquivo_direita_positiva2 = "imagens\\cano_amassado_vista_1\\3_2\\direita\\04.jpg";

			nome_arquivo_esquerda_negativa1 = "imagens\\cano_amassado_vista_1\\3_2\\esquerda\\01.jpg";
			nome_arquivo_esquerda_negativa2 = "imagens\\cano_amassado_vista_1\\3_2\\esquerda\\02.jpg";
			nome_arquivo_esquerda_positiva1 = "imagens\\cano_amassado_vista_1\\3_2\\esquerda\\03.jpg";
			nome_arquivo_esquerda_positiva2 = "imagens\\cano_amassado_vista_1\\3_2\\esquerda\\04.jpg";

			nome_arquivo_saida_direita1_f	= "out\\direita_cano_amassado_vista1_1_faixas.bmp";
			nome_arquivo_saida_direita2_f	= "out\\direita_cano_amassado_vista1_2_faixas.bmp";
			nome_arquivo_saida_esquerda1_f	= "out\\esquerda_cano_amassado_vista1_1_faixas.bmp";
			nome_arquivo_saida_esquerda2_f	= "out\\esquerda_cano_amassado_vista1_2_faixas.bmp";

			nome_arquivo_saida_direita1_fn	= "out\\direita_cano_amassado_vista1_1_faixas_negativa.bmp";
			nome_arquivo_saida_direita2_fn	= "out\\direita_cano_amassado_vista1_2_faixas_negativa.bmp";
			nome_arquivo_saida_esquerda1_fn	= "out\\esquerda_cano_amassado_vista1_1_faixas_negativa.bmp";
			nome_arquivo_saida_esquerda2_fn	= "out\\esquerda_cano_amassado_vista1_2_faixas_negativa.bmp";

			nome_arquivo_saida_direita1		= "out\\transicao_direita_cano_amassado_vista1_1.jpg";
			nome_arquivo_saida_direita2		= "out\\transicao_direita_cano_amassado_vista1_2.jpg";
			nome_arquivo_saida_esquerda1	= "out\\transicao_esquerda_cano_amassado_vista1_1.jpg";
			nome_arquivo_saida_esquerda2	= "out\\transicao_esquerda_cano_amassado_vista1_2.jpg";
		break;
		// cano amassado 2
		case (2):
			nome_arquivo_direita_negativa1 = "imagens\\cano_amassado_vista_2\\3_2\\direita\\01.jpg";
			nome_arquivo_direita_negativa2 = "imagens\\cano_amassado_vista_2\\3_2\\direita\\02.jpg";
			nome_arquivo_direita_positiva1 = "imagens\\cano_amassado_vista_2\\3_2\\direita\\03.jpg";
			nome_arquivo_direita_positiva2 = "imagens\\cano_amassado_vista_2\\3_2\\direita\\04.jpg";

			nome_arquivo_esquerda_negativa1 = "imagens\\cano_amassado_vista_2\\3_2\\esquerda\\01.jpg";
			nome_arquivo_esquerda_negativa2 = "imagens\\cano_amassado_vista_2\\3_2\\esquerda\\02.jpg";
			nome_arquivo_esquerda_positiva1 = "imagens\\cano_amassado_vista_2\\3_2\\esquerda\\03.jpg";
			nome_arquivo_esquerda_positiva2 = "imagens\\cano_amassado_vista_2\\3_2\\esquerda\\04.jpg";

			nome_arquivo_saida_direita1_f	= "out\\direita_cano_amassado_vista2_1_faixas.bmp";
			nome_arquivo_saida_direita2_f	= "out\\direita_cano_amassado_vista2_2_faixas.bmp";
			nome_arquivo_saida_esquerda1_f	= "out\\esquerda_cano_amassado_vista2_1_faixas.bmp";
			nome_arquivo_saida_esquerda2_f	= "out\\esquerda_cano_amassado_vista2_2_faixas.bmp";

			nome_arquivo_saida_direita1_fn	= "out\\direita_cano_amassado_vista2_1_faixas_negativa.bmp";
			nome_arquivo_saida_direita2_fn	= "out\\direita_cano_amassado_vista2_2_faixas_negativa.bmp";
			nome_arquivo_saida_esquerda1_fn	= "out\\esquerda_cano_amassado_vista2_1_faixas_negativa.bmp";
			nome_arquivo_saida_esquerda2_fn	= "out\\esquerda_cano_amassado_vista2_2_faixas_negativa.bmp";

			nome_arquivo_saida_direita1		= "out\\transicao_direita_cano_amassado_vista2_1.jpg";
			nome_arquivo_saida_direita2		= "out\\transicao_direita_cano_amassado_vista2_2.jpg";
			nome_arquivo_saida_esquerda1	= "out\\transicao_esquerda_cano_amassado_vista2_1.jpg";
			nome_arquivo_saida_esquerda2	= "out\\transicao_esquerda_cano_amassado_vista2_2.jpg";
		break;
		// cano amassado 3
		case (3):
			nome_arquivo_direita_negativa1 = "imagens\\cano_amassado_vista_3\\3_2\\direita\\01.jpg";
			nome_arquivo_direita_negativa2 = "imagens\\cano_amassado_vista_3\\3_2\\direita\\02.jpg";
			nome_arquivo_direita_positiva1 = "imagens\\cano_amassado_vista_3\\3_2\\direita\\03.jpg";
			nome_arquivo_direita_positiva2 = "imagens\\cano_amassado_vista_3\\3_2\\direita\\04.jpg";

			nome_arquivo_esquerda_negativa1 = "imagens\\cano_amassado_vista_3\\3_2\\esquerda\\01.jpg";
			nome_arquivo_esquerda_negativa2 = "imagens\\cano_amassado_vista_3\\3_2\\esquerda\\02.jpg";
			nome_arquivo_esquerda_positiva1 = "imagens\\cano_amassado_vista_3\\3_2\\esquerda\\03.jpg";
			nome_arquivo_esquerda_positiva2 = "imagens\\cano_amassado_vista_3\\3_2\\esquerda\\04.jpg";

			nome_arquivo_saida_direita1_f	= "out\\direita_cano_amassado_vista3_1_faixas.bmp";
			nome_arquivo_saida_direita2_f	= "out\\direita_cano_amassado_vista3_2_faixas.bmp";
			nome_arquivo_saida_esquerda1_f	= "out\\esquerda_cano_amassado_vista3_1_faixas.bmp";
			nome_arquivo_saida_esquerda2_f	= "out\\esquerda_cano_amassado_vista3_2_faixas.bmp";

			nome_arquivo_saida_direita1_fn	= "out\\direita_cano_amassado_vista3_1_faixas_negativa.bmp";
			nome_arquivo_saida_direita2_fn	= "out\\direita_cano_amassado_vista3_2_faixas_negativa.bmp";
			nome_arquivo_saida_esquerda1_fn	= "out\\esquerda_cano_amassado_vista3_1_faixas_negativa.bmp";
			nome_arquivo_saida_esquerda2_fn	= "out\\esquerda_cano_amassado_vista3_2_faixas_negativa.bmp";

			nome_arquivo_saida_direita1		= "out\\transicao_direita_cano_amassado_vista3_1.jpg";
			nome_arquivo_saida_direita2		= "out\\transicao_direita_cano_amassado_vista3_2.jpg";
			nome_arquivo_saida_esquerda1	= "out\\transicao_esquerda_cano_amassado_vista3_1.jpg";
			nome_arquivo_saida_esquerda2	= "out\\transicao_esquerda_cano_amassado_vista3_2.jpg";
		break;
	}

		// imagem 1 direita
		imgPositiva1_direita = imFileImageLoad( nome_arquivo_direita_positiva1 , 0 , &erro );
		if ( erro == 1 )
			return 0;
		imgNegativa = imFileImageLoad( nome_arquivo_direita_negativa1 , 0 , &erro );
		if ( erro == 1 )
			return 0;
		// calcula a transicao
		imgTransicaoDireita = encontraTransicao ( imgPositiva1_direita , imgNegativa , nome_arquivo_saida_direita1_f , nome_arquivo_saida_direita1_fn );
		// salva uma imagem
		salvaImagem( nome_arquivo_saida_direita1 , imgPositiva1_direita->width , imgPositiva1_direita->height , imgTransicaoDireita );
		// libera a imagem negativa
		imImageDestroy(imgNegativa);

			// imagem 1 direita
			imgPositiva1_direita = imFileImageLoad( nome_arquivo_saida_direita1_f , 0 , &erro );
			if ( erro == 1 )
				return 0;
			imgNegativa = imFileImageLoad( nome_arquivo_saida_direita1_fn , 0 , &erro );
			if ( erro == 1 )
				return 0;
			// calcula a transicao
			imgTransicaoDireita = encontraTransicao ( imgPositiva1_direita , imgNegativa , nome_arquivo_saida_direita1_f , nome_arquivo_saida_direita1_fn );
			// salva uma imagem
			salvaImagem( nome_arquivo_saida_direita1 , imgPositiva1_direita->width , imgPositiva1_direita->height , imgTransicaoDireita );
			// libera a imagem negativa
			imImageDestroy(imgNegativa);


	// imagem 2 direita
	imgPositiva2_direita = imFileImageLoad( nome_arquivo_direita_positiva2 , 0 , &erro );
	if ( erro == 1 )
		return 0;
	imgNegativa = imFileImageLoad( nome_arquivo_direita_negativa2 , 0 , &erro );
	if ( erro == 1 )
		return 0;
	// calcula a transicao
	imgTransicao = encontraTransicao ( imgPositiva2_direita , imgNegativa , nome_arquivo_saida_direita2_f , nome_arquivo_saida_direita2_fn );
	// salva uma imagem
	salvaImagem( nome_arquivo_saida_direita2 , imgPositiva2_direita->width , imgPositiva2_direita->height , imgTransicao );
	// libera a imagem negativa
	imImageDestroy(imgNegativa);

			// imagem 2 direita
			imgPositiva2_direita = imFileImageLoad( nome_arquivo_saida_direita2_f , 0 , &erro );
			if ( erro == 1 )
				return 0;
			imgNegativa = imFileImageLoad( nome_arquivo_saida_direita2_fn , 0 , &erro );
			if ( erro == 1 )
				return 0;
			// calcula a transicao
			imgTransicao = encontraTransicao ( imgPositiva2_direita , imgNegativa , nome_arquivo_saida_direita2_f , nome_arquivo_saida_direita2_fn );
			// salva uma imagem
			salvaImagem( nome_arquivo_saida_direita2 , imgPositiva2_direita->width , imgPositiva2_direita->height , imgTransicao );
			// libera a imagem negativa
			imImageDestroy(imgNegativa);


	// imagem 1 esquerda
	imgPositiva1_esquerda = imFileImageLoad( nome_arquivo_esquerda_positiva1 , 0 , &erro );
	if ( erro == 1 )
		return 0;
	imgNegativa = imFileImageLoad( nome_arquivo_esquerda_negativa1 , 0 , &erro );
	if ( erro == 1 )
		return 0;
	// calcula a transicao
	imgTransicaoEsquerda = encontraTransicao ( imgPositiva1_esquerda , imgNegativa , nome_arquivo_saida_esquerda1_f , nome_arquivo_saida_esquerda1_fn);
	// salva uma imagem
	salvaImagem( nome_arquivo_saida_esquerda1 , imgPositiva1_esquerda->width , imgPositiva1_esquerda->height , imgTransicaoEsquerda );
	// libera a imagem negativa
	imImageDestroy(imgNegativa);

			// imagem 1 esquerda
			imgPositiva1_esquerda = imFileImageLoad( nome_arquivo_saida_esquerda1_f , 0 , &erro );
			if ( erro == 1 )
				return 0;
			imgNegativa = imFileImageLoad( nome_arquivo_saida_esquerda1_fn , 0 , &erro );
			if ( erro == 1 )
				return 0;
			// calcula a transicao
			imgTransicaoEsquerda = encontraTransicao ( imgPositiva1_esquerda , imgNegativa , nome_arquivo_saida_esquerda1_f , nome_arquivo_saida_esquerda1_fn);
			// salva uma imagem
			salvaImagem( nome_arquivo_saida_esquerda1 , imgPositiva1_esquerda->width , imgPositiva1_esquerda->height , imgTransicaoEsquerda );
			// libera a imagem negativa
			imImageDestroy(imgNegativa);

	// imagem 2 esquerda
	imgPositiva2_esquerda = imFileImageLoad( nome_arquivo_esquerda_positiva2 , 0 , &erro );
	if ( erro == 1 )
		return 0;
	imgNegativa = imFileImageLoad( nome_arquivo_esquerda_negativa2 , 0 , &erro );
	if ( erro == 1 )
		return 0;
	// calcula a transicao
	imgTransicao = encontraTransicao ( imgPositiva2_esquerda , imgNegativa , nome_arquivo_saida_esquerda2_f , nome_arquivo_saida_esquerda2_fn);
	// salva uma imagem
	salvaImagem( nome_arquivo_saida_esquerda2 , imgPositiva2_esquerda->width , imgPositiva2_esquerda->height , imgTransicao );
	// libera a imagem negativa
	imImageDestroy(imgNegativa);

			// imagem 2 esquerda
			imgPositiva2_esquerda = imFileImageLoad( nome_arquivo_saida_esquerda2_f , 0 , &erro );
			if ( erro == 1 )
				return 0;
			imgNegativa = imFileImageLoad( nome_arquivo_saida_esquerda2_fn , 0 , &erro );
			if ( erro == 1 )
				return 0;
			// calcula a transicao
			imgTransicao = encontraTransicao ( imgPositiva2_esquerda , imgNegativa , nome_arquivo_saida_esquerda2_f , nome_arquivo_saida_esquerda2_fn);
			// salva uma imagem
			salvaImagem( nome_arquivo_saida_esquerda2 , imgPositiva2_esquerda->width , imgPositiva2_esquerda->height , imgTransicao );
			// libera a imagem negativa
			imImageDestroy(imgNegativa);

	return 1;
}


void encontraLinhaEpipolarPonto( int x , int y , int faixa, imImage *imgDireita )
{
	float x1,y1,x2,y2;
	int x_proj, y_proj;
	vec3 esquerda, direita;
	float epiLine1[3];
	float epiLine2[3];
	int width = imgDireita->width;
	int height = imgDireita->height;

	// calcula as linhas epipolares
	epiLine1[0]=x-(float)width/2;
	epiLine1[1]=(float)height/2-y;
	epiLine1[2]=1;

	MulMatrix3x3Vec3( MF_esquerda , epiLine1 , epiLine2 );

	direita.x = epiLine2[0];
	direita.y = epiLine2[1];
	direita.z = epiLine2[2];

	epiLine1[0]=0;
	epiLine1[1]=-direita.z/direita.y;
	epiLine1[2]=1;

	MulMatrix3x3Vec3( MF_direita , epiLine1 , epiLine2 );

	esquerda.x=epiLine2[0];
	esquerda.y=epiLine2[1];
	esquerda.z=epiLine2[2];

	epi[ qtdEpi ].epiLine[0] = esquerda.x;
	epi[ qtdEpi ].epiLine[1] = esquerda.y;
	epi[ qtdEpi ].epiLine[2] = esquerda.z;

	epi[ qtdEpi ].epiLinePROJ[0] = direita.x;
	epi[ qtdEpi ].epiLinePROJ[1] = direita.y;
	epi[ qtdEpi ].epiLinePROJ[2] = direita.z;

	epi[ qtdEpi ].x = x;
	epi[ qtdEpi ].y = y;

	// "desenha" as linhas (esquerda)
	x1=-(float)width/2;
	y1=-(esquerda.x*x1+esquerda.z)/esquerda.y;
	x2=(float)width/2;
	y2=-(esquerda.x*x2+esquerda.z)/esquerda.y;

	epi[ qtdEpi ].x1 = 0;
	epi[ qtdEpi ].y1 = (int)(y1+height/2);
	epi[ qtdEpi ].x2 = width;
	epi[ qtdEpi ].y2 = (int)(y2+height/2);
	epi[ qtdEpi ].faixa = faixa;

	// nao precisa encontrar nada.. pq ja é o ponto
	//desenhaEpipolar( 0    , (int)(y1+height/2) , width , (int)(y2+height/2) , imagem_esquerda_in,  imagem_esquerda_out , faixa );
	//BresLine( epi[ qtdEpi ].x1 , epi[ qtdEpi ].y1 , epi[ qtdEpi ].x2 , epi[ qtdEpi ].y2 , epi[ qtdEpi ].faixa , Re , Ge , Be );

	// "desenha" as linhas da (direita)
	x1=-(float)width/2;
	y1=-(direita.x*x1+direita.z)/direita.y;
	x2=(float)width/2;
	y2=-(direita.x*x2+direita.z)/direita.y;

	epi[ qtdEpi ].x1_proj = 0;
	epi[ qtdEpi ].y1_proj = (int)(y1+height/2);
	epi[ qtdEpi ].x2_proj = width;
	epi[ qtdEpi ].y2_proj = (int)(y2+height/2);

	//BresLine( epi[ qtdEpi ].x1_proj , epi[ qtdEpi ].y1_proj , epi[ qtdEpi ].x2_proj , epi[ qtdEpi ].y2_proj , epi[ qtdEpi ].faixa , R , G , B );

	if ( encontraRelacaoEpipolar(0    , (int)(y1+height/2) , width , (int)(y2+height/2) , imgDireita , faixa , &x_proj, &y_proj ) )
	{
		epi[ qtdEpi ].x_proj = x_proj;
		//
		epi[ qtdEpi ].y_proj = (height - y_proj);
		qtdEpi++;
	}
}

void calculaLinhaEpipolar( int width, int height , char *img_esquerda, char *img_direita )
{
	unsigned char* R_esquerda;
	unsigned char* G_esquerda;
	unsigned char* B_esquerda;
	int i, j, erro;
	imImage *imgDireita, *imgEsquerda;

	imgEsquerda  = imFileImageLoad( img_esquerda , 0 , &erro );
	R_esquerda	= (imbyte*)imgEsquerda->data[0];
	G_esquerda	= (imbyte*)imgEsquerda->data[1];
	B_esquerda	= (imbyte*)imgEsquerda->data[2];   
	
	imgDireita	= imFileImageLoad( img_direita , 0 , &erro );
		
	for ( i = 0 ; i < height ; i++ )
	{
		for (  j = 0 ; j < width ; j++ )
		{
			// se houver transicao, calcular a epipolar
			if ( R_esquerda[(i*width)+j] == 255 && G_esquerda[(i*width)+j] == 255 && B_esquerda[(i*width)+j] != 205 )
			{
				encontraLinhaEpipolarPonto( j , (2048-i) , B_esquerda[(i*width)+j], imgDireita );
			}
		}
	}
}


void salvaPontos(void)
{
	int i;
	FILE *arq;

	
	arq = fopen("out\\pontos.txt","wt");

	for ( i = 0 ; i < qtdEpi ; i++ )
		fprintf( arq , "%d	%d	%d	%d	%d	%d	%f	%f	%f\n", (int)epi[i].x , (int)epi[i].y , 0 , (int)epi[i].x_proj , (int)epi[i].y_proj , 0, (float)epi[i].xe, (float)epi[i].ye, (float)epi[i].ze );

	fclose(arq);
}

void stereo(void)
{
	int i;
	Vector p1;
	Matrix Ra;
	Matrix Rb;
	Vector eye_esquerda;
	Vector eye_direita;

	eye_esquerda.x = cam_posx_esquerda;
	eye_esquerda.y = cam_posy_esquerda;
	eye_esquerda.z = cam_posz_esquerda;

	eye_direita.x = cam_posx_direita ;
	eye_direita.y = cam_posy_direita;
	eye_direita.z = cam_posz_direita;


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

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

	for ( i = 0 ; i < qtdEpi ; i++ )
	{
		p1 = stereoReconstruct(	epi[i].x	 , epi[i].y		, f_esquerda, Ra, eye_esquerda, 3072/2, 2048/2,
										epi[i].x_proj, epi[i].y_proj, f_direita , Rb, eye_direita , 3072/2, 2048/2);

		epi[i].xe = p1.x;
		epi[i].ye = p1.y;
		epi[i].ze = p1.z;
	}
}

int main(int argc, char* argv[]) 
{
	int retorno;
	char aux[500];

	menuCalibracao();
	printf("\nCalibrando a camera........");
	retorno = calibracao();
	printf("concluido");
	printf("\n\tPosicao da camera direita: (%f,%f,%f)", cam_posx_direita, cam_posy_direita, cam_posz_direita);
	printf("\n\tPosicao da camera direita: (%f,%f,%f)", cam_posx_esquerda, cam_posy_esquerda, cam_posz_esquerda);
	printf("\n\tResultados em 'calib_direita_XXXX.jpg' e 'calib_esquerda_XXXX.jpg'");
	printf("\n\tParamentros da camera gravado em: 'calib_cam_XXXXXX.txt'");
	
	if ( retorno == 1 )
	{
		menu();
		printf("\nLocalizando transicao entre faixas........");
		retorno = encontraTransicaoFaixas();
		if ( retorno == 1 )
		{
			printf("concluido");
			printf("\n\tResultado em 'transicao_XXXX.jpg'");
			printf("\nCriando arquivo de reconhecimento de faixas por linha........");
			openBcsl(3);
				pixelFaixasDireita = criaArquivoFaixasLinha(  "out\\faixas_linhas_direita.txt" , "out\\faixas_linhas_direita.bmp", imgPositiva1_direita , imgPositiva2_direita , imgTransicaoDireita );
				imImageDestroy(imgPositiva1_direita);
				imImageDestroy(imgPositiva2_direita);
				free(imgTransicaoDireita);
				pixelFaixasEsquerda = criaArquivoFaixasLinha( "out\\faixas_linhas_esquerda.txt" , "out\\faixas_linhas_esquerda.bmp", imgPositiva1_esquerda, imgPositiva2_esquerda, imgTransicaoEsquerda );
				imImageDestroy(imgPositiva1_esquerda);
				imImageDestroy(imgPositiva2_esquerda);
				free(imgTransicaoEsquerda);
			closeBcsl();
			printf("concluido");
			printf("\n\tResultado em 'faixas_linhas_direita.txt' e 'faixas_linhas_esquerda.txt'");

				free(pixelFaixasDireita);
				free(pixelFaixasEsquerda);

		calculaLinhaEpipolar( 3072, 2048 , "out\\faixas_linhas_esquerda.bmp" , "out\\faixas_linhas_direita.bmp" );
		stereo();
		salvaPontos();
		}
	}

	printf("\n\nFim da geracao dos arquivos de saida !!!!");
	gets(aux);
	gets(aux);

	return 0;
}