//=======================================================================
//Matrix4x4.java - Implements a simple, _immutable_ 4x4 matrix class
//Author: Chris DeCoro (cdecoro@cat.nyu.edu)
//=======================================================================

package Geometry;

public class Matrix4x4 implements Cloneable
{
	//Default constructor
	//Sets matrix to identity
	public Matrix4x4()
	{
		initArray();
		matrix[0][0] =1;
		matrix[1][1] =1;
		matrix[2][2] =1;
		matrix[3][3] =1;
	}

	//"Copy constructor"
	//Since we can't modify a matrix, this ctor is probably worthless
	public Matrix4x4(Matrix4x4 copy)
	{
		this.matrix = (double[][])copy.matrix.clone();
	}
	
	//Initializing constructor - scalars
	public Matrix4x4(double v11, double v12, double v13, double v14,
			double v21, double v22, double v23, double v24,
			double v31, double v32, double v33, double v34,
			double v41, double v42, double v43, double v44)
	{
		initArray();

		matrix[0][0] = v11;
		matrix[0][1] = v12;
		matrix[0][2] = v13;
		matrix[0][3] = v14;

		matrix[1][0] = v21;
		matrix[1][1] = v22;
		matrix[1][2] = v23;
		matrix[1][3] = v24;

		matrix[2][0] = v31;
		matrix[2][1] = v32;
		matrix[2][2] = v33;
		matrix[2][3] = v34;

		matrix[3][0] = v41;
		matrix[3][1] = v42;
		matrix[3][2] = v43;
		matrix[3][3] = v44;
	}

	//Initializing constructor - single-dimensional array
	//Assumes row-major ordering (column idx increases faster)
	public Matrix4x4(double[] array)
	{
		if( array.length != 9 && array.length != 16 )
		{
			throw new RuntimeException("Array.length must == 9 or 16");
		}

		initArray();

		if( array.length == 16 )
		{
			matrix[0][0] = array[0];
			matrix[0][1] = array[1];
			matrix[0][2] = array[2];
			matrix[0][3] = array[3];

			matrix[1][0] = array[4];
			matrix[1][1] = array[5];
			matrix[1][2] = array[6];
			matrix[1][3] = array[7];

			matrix[2][0] = array[8];
			matrix[2][1] = array[9];
			matrix[2][2] = array[10];
			matrix[2][3] = array[11];

			matrix[3][0] = array[12];
			matrix[3][1] = array[13];
			matrix[3][2] = array[14];
			matrix[3][3] = array[15];
		}		
		else if( array.length == 9 )
		{
			matrix[0][0] = array[0];
			matrix[0][1] = array[1];
			matrix[0][2] = array[2];
			matrix[0][3] = 0;

			matrix[1][0] = array[3];
			matrix[1][1] = array[4];
			matrix[1][2] = array[5];
			matrix[1][3] = 0;

			matrix[2][0] = array[6];
			matrix[2][1] = array[7];
			matrix[2][2] = array[8];
			matrix[2][3] = 0;

			matrix[3][0] = array[9];
			matrix[3][1] = array[10];
			matrix[3][2] = array[11];
			matrix[3][3] = 1;
		}
	}
	
	//Clone - overrides Object.clone
	//As with the copy ctor, for immutable objects this maybe worthless
	public Object clone()
	{
		Matrix4x4 retval = new Matrix4x4();
		retval.matrix = (double[][])matrix.clone();
		return retval;
	}
	
	//Element Accessor
	//Returns a single element of the matrix
	public double get(int i, int j)
	{
		if( i < 0 || i > 3 || j < 0 || j > 3 )
			throw new RuntimeException("Index out of bounds");
		else
			return matrix[i][j];
	}

	//Array accessor
	//Returns a duplicate of the internal 2d matrix structure
	public double[][] array()
	{
		//make sure to return a duplicate so they cant change it
		//JAVA NEEDS CONST!!!
		return (double[][])matrix.clone();
	}

