#ifndef __View_h
#define __View_h
#include "View.h"
#endif

void View::compute3DLines()
{
	int size = pictures[0]->lines.size();
	for (int i = 0;i < size ;i++ )
	{
		double l_1[6];
		double l_2[6];
		double l_3[6];
		
		//Line *ln1 = pictures[0]->lines[i];
		//Line *ln2 = pictures[1]->lines[i];

		Line *ln1 = pictures[1]->lines[i];
		Line *ln2 = pictures[2]->lines[i];

		l_1[0] = ( ln1->plane[1] * ln2->plane[2] ) - ( ln1->plane[2]  * ln2->plane[1] );
		l_1[1] = ( ln1->plane[2] * ln2->plane[0] ) - ( ln1->plane[0]  * ln2->plane[2] );
		l_1[2] = ( ln1->plane[0] * ln2->plane[1] ) - ( ln1->plane[1]  * ln2->plane[0] );
		l_1[3] = ( ln1->plane[0] * ln2->plane[3] ) - ( ln1->plane[3]  * ln2->plane[0] );
		l_1[4] = ( ln1->plane[1] * ln2->plane[3] ) - ( ln1->plane[3]  * ln2->plane[1] );
		l_1[5] = ( ln1->plane[2] * ln2->plane[3] ) - ( ln1->plane[3]  * ln2->plane[2] );
		

		ThreeDLines *l3_1 = new ThreeDLines(l_1[0],l_1[1],l_1[2],l_1[3],l_1[4],l_1[5]);
		threeDLines1.push_back(l3_1);
		cout << "uncorrected line cord"	 << "\n";
		cout << l_1[0] << "\n";
		cout << l_1[1] << "\n";
		cout << l_1[2] << "\n";
		cout << l_1[3] << "\n";
		cout << l_1[4] << "\n";
		cout << l_1[5] << endl;

	}
	
//	error();
}

void View::error()
{
	// we need to compute La*La transpose + Lb* Lb Transpose + Lab * Lab Transpose
	// Declaring result 6* 6 matrices for the above

	double **LaLat, **LbLbt, **LabLabt;

	// allocating space
	LaLat = (double **)malloc(6 * sizeof(double *));
		 for(int i = 0; i < 6; i++)
			LaLat[i] = (double *)malloc(6 * sizeof(double));

 	LbLbt = (double **)malloc(6 * sizeof(double *));
		 for(int i = 0; i < 6; i++)
			LbLbt[i] = (double *)malloc(6 * sizeof(double));

	LabLabt = (double **)malloc(6 * sizeof(double *));
		 for(int i = 0; i < 6; i++)
			LabLabt[i] = (double *)malloc(6 * sizeof(double));



	// following code computes Mi for all the lines
	for(int i = 0; i < 3; i++)
		for(int j = 0; j < pictures[i]->lines.size(); j++)
		{

		   for(int a=0; a < 6; a++)
			   for(int b=0; b<6;b++)
				{
				   LaLat[a][b] = 0.0;
				   LbLbt[a][a] = 0.0;
				   LabLabt[a][b] = 0.0;
				}
		   MatrixMaths::multiply(pictures[i]->lines[j]->lA,LaLat);
		   MatrixMaths::multiply(pictures[i]->lines[j]->lB,LbLbt);
           MatrixMaths::multiply(pictures[i]->lines[j]->pluckerCord,LabLabt);

		   MatrixMaths::add(LaLat,pictures[i]->lines[j]->errorMatrix);
		   MatrixMaths::add(LbLbt,pictures[i]->lines[j]->errorMatrix);
		   MatrixMaths::add(LabLabt,pictures[i]->lines[j]->errorMatrix);		   
			
		}

	


	// following code computes Mg for all the lines
 	for(int j = 0; j < threeDLines1.size(); j++)
	{
	   for(int i = 0; i < 3; i++)
		{
			MatrixMaths::add(pictures[i]->lines[j]->errorMatrix,threeDLines1[j]->errorMatrix);
	    }		
	}

	

	// following code computes the eigen vector and the eigen values for Mg
	for(int j = 0; j < threeDLines1.size(); j++)
	{
		double **EigVec, *EigVal;
		int minEigen = 0;
		GN_Matrix *pEng;
		pEng = new GN_Matrix;

        // allocating space for EigenVec and EigenVal
		EigVec = (double **)malloc(6 * sizeof(double *));
		 for(int i = 0; i < 6; i++)
			EigVec[i] = (double *)malloc(6 * sizeof(double));

	    EigVal = new double[6];

		pEng->Eigen(threeDLines1[j]->errorMatrix, EigVec, EigVal, 6);
		
		// find the unit eigen vector corresponding with the smallest eigen value
		minEigen = MatrixMaths::min(EigVal);
		for (int k = 0; k<6 ; k++)
		{
			threeDLines1[j]->unitEigenVector[k] = EigVec[k][minEigen];
		}

	}
	
	// following code finds the unit eigen vector
	for(int j = 0; j < threeDLines1.size(); j++)
	{
		double div = 0.0;
		for (int i = 0;i <6 ;i++ )
		{
			div = div + pow(threeDLines1[j]->unitEigenVector[i],2);
		}
		div = sqrt(div);

		for (int i = 0;i <6 ;i++ )
		{
			threeDLines1[j]->unitEigenVector[i] = (threeDLines1[j]->unitEigenVector[i]) / div;
		}
        
	}

	// following code reprojects the vector back to plucker
	for(int j = 0; j < threeDLines1.size(); j++)
	{
			double div = 0.0;
			double Ux = threeDLines1[j]->unitEigenVector[3];
			double Uy = threeDLines1[j]->unitEigenVector[4];
			double Uz = threeDLines1[j]->unitEigenVector[5];
			double Mx = threeDLines1[j]->unitEigenVector[0];
			double My = threeDLines1[j]->unitEigenVector[1];
			double Mz = threeDLines1[j]->unitEigenVector[2];

			div = sqrt(pow(Ux,2) + pow(Uy,2) + pow(Uz,2) );

			//this is U, Q=(U,M)
			threeDLines1[j]->correctedLine[0] = Ux / div; //U'x
			threeDLines1[j]->correctedLine[1] = Uy / div; //U'y
			threeDLines1[j]->correctedLine[2] = Uz / div; //U'z 

			//this is M, Q=(U,M)
			threeDLines1[j]->correctedLine[3] = ( (Uz*Mx - Ux*Mz) * threeDLines1[j]->correctedLine[2])
						- ((Ux*My - Uy*Mx) * threeDLines1[j]->correctedLine[1] );

			threeDLines1[j]->correctedLine[4] = ( (Ux*My - Uy*Mx) * threeDLines1[j]->correctedLine[0])
						- ((Uy*Mz - Uz*My) * threeDLines1[j]->correctedLine[2] );

			
			threeDLines1[j]->correctedLine[5] = ( (Uy*Mz - Uz*My) * threeDLines1[j]->correctedLine[1])
						- ((Uz*Mx - Ux*Mz) * threeDLines1[j]->correctedLine[0] );	

    }


}


