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

public class kmVec3 {

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

	public static kmVec3 kmVec3Fill(kmVec3 pOut, float x, float y, float z) {
		pOut.x = x;
		pOut.y = y;
		pOut.z = z;
		return pOut;
	}

	/** Returns the length of the vector */
	public static float kmVec3Length(final kmVec3 pIn) {
		return (float) Math.sqrt(Utility.kmSQR(pIn.x) + Utility.kmSQR(pIn.y) + Utility.kmSQR(pIn.z));
	}

	/** Returns the square of the length of the vector */
	public static float kmVec3LengthSq(final kmVec3 pIn) {
		return kmSQR(pIn.x) + kmSQR(pIn.y) + kmSQR(pIn.z);
	}

	/** Returns the vector passed in set to unit length */
	public static kmVec3 kmVec3Normalize(kmVec3 pOut, final kmVec3 pIn) {
		float l = 1.0f / kmVec3Length(pIn);

		pOut.x = pIn.x * l;
		pOut.y = pIn.y * l;
		pOut.z = pIn.z * l;

		return pOut;
	}

	/** Returns a vector perpendicular to 2 other vectors */
	public static kmVec3 kmVec3Cross(kmVec3 pOut, final kmVec3 pV1, final kmVec3 pV2) {
		kmVec3 v = new kmVec3();

		v.x = (pV1.y * pV2.z) - (pV1.z * pV2.y);
		v.y = (pV1.z * pV2.x) - (pV1.x * pV2.z);
		v.z = (pV1.x * pV2.y) - (pV1.y * pV2.x);

		pOut.x = v.x;
		pOut.y = v.y;
		pOut.z = v.z;

		return pOut;
	}

	/** Returns the cosine of the angle between 2 vectors */
	public static float kmVec3Dot(final kmVec3 pV1, final kmVec3 pV2) {
		return (  pV1.x * pV2.x
				+ pV1.y * pV2.y
				+ pV1.z * pV2.z );
	}

	/** Adds 2 vectors and returns the result */
	public static kmVec3 kmVec3Add(kmVec3 pOut, final kmVec3 pV1, final kmVec3 pV2) {
		pOut.x = pV1.x + pV2.x;
		pOut.y = pV1.y + pV2.y;
		pOut.z = pV1.z + pV2.z;

		return pOut;
	}

	/** Subtracts 2 vectors and returns the result */
	public static kmVec3 kmVec3Subtract(kmVec3 pOut, final kmVec3 pV1, final kmVec3 pV2) {
		pOut.x = pV1.x - pV2.x;
		pOut.y = pV1.y - pV2.y;
		pOut.z = pV1.z - pV2.z;

		return pOut;
	}

	/** Transforms a vector (assuming w=1) by a given matrix */
	public static kmVec3 kmVec3Transform(kmVec3 pOut, final kmVec3 pV, final kmMat4 pM) {
		/*
			a = (Vx, Vy, Vz, 1)
			b = (a*M)T
			Out = (bx, by, bz)
		*/

		pOut.x = pV.x * pM.mat[0] + pV.y * pM.mat[4] + pV.z * pM.mat[8] + pM.mat[12];
		pOut.y = pV.x * pM.mat[1] + pV.y * pM.mat[5] + pV.z * pM.mat[9] + pM.mat[13];
		pOut.z = pV.x * pM.mat[2] + pV.y * pM.mat[6] + pV.z * pM.mat[10] + pM.mat[14];

		return pOut;
	}

	/**Transforms a 3D normal by a given matrix */
	public static kmVec3 kmVec3TransformNormal(kmVec3 pOut, final kmVec3 pV, final kmMat4 pM) {
		/*
			a = (Vx, Vy, Vz, 0)
			b = (a*M)T
			Out = (bx, by, bz)
		*/
		//Omits the translation, only scaling + rotating

		kmVec3 v = new kmVec3();

		v.x = pV.x * pM.mat[0] + pV.y * pM.mat[4] + pV.z * pM.mat[8];
		v.y = pV.x * pM.mat[1] + pV.y * pM.mat[5] + pV.z * pM.mat[9];
		v.z = pV.x * pM.mat[2] + pV.y * pM.mat[6] + pV.z * pM.mat[10];

		pOut.x = v.x;
		pOut.y = v.y;
		pOut.z = v.z;

		return pOut;
	}

