/*
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 java.nio.FloatBuffer;
import java.util.Arrays;

public class kmMat3 {

	public float [] mat = new float[9];

	public static kmMat3 kmMat3Fill(kmMat3 pOut, final float [] pMat) {
		System.arraycopy(pMat, 0, pOut.mat, 0, 9);
		return pOut;
	}

	public static kmMat3 kmMat3Adjugate(kmMat3 pOut, final kmMat3 pIn) {
		pOut.mat[0] = pIn.mat[4] * pIn.mat[8] - pIn.mat[5] * pIn.mat[7];
		pOut.mat[1] = pIn.mat[2] * pIn.mat[7] - pIn.mat[1] * pIn.mat[8];
		pOut.mat[2] = pIn.mat[1] * pIn.mat[5] - pIn.mat[2] * pIn.mat[4];
		pOut.mat[3] = pIn.mat[5] * pIn.mat[6] - pIn.mat[3] * pIn.mat[8];
		pOut.mat[4] = pIn.mat[0] * pIn.mat[8] - pIn.mat[2] * pIn.mat[6];
		pOut.mat[5] = pIn.mat[2] * pIn.mat[3] - pIn.mat[0] * pIn.mat[5];
		pOut.mat[6] = pIn.mat[3] * pIn.mat[7] - pIn.mat[4] * pIn.mat[6];
		pOut.mat[7] = pIn.mat[1] * pIn.mat[6] - pIn.mat[0] * pIn.mat[7];
		pOut.mat[8] = pIn.mat[0] * pIn.mat[4] - pIn.mat[1] * pIn.mat[3];

		return pOut;
	}


	/** Sets pOut to an identity matrix returns pOut*/
	public static kmMat3 kmMat3Identity(kmMat3 pOut) {
		System.arraycopy(identity, 0, pOut.mat, 0, 9);
		return pOut;
	}

	public static kmMat3 kmMat3Inverse(kmMat3 pOut, final float pDeterminate, final kmMat3 pM) {
		float detInv;
		kmMat3 adjugate = new kmMat3();

		if(pDeterminate == 0.0f) {
			return null;
		}

		detInv = 1.0f / pDeterminate;

		kmMat3Adjugate(adjugate, pM);
		kmMat3ScalarMultiply(pOut, adjugate, detInv);

		return pOut;
	}


	/** Returns true if pIn is an identity matrix */
	public static boolean kmMat3IsIdentity(final kmMat3 pIn) {
		return Arrays.equals(identity, pIn.mat);
	}

	/** Sets pOut to the transpose of pIn, returns pOut */
	public static kmMat3 kmMat3Transpose(kmMat3 pOut, final kmMat3 pIn) {
		int z, x;
		for (z = 0; z < 3; ++z) {
			for (x = 0; x < 3; ++x) {
				pOut.mat[(z * 3) + x] = pIn.mat[(x * 3) + z];
			}
		}

		return pOut;
	}

	public static float kmMat3Determinant(final kmMat3 pIn) {
		float output;
		/*
			calculating the determinant following the rule of sarus,
			    | 0  3  6 | 0  3 |
			m = | 1  4  7 | 1  4 |
			    | 2  5  8 | 2  5 |
			now sum up the products of the diagonals going to the right (i.e. 0,4,8)
			and subtract the products of the other diagonals (i.e. 2,4,6)
		*/

		output = pIn.mat[0] * pIn.mat[4] * pIn.mat[8] + pIn.mat[1] * pIn.mat[5] * pIn.mat[6] + pIn.mat[2] * pIn.mat[3] * pIn.mat[7];
		output -= pIn.mat[2] * pIn.mat[4] * pIn.mat[6] + pIn.mat[0] * pIn.mat[5] * pIn.mat[7] + pIn.mat[1] * pIn.mat[3] * pIn.mat[8];

		return output;
	}


	/* Multiplies pM1 with pM2, stores the result in pOut, returns pOut */
	public static kmMat3 kmMat3Multiply(kmMat3 pOut, final kmMat3 pM1, final kmMat3 pM2) {
		float [] mat = new float[9];

		final float [] m1 = pM1.mat, m2 = pM2.mat;

		mat[0] = m1[0] * m2[0] + m1[3] * m2[1] + m1[6] * m2[2];
		mat[1] = m1[1] * m2[0] + m1[4] * m2[1] + m1[7] * m2[2];
		mat[2] = m1[2] * m2[0] + m1[5] * m2[1] + m1[8] * m2[2];

		mat[3] = m1[0] * m2[3] + m1[3] * m2[4] + m1[6] * m2[5];
		mat[4] = m1[1] * m2[3] + m1[4] * m2[4] + m1[7] * m2[5];
		mat[5] = m1[2] * m2[3] + m1[5] * m2[4] + m1[8] * m2[5];

		mat[6] = m1[0] * m2[6] + m1[3] * m2[7] + m1[6] * m2[8];
		mat[7] = m1[1] * m2[6] + m1[4] * m2[7] + m1[7] * m2[8];
		mat[8] = m1[2] * m2[6] + m1[5] * m2[7] + m1[8] * m2[8];

		System.arraycopy(mat, 0, pOut.mat, 0, 9);

		return pOut;
	}


	public static kmMat3 kmMat3ScalarMultiply(kmMat3 pOut, final kmMat3 pM, final float pFactor) {
		for(int i = 0; i < 9; i++) {
			pOut.mat[i] = pM.mat[i] * pFactor;
		}

		return pOut;
	}


	public static kmMat3 kmMat3RotationAxisAngle(kmMat3 pOut, final kmVec3 axis, float radians) {
		float rcos = (float) Math.cos(radians);
		float rsin = (float) Math.sin(radians);

		pOut.mat[0] = rcos + axis.x * axis.x * (1 - rcos);
		pOut.mat[1] = axis.z * rsin + axis.y * axis.x * (1 - rcos);
		pOut.mat[2] = -axis.y * rsin + axis.z * axis.x * (1 - rcos);

		pOut.mat[3] = -axis.z * rsin + axis.x * axis.y * (1 - rcos);
		pOut.mat[4] = rcos + axis.y * axis.y * (1 - rcos);
		pOut.mat[5] = axis.x * rsin + axis.z * axis.y * (1 - rcos);

		pOut.mat[6] = axis.y * rsin + axis.x * axis.z * (1 - rcos);
		pOut.mat[7] = -axis.x * rsin + axis.y * axis.z * (1 - rcos);
		pOut.mat[8] = rcos + axis.z * axis.z * (1 - rcos);

		return pOut;
	}

	public static kmVec3 kmMat3RotationToAxisAngle(kmVec3 pAxis, FloatBuffer radians, final kmMat3 pIn) {
		/*Surely not this easy?*/
		kmQuaternion temp = new kmQuaternion();
		kmQuaternion.kmQuaternionRotationMatrix(temp, pIn);
		kmQuaternion.kmQuaternionToAxisAngle(temp, pAxis, radians);
		return pAxis;
	}

	/** Assigns the value of pIn to pOut */
	public static kmMat3 kmMat3Assign(kmMat3 pOut, final kmMat3 pIn) {
		assert pOut != pIn : "You have tried to self-assign!!";

		System.arraycopy(pIn.mat, 0, pOut.mat, 0, 9);

		return pOut;
	}


	/** Returns true if the 2 matrices are equal (approximately) */
	public static boolean kmMat3AreEqual(final kmMat3 pM1, final kmMat3 pM2) {
		if (pM1 == pM2) {
			return true;
		}

		for(int i = 0; i < 9; ++i) {
			if (! (pM1.mat[i] + kmEpsilon > pM2.mat[i] &&
				pM1.mat[i] - kmEpsilon < pM2.mat[i])) {
				return false;
			}
		}

		return true;
	}

	/**
	 * Builds an X-axis rotation matrix and stores it in pOut, returns pOut
	 */
	public static kmMat3 kmMat3RotationX(kmMat3 pOut, final float radians) {
		/*
			    |  1  0       0      |
			M = |  0  cos(A) -sin(A) |
			    |  0  sin(A)  cos(A) |
		 */

		pOut.mat[0] = 1.0f;
		pOut.mat[1] = 0.0f;
		pOut.mat[2] = 0.0f;

		pOut.mat[3] = 0.0f;
		pOut.mat[4] = (float) Math.cos(radians);
		pOut.mat[5] = (float) Math.sin(radians);

		pOut.mat[6] = 0.0f;
		pOut.mat[7] = -(float) Math.sin(radians);
		pOut.mat[8] = (float) Math.cos(radians);

		return pOut;
	}

	/**
	 * Builds a rotation matrix using the rotation around the Y-axis
	 * The result is stored in pOut, pOut is returned.
	 */
	public static kmMat3 kmMat3RotationY(kmMat3 pOut, final float radians) {
		/*
			    |  cos(A)  0   sin(A) |
			M = |  0       1   0      |
			    | -sin(A)  0   cos(A) |
		 */

		pOut.mat[0] = (float) Math.cos(radians);
		pOut.mat[1] = 0.0f;
		pOut.mat[2] = -(float) Math.sin(radians);

		pOut.mat[3] = 0.0f;
		pOut.mat[4] = 1.0f;
		pOut.mat[5] = 0.0f;

		pOut.mat[6] = (float) Math.sin(radians);
		pOut.mat[7] = 0.0f;
		pOut.mat[8] = (float) Math.cos(radians);

		return pOut;
	}

	/**
	 * Builds a rotation matrix around the Z-axis. The resulting
	 * matrix is stored in pOut. pOut is returned.
	 */
	public static kmMat3 kmMat3RotationZ(kmMat3 pOut, final float radians) {
		/*
			    |  cos(A)  -sin(A)   0  |
			M = |  sin(A)   cos(A)   0  |
			    |  0        0        1  |
		 */

		pOut.mat[0] = (float) Math.cos(radians);
		pOut.mat[1] = -(float) Math.sin(radians);
		pOut.mat[2] = 0.0f;

		pOut.mat[3] = (float) Math.sin(radians);;
		pOut.mat[4] = (float) Math.cos(radians);
		pOut.mat[5] = 0.0f;

		pOut.mat[6] = 0.0f;
		pOut.mat[7] = 0.0f;
		pOut.mat[8] = 1.0f;

		return pOut;
	}


	/* Rotation around the z axis so everything stays planar in XY */
	public static kmMat3 kmMat3Rotation(kmMat3 pOut, final float radians) {
		/*
			    |  cos(A)  -sin(A)   0  |
			M = |  sin(A)   cos(A)   0  |
			    |  0        0        1  |
		*/

		pOut.mat[0] = (float) Math.cos(radians);
		pOut.mat[1] = (float) Math.sin(radians);
		pOut.mat[2] = 0.0f;

		pOut.mat[3] = -(float) Math.sin(radians);;
		pOut.mat[4] = (float) Math.cos(radians);
		pOut.mat[5] = 0.0f;

		pOut.mat[6] = 0.0f;
		pOut.mat[7] = 0.0f;
		pOut.mat[8] = 1.0f;

		return pOut;
	}

	/** Builds a scaling matrix */
	public static kmMat3 kmMat3Scaling(kmMat3 pOut, final float x, final float y) {
//		memset(pOut->mat, 0, sizeof(float) * 9);
		kmMat3Identity(pOut);
		pOut.mat[0] = x;
		pOut.mat[4] = y;

		return pOut;
	}


	public static kmMat3 kmMat3Translation(kmMat3 pOut, final float x, final float y) {
//		memset(pOut->mat, 0, sizeof(float) * 9);
		kmMat3Identity(pOut);
		pOut.mat[6] = x;
		pOut.mat[7] = y;
//		pOut->mat[8] = 1.0;

		return pOut;
	}

	public static kmMat3 kmMat3RotationQuaternion(kmMat3 pOut, final kmQuaternion pIn) {
		if (pIn == null || pOut == null) {
			return null;
		}

		// First row
		pOut.mat[0] = 1.0f - 2.0f * (pIn.y * pIn.y + pIn.z * pIn.z);
		pOut.mat[1] = 2.0f * (pIn.x * pIn.y - pIn.w * pIn.z);
		pOut.mat[2] = 2.0f * (pIn.x * pIn.z + pIn.w * pIn.y);

		// Second row
		pOut.mat[3] = 2.0f * (pIn.x * pIn.y + pIn.w * pIn.z);
		pOut.mat[4] = 1.0f - 2.0f * (pIn.x * pIn.x + pIn.z * pIn.z);
		pOut.mat[5] = 2.0f * (pIn.y * pIn.z - pIn.w * pIn.x);

		// Third row
		pOut.mat[6] = 2.0f * (pIn.x * pIn.z - pIn.w * pIn.y);
		pOut.mat[7] = 2.0f * (pIn.y * pIn.z + pIn.w * pIn.x);
		pOut.mat[8] = 1.0f - 2.0f * (pIn.x * pIn.x + pIn.y * pIn.y);

		return pOut;
	}

	private static final float[] identity = {	1.0f, 0.0f, 0.0f,
												0.0f, 1.0f, 0.0f,
												0.0f, 0.0f, 1.0f	};
}
