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


extern int WIDTH, HEIGHT;


void encontrarNuvemPontos(std::vector<Ponto> &pontos3D, MATRIZ_ID pontosCam1[36][36], noMat **pontosCam2, 
						  float df_cam1, float pos_cam1[3], float R_cam1[9], Ponto c_cam1, 
						  float df_cam2, float pos_cam2[3], float R_cam2[9], Ponto c_cam2)
{
	int i,j;
	Ponto px_cam1;
	

	Matrix r_matrix_cam1 = algMatriz_r(R_cam1);
    Matrix r_matrix_cam2 = algMatriz_r(R_cam2);

    Vector eye_cam1 = algVector(pos_cam1[0], pos_cam1[1], pos_cam1[2], 1.0);
    Vector eye_cam2 = algVector(pos_cam2[0], pos_cam2[1], pos_cam2[2], 1.0);

	for(i=0;i<HEIGHT;i++)
	{
		for(j=0;j<WIDTH;j++)
		{
			if ((pontosCam2[i][j].x != -1)&&(pontosCam2[i][j].y != -1))
			{
				if (pontosCam1[pontosCam2[i][j].x][pontosCam2[i][j].y].flag == 1)
				{
					px_cam1.x = pontosCam1[pontosCam2[i][j].x][pontosCam2[i][j].y].posX;
					px_cam1.y = pontosCam1[pontosCam2[i][j].x][pontosCam2[i][j].y].posY;
					pontosCam1[pontosCam2[i][j].x][pontosCam2[i][j].y].flag = 2;

					Vector p_aux = stereoReconstruct(px_cam1.x, px_cam1.y, df_cam1, r_matrix_cam1, eye_cam1, c_cam1.x, c_cam1.y, j, i, df_cam2, r_matrix_cam2, eye_cam2, c_cam2.x, c_cam2.y);
					if (!((p_aux.x == 1000000)&&(p_aux.y == 1000000)&&(p_aux.z == 1000000)))	// para não considerar pontos quando os raios estiverem muito separados
					{
						// converte da cam1 para o sistema de coordenadas do mundo
						p_aux = algAdd(eye_cam1, algTransf( algTransp( r_matrix_cam1 ), p_aux));

						Ponto p3D = {p_aux.x,p_aux.y,p_aux.z};
						pontos3D.push_back(p3D);
					}
				}
			}
		}
	}
}