/*******************************************************************************
 * This library is free software; you can redistribute it and/or
 * modify it under the terms of the GNU Lesser General Public
 * License as published by the Free Software Foundation; either
 * version 2.1 of the License, or (at your option) any later version.
 * 
 * This library is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
 * GNU Lesser General Public License for more details.
 * 
 * You should have received a copy of the GNU Lesser General Public
 * License along with this library; if not, write to the Free Software
 * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307
 * USA
 * 
 * Contact Info:
 * 	Bruce Donald
 * 	Duke University
 * 	Department of Computer Science
 * 	Levine Science Research Center (LSRC)
 * 	Durham
 * 	NC 27708-0129 
 * 	USA
 * 	brd@cs.duke.edu
 * 
 * Copyright (C) 2011 Jeffrey W. Martin and Bruce R. Donald
 * 
 * <signature of Bruce Donald>, April 2011
 * Bruce Donald, Professor of Computer Science
 ******************************************************************************/


package edu.duke.donaldLab.jdshot.grid;

import edu.duke.donaldLab.share.geom.Vector3;
import edu.duke.donaldLab.share.math.CompareReal;
import edu.duke.donaldLab.share.math.Matrix3;

public class Axes
{
	/**************************
	 *   Static Methods
	 **************************/
	
	public static void getRotationVector( Vector3 out, double phi, double theta )
	{
		getRotationVector( out, phi, theta, new Matrix3() );
	}
	
	public static void getRotationVector( Vector3 out, double phi, double theta, Matrix3 temp )
	{
		getTransformationR(
			temp,
			Math.cos( phi ),
			Math.sin( phi ),
			Math.cos( theta ),
			Math.sin( theta )
		);
		Vector3.getUnitZ( out );
		temp.multiply( out );
	}
	
	public static void getFlipVector( Vector3 out, double phi, double theta, double rho )
	{
		getFlipVector( out, phi, theta, rho, new Matrix3() );
	}
	
	public static void getFlipVector( Vector3 out, double phi, double theta, double rho, Matrix3 temp )
	{
		getTransformationF(
			temp,
			Math.cos( phi ),
			Math.sin( phi ),
			Math.cos( theta ),
			Math.sin( theta ),
			Math.cos( rho ),
			Math.sin( rho )
		);
		Vector3.getUnitX( out );
		temp.multiply( out );
	}
	
	public static void getCVector( Vector3 out, double phi, double theta, double rho, double halfBeta )
	{
		getCVector( out, phi, theta, rho, halfBeta, new Matrix3(), new Matrix3() );
	}
	
	public static void getCVector( Vector3 out, double phi, double theta, double rho, double halfBeta, Matrix3 tempa, Matrix3 tempb )
	{
		/* Jeff: 5/27/2008 - NOTE:
			r = R_z(phi) R_y(theta) z
			f = R_z(phi) R_y(theta) R_z(rho) x
			c = R_r(beta/2) f
		*/
		
		double cosPhi = Math.cos( phi );
		double sinPhi = Math.sin( phi );
		double cosTheta = Math.cos( theta );
		double sinTheta = Math.sin( theta );
		double cosRho = Math.cos( rho );
		double sinRho = Math.sin( rho );
		double cosHalfBeta = Math.cos( halfBeta );
		double sinHalfBeta = Math.sin( halfBeta );
		
		getTransformationR( tempa, cosPhi, sinPhi, cosTheta, sinTheta );
		Vector3.getUnitZ( out );
		tempa.multiply( out );
		getTransformationC(
			tempb,
			out,
			cosHalfBeta,
			sinHalfBeta
		);
		
		getTransformationF( tempa, cosPhi, sinPhi, cosTheta, sinTheta, cosRho, sinRho );
		Vector3.getUnitX( out );
		tempa.multiply( out );
		tempb.multiply( out );
	}
	
	public static double[] getAnglesFromRotationVector( Vector3 r )
	{
		double theta = Math.acos( r.z );
		
		// sometimes the inside bit is almost one, but just a little over or under. That's ok
		double phi = Math.acos( hardClamp( r.x / Math.sqrt( 1 - r.z * r.z ), -1.0, 1.0 ) );
		
		if( r.y < 0.0 )
		{
			phi = 2.0 * Math.PI - phi;
		}
		
		// HACKHACK: if phi is undefined, just pretend it's zero instead
		if( isThetaBad( theta ) )
		{
			phi = 0.0;
		}
		
		return new double[] { phi, theta };
	}
	
	public static double[] getAngles( Vector3 r, Vector3 f )
	{
		// get theta and phi from the rotation vector
		double[] angles = getAnglesFromRotationVector( r );
		double phi = angles[0];
		double theta = angles[1];
		
		// now just get rho (geometrically!!)
		double rho = 0.0;
		{
			Vector3 bizarroX = new Vector3();
			Vector3 bizarroY = new Vector3();
			Matrix3 transformationF = new Matrix3();
			
			getFlipVector( bizarroX, phi, theta, 0.0 );
			Vector3.getUnitY( bizarroY );
			getTransformationF(
				transformationF,
				Math.cos( phi ),
				Math.sin( phi ),
				Math.cos( theta ),
				Math.sin( theta ),
				Math.cos( 0.0 ),
				Math.sin( 0.0 )
			);
			transformationF.multiply( bizarroY );
			
			rho = Math.acos( hardClamp( bizarroX.getDot( f ), -1.0, 1.0 ) );
			if( bizarroY.getDot( f ) < 0 )
			{
				rho = 2.0 * Math.PI - rho;
			}
			
			// normalize rho
			if( rho >= 2.0 * Math.PI )
			{
				rho -= 2.0 * Math.PI;
			}
		}
		
		return new double[] { phi, theta, rho };
	}
	
	
	/**************************
	 *   Static Functions
	 **************************/
	
	private static void getTransformationR( Matrix3 matrix, double cosPhi, double sinPhi, double cosTheta, double sinTheta )
	{
		matrix.set(
			cosPhi * cosTheta,	-sinPhi,	cosPhi * sinTheta,
			sinPhi * cosTheta,	cosPhi,		sinPhi * sinTheta,
			-sinTheta,			0.0,		cosTheta
		);
	}
	
	private static void getTransformationF( Matrix3 matrix, double cosPhi, double sinPhi, double cosTheta, double sinTheta, double cosRho, double sinRho )
	{
		matrix.set(
			cosPhi * cosTheta * cosRho - sinPhi * sinRho,	-cosPhi * cosTheta * sinRho - sinPhi * cosRho,	cosPhi * sinTheta,
			sinPhi * cosTheta * cosRho + cosPhi * sinRho,	-sinPhi * cosTheta * sinRho + cosPhi * cosRho,	sinPhi * sinTheta,
			-sinTheta * cosRho,								sinTheta * sinRho,								cosTheta
		);
	}
	
	private static void getTransformationC( Matrix3 matrix, Vector3 r, double cosHalfBeta, double sinHalfBeta )
	{
		Matrix3.getRotation( matrix, r, cosHalfBeta, sinHalfBeta );
	}
	
	private static double hardClamp( double val, double min, double max )
	{
		if( val < min )
		{
			val = min;
		}
		else if( val > max )
		{
			val = max;
		}
		return val;
	}
	
	private static boolean isThetaBad( double theta )
	{
		return CompareReal.eq( theta, 0.0 ) || CompareReal.eq( theta, Math.PI );
	}
}