	//Equality test - overrides Object.equals
	public boolean equals(Object obj)
	{
		if( obj instanceof Matrix4x4 )
		{
			Matrix4x4 rhs = (Matrix4x4)obj;
			boolean eq = true;
OUTER:
			for(int i=0; i<3; i++)
				for(int j=0; j<3; j++)
					if( matrix[i][j] != rhs.matrix[i][j] )
					{
						eq = false;
						break OUTER;
					}
			return eq;
		}
		else
		{
			return false;
		}
	}

	//String conversion - overrides Object.toString
	//Turns matrix into four rows of doubles - includes linebreaks
	public String toString()
	{
		return  "| " + matrix[0][0] + " " + matrix[0][1] + " " + matrix[0][2] + " " + matrix[0][3] + " |\n" +
			"| " + matrix[1][0] + " " + matrix[1][1] + " " + matrix[1][2] + " " + matrix[1][3] + " |\n" +
			"| " + matrix[2][0] + " " + matrix[2][1] + " " + matrix[2][2] + " " + matrix[2][3] + " |\n" +
			"| " + matrix[3][0] + " " + matrix[3][1] + " " + matrix[3][2] + " " + matrix[3][3] + " |";
	}
	
	//Matrix-Matrix Addition
	public Matrix4x4 add(Matrix4x4 rhs)
	{
		Matrix4x4 retval = new Matrix4x4();

		for(int i=0; i<4; i++)
			for(int j=0; j<4; j++)
				retval.matrix[i][j] = matrix[i][j] + rhs.matrix[i][j];	

		return retval;
	}

	//Matrix-Matrix Subtraction
	public Matrix4x4 sub(Matrix4x4 rhs)
	{
		Matrix4x4 retval = new Matrix4x4();

		for(int i=0; i<4; i++)
			for(int j=0; j<4; j++)
				retval.matrix[i][j] = matrix[i][j] - rhs.matrix[i][j];	

		return retval;
	}

	//Matrix-Scalar Multiplication
	public Matrix4x4 mul(double c)
	{
		Matrix4x4 retval = new Matrix4x4();
		for(int i=0; i<4; i++)
			for(int j=0; j<4; j++)
				retval.matrix[i][j] = c*matrix[i][j];
		return retval;
	}

	//Matrix-Scalar Division
	public Matrix4x4 div(double c)
	{
		Matrix4x4 retval = new Matrix4x4();
		for(int i=0; i<4; i++)
			for(int j=0; j<4; j++)
				retval.matrix[i][j] = matrix[i][j]/c;
		return retval;
	}

	//Matrix-Vector Multiplication (Application)
	public Vector3 mul(Vector3 vec)
	{
		//Create a new vector and make it 4d homogenous
		double vectorOut[] = new double[4];
		double vectorIn[] = new double[4];
		vectorIn[0] = vec.xyz[0];
		vectorIn[1] = vec.xyz[1];
		vectorIn[2] = vec.xyz[2];
		vectorIn[3] = 1;

		for(int i=0; i<4; i++)
		{
			vectorOut[i] = 0;
			for(int j=0; j<4; j++)
				vectorOut[i] += vectorIn[j]*matrix[i][j];
		}
		
		//Unhomogenize the result vector and return a 3d vector
		Vector3 retval = new Vector3();
		retval.xyz[0] = vectorOut[0] / vectorOut[3];
		retval.xyz[1] = vectorOut[1] / vectorOut[3];
		retval.xyz[2] = vectorOut[2] / vectorOut[3];
		return retval;
	}

	//Matrix-Vector Multiplication (Application)
	public Vector4 mul(Vector4 vec)
	{
		//Create a new vector and make it 4d homogenous
		double vectorOut[] = new double[4];
		double vectorIn[] = new double[4];
		vectorIn[0] = vec.xyz[0];
		vectorIn[1] = vec.xyz[1];
		vectorIn[2] = vec.xyz[2];
		vectorIn[3] = vec.xyz[3];

		for(int i=0; i<4; i++)
		{
			vectorOut[i] = 0;
			for(int j=0; j<4; j++)
				vectorOut[i] += vectorIn[j]*matrix[i][j];
		}
		
		return new Vector4(vectorOut);
	}

