/*
Copyright (c) 2008, Luke Benstead.
All rights reserved.

Redistribution and use in source and binary forms, with or without modification,
are permitted provided that the following conditions are met:

    * Redistributions of source code must retain the above copyright notice,
      this list of conditions and the following disclaimer.
    * Redistributions in binary form must reproduce the above copyright notice,
      this list of conditions and the following disclaimer in the documentation
      and/or other materials provided with the distribution.

THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR
ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON
ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/

package org.cocos2d.kazmath;

import static org.cocos2d.kazmath.Utility.kmEpsilon;
import static org.cocos2d.kazmath.Utility.kmDegreesToRadians;
import static org.cocos2d.kazmath.Utility.kmPI;
import static org.cocos2d.kazmath.Utility.kmSQR;

import java.nio.FloatBuffer;

public class kmQuaternion {

	public float x;
	public float y;
	public float z;
	public float w;

	///< Returns pOut, sets pOut to the conjugate of pIn
	public static kmQuaternion kmQuaternionConjugate(kmQuaternion pOut, final kmQuaternion pIn) {
		pOut.x = -pIn.x;
		pOut.y = -pIn.y;
		pOut.z = -pIn.z;
		pOut.w = pIn.w;

		return pOut;
	}

	///< Returns the dot product of the 2 quaternions
	public static float kmQuaternionDot(final kmQuaternion q1, final kmQuaternion q2) {
		// A dot B = B dot A = AtBt + AxBx + AyBy + AzBz

		return (q1.w * q2.w +
				q1.x * q2.x +
				q1.y * q2.y +
				q1.z * q2.z);
	}

	///< Returns the exponential of the quaternion
	public static kmQuaternion kmQuaternionExp(kmQuaternion pOut, final kmQuaternion pIn) {
		assert false : "";

		return pOut;
	}

	///< Makes the passed quaternion an identity quaternion
	public static kmQuaternion kmQuaternionIdentity(kmQuaternion pOut) {
		pOut.x = 0.0f;
		pOut.y = 0.0f;
		pOut.z = 0.0f;
		pOut.w = 1.0f;

		return pOut;
	}

	///< Returns the inverse of the passed Quaternion
	public static kmQuaternion kmQuaternionInverse(kmQuaternion pOut,
			final kmQuaternion pIn) {
		float l = kmQuaternionLength(pIn);
		kmQuaternion tmp = new kmQuaternion();

		if (Math.abs(l) > kmEpsilon) {
			pOut.x = 0.0f;
			pOut.y = 0.0f;
			pOut.z = 0.0f;
			pOut.w = 0.0f;

			return pOut;
		}

		///Get the conjugute and divide by the length
		kmQuaternionScale(pOut,
				kmQuaternionConjugate(tmp, pIn), 1.0f / l);

		return pOut;
	}


	///< Returns true if the quaternion is an identity quaternion
	public static boolean kmQuaternionIsIdentity(final kmQuaternion pIn) {
		return (pIn.x == 0.0f && pIn.y == 0.0f && pIn.z == 0.0f && pIn.w == 1.0f);
	}

	///< Returns the length of the quaternion
	public static float kmQuaternionLength(final kmQuaternion pIn) {
		return (float) Math.sqrt(kmQuaternionLengthSq(pIn));
	}

	///< Returns the length of the quaternion squared (prevents a sqrt)
	public static float kmQuaternionLengthSq(final kmQuaternion pIn) {
		return	pIn.x * pIn.x + pIn.y * pIn.y +
				pIn.z * pIn.z + pIn.w * pIn.w;
	}

	///< Returns the natural logarithm
	public static kmQuaternion kmQuaternionLn(kmQuaternion pOut, final kmQuaternion pIn) {
		/*
			A unit quaternion, is defined by:
			Q == (cos(theta), sin(theta) * v) where |v| = 1
			The natural logarithm of Q is, ln(Q) = (0, theta * v)
		*/

		assert false : "";

		return pOut;
	}

	///< Multiplies 2 quaternions together
	public static kmQuaternion kmQuaternionMultiply(kmQuaternion pOut, final kmQuaternion q1, final kmQuaternion q2) {
		pOut.w = q1.w * q2.w - q1.x * q2.x - q1.y * q2.y - q1.z * q2.z;
		pOut.x = q1.w * q2.x + q1.x * q2.w + q1.y * q2.z - q1.z * q2.y;
		pOut.y = q1.w * q2.y + q1.y * q2.w + q1.z * q2.x - q1.x * q2.z;
		pOut.z = q1.w * q2.z + q1.z * q2.w + q1.x * q2.y - q1.y * q2.x;

		return pOut;
	}

	///< Normalizes a quaternion
	public static kmQuaternion kmQuaternionNormalize(kmQuaternion pOut, final kmQuaternion pIn) {
		float length = kmQuaternionLength(pIn);
		assert(Math.abs(length) > kmEpsilon);
		kmQuaternionScale(pOut, pIn, 1.0f / length);

		return pOut;
	}

	///< Rotates a quaternion around an axis
	public static kmQuaternion kmQuaternionRotationAxis(kmQuaternion pOut, final kmVec3 pV, float angle) {
		float rad = angle * 0.5f;
		float scale = (float) Math.sin(rad);

		pOut.w = (float) Math.cos(rad);
		pOut.x = pV.x * scale;
		pOut.y = pV.y * scale;
		pOut.z = pV.z * scale;

		return pOut;
	}

	///< Creates a quaternion from a rotation matrix
	public static kmQuaternion kmQuaternionRotationMatrix(kmQuaternion pOut, final kmMat3 pIn) {
		/*
			Note: The OpenGL matrices are transposed from the description below
			taken from the Matrix and Quaternion FAQ

				if ( mat[0] > mat[5] && mat[0] > mat[10] )  {	// Column 0:
					S  = sqrt( 1.0 + mat[0] - mat[5] - mat[10] ) * 2;
					X = 0.25 * S;
					Y = (mat[4] + mat[1] ) / S;
					Z = (mat[2] + mat[8] ) / S;
					W = (mat[9] - mat[6] ) / S;
				} else if ( mat[5] > mat[10] ) {			// Column 1:
					S  = sqrt( 1.0 + mat[5] - mat[0] - mat[10] ) * 2;
					X = (mat[4] + mat[1] ) / S;
					Y = 0.25 * S;
					Z = (mat[9] + mat[6] ) / S;
					W = (mat[2] - mat[8] ) / S;
				} else {						// Column 2:
					S  = sqrt( 1.0 + mat[10] - mat[0] - mat[5] ) * 2;
					X = (mat[2] + mat[8] ) / S;
					Y = (mat[9] + mat[6] ) / S;
					Z = 0.25 * S;
					W = (mat[4] - mat[1] ) / S;
				}
			*/

		float x, y, z, w;
		float [] pMatrix;
		float [] m4x4 = new float[16];
		float scale = 0.0f;
		float diagonal = 0.0f;

		if(pIn == null) {
			return null;
		}

		/*	0 3 6
			1 4 7
			2 5 8

			0 1 2 3
			4 5 6 7
			8 9 10 11
			12 13 14 15*/

		m4x4[0]  = pIn.mat[0];
		m4x4[1]  = pIn.mat[3];
		m4x4[2]  = pIn.mat[6];
		m4x4[4]  = pIn.mat[1];
		m4x4[5]  = pIn.mat[4];
		m4x4[6]  = pIn.mat[7];
		m4x4[8]  = pIn.mat[2];
		m4x4[9]  = pIn.mat[5];
		m4x4[10] = pIn.mat[8];
		m4x4[15] = 1;
		pMatrix = m4x4;

		diagonal = pMatrix[0] + pMatrix[5] + pMatrix[10] + 1;

		if(diagonal > kmEpsilon) {
			// Calculate the scale of the diagonal
			scale = (float) Math.sqrt(diagonal ) * 2;

			// Calculate the x, y, x and w of the quaternion through the respective equation
			x = ( pMatrix[9] - pMatrix[6] ) / scale;
			y = ( pMatrix[2] - pMatrix[8] ) / scale;
			z = ( pMatrix[4] - pMatrix[1] ) / scale;
			w = 0.25f * scale;
		} else {
			// If the first element of the diagonal is the greatest value
			if ( pMatrix[0] > pMatrix[5] && pMatrix[0] > pMatrix[10] ) {
				// Find the scale according to the first element, and double that value
				scale = (float) Math.sqrt( 1.0f + pMatrix[0] - pMatrix[5] - pMatrix[10] ) * 2.0f;

				// Calculate the x, y, x and w of the quaternion through the respective equation
				x = 0.25f * scale;
				y = (pMatrix[4] + pMatrix[1] ) / scale;
				z = (pMatrix[2] + pMatrix[8] ) / scale;
				w = (pMatrix[9] - pMatrix[6] ) / scale;
			}
			// Else if the second element of the diagonal is the greatest value
			else if (pMatrix[5] > pMatrix[10]) {
				// Find the scale according to the second element, and double that value
				scale = (float) Math.sqrt( 1.0f + pMatrix[5] - pMatrix[0] - pMatrix[10] ) * 2.0f;

				// Calculate the x, y, x and w of the quaternion through the respective equation
				x = (pMatrix[4] + pMatrix[1] ) / scale;
				y = 0.25f * scale;
				z = (pMatrix[9] + pMatrix[6] ) / scale;
				w = (pMatrix[2] - pMatrix[8] ) / scale;
			}
			// Else the third element of the diagonal is the greatest value
			else {
				// Find the scale according to the third element, and double that value
				scale  = (float) Math.sqrt( 1.0f + pMatrix[10] - pMatrix[0] - pMatrix[5] ) * 2.0f;

				// Calculate the x, y, x and w of the quaternion through the respective equation
				x = (pMatrix[2] + pMatrix[8] ) / scale;
				y = (pMatrix[9] + pMatrix[6] ) / scale;
				z = 0.25f * scale;
				w = (pMatrix[4] - pMatrix[1] ) / scale;
			}
		}

		pOut.x = x;
		pOut.y = y;
		pOut.z = z;
		pOut.w = w;

		return pOut;
	}

	///< Create a quaternion from yaw, pitch and roll
	public static kmQuaternion kmQuaternionRotationYawPitchRoll(kmQuaternion pOut, float yaw, float pitch, float roll) {
		float ex, ey, ez;		// temp half euler angles
		float cr, cp, cy, sr, sp, sy, cpcy, spsy;		// temp vars in roll,pitch yaw

		ex = kmDegreesToRadians(pitch) / 2.0f;	// convert to rads and half them
		ey = kmDegreesToRadians(yaw) / 2.0f;
		ez = kmDegreesToRadians(roll) / 2.0f;

		cr = (float) Math.cos(ex);
		cp = (float) Math.cos(ey);
		cy = (float) Math.cos(ez);

		sr = (float) Math.sin(ex);
		sp = (float) Math.sin(ey);
		sy = (float) Math.sin(ez);

		cpcy = cp * cy;
		spsy = sp * sy;

		pOut.w = cr * cpcy + sr * spsy;

		pOut.x = sr * cpcy - cr * spsy;
		pOut.y = cr * sp * cy + sr * cp * sy;
		pOut.z = cr * cp * sy - sr * sp * cy;

		kmQuaternionNormalize(pOut, pOut);

		return pOut;
	}

	///< Interpolate between 2 quaternions
	public static kmQuaternion kmQuaternionSlerp(kmQuaternion pOut, final kmQuaternion q1, final kmQuaternion q2, float t) {

		/*	float CosTheta = Q0.DotProd(Q1);
			float Theta = acosf(CosTheta);
			float SinTheta = sqrtf(1.0f-CosTheta*CosTheta);

			float Sin_T_Theta = sinf(T*Theta)/SinTheta;
			float Sin_OneMinusT_Theta = sinf((1.0f-T)*Theta)/SinTheta;

			Quaternion Result = Q0*Sin_OneMinusT_Theta;
			Result += (Q1*Sin_T_Theta);

			return Result;*/

		if (q1.x == q2.x &&
			q1.y == q2.y &&
			q1.z == q2.z &&
			q1.w == q2.w) {

			pOut.x = q1.x;
			pOut.y = q1.y;
			pOut.z = q1.z;
			pOut.w = q1.w;

			return pOut;
		} else {
			float ct = kmQuaternionDot(q1, q2);
			float theta = (float) Math.acos(ct);
			float st = (float) Math.sqrt(1.0 - kmSQR(ct));

			float stt = (float) Math.sin(t * theta) / st;
			float somt = (float) Math.sin((1.0 - t) * theta) / st;

			kmQuaternion temp = new kmQuaternion();
			kmQuaternion temp2 = new kmQuaternion();
			kmQuaternionScale(temp, q1, somt);
			kmQuaternionScale(temp2, q2, stt);
			kmQuaternionAdd(pOut, temp, temp2);
		}
		return pOut;
	}

	///< Get the axis and angle of rotation from a quaternion
	public static void kmQuaternionToAxisAngle(final kmQuaternion pIn, kmVec3 pAxis, FloatBuffer pAngle) {
		float tempAngle;		// temp angle
		float scale;			// temp vars

		tempAngle = (float) Math.acos(pIn.w);
		scale = (float) Math.sqrt(kmSQR(pIn.x) + kmSQR(pIn.y) + kmSQR(pIn.z));

		if (((scale > -kmEpsilon) && scale < kmEpsilon)
			|| (scale < 2*kmPI + kmEpsilon && scale > 2*kmPI - kmEpsilon))        // angle is 0 or 360 so just simply set axis to 0,0,1 with angle 0
		{
			pAngle.put(0, 0.0f);

			pAxis.x = 0.0f;
			pAxis.y = 0.0f;
			pAxis.z = 1.0f;
		} else {
			pAngle.put(0, tempAngle * 2.0f);	// angle in radians

			pAxis.x = pIn.x / scale;
			pAxis.y = pIn.y / scale;
			pAxis.z = pIn.z / scale;
			kmVec3.kmVec3Normalize(pAxis, pAxis);
		}
	}

	///< Scale a quaternion
	public static kmQuaternion kmQuaternionScale(kmQuaternion pOut, final kmQuaternion pIn, float s) {
		pOut.x = pIn.x * s;
		pOut.y = pIn.y * s;
		pOut.z = pIn.z * s;
		pOut.w = pIn.w * s;

		return pOut;
	}

	public static kmQuaternion kmQuaternionAssign(kmQuaternion pOut, final kmQuaternion pIn) {
		pOut.x = pIn.x;
		pOut.y = pIn.y;
		pOut.z = pIn.z;
		pOut.w = pIn.w;

		return pOut;
	}

	public static kmQuaternion kmQuaternionAdd(kmQuaternion pOut, final kmQuaternion pQ1, final kmQuaternion pQ2) {
		pOut.x = pQ1.x + pQ2.x;
		pOut.y = pQ1.y + pQ2.y;
		pOut.z = pQ1.z + pQ2.z;
		pOut.w = pQ1.w + pQ2.w;

		return pOut;
	}

	/** Adapted from the OGRE engine!
	 * 
	 * Gets the shortest arc quaternion to rotate this vector to the destination
	 * vector.
	 * @remarks
	 *     If you call this with a dest vector that is close to the inverse
	 *     of this vector, we will rotate 180 degrees around the 'fallbackAxis'
	 *     (if specified, or a generated axis if not) since in this case
	 *     ANY axis of rotation is valid.
	 */
	public static kmQuaternion kmQuaternionRotationBetweenVec3(kmQuaternion pOut, final kmVec3 vec1, final kmVec3 vec2, final kmVec3 fallback) {
		kmVec3 v1 = new kmVec3();
		kmVec3 v2 = new kmVec3();
		float a;

		kmVec3.kmVec3Assign(v1, vec1);
		kmVec3.kmVec3Assign(v2, vec2);

		kmVec3.kmVec3Normalize(v1, v1);
		kmVec3.kmVec3Normalize(v2, v2);

		a = kmVec3.kmVec3Dot(v1, v2);

		if (a >= 1.0) {
			kmQuaternionIdentity(pOut);
			return pOut;
		}

		if (a < (1e-6f - 1.0f))    {
			if (Math.abs(kmVec3.kmVec3LengthSq(fallback)) < kmEpsilon) {
				kmQuaternionRotationAxis(pOut, fallback, kmPI);
			} else {
				kmVec3 axis = new kmVec3();
				kmVec3 X = new kmVec3();
				X.x = 1.0f;
				X.y = 0.0f;
				X.z = 0.0f;

				kmVec3.kmVec3Cross(axis, X, vec1);

				//If axis is zero
				if (Math.abs(kmVec3.kmVec3LengthSq(axis)) < kmEpsilon) {
					kmVec3 Y = new kmVec3();
					Y.x = 0.0f;
					Y.y = 1.0f;
					Y.z = 0.0f;

					kmVec3.kmVec3Cross(axis, Y, vec1);
				}

				kmVec3.kmVec3Normalize(axis, axis);

				kmQuaternionRotationAxis(pOut, axis, kmPI);
			}
		} else {
			float s = (float) Math.sqrt((1+a) * 2);
			float invs = 1 / s;

			kmVec3 c = new kmVec3();
			kmVec3.kmVec3Cross(c, v1, v2);

			pOut.x = c.x * invs;
			pOut.y = c.y * invs;
			pOut.z = c.z * invs;
			pOut.w = s * 0.5f;

			kmQuaternionNormalize(pOut, pOut);
		}

		return pOut;
	}

	public static kmVec3 kmQuaternionMultiplyVec3(kmVec3 pOut, final kmQuaternion q, final kmVec3 v) {
		kmVec3 uv = new kmVec3();
		kmVec3 uuv = new kmVec3();
		kmVec3 qvec = new kmVec3();

		qvec.x = q.x;
		qvec.y = q.y;
		qvec.z = q.z;

		kmVec3.kmVec3Cross(uv, qvec, v);
		kmVec3.kmVec3Cross(uuv, qvec, uv);

		kmVec3.kmVec3Scale(uv, uv, (2.0f * q.w));
		kmVec3.kmVec3Scale(uuv, uuv, 2.0f);

		kmVec3.kmVec3Add(pOut, v, uv);
		kmVec3.kmVec3Add(pOut, pOut, uuv);

		return pOut;
	}
}
