#include "bbPackRecalageTransform3D3PointsBox.h"
#include "bbPackRecalagePackage.h"
namespace bbPackRecalage
{

BBTK_ADD_BLACK_BOX_TO_PACKAGE(PackRecalage,Transform3D3PointsBox)
BBTK_BLACK_BOX_IMPLEMENTATION(Transform3D3PointsBox,bbtk::AtomicBlackBox);
void Transform3D3PointsBox::Process()
{
	if(!bbGetInputInX1().empty() && bbGetInputInX1().size() >= 3 && !bbGetInputInX2().empty() && bbGetInputInX2().size() >= 3)
	{
		double A1[3];
		double B1[3];
		double O1[3];

		double A2[3];
		double B2[3];
		double O2[3];
		
		//In case there is an order in the points
		if(!bbGetInputLabels1().empty() && !bbGetInputLabels2().empty() && bbGetInputLabels1()[0].compare("1") == 0 && bbGetInputLabels1()[1].compare("2") == 0 && bbGetInputLabels1()[2].compare("3") == 0)
		{
			int i;
			for(i = 0; i < 3; i++)
			{
				if(bbGetInputLabels1()[i].compare("1") == 0)
				{	
					O1[0] = bbGetInputInX1()[i];
					O1[1] = bbGetInputInY1()[i];
					O1[2] = bbGetInputInZ1()[i];
				}
				else if(bbGetInputLabels1()[i].compare("2") == 0)
				{
					A1[0] = bbGetInputInX1()[i];
					A1[1] = bbGetInputInY1()[i];
					A1[2] = bbGetInputInZ1()[i];
				}
				else if(bbGetInputLabels1()[i].compare("3") == 0)
				{
					B1[0] = bbGetInputInX1()[i];
					B1[1] = bbGetInputInY1()[i];
					B1[2] = bbGetInputInZ1()[i];
				}
			}			
			for(i = 0; i < 3; i++)
			{
				if(bbGetInputLabels2()[i].compare("1") == 0)
				{	
					O2[0] = bbGetInputInX2()[i];
					O2[1] = bbGetInputInY2()[i];
					O2[2] = bbGetInputInZ2()[i];
				}
				else if(bbGetInputLabels2()[i].compare("2") == 0)
				{
					A2[0] = bbGetInputInX2()[i];
					A2[1] = bbGetInputInY2()[i];
					A2[2] = bbGetInputInZ2()[i];
				}
				else if(bbGetInputLabels2()[i].compare("3") == 0)
				{
					B2[0] = bbGetInputInX2()[i];
					B2[1] = bbGetInputInY2()[i];
					B2[2] = bbGetInputInZ2()[i];
				}
			}				
		}
		else
		{
			O1[0] = bbGetInputInX1()[0];
			O1[1] = bbGetInputInY1()[0];
			O1[2] = bbGetInputInZ1()[0];

			A1[0] = bbGetInputInX1()[1];
			A1[1] = bbGetInputInY1()[1];
			A1[2] = bbGetInputInZ1()[1];

			B1[0] = bbGetInputInX1()[2];
			B1[1] = bbGetInputInY1()[2];
			B1[2] = bbGetInputInZ1()[2];

			O2[0] = bbGetInputInX2()[0];
			O2[1] = bbGetInputInY2()[0];
			O2[2] = bbGetInputInZ2()[0];

			A2[0] = bbGetInputInX2()[1];
			A2[1] = bbGetInputInY2()[1];
			A2[2] = bbGetInputInZ2()[1];

			B2[0] = bbGetInputInX2()[2];
			B2[1] = bbGetInputInY2()[2];
			B2[2] = bbGetInputInZ2()[2];
		}

		//Create the new center point
		double * centerPoint;
		centerPoint[0] = O2[0];
		centerPoint[1] = O2[1];
		centerPoint[2] = O2[2];

		transformer->SetSecondTranslation(centerPoint);

		centerPoint[0] = O1[0];
		centerPoint[1] = O1[1];
		centerPoint[2] = O1[2];

		transformer->SetFirstTranslation(centerPoint);

		//Create the first vector
		double* vector1;
		vector1 = planes->makeVector(O1, A1);
		
		//create the second vector
		double* vector2;
		vector2 = planes->makeVector(O2, A2);

		//The rotation axis
		double* axis = planes->getCrossProduct(vector2, vector1);

		//Normalize the axis
		axis = planes->getNormal(axis);

		//Normalize the 2 vectors for calculating the dot product
		double* vector1N = planes->getNormal(vector1);
		double* vector2N = planes->getNormal(vector2);

		//Dot product
		double angle = planes->getDotProduct(vector2N, vector1N);

		//Convert the dot product to radians
		angle = acos(angle);

		//Convert from Radians to Degrees (necesary for the transformation)
		angle = vtkMath::DegreesFromRadians(angle);

		//Set the cross product and dot product of the first rotation
		transformer->SetRotationAxis(axis);
		transformer->SetAngle(angle);

		//Sets the second rotation axis (defined by the first vector) and the second angle as input
		transformer->SetSecondRotationAxis(vector1);

		///////////////////////////////
		//Second Rotation calculations start now (adjust the angle between the planes)
		/////////////////////////////////

		//Obtain the resultant transformation without moving the image first
		vtkMatrix4x4* matrix = transformer->GetFirstResult()->GetMatrix();

		double newMatrix[3][3];

		int i,j;
		for(i = 0; i < 3; i ++)
		{
			for(j = 0; j < 3; j ++)
			{
				newMatrix[i][j] = matrix->GetElement(i,j);
			}
		}

		//Calculates the new position of the points after the first transformation
		double A2N[3]; 
		A2N[0] = A2[0]*newMatrix[0][0] + A2[0]*newMatrix[0][1] + A2[0]*newMatrix[0][2];
		A2N[1] = A2[1]*newMatrix[1][0] + A2[0]*newMatrix[1][1] + A2[0]*newMatrix[1][2];
		A2N[2] = A2[2]*newMatrix[2][0] + A2[0]*newMatrix[2][1] + A2[0]*newMatrix[2][2];

		double B2N[3]; 
		B2N[0] = B2[0]*newMatrix[0][0] + B2[0]*newMatrix[0][1] + B2[0]*newMatrix[0][2];
		B2N[1] = B2[1]*newMatrix[1][0] + B2[0]*newMatrix[1][1] + B2[0]*newMatrix[1][2];
		B2N[2] = B2[2]*newMatrix[2][0] + B2[0]*newMatrix[2][1] + B2[0]*newMatrix[2][2];

		double O2N[3]; 
		O2N[0] = O2[0]*newMatrix[0][0] + O2[0]*newMatrix[0][1] + O2[0]*newMatrix[0][2];
		O2N[1] = O2[1]*newMatrix[1][0] + O2[0]*newMatrix[1][1] + O2[0]*newMatrix[1][2];
		O2N[2] = O2[2]*newMatrix[2][0] + O2[0]*newMatrix[2][1] + O2[0]*newMatrix[2][2];

		///////////////////////////////////////////////////////////////////////////////
		//Creation of the two planes for the second transformation
		///////////////////////////////////////////////////////////////////////////////

		double * vector1A;
		vector1A = planes->makeVector(O1, A1);

		double * vector1B;
		vector1B = planes->makeVector(O1, B1);

		double * vector2AN;
		vector2AN = planes->makeVector(O2N, A2N);

		double * vector2BN;
		vector2BN = planes->makeVector(O2N, B2N);

		//Normal Plane 1
		double* crossPlaneA = planes->getCrossProduct(vector1A, vector1B);

		//Normal Plane 2
		double* crossPlaneB = planes->getCrossProduct(vector2AN, vector2BN);

		//Normalize the vectors		
		crossPlaneA = planes->getNormal(crossPlaneA);
		crossPlaneB = planes->getNormal(crossPlaneB);

		//Angle between the planes
		double anglePlanes = planes->getDotProduct(crossPlaneB, crossPlaneA);

		//Make necessary changes to the angle
		anglePlanes = acos(anglePlanes);

		anglePlanes = vtkMath::DegreesFromRadians(anglePlanes);

		transformer->SetSecondAngle(-anglePlanes);

		// The calculation of the transformations are made
		transformer->Run();

		// We get the results of transformer and set it as result of this box
		bbSetOutputOut( transformer->GetResult() );
	}
	else
	{
		bbSetOutputOut( NULL );
	}
}
void Transform3D3PointsBox::bbUserSetDefaultValues()
{
	std::vector<int> empty;
	std::vector<std::string> emptyString;
	bbSetInputInX1(empty);
	bbSetInputInY1(empty);
	bbSetInputInZ1(empty);
	bbSetInputInX2(empty);
	bbSetInputInY2(empty);
	bbSetInputInZ2(empty);
	bbSetInputLabels1(emptyString);
	bbSetInputLabels2(emptyString);
	bbSetOutputOut(NULL);
}
void Transform3D3PointsBox::bbUserInitializeProcessing()
{
	//We initialize the transformer
	transformer=new Transformer3D(); 

	//We initialize the plane operator
	planes=new PlanesOperations(); 
}
void Transform3D3PointsBox::bbUserFinalizeProcessing()
{
	//We delete the transformer
	delete transformer;

	//We delete the plane operator
	delete planes;
}
}
// EO namespace bbPackRecalage