void View::compute3DPoints()
{
	int picNo = 0;
	for(int i = 0; i < threeDLines1.size(); i++)
	{
		 for(int j = 0; j < pictures[picNo]->lines.size(); j++)
		 {
			 if(j != i)
			 {
				 if(pictures[0]->lines[j]->source->isEquals(pictures[0]->lines[i]->source) || pictures[0]->lines[j]->destination->isEquals(pictures[0]->lines[i]->source) )
				 {
					    //cout << "Line " << i << "Intersecting with line " << j << "for source " << endl;
						threeDLines1[i]->computeSource(pictures[picNo]->lines[j],threeDLines1[j]);
				 }
				 if(pictures[0]->lines[j]->source->isEquals(pictures[0]->lines[i]->destination) || pictures[0]->lines[j]->destination->isEquals(pictures[0]->lines[i]->destination) )
				 {
					    //cout << "Line " << i << "Intersecting with line " << j << "for dest " << endl;
 						threeDLines1[i]->computeDestination(pictures[picNo]->lines[j],threeDLines1[j]);
				 }
			 }
		 }
	}

/*threeDLines1[0]->computeSource(pictures[2]->lines[3],threeDLines1[3]);
threeDLines1[1]->computeSource(pictures[2]->lines[0],threeDLines1[0]);
threeDLines1[2]->computeSource(pictures[2]->lines[1],threeDLines1[1]);
threeDLines1[3]->computeSource(pictures[2]->lines[2],threeDLines1[2]);

threeDLines1[0]->computeDestination(pictures[2]->lines[1],threeDLines1[1]);
threeDLines1[1]->computeDestination(pictures[2]->lines[2],threeDLines1[2]);
threeDLines1[2]->computeDestination(pictures[2]->lines[3],threeDLines1[3]);
threeDLines1[3]->computeDestination(pictures[2]->lines[0],threeDLines1[0]);*/


for(int i = 0 ; i < threeDLines1.size(); i++)
	{
	cout << "source " << threeDLines1[i]->source2[0] << " " << threeDLines1[i]->source2[1] << " " 
		<< threeDLines1[i]->source2[2] << endl;
		
	cout << "destination " << threeDLines1[i]->destination2[0] << " " << threeDLines1[i]->destination2[1] << " " 
		<< threeDLines1[i]->destination2[2] << endl;
	}

// following code finds the average of points to get four points 
    picNo = 0;
	for(int i = 0; i < threeDLines1.size(); i++)
	{
		 for(int j = 0; j < pictures[picNo]->lines.size(); j++)
		 {
			 if(j != i)
			 {
				 if (pictures[0]->lines[j]->destination->isEquals(pictures[0]->lines[i]->source))
				 {
						double* averagePoint = new double[3];
						MatrixMaths::average(threeDLines1[i]->source2,threeDLines1[j]->destination2,averagePoint);
						if(MatrixMaths::contains(Points,averagePoint) == false)
								Points.push_back(averagePoint);
	
				 }
		 	 }
		 }
	}



}

