/*******************************************************************************
 * 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.share.math;

import edu.duke.donaldLab.share.geom.Vector3;

public class Quaternion
{
	/**************************
	 *   Definitions
	 **************************/
	
	public static final int Dimension = 4;
	
	
	/**************************
	 *   Fields
	 **************************/
	
	public double a;
	public double b;
	public double c;
	public double d;
	
	
	/**************************
	 *   Constructors
	 **************************/
	
	public Quaternion( )
	{
		set( 1.0, 0.0, 0.0, 0.0 );
	}
	
	public Quaternion( double a, double b, double c, double d )
	{
		set( a, b, c, d );
	}
	
	public Quaternion( Vector3 v )
	{
		set( v );
	}
	
	public Quaternion( Quaternion other )
	{
		set( other );
	}
	
	
	/**************************
	 *   Accessors
	 **************************/
	
	public double get( int i )
	{
		switch( i )
		{
			case 0: return a;
			case 1: return b;
			case 2: return c;
			case 3: return d;
		}
		
		assert( false ) : "Invalid index: " + i;
		
		// just to make the compiler happy
		return Double.NaN;
	}
	
	public void set( int i, double val )
	{
		switch( i )
		{
			case 0: a = val; return;
			case 1: b = val; return;
			case 2: c = val; return;
			case 3: d = val; return;
		}
		
		assert( false ) :  "Invalid index: " + i;
	}
	
	public void set( double a, double b, double c, double d )
	{
		this.a = a;
		this.b = b;
		this.c = c;
		this.d = d;
	}
	
	public void set( Quaternion other )
	{
		set( other.a, other.b, other.c, other.d );
	}
	
	public void set( Vector3 v )
	{
		set( 0.0, v.x, v.y, v.z );
	}
	 
	
	/**************************
	 *   Static Methods
	 **************************/
	
	public static void getRotation( Quaternion q, Vector3 axis, double angle )
	{
		double halfAngle = angle / 2.0;
		double multiplier = Math.sin( halfAngle ) / axis.getLength();
		q.set(
			Math.cos( halfAngle ),
			multiplier * axis.x,
			multiplier * axis.y,
			multiplier * axis.z
		);
		q.normalize();
	}
	
	public static void getRotationByPi( Quaternion q, Vector3 axis )
	{
		q.set( axis );
		q.normalize();
	}
	
	public static void getRotation( Quaternion q, Vector3 from, Vector3 to )
	{
		// calculate the rotation axis
		Vector3 fromUnit = new Vector3( from );
		fromUnit.normalize();
		Vector3 toUnit = new Vector3( to );
		toUnit.normalize();
		Vector3 axis = new Vector3();
		fromUnit.getCross( axis, toUnit );
		axis.normalize();
		
		// if the axis is full of NaNs, from must be too similar to to, so just return the identity
		if( !CompareReal.eq( axis.getSquaredLength(), 1.0 ) )
		{
			getIdentity( q );
			return;
		}
		
		// calculate the rotation angle
		double angle = Math.acos( Math.min( fromUnit.getDot( toUnit ), 1.0 ) );
		
		getRotation( q, axis, angle );
	}
	
	public static Quaternion getIdentity( )
	{
		return new Quaternion( 1.0, 0.0, 0.0, 0.0 );
	}
	
	public static void getIdentity( Quaternion q )
	{
		q.set( 1.0, 0.0, 0.0, 0.0 );
	}
	
	
	/**************************
	 *   Methods
	 **************************/
	
	public double toAxisAngle( Vector3 axis )
	{
		double multiplier = Math.sqrt( 1.0 - a*a );
		axis.set(
			b / multiplier,
			c / multiplier,
			d / multiplier
		);
		return Math.acos( a ) * 2.0;
	}
	
	public void negate( )
	{
		// this doesn't really make any sense to do in "the real world"
		// but it's useful for testing
		a = -a;
		b = -b;
		c = -c;
		d = -d;
	}
	
	public double getSquaredLength( )
	{
		return a*a + b*b + c*c + d*d;
	}
	
	public double getLength( )
	{
		return Math.sqrt( getSquaredLength() );
	}
	
	public void normalize( )
	{
		double length = getLength();
		a /= length;
		b /= length;
		c /= length;
		d /= length;
	}
	
	public void add( Quaternion other )
	{
		a += other.a;
		b += other.b;
		c += other.c;
		d += other.d;
	}
	
	public void subtract( Quaternion other )
	{
		a -= other.a;
		b -= other.b;
		c -= other.c;
		d -= other.d;
	}
	
	public void multiply( Quaternion other )
	{
		/*
			q1q2
			= (a1a2 − b1b2 − c1c2 − d1d2)
			+ (a1b2 + b1a2 + c1d2 − d1c2)i
			+ (a1c2 − b1d2 + c1a2 + d1b2)j
			+ (a1d2 + b1c2 − c1b2 + d1a2)k
			other = q1
			this = q2
		*/
		set(
			other.a * a - other.b * b - other.c * c - other.d * d,
			other.a * b + other.b * a + other.c * d - other.d * c,
			other.a * c - other.b * d + other.c * a + other.d * b,
			other.a * d + other.b * c - other.c * b + other.d * a
		);
	}
	
	public void conjugate( )
	{
		b = -b;
		c = -c;
		d = -d;
	}
	
	public double getDot( Quaternion other )
	{
		return a * other.a + b * other.b + c * other.c + d * other.d;
	}
	
	public String toString( )
	{
		return "( " + a + ", " + b + "i, " + c + "j, " + d + "k )";
	}
	
	public void rotate( Vector3 out )
	{
		// v' = qvq'
		
		// first, compute i = vq'
		double ia = out.x*b + out.y*c + out.z*d;
		double ib = out.x*a - out.y*d + out.z*c;
		double ic = out.x*d + out.y*a - out.z*b;
		double id = -out.x*c + out.y*b + out.z*a;
		
		// finally, compute o = qvq'
		out.set(
			a*ib + b*ia + c*id - d*ic,
			a*ic - b*id + c*ia + d*ib,
			a*id + b*ic - c*ib + d*ia
		);
	}
	
	public double getDistance( Quaternion other )
	{
		/* Jeff: 12/05/2008 - NOTE:
			The standard quaternion distance formula (length of the great arc) is
			d = ||log(p^*q)|| = acos(p.q) = angle of rotation about an axis for quaternion p^*q
			
			However, I only care about unit quaternions and this lets q and -q have a nonzero
			distance is annoying because they represent the same rotation.
			
			Therefore, I changed the distance formula to the following which fixes this problem.
			d = 2*acos(|p.q|)
		*/
		
		double aCopy = Math.abs( getDot( other ) );
		
		// HACKHACK: sometimes copy.a isn't strictly less than 1.0
		if( aCopy > 1.0 )
		{
			aCopy = 1.0;
		}
		
		return Math.acos( aCopy );
	}
}