	//Obscene Hack Matrix-Vector Multiplication (Application)
	//This is a cheap hack to try to gain some additional speed in the vertex() function
	//We have a pre-allocated array which we return (this is terrible coupling)
	//We also assume that the row[2] = [ 0 0 0 x ] (makes it special purpose to this app)
	//We also assume that the row[3] = [ 0 0 0 1 ] (worthless for projection)
	//Finally, assumes w==1 (ok, it will be 1 anyway, so its not too bad)
	public double[] fastMul(Vector3 vec)
	{
		retval[0] = matrix[0][0]*vec.xyz[0] + matrix[0][1]*vec.xyz[1] + matrix[0][2]*vec.xyz[2] + matrix[0][3];
		retval[1] = matrix[1][0]*vec.xyz[0] + matrix[1][1]*vec.xyz[1] + matrix[1][2]*vec.xyz[2] + matrix[1][3];
		retval[2] = matrix[2][3];
		return retval;
	}
			
	//Matrix-Matrix Left-multiplication
	//Given matrix this, mat; performs mat*this
	public Matrix4x4 leftmul(Matrix4x4 mat)
	{
		return mat.mul(this);
	}

	//Matrix-Matrix Right-multiplication
	public Matrix4x4 mul(Matrix4x4 mat)
	{
		Matrix4x4 retval = new Matrix4x4();
		for(int i=0; i<4; i++)
		{
			for(int j=0; j<4; j++)
			{
				retval.matrix[i][j] = 0;
				for(int k=0; k<4; k++)
				{
					retval.matrix[i][j] += matrix[i][k]*mat.matrix[k][j];
				}
			}
		}
		return retval;
	}

	//Apply Translation to Matrix
	public Matrix4x4 translate(double dx, double dy, double dz)
	{
		Matrix4x4 trn = new Matrix4x4();
		trn.matrix[0][3] = dx;
		trn.matrix[1][3] = dy;
		trn.matrix[2][3] = dz;
		//return trn.mul(this);
		return trn.leftmul(this);
	}

	//Apply Scale to Matrix
	public Matrix4x4 scale(double scaleX, double scaleY, double scaleZ)
	{
		Matrix4x4 scl = new Matrix4x4();
		scl.matrix[0][0] = scaleX;
		scl.matrix[1][1] = scaleY;
		scl.matrix[2][2] = scaleZ;
		//return scl.mul(this);
		return scl.leftmul(this);
	}
	
	//Apply Rotation about X to Matrix
	public Matrix4x4 rotateX(double degtheta)
	{
		double theta = degtheta * Math.PI / 180.0;
		Matrix4x4 rot = new Matrix4x4();
		rot.matrix[1][1] = rot.matrix[2][2] = Math.cos(theta);
		rot.matrix[2][1] = Math.sin(theta);
		rot.matrix[1][2] = -rot.matrix[2][1];
		//return rot.mul(this);
		return rot.leftmul(this);
	}

	//Apply Rotation about Y to Matrix
	public Matrix4x4 rotateY(double degtheta)
	{
		double theta = degtheta * Math.PI / 180.0;
		Matrix4x4 rot = new Matrix4x4();
		rot.matrix[0][0] = rot.matrix[2][2] = Math.cos(theta);
		rot.matrix[0][2] = Math.sin(theta);
		rot.matrix[2][0] = -rot.matrix[0][2];
		//return rot.mul(this);
		return rot.leftmul(this);
	}

	//Apply Rotation about Z to Matrix
	public Matrix4x4 rotateZ(double degtheta)
	{
		double theta = degtheta * Math.PI / 180.0;
		Matrix4x4 rot = new Matrix4x4();
		rot.matrix[0][0] = rot.matrix[1][1] = Math.cos(theta);
		rot.matrix[1][0] = Math.sin(theta);
		rot.matrix[0][1] = -rot.matrix[1][0];
		//return rot.mul(this);
		return rot.leftmul(this);
	}

