package destr.math;

public class Matrix4f {	
	float[]data;
	public Matrix4f(float[]data) {
		if(data.length > 16) 
			System.err.println(this.getClass().getName()+" length: "+data.length+" > 16");
		this.data = data;
	}
	
	public Matrix4f() {
		// По умолчанию единичная матрица
		data = new float[]{
			1, 0, 0, 0,
			0, 1, 0, 0,
			0, 0, 1, 0,
			0, 0, 0, 1,
		};
	}
	
	public float[] toArray() {
		return data;
	}
	
	public Matrix4f clone() {
		return new Matrix4f(data.clone());
	}
	
	//---
	
	public void multiply(Matrix4f other) {
		multiply(other.toArray());
	}
	
	public void multiply(float[]data2) {
		float a, b, c;
		
		a = data[ 4];
		b = data[ 8];
		c = data[12];
		data[12] = data[0]*data2[12]+ a*data2[13]+ b*data2[14]+	c*data2[15];
		data[ 8] = data[0]*data2[ 8]+ a*data2[ 9]+ b*data2[10]+ c*data2[11];
		data[ 4] = data[0]*data2[ 4]+ a*data2[ 5]+ b*data2[ 6]+ c*data2[ 7];
		data[ 0] = data[0]*data2[ 0]+ a*data2[ 1]+ b*data2[ 2]+	c*data2[ 3];
		
		a = data[ 5];
		b = data[ 9];
		c = data[13];
		data[13] = data[1]*data2[12]+ a*data2[13]+ b*data2[14]+	c*data2[15];
		data[ 9] = data[1]*data2[ 8]+ a*data2[ 9]+ b*data2[10]+ c*data2[11];
		data[ 5] = data[1]*data2[ 4]+ a*data2[ 5]+ b*data2[ 6]+ c*data2[ 7];
		data[ 1] = data[1]*data2[ 0]+ a*data2[ 1]+ b*data2[ 2]+	c*data2[ 3];
		
		a = data[ 6];
		b = data[10];
		c = data[14];
		data[14] = data[2]*data2[12]+ a*data2[13]+ b*data2[14]+ c*data2[15];
		data[10] = data[2]*data2[ 8]+ a*data2[ 9]+ b*data2[10]+ c*data2[11];
		data[ 6] = data[2]*data2[ 4]+ a*data2[ 5]+ b*data2[ 6]+ c*data2[ 7];
		data[ 2] = data[2]*data2[ 0]+ a*data2[ 1]+ b*data2[ 2]+ c*data2[ 3];
		
		a = data[ 7];
		b = data[11];
		c = data[15];
		data[15] = data[3]*data2[12]+ a*data2[13]+ b*data2[14]+ c*data2[15];
		data[11] = data[3]*data2[ 8]+ a*data2[ 9]+ b*data2[10]+ c*data2[11];
		data[ 7] = data[3]*data2[ 4]+ a*data2[ 5]+ b*data2[ 6]+ c*data2[ 7];
		data[ 3] = data[3]*data2[ 0]+ a*data2[ 1]+ b*data2[ 2]+ c*data2[ 3];
	}
	
	public void moveForward(float delta){
		float mDelta = delta / (float)Math.sqrt(data[2]*data[2] + data[6]*data[6] + data[10]*data[10]);
		data[12] += data[ 2] * mDelta;
		data[13] += data[ 6] * mDelta;
		data[14] += data[10] * mDelta;
	}
	public void moveBack(float delta){
		float mDelta = delta / (float)Math.sqrt(data[2]*data[2] + data[6]*data[6] + data[10]*data[10]);
		data[12] -= data[ 2] * mDelta;
		data[13] -= data[ 6] * mDelta;
		data[14] -= data[10] * mDelta;
	}
	
	public void moveUp(float delta){
		float mDelta = delta / (float)Math.sqrt(data[2]*data[2] + data[6]*data[6] + data[10]*data[10]);
		data[12] += data[ 1] * mDelta;
		data[13] += data[ 5] * mDelta;
		data[14] += data[ 9] * mDelta;
	}
	public void moveDown(float delta){
		float mDelta = delta / (float)Math.sqrt(data[2]*data[2] + data[6]*data[6] + data[10]*data[10]);
		data[12] -= data[ 1] * mDelta;
		data[13] -= data[ 5] * mDelta;
		data[14] -= data[ 9] * mDelta;
	}
	
	public void moveLeft(float delta){
		float mDelta = delta / (float)Math.sqrt(data[2]*data[2] + data[6]*data[6] + data[10]*data[10]);
		data[12] += data[ 0] * mDelta;
		data[13] += data[ 4] * mDelta;
		data[14] += data[ 8] * mDelta;
	}
	public void moveRight(float delta){
		float mDelta = delta / (float)Math.sqrt(data[2]*data[2] + data[6]*data[6] + data[10]*data[10]);
		data[12] -= data[ 0] * mDelta;
		data[13] -= data[ 4] * mDelta;
		data[14] -= data[ 8] * mDelta;
	}
	
	public void translite(float[] vector){
		this.multiply(new float[]{
			1,			0,			0,			0,
			0,			1,			0,			0,
			0,			0,			1,			0,
			vector[0],	vector[1],	vector[2],	1,
		});
		
	}
	