	/**Transforms a 3D vector by a given matrix, projecting the result back into w = 1. */
	public static kmVec3 kmVec3TransformCoord(kmVec3 pOut, final kmVec3 pV, final kmMat4 pM) {
		/*
			a = (Vx, Vy, Vz, 1)
			b = (a*M)T
			Out = 1/bw(bx, by, bz)
		*/

		kmVec4 v = new kmVec4();
		kmVec4 inV = new kmVec4();
		kmVec4.kmVec4Fill(inV, pV.x, pV.y, pV.z, 1.0f);

		kmVec4.kmVec4Transform(v, inV, pM);

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

		return pOut;
	}

	/**
	 * Scales a vector to length s. Does not normalize first,
	 * you should do that!
	 */
	public static kmVec3 kmVec3Scale(kmVec3 pOut, final kmVec3 pIn, final float s) {
		pOut.x = pIn.x * s;
		pOut.y = pIn.y * s;
		pOut.z = pIn.z * s;

		return pOut;
	}

	/**
	 * Returns KM_TRUE if the 2 vectors are approximately equal
	 */
	public static int kmVec3AreEqual(final kmVec3 p1, final kmVec3 p2) {
		if(	(p1.x < (p2.x + kmEpsilon) && p1.x > (p2.x - kmEpsilon)) &&
			(p1.y < (p2.y + kmEpsilon) && p1.y > (p2.y - kmEpsilon)) &&
			(p1.z < (p2.z + kmEpsilon) && p1.z > (p2.z - kmEpsilon))) {
			return 1;
		}

		return 0;
	}

	public static kmVec3 kmVec3InverseTransform(kmVec3 pOut, final kmVec3 pV, final kmMat4 pM) {
		kmVec3 v1 = new kmVec3();
		kmVec3 v2 = new kmVec3();

		v1.x = pV.x - pM.mat[12];
		v1.y = pV.y - pM.mat[13];
		v1.z = pV.z - pM.mat[14];

		v2.x = v1.x * pM.mat[0] + v1.y * pM.mat[1] + v1.z * pM.mat[2];
		v2.y = v1.x * pM.mat[4] + v1.y * pM.mat[5] + v1.z * pM.mat[6];
		v2.z = v1.x * pM.mat[8] + v1.y * pM.mat[9] + v1.z * pM.mat[10];

		pOut.x = v2.x;
		pOut.y = v2.y;
		pOut.z = v2.z;

		return pOut;
	}

	public static kmVec3 kmVec3InverseTransformNormal(kmVec3 pOut, final kmVec3 pV, final kmMat4 pM) {
		kmVec3 v = new kmVec3();

		v.x = pV.x * pM.mat[0] + pV.y * pM.mat[1] + pV.z * pM.mat[2];
		v.y = pV.x * pM.mat[4] + pV.y * pM.mat[5] + pV.z * pM.mat[6];
		v.z = pV.x * pM.mat[8] + pV.y * pM.mat[9] + pV.z * pM.mat[10];

		pOut.x = v.x;
		pOut.y = v.y;
		pOut.z = v.z;

		return pOut;
	}

	/**
	 * Assigns pIn to pOut. Returns pOut. If pIn and pOut are the same
	 * then nothing happens but pOut is still returned
	 */
	public static kmVec3 kmVec3Assign(kmVec3 pOut, final kmVec3 pIn) {
		if (pOut == pIn) {
			return pOut;
		}

		pOut.x = pIn.x;
		pOut.y = pIn.y;
		pOut.z = pIn.z;

		return pOut;
	}

	/**
	 * Sets all the elements of pOut to zero. Returns pOut.
	 */
	public static kmVec3 kmVec3Zero(kmVec3 pOut) {
		pOut.x = 0.0f;
		pOut.y = 0.0f;
		pOut.z = 0.0f;

		return pOut;
	}
}