void View::computePlanes(vector<double*> P)
{
   vector<double*> Planes;
   
   int k = P.size();
   for(int i=0; i < k; i++)
   {
	   int l=0,m=0,n=0;
	   double *line = new double[6];
	   double *cross = new double[3];
	   double *plane = new double[4];
	   
	   l = i % k;
	   m = (i + 1) % k;
	   n = (i + 2) % k;
	   
	   line[0] = P[l][0] - P[m][0];
	   line[1] = P[l][1] - P[m][1];
	   line[2] = P[l][2] - P[m][2];
	   line[3] = ( P[l][1] * P[m][2] ) - ( P[l][2] * P[m][1]);
	   line[4] = ( P[l][2] * P[m][0] ) - ( P[l][0] * P[m][2]);
	   line[5] = ( P[l][0] * P[m][1] ) - ( P[l][1] * P[m][0]);
	   
	   // Cross product of U and P
	   cross[0] = ( line[1] * P[n][2]) - (line[2] * P[n][1]);	
	   cross[1] = ( line[2] * P[n][0]) - (line[0] * P[n][2]);
	   cross[2] = ( line[0] * P[n][1]) - (line[1] * P[n][0]);

	   // (U*P -Vw)
	   plane[0] = cross[0] - line[3];
	   plane[1] = cross[1] - line[4];
	   plane[2] = cross[2] - line[5];

	   //V.P
	   plane[3] = (line[3]*P[n][0]) + (line[4]*P[n][1]) + (line[5]*P[n][2]);

       Planes.push_back(plane);
	   delete cross; delete line;

   } 

	double* finalPlane = new double[4];
	for (int i = 0; i < 4 ; i++ )
		finalPlane[i] = 0.0;

	for(int i = 0; i < Planes.size(); i++)
	{
		finalPlane[0] = finalPlane[0] + Planes[i][0];
		finalPlane[1] = finalPlane[1] + Planes[i][1];
		finalPlane[2] = finalPlane[2] + Planes[i][2];
		finalPlane[3] = finalPlane[3] + Planes[i][3];
	}

	finalPlane[0] = finalPlane[0] / Planes.size();
	finalPlane[1] = finalPlane[1] / Planes.size();
	finalPlane[2] = finalPlane[2] / Planes.size();
	finalPlane[3] = finalPlane[3] / Planes.size();

    finalPlanes.push_back(finalPlane);
	//delete Planes;
}



void View::computePoints(int lineStart, int lineEnd, int planeNo)
{
  for(int i = lineStart ; i <= lineEnd; i++)
  {
	 double Ux = pictures[0]->lines[i]->lB[0];
  	 double Uy = pictures[0]->lines[i]->lB[1];
 	 double Uz = pictures[0]->lines[i]->lB[2];
 	 double Vx = pictures[0]->lines[i]->lB[3];
 	 double Vy = pictures[0]->lines[i]->lB[4];
 	 double Vz = pictures[0]->lines[i]->lB[5];
     double Nx = finalPlanes[planeNo][0];
     double Ny = finalPlanes[planeNo][1];
     double Nz = finalPlanes[planeNo][2];
     double n = finalPlanes[planeNo][3];

	double* finalPoint = new double[3];
	//(VxN-Un:U.N) is the point where L(U:V) intersects plane [N:n] not parallel to L.
	double temp = (Ux * Nx) + (Uy * Ny) + (Uz * Nz);
	finalPoint[0] = ( (Vy * Nz) - (Vz * Ny) - (Ux * n) ) / temp;
	finalPoint[1] = ( (Vz * Nx) - (Vx * Nz) - (Uy * n) ) / temp;
	finalPoint[2] = ( (Vx * Ny) - (Vx * Ny) - (Uz * n) ) / temp;

    finalPoints.push_back(finalPoint);
	cout << "final points with plane no " << planeNo << " " << finalPoint[0] << " " << finalPoint[1] << " " 
	    << finalPoint[2] << endl;
  }

}