	public String toString(){
		return super.toString()+'\n'+
		data[ 0] + ", " + data[ 1] + ", " + data[ 2] + ", " + data[ 3] + ",\n" + 
		data[ 4] + ", " + data[ 5] + ", " + data[ 6] + ", " + data[ 7] + ",\n" +
		data[ 8] + ", " + data[ 9] + ", " + data[10] + ", " + data[11] + ",\n" +
		data[12] + ", " + data[13] + ", " + data[14] + ", " + data[15] + ",\n" ;
	}
	
	
	public static Matrix4f makeFrustum(
		float l,// left
		float r,// right
		float b,// button
		float t,// top
		float n,// near
		float f // far
	) {
		
		float	t1 = 2.0f * n,
				t2 = r - l,
				t3 = t - b,
				t4 = f - n;
		
		return new Matrix4f(new float[]{
			t1/t2,		0,			0,			0,
			0,			t1/t3,		0,			0,
			(r+l)/t2,	(t+b)/t3,	(-f-n)/t4,	-1,
			0,			0,			-t1*f/t4,	0,
		});
	}
	
	public static float[] lookAt(float[] position, float[] center3D, float[] upVector3D){
		/*
		float[] back = Vector3f.normalize(Vector3f.subtract(center3D, position));
		float[]	left = Vector3f.normalize(Vector3f.normal(back, upVector3D));
		float[]	up = Vector3f.normal(left, back);
		
		return multiply(new float[]{
			left[0],	up[0],	-back[0],	0,
			left[1],	up[1],	-back[1],	0,
			left[2],	up[2],	-back[2],	0,
			0,			0,		0,			1,
		}, new float[]{
			1,0,0,0,
			0,1,0,0,
			0,0,1,0,
			-position[0],-position[1],-position[2],1,
		});*/
		
		//http://gamedev.chat.ru/articles/a0029.html
		float[] fwd = Vector3f.normalize(Vector3f.subtract( position, center3D));
		float[]	left = Vector3f.normalize(Vector3f.normal( upVector3D, fwd));
		float[]	up = Vector3f.normal(fwd, left);
		return new float[]{
			left[0],	up[0],	fwd[0],	0,
			left[1],	up[1],	fwd[1],	0,
			left[2],	up[2],	fwd[2],	0,
			-Vector3f.dot(position, left),
			-Vector3f.dot(position, up),
			-Vector3f.dot(position, fwd),1,
		};
	}
	
	public static Matrix4f makeByCols(float[]a, float[]b, float[]c){
		return new Matrix4f(new float[]{
			a[0],	b[0],	c[0],	0,
			a[1],	b[1],	c[1],	0,
			a[2],	b[2],	c[2],	0,
			0,	0,	0,	1,
		});
	}
	
	public static Matrix4f multiply(Matrix4f matrix1, Matrix4f matrix2){
		return new Matrix4f(multiply(matrix1.toArray(), matrix2.toArray()));
	}
	
	public static float[] multiply(float[]data1, float[]data2){
		return new float []{
			data1[0]*data2[ 0]+data1[4]*data2[ 1]+data1[ 8]*data2[ 2]+data1[12]*data2[ 3],
			data1[1]*data2[ 0]+data1[5]*data2[ 1]+data1[ 9]*data2[ 2]+data1[13]*data2[ 3],
			data1[2]*data2[ 0]+data1[6]*data2[ 1]+data1[10]*data2[ 2]+data1[14]*data2[ 3],
			data1[3]*data2[ 0]+data1[7]*data2[ 1]+data1[11]*data2[ 2]+data1[15]*data2[ 3],
			data1[0]*data2[ 4]+data1[4]*data2[ 5]+data1[ 8]*data2[ 6]+data1[12]*data2[ 7],
			data1[1]*data2[ 4]+data1[5]*data2[ 5]+data1[ 9]*data2[ 6]+data1[13]*data2[ 7],					
			data1[2]*data2[ 4]+data1[6]*data2[ 5]+data1[10]*data2[ 6]+data1[14]*data2[ 7],					
			data1[3]*data2[ 4]+data1[7]*data2[ 5]+data1[11]*data2[ 6]+data1[15]*data2[ 7],
			data1[0]*data2[ 8]+data1[4]*data2[ 9]+data1[ 8]*data2[10]+data1[12]*data2[11],					
			data1[1]*data2[ 8]+data1[5]*data2[ 9]+data1[ 9]*data2[10]+data1[13]*data2[11],
			data1[2]*data2[ 8]+data1[6]*data2[ 9]+data1[10]*data2[10]+data1[14]*data2[11],					
			data1[3]*data2[ 8]+data1[7]*data2[ 9]+data1[11]*data2[10]+data1[15]*data2[11],
			data1[0]*data2[12]+data1[4]*data2[13]+data1[ 8]*data2[14]+data1[12]*data2[15],
			data1[1]*data2[12]+data1[5]*data2[13]+data1[ 9]*data2[14]+data1[13]*data2[15],
			data1[2]*data2[12]+data1[6]*data2[13]+data1[10]*data2[14]+data1[14]*data2[15],
			data1[3]*data2[12]+data1[7]*data2[13]+data1[11]*data2[14]+data1[15]*data2[15],
		};
	}
}