/*
# ---------------------------------------------------------------------
#
# 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 "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)
	{
		double a[3];		
		
		/*First Vector*/
		a[0] = bbGetInputInX()[1]-bbGetInputInX()[0];
		a[1] = bbGetInputInY()[1]-bbGetInputInY()[0];
		a[2] = bbGetInputInZ()[1]-bbGetInputInZ()[0];

		double b[3];

		/*Second Vector*/
		b[0] = bbGetInputInX()[2]-bbGetInputInX()[0];
		b[1] = bbGetInputInY()[2]-bbGetInputInY()[0];
		b[2] = bbGetInputInZ()[2]-bbGetInputInZ()[0];

		double normal[3];

		normal[0]=(a[1]*b[2])-(a[2]*b[1]);
		normal[1]=(a[2]*b[0])-(a[0]*b[2]);
		normal[2]=(a[0]*b[1])-(a[1]*b[0]);

		vtkMath::Normalize(normal);

		std::cout << "Normal axis : " << "X: " << normal[0] << " Y: " << normal[1] << " Z: " << normal[2] << std::endl;
		
		/*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;		

		/*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;

		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);
	}
	else
	{
		transform->Identity();
		transform->Update();
		bbSetOutputOut(transform);
	}
}
void PlaneReorientationBox::bbUserSetDefaultValues()
{
	std::vector<int> nullVector;
	bbSetInputInX(nullVector);
	bbSetInputInY(nullVector);
	bbSetInputInZ(nullVector);
}
void PlaneReorientationBox::bbUserInitializeProcessing()
{
	transform = vtkTransform::New();
}
void PlaneReorientationBox::bbUserFinalizeProcessing()
{
	transform->Delete(); 
}
}
// EO namespace bbPackRecalage