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

BBTK_ADD_BLACK_BOX_TO_PACKAGE(PackRecalage,PlaneReorientationBox)
BBTK_BLACK_BOX_IMPLEMENTATION(PlaneReorientationBox,bbtk::AtomicBlackBox);
void PlaneReorientationBox::Process()
{
	if(!bbGetInputInX().empty() && bbGetInputInX().size() == 3 && !bbGetInputInY().empty() && bbGetInputInY().size() == 3 && !bbGetInputInZ().empty() && bbGetInputInZ().size() == 3)
	{
		/*Unitary Vector in Y*/
		double y[3];
		y[0] = 0;
		y[1] = 1;
		y[2] = 0;

		/*Unitary Vector in X*/
		double x[3];
		x[0] = 1;
		x[1] = 0;
		x[2] = 0;

		double normal[3];

		/*Normal vector without its z factor*/
		double normalZ[3];
		normalZ[0] = normal[0];
		normalZ[1] = normal[1];
		normalZ[2] = 0;

		/*Normal vector without its y factor*/
		double normalY[3];
		normalY[0] = normal[0];
		normalY[1] = 0;
		normalY[2] = normal[2];

		/*Angle for the rotation in Z*/
		double angleZ = acos(vtkMath::Dot(normalZ,y)/vtkMath::Norm(normalZ));		

		/*Angle for the rotation in Y*/
		double angleY = acos(vtkMath::Dot(x,normalY)/vtkMath::Norm(normalY));

		/*Convert from Radians to Degrees*/
		angleZ = vtkMath::DegreesFromRadians(angleZ);
		angleY = vtkMath::DegreesFromRadians(angleY);
		std::cout << "Angle for Z: " << angleZ << std::endl;
		std::cout << "Angle for Y: " << angleY << std::endl;

		vtkTransform *transform = vtkTransform::New();
		transform->Identity();
		if(normal[0] < 0)
		{
			transform->RotateWXYZ(angleZ,0,0,1);
		}
		else
		{
			transform->RotateWXYZ(angleZ,0,0,-1);
		}
		if(normal[1]<0)
		{
			transform->RotateWXYZ(angleY,0,1,0);
		}
		else
		{
			if(angleY != 180){transform->RotateWXYZ(angleY,0,-1,0);}
		}
		//transform->Translate(bbGetInputCenterPoint()[0],bbGetInputCenterPoint()[1],bbGetInputCenterPoint()[2]);
		transform->Update();

		/*Set output and pray to god that it works :P*/
		bbSetOutputOut(transform);
	}
}
void PlaneReorientationBox::bbUserSetDefaultValues()
{
	std::vector<int> nullVector;
	bbSetInputInX(nullVector);
	bbSetInputInY(nullVector);
	bbSetInputInZ(nullVector);
}
void PlaneReorientationBox::bbUserInitializeProcessing()
{
 
//  THE INITIALIZATION METHOD BODY : 
//    Here does nothing  
//    but this is where you should allocate the internal/output pointers  
//    if any  
 
  
}
void PlaneReorientationBox::bbUserFinalizeProcessing()
{
 
//  THE FINALIZATION METHOD BODY : 
//    Here does nothing  
//    but this is where you should desallocate the internal/output pointers  
//    if any 
  
}
}
// EO namespace bbPackRecalage