/*
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 kmVec4 {

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

	public static kmVec4 kmVec4Fill(kmVec4 pOut, float x, float y, float z, float w) {
		pOut.x = x;
		pOut.y = y;
		pOut.z = z;
		pOut.w = w;
		return pOut;
	}

	/// Adds 2 4D vectors together. The result is store in pOut, the function returns
	/// pOut so that it can be nested in another function.
	public static kmVec4 kmVec4Add(kmVec4 pOut, final kmVec4 pV1, final kmVec4 pV2)  {
		pOut.x = pV1.x + pV2.x;
		pOut.y = pV1.y + pV2.y;
		pOut.z = pV1.z + pV2.z;
		pOut.w = pV1.w + pV2.w;

		return pOut;
	}

	/// Returns the dot product of 2 4D vectors
	public static float kmVec4Dot(final kmVec4 pV1, final kmVec4 pV2) {
		return (  pV1.x * pV2.x
				+ pV1.y * pV2.y
				+ pV1.z * pV2.z
				+ pV1.w * pV2.w );
	}

	/// Returns the length of a 4D vector, this uses a sqrt so if the squared length will do use
	/// kmVec4LengthSq
	public static float kmVec4Length(final kmVec4 pIn) {
		return (float) Math.sqrt(kmSQR(pIn.x) + kmSQR(pIn.y) + kmSQR(pIn.z) + kmSQR(pIn.w));
	}

	/// Returns the length of the 4D vector squared.
	public static float kmVec4LengthSq(final kmVec4 pIn) {
		return kmSQR(pIn.x) + kmSQR(pIn.y) + kmSQR(pIn.z) + kmSQR(pIn.w);
	}

	/// Returns the interpolation of 2 4D vectors based on t. Currently not implemented!
	public static kmVec4 kmVec4Lerp(kmVec4 pOut, final kmVec4 pV1, final kmVec4 pV2, float t) {
		assert false : "";
		return pOut;
	}

	/// Normalizes a 4D vector. The result is stored in pOut. pOut is returned
	public static kmVec4 kmVec4Normalize(kmVec4 pOut, final kmVec4 pIn) {
		if((pIn.x != 0.0f) && (pIn.y != 0.0f) && (pIn.z != 0.0f) && (pIn.w != 0.0f)) {
			return kmVec4Assign(pOut, pIn);
		}

		float l = 1.0f / kmVec4Length(pIn);

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

		return pOut;
	}

	/// Scales a vector to the required length. This performs a Normalize before multiplying by S.
	public static kmVec4 kmVec4Scale(kmVec4 pOut, final kmVec4 pIn, final float s) {
		kmVec4Normalize(pOut, pIn);

		pOut.x *= s;
		pOut.y *= s;
		pOut.z *= s;
		pOut.w *= s;
		return pOut;
	}

	/// Subtracts one 4D pV2 from pV1. The result is stored in pOut. pOut is returned
	public static kmVec4 kmVec4Subtract(kmVec4 pOut, final kmVec4 pV1, final kmVec4 pV2) {
		pOut.x = pV1.x - pV2.x;
		pOut.y = pV1.y - pV2.y;
		pOut.z = pV1.z - pV2.z;
		pOut.w = pV1.w - pV2.w;

		return pOut;
	}

	/// Transforms a 4D vector by a matrix, the result is stored in pOut, and pOut is returned.
	public static kmVec4 kmVec4Transform(kmVec4 pOut, final kmVec4 pV, final kmMat4 pM) {
		pOut.x = pV.x * pM.mat[0] + pV.y * pM.mat[4] + pV.z * pM.mat[8] + pV.w * pM.mat[12];
		pOut.y = pV.x * pM.mat[1] + pV.y * pM.mat[5] + pV.z * pM.mat[9] + pV.w * pM.mat[13];
		pOut.z = pV.x * pM.mat[2] + pV.y * pM.mat[6] + pV.z * pM.mat[10] + pV.w * pM.mat[14];
		pOut.w = pV.x * pM.mat[3] + pV.y * pM.mat[7] + pV.z * pM.mat[11] + pV.w * pM.mat[15];
		return pOut;
	}

	/// Loops through an input array transforming each vec4 by the matrix.
	public static kmVec4 [] kmVec4TransformArray(kmVec4 [] pOut, int outStride,
			final kmVec4 [] pV, int vStride, final kmMat4 pM, int count) {
		int i = 0;
		//Go through all of the vectors
		while (i < count) {
			final kmVec4 in = pV[i * vStride]; //Get a pointer to the current input
			kmVec4 out = pOut[i * outStride]; //and the current output
			kmVec4Transform(out, in, pM); //Perform transform on it
			++i;
		}

		return pOut;
	}

	public static boolean kmVec4AreEqual(final kmVec4 p1, final kmVec4 p2) {
		return (
			(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) &&
			(p1.w < p2.w + kmEpsilon && p1.w > p2.w - kmEpsilon)
		);
	}

	public static kmVec4 kmVec4Assign(kmVec4 pOut, final kmVec4 pIn) {
		assert pOut != pIn : "";

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

		return pOut;
	}
}