	//Apply Rotation about arbitrary axis to Matrix
	public Matrix4x4 rotate(Vector3 axis, double degtheta)
	{
		double theta = degtheta * Math.PI / 180.0;
		double x, y, z, s, c, t;
		x = axis.x();
		y = axis.y();
		z = axis.z();
		s = Math.sin(theta);
		//c = Math.cos(theta);
		c = Math.sqrt(1 - s*s); //this may be faster than the previous line
		t = 1 - c;

		return (new Matrix4x4(
			t*x*x + c,   t*x*y + s*z, t*x*z - s*y, 0,
			t*x*y - s*z, t*y*y + c,   t*y*z + s*x, 0,
			t*x*z + s*y, t*y*z - s*x, t*z*z + c,   0,
			0,           0,           0,           1
			)).leftmul(this);
	}

	/*
	public double set(int i, int j, double value)
	{
		if( i < 0 || i > 3 || j < 0 || j > 3 )
			throw new RuntimeException("Index out of bounds");
		else
			return matrix[i][j] = value;
	}
	*/

	//Matrix Transpose
	public Matrix4x4 transpose()
	{
		return new Matrix4x4(
			matrix[0][0], matrix[1][0], matrix[2][0], matrix[3][0], 
			matrix[0][1], matrix[1][1], matrix[2][1], matrix[3][1], 
			matrix[0][2], matrix[1][2], matrix[2][2], matrix[3][2], 
			matrix[0][3], matrix[1][3], matrix[2][3], matrix[3][3] );
	}

