/*
# ---------------------------------------------------------------------
#
# Copyright (c) CREATIS (Centre de Recherche en Acquisition et Traitement de l'Image 
#                        pour la Santé)
# Authors : Eduardo Davila, Frederic Cervenansky, Claire Mouton
#
#  This software is governed by the CeCILL-B license under French law and 
#  abiding by the rules of distribution of free software. You can  use, 
#  modify and/ or redistribute the software under the terms of the CeCILL-B 
#  license as circulated by CEA, CNRS and INRIA at the following URL 
#  http://www.cecill.info/licences/Licence_CeCILL-B_V1-en.html 
#  or in the file LICENSE.txt.
#
#  As a counterpart to the access to the source code and  rights to copy,
#  modify and redistribute granted by the license, users are provided only
#  with a limited warranty  and the software's author,  the holder of the
#  economic rights,  and the successive licensors  have only  limited
#  liability. 
#
#  The fact that you are presently reading this means that you have had
#  knowledge of the CeCILL-B license and that you accept its terms.
# ------------------------------------------------------------------------   
*/


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

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

		double A2[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 && bbGetInputLabels2()[0].compare("1") == 0 && bbGetInputLabels2()[1].compare("2") == 0)
		{
			int i;
			for(i = 0; i < 2; 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];
				}				
			}			
			for(i = 0; i < 2; 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
		{
			O1[0] = bbGetInputInX1()[0];
			O1[1] = bbGetInputInY1()[0];
			O1[2] = bbGetInputInZ1()[0];

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

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

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

		//Create the new center point
		double * centerPoint;
		centerPoint = new double[3];

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

		transformer->SetFirstTranslation(centerPoint);

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

		transformer->SetSecondTranslation(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
		vector1 = planes->getNormal(vector1);
		vector2 = planes->getNormal(vector2);

		//Dot product
		double angle = planes->getDotProduct(vector2, vector1);

		//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);
		transformer->SetSecondAngle(bbGetInputSecondAngle());

		// 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 Transform3D2PointsBox::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);
	bbSetInputSecondAngle(0);
	bbSetOutputOut(NULL);
}
void Transform3D2PointsBox::bbUserInitializeProcessing()
{
	//We initialize the transformer
	transformer=new Transformer3D(); 

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

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