/*******************************************************************************
 * 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.test.grid;

import edu.duke.donaldLab.jdshot.grid.Axes;
import edu.duke.donaldLab.jdshot.test.ExtendedTestCase;
import edu.duke.donaldLab.share.geom.Vector3;
import edu.duke.donaldLab.share.math.CompareReal;
import edu.duke.donaldLab.share.math.Matrix3;
import edu.duke.donaldLab.share.math.PointIterator;

public class TestAxes extends ExtendedTestCase
{
	/* UNDONE: implement this!!
	public void testGetCVector( )
	{
	}
	*/
	
	public void testGetRotationVector( )
	{
		Matrix3 rotY = new Matrix3();
		Matrix3 rotZ = new Matrix3();
		Vector3 r = new Vector3();
		Vector3 v = new Vector3();
		
		// r = R_z(phi) R_y(theta) z
		PointIterator iter = new PointIterator(
			2,
			new int[] { 10, 10 },
			new double[] { 0.0, 0.0 },
			new double[] { 2.0 * Math.PI, Math.PI }
		);
		while( iter.hasNext() )
		{
			double[] angles = iter.next();
			double phi = angles[0];
			double theta = angles[1];
			
			Matrix3.getRotation( rotY, Vector3.getUnitY(), theta );
			Matrix3.getRotation( rotZ, Vector3.getUnitZ(), phi );
			
			Vector3.getUnitZ( r );
			rotY.multiply( r );
			rotZ.multiply( r );
			
			Axes.getRotationVector( v, phi, theta );
			assertEquals( r, v );
		}
	}
	
	public void testGetFlipVector( )
	{
		Matrix3 rotZa = new Matrix3();
		Matrix3 rotY = new Matrix3();
		Matrix3 rotZb = new Matrix3();
		Vector3 f = new Vector3();
		Vector3 v = new Vector3();
		
		// f = R_z(phi) R_y(theta) R_z(rho) x
		PointIterator iter = new PointIterator(
			3,
			new int[] { 10, 10, 10 },
			new double[] { 0.0, 0.0, 0.0 },
			new double[] { 2.0 * Math.PI, Math.PI, 2.0 * Math.PI }
		);
		while( iter.hasNext() )
		{
			double[] angles = iter.next();
			double phi = angles[0];
			double theta = angles[1];
			double rho = angles[2];
			
			Matrix3.getRotation( rotZa, Vector3.getUnitZ(), rho );
			Matrix3.getRotation( rotY, Vector3.getUnitY(), theta );
			Matrix3.getRotation( rotZb, Vector3.getUnitZ(), phi );
			
			Vector3.getUnitX( f );
			rotZa.multiply( f );
			rotY.multiply( f );
			rotZb.multiply( f );
			
			Axes.getFlipVector( v, phi, theta, rho );
			assertEquals( f, v );
		}
	}

	public void testPerpendicularity( )
	{
		checkPerpendicularity( new Vector3() );
		checkPerpendicularity( new Vector3( Math.PI / 6.0, 0.0, 0.0 ) );
		checkPerpendicularity( new Vector3( 0.0, Math.PI / 6.0, 0.0 ) );
		checkPerpendicularity( new Vector3( 0.0, 0.0, Math.PI / 6.0 ) );
		checkPerpendicularity( new Vector3( Math.PI / 6.0, Math.PI / 6.0, 0.0 ) );
		checkPerpendicularity( new Vector3( Math.PI / 6.0, Math.PI / 6.0, Math.PI / 6.0 ) );
	}
	
	private void checkPerpendicularity( Vector3 angles )
	{
		double halfBeta = 2.0 * Math.PI / 4.0;
		Vector3 r = new Vector3();
		Vector3 f = new Vector3();
		Vector3 c = new Vector3();
		Axes.getRotationVector( r, angles.x, angles.y );
		Axes.getFlipVector( f, angles.x, angles.y, angles.z );
		Axes.getCVector( c, angles.x, angles.y, angles.z, halfBeta );
		
		assertEquals( 0.0, r.getDot( f ), CompareReal.getEpsilon() );
		assertEquals( 0.0, r.getDot( c ), CompareReal.getEpsilon() ); 
	}
	
	public void testGetAnglesFromRotationVector( )
	{
		Vector3 r = new Vector3();
		
		PointIterator iter = new PointIterator(
			2,
			new int[] { 20, 10 },
			new double[] { 0.0, 0.0 },
			new double[] { 2.0 * Math.PI, Math.PI }
		);
		while( iter.hasNext() )
		{
			double[] angles = iter.next();
			double phi = angles[0];
			double theta = angles[1];
			
			Axes.getRotationVector( r, phi, theta );
			
			double[] anglesAgain = Axes.getAnglesFromRotationVector( r );
			double phiAgain = anglesAgain[0];
			double thetaAgain = anglesAgain[1];
			
			// TEMP
			//System.out.println( "angles:\tphi=" + phi + "\ttheta=" + theta );
			//System.out.println( "\tphi=" + phiAgain + "\ttheta=" + thetaAgain );
			//System.out.println( "\tr = " + r );
			
			// NOTE: if theta is zero, phi is really undefined
			if( theta != 0.0 && theta != Math.PI )
			{
				assertEquals( phi, phiAgain, CompareReal.getEpsilon() );
			}
			assertEquals( theta, thetaAgain, CompareReal.getEpsilon() );
		}
	}
	
	public void testGetAnglesByAngle( )
	{
		Vector3 r = new Vector3();
		Vector3 f = new Vector3();
		Vector3 rAgain = new Vector3();
		Vector3 fAgain = new Vector3();
		
		PointIterator iter = new PointIterator(
			3,
			new int[] { 20, 10, 20 },
			new double[] { 0.0, 0.0, 0.0 },
			new double[] { 2.0 * Math.PI, Math.PI, 2.0 * Math.PI }
		);
		int counter = 0;
		while( iter.hasNext() )
		{
			double[] angles = iter.next();
			double phi = angles[0];
			double theta = angles[1];
			double rho = angles[2];
			
			Axes.getRotationVector( r, phi, theta );
			Axes.getFlipVector( f, phi, theta, rho );
			
			double[] anglesAgain = Axes.getAngles( r, f );
			double phiAgain = anglesAgain[0];
			double thetaAgain = anglesAgain[1];
			double rhoAgain = anglesAgain[2];
			
			// TEMP
			//System.out.println( counter + ":\tphi=" + phi + "\ttheta=" + theta + "\trho=" + rho );
			//System.out.println( "\tphi=" + phiAgain + "\ttheta=" + thetaAgain + "\trho=" + rhoAgain );
			//System.out.println( "\tr = " + r );
			//System.out.println( "\tf = " + f );
			
			// NOTE: if theta is zero, phi and rho are really undefined
			if( theta != 0.0 && theta != Math.PI )
			{
				assertEqualsAngle( phi, phiAgain );
				assertEqualsAngle( rho, rhoAgain );
			}
			assertEqualsAngle( theta, thetaAgain );
			
			Axes.getRotationVector( rAgain, phiAgain, thetaAgain );
			Axes.getFlipVector( fAgain, phiAgain, thetaAgain, rhoAgain );
			
			assertEquals( r, rAgain );
			assertEquals( f, fAgain );
			
			counter++;
		}
	}
	
	/* Jeff: 6/09/2008 - NOTE: this test will always fail. The Euler angles parameterization is singular near the poles.
	public void testGetAnglesExample( )
	{
		Vector r = new Vector( 0.008852507964820247, 0.004291695599757736, 0.9996697853261697 );
		Vector f = new Vector( 0.9999429376258302, -0.006014012301501438, -0.008829108010589103 );
		
		double[] anglesAgain = Axes.getAngles( r, f );
		double phiAgain = anglesAgain[0];
		double thetaAgain = anglesAgain[1];
		double rhoAgain = anglesAgain[2];
		
		Vector rAgain = Axes.getRotationVector( phiAgain, thetaAgain );
		Vector fAgain = Axes.getFlipVector( phiAgain, thetaAgain, rhoAgain );
		
		assertEquals( r, rAgain );
		assertEquals( f, fAgain );
	}
	*/
}