	//Matrix Inversion using Cramer's Method
	//Computes Adjoint matrix divided by determinant
	//Code modified from http://www.intel.com/design/pentiumiii/sml/24504301.pdf
	//Turns out we don't need this after all
	public Matrix4x4 inverse()
	{
		double[] mat = new double[16];
		double[] dst = new double[16];

		//Copy all of the elements into the linear array
		int k=0;
		for(int i=0; i<4; i++)
			for(int j=0; j<4; j++)
				mat[k++] = matrix[i][j];

		double[] tmp = new double[12];

		/*temparrayforpairs*/
		double src[] = new double[16];

		/*arrayoftransposesourcematrix*/
		double det;

		/*determinant*/
		/*transposematrix*/
		for(int i=0;i<4;i++) //>
		{
			src[i]=mat[i*4];
			src[i+4]=mat[i*4+1];
			src[i+8]=mat[i*4+2];
			src[i+12]=mat[i*4+3];
		}
			
		/*calculatepairsforfirst8elements(cofactors)*/
		tmp[0]=src[10]*src[15];
		tmp[1]=src[11]*src[14];
		tmp[2]=src[9]*src[15];
		tmp[3]=src[11]*src[13];
		tmp[4]=src[9]*src[14];
		tmp[5]=src[10]*src[13];
		tmp[6]=src[8]*src[15];
		tmp[7]=src[11]*src[12];
		tmp[8]=src[8]*src[14];
		tmp[9]=src[10]*src[12];
		tmp[10]=src[8]*src[13];
		tmp[11]=src[9]*src[12];

		/*calculatefirst8elements(cofactors)*/
		dst[0]=tmp[0]*src[5]+tmp[3]*src[6]+tmp[4]*src[7];
		dst[0]-=tmp[1]*src[5]+tmp[2]*src[6]+tmp[5]*src[7];
		dst[1]=tmp[1]*src[4]+tmp[6]*src[6]+tmp[9]*src[7];
		dst[1]-=tmp[0]*src[4]+tmp[7]*src[6]+tmp[8]*src[7];
		dst[2]=tmp[2]*src[4]+tmp[7]*src[5]+tmp[10]*src[7];
		dst[2]-=tmp[3]*src[4]+tmp[6]*src[5]+tmp[11]*src[7];
		dst[3]=tmp[5]*src[4]+tmp[8]*src[5]+tmp[11]*src[6];
		dst[3]-=tmp[4]*src[4]+tmp[9]*src[5]+tmp[10]*src[6];
		dst[4]=tmp[1]*src[1]+tmp[2]*src[2]+tmp[5]*src[3];
		dst[4]-=tmp[0]*src[1]+tmp[3]*src[2]+tmp[4]*src[3];
		dst[5]=tmp[0]*src[0]+tmp[7]*src[2]+tmp[8]*src[3];
		dst[5]-=tmp[1]*src[0]+tmp[6]*src[2]+tmp[9]*src[3];
		dst[6]=tmp[3]*src[0]+tmp[6]*src[1]+tmp[11]*src[3];
		dst[6]-=tmp[2]*src[0]+tmp[7]*src[1]+tmp[10]*src[3];
		dst[7]=tmp[4]*src[0]+tmp[9]*src[1]+tmp[10]*src[2];
		dst[7]-=tmp[5]*src[0]+tmp[8]*src[1]+tmp[11]*src[2];

		/*calculatepairsforsecond8elements(cofactors)*/
		tmp[0]=src[2]*src[7];
		tmp[1]=src[3]*src[6];
		tmp[2]=src[1]*src[7];
		tmp[3]=src[3]*src[5];
		tmp[4]=src[1]*src[6];
		tmp[5]=src[2]*src[5];
		tmp[6]=src[0]*src[7];
		tmp[7]=src[3]*src[4];
		tmp[8]=src[0]*src[6];
		tmp[9]=src[2]*src[4];
		tmp[10]=src[0]*src[5];
		tmp[11]=src[1]*src[4];

		/*calculatesecond8elements(cofactors)*/
		dst[8]=tmp[0]*src[13]+tmp[3]*src[14]+tmp[4]*src[15];
		dst[8]-=tmp[1]*src[13]+tmp[2]*src[14]+tmp[5]*src[15];
		dst[9]=tmp[1]*src[12]+tmp[6]*src[14]+tmp[9]*src[15];
		dst[9]-=tmp[0]*src[12]+tmp[7]*src[14]+tmp[8]*src[15];
		dst[10]=tmp[2]*src[12]+tmp[7]*src[13]+tmp[10]*src[15];
		dst[10]-=tmp[3]*src[12]+tmp[6]*src[13]+tmp[11]*src[15];
		dst[11]=tmp[5]*src[12]+tmp[8]*src[13]+tmp[11]*src[14];
		dst[11]-=tmp[4]*src[12]+tmp[9]*src[13]+tmp[10]*src[14];
		dst[12]=tmp[2]*src[10]+tmp[5]*src[11]+tmp[1]*src[9];
		dst[12]-=tmp[4]*src[11]+tmp[0]*src[9]+tmp[3]*src[10];
		dst[13]=tmp[8]*src[11]+tmp[0]*src[8]+tmp[7]*src[10];
		dst[13]-=tmp[6]*src[10]+tmp[9]*src[11]+tmp[1]*src[8];
		dst[14]=tmp[6]*src[9]+tmp[11]*src[11]+tmp[3]*src[8];
		dst[14]-=tmp[10]*src[11]+tmp[2]*src[8]+tmp[7]*src[9];
		dst[15]=tmp[10]*src[10]+tmp[4]*src[8]+tmp[9]*src[9];
		dst[15]-=tmp[8]*src[9]+tmp[11]*src[10]+tmp[5]*src[8];

		/*calculatedeterminant*/
		det=src[0]*dst[0]+src[1]*dst[1]+src[2]*dst[2]+src[3]*dst[3];

		/*calculatematrixinverse*/
		det=1/det;

		for(int j=0;j<16;j++) //>
			dst[j]*=det;

		//Copy everything into the output
		Matrix4x4 ret = new Matrix4x4();
		k = 0;
		for(int i=0; i<4; i++)
			for(int j=0; j<4; j++)
				ret.matrix[i][j] = dst[k++];
		return ret;
	}

	//Private initialization function
	private final void initArray()
	{
		matrix = new double[4][];
		matrix[0] = new double[4];
		matrix[1] = new double[4];
		matrix[2] = new double[4];
		matrix[3] = new double[4];
	}

	private double cosFromSine(double cos)
	{
		/*
		sine^2 + cos^2 = 1
		sine^2 = 1 - cos^2
		sine = sqrt(1 - cos^2)
		*/

		return Math.sqrt(1 - cos*cos);
	}
	
	/*package*/ double[][] matrix;

	static double[] retval = new double[3];

}