/**
 * Matrix: Generic matrix class
 * @author Daniel Staudigel
 */

public class Matrix implements Transformation{
	/**
	 * public m, so it can be accessed by others
	 */
	public double m[] = new double[16];

	/**
	 * Initializes an identity matrix
	 */

	public Matrix() {
		//m = new double[16];
		for(int i = 0 ; i < 16 ; ++i ) {
			m[i] = (i%4 == (i/4))?1:0;
		}
	}
	
	/**
	 * Initialize with double array
	 * @param mp Matrix array
	 */

	public Matrix(double mp[]) {
		//m = new double[16];
		if(mp.length != 16) {
			throw new IllegalArgumentException("Invalid Matrix Size (Must be 16)");
		}
		for(int i = 0 ; i < 16 ; ++i) {
			m[i] = mp[i];
		}
	}
	
	/**
	 * Initialize with a double array
	 * @param mp double array
	 * @param transpose Transpose the array?
	 */

	public Matrix(double mp[],boolean transpose) {
		//m = new double[16];
		if(mp.length != 16) {
			throw new IllegalArgumentException("Invalid Matrix Size (Must be 16)");
		}
		for(int i = 0 ; i < 16 ; ++i) {
			if(transpose) {
				m[i] = mp[(i/4)+(i%4)*4];
			} else {
				m[i] = mp[i];
			}
		}
	}
	
	/**
	 * Initialize with a copy of a matrix
	 * @param mp Matrix to copy
	 */
	
	public Matrix(Matrix mp) {
		//m = new double[16];
		
		for(int i = 0 ; i < 16 ; ++i) {
			m[i] = mp.m[i];
		}
	}
	
	/**
	 * Initialize with a copy of a matrix
	 * @param mp Matrix to copy
	 * @param transpose Flag to copy as the transpose of the matrix
	 */
	
	public Matrix(Matrix mp,boolean transpose) {
		//m = new double[16];
		
		for(int i = 0 ; i < 16 ; ++i) {
			if(transpose) {
				m[i] = mp.m[(i/4)+(i%4)*4];
			} else {
				m[i] = mp.m[i];
			}
		}
	}
	
	/**
	 * Create a transformation matrix from a quaternion
	 * @param quat
	 */
	
	public Matrix(Quaternion quat) {
		this.set(Vector.zero,quat);
	}
	
	public void setRotation(Quaternion quat) {
		double wx, wy, wz, xx, yy, yz, xy, xz, zz, x2, y2, z2;
	    
	    //System.out.printf("Quat: %.2f %.2f %.2f %.2f",quat.x,quat.y,quat.z,quat.w);
		
		x2 = quat.x + quat.x;
	    y2 = quat.y + quat.y;
	    z2 = quat.z + quat.z;
	    xx = quat.x * x2;
	    xy = quat.x * y2;
	    xz = quat.x * z2;
	    yy = quat.y * y2;
	    yz = quat.y * z2;
	    zz = quat.z * z2;
	    wx = quat.w * x2;
	    wy = quat.w * y2;
	    wz = quat.w * z2;
	    
	    m[0] = 1.0 - (yy + zz);
	    m[4] = xy - wz;
	    m[8] = xz + wy;
	    
	    m[1] = xy + wz;
	    m[5] = 1.0 - (xx + zz);
	    m[9] = yz - wx;
	    
	    m[2] = xz - wy;
	    m[6] = yz + wx;
	    m[10] = 1.0 - (xx + yy);
	}
	
	public Matrix(Vector omega) {
		m = new double[16];
		double radians = omega.length();
		Vector axis = Vector.mul(omega,1/radians);
		
		double s = Math.sin(radians);
	    double c = Math.cos(radians);
	    double t = (1-c);
	    
	    m[ 0] = (axis.x * axis.x) * t + c;
	    m[ 1] = (axis.y * axis.x) * t + (axis.z * s);
	    m[ 2] = (axis.z * axis.x) * t - (axis.y * s);
	    m[ 3] = 0;
	    
	    m[ 4] = (axis.x * axis.y) * t - (axis.z * s);
	    m[ 5] = (axis.y * axis.y) * t + c;
	    m[ 6] = (axis.z * axis.y) * t + (axis.x * s);
	    m[ 7] = 0;
	    
	    m[ 8] = (axis.x * axis.z) * t + (axis.y * s);
	    m[ 9] = (axis.y * axis.z) * t - (axis.x * s);
	    m[10] = (axis.z * axis.z) * t + c;
	    m[11] = 0;
	    
	    m[12] = m[13] = m[14] = 0;
	    m[15] = 1;
	}
	
	public Matrix(Vector axis,double radians) {
		m = new double[16];
		
		double s = Math.sin(radians);
	    double c = Math.cos(radians);
	    double t = (1-c);
	    
	    m[ 0] = (axis.x * axis.x) * t + c;
	    m[ 1] = (axis.y * axis.x) * t + (axis.z * s);
	    m[ 2] = (axis.z * axis.x) * t - (axis.y * s);
	    m[ 3] = 0;
	    
	    m[ 4] = (axis.x * axis.y) * t - (axis.z * s);
	    m[ 5] = (axis.y * axis.y) * t + c;
	    m[ 6] = (axis.z * axis.y) * t + (axis.x * s);
	    m[ 7] = 0;
	    
	    m[ 8] = (axis.x * axis.z) * t + (axis.y * s);
	    m[ 9] = (axis.y * axis.z) * t - (axis.x * s);
	    m[10] = (axis.z * axis.z) * t + c;
	    m[11] = 0;
	    
	    m[12] = m[13] = m[14] = 0;
	    m[15] = 1;
	}
	
	/**
	 * Generate a rotation matrix from 3 axes
	 * @param x new X axis in terms of old coords.
	 * @param y new Y axis in terms of old coords.
	 * @param z new Z axis in terms of old coords.
	 * @param t translation to add in
	 */
	
	public Matrix(Vector x,Vector y,Vector z,Vector t) {
		m = new double[16];
		m[ 0] = x.x; m[ 4] = x.y; m[ 8] = x.z; m[12] = t.x;
		m[ 1] = y.x; m[ 5] = y.y; m[ 9] = y.z; m[13] = t.y;
		m[ 2] = z.x; m[ 6] = z.y; m[10] = z.z; m[14] = t.z;
		m[ 3] = 0;   m[ 7] = 0;   m[11] = 0;   m[15] = 1;
	}
	
	
	public Matrix(Vector _position, Quaternion _rotation) {
		set(_position,_rotation);
	}

	/**
	 * Multiply two matrices
	 * @param b Mat. to multiply
	 * @return result of multiply
	 */
	
	public Matrix mul(Matrix b) {
		Matrix r = new Matrix();
		
	    r.m[ 0] = m[0]*b.m[ 0] + m[4]*b.m[ 1] + m[ 8]*b.m[2] + m[12]*b.m[3];
	    r.m[ 1] = m[1]*b.m[ 0] + m[5]*b.m[ 1] + m[ 9]*b.m[2] + m[13]*b.m[3];
	    r.m[ 2] = m[2]*b.m[ 0] + m[6]*b.m[ 1] + m[10]*b.m[2] + m[14]*b.m[3];
	    r.m[ 3] = m[3]*b.m[ 0] + m[7]*b.m[ 1] + m[11]*b.m[2] + m[15]*b.m[3];
	    
	    r.m[ 4] = m[0]*b.m[ 4] + m[4]*b.m[ 5] + m[ 8]*b.m[6] + m[12]*b.m[7];
	    r.m[ 5] = m[1]*b.m[ 4] + m[5]*b.m[ 5] + m[ 9]*b.m[6] + m[13]*b.m[7];
	    r.m[ 6] = m[2]*b.m[ 4] + m[6]*b.m[ 5] + m[10]*b.m[6] + m[14]*b.m[7];
	    r.m[ 7] = m[3]*b.m[ 4] + m[7]*b.m[ 5] + m[11]*b.m[6] + m[15]*b.m[7];
	    
	    r.m[ 8] = m[0]*b.m[ 8] + m[4]*b.m[ 9] + m[ 8]*b.m[10] + m[12]*b.m[11];
	    r.m[ 9] = m[1]*b.m[ 8] + m[5]*b.m[ 9] + m[ 9]*b.m[10] + m[13]*b.m[11];
	    r.m[10] = m[2]*b.m[ 8] + m[6]*b.m[ 9] + m[10]*b.m[10] + m[14]*b.m[11];
	    r.m[11] = m[3]*b.m[ 8] + m[7]*b.m[ 9] + m[11]*b.m[10] + m[15]*b.m[11];
	    
	    r.m[12] = m[0]*b.m[12] + m[4]*b.m[13] + m[ 8]*b.m[14] + m[12]*b.m[15];
	    r.m[13] = m[1]*b.m[12] + m[5]*b.m[13] + m[ 9]*b.m[14] + m[13]*b.m[15];
	    r.m[14] = m[2]*b.m[12] + m[6]*b.m[13] + m[10]*b.m[14] + m[14]*b.m[15];
	    r.m[15] = m[3]*b.m[12] + m[7]*b.m[13] + m[11]*b.m[14] + m[15]*b.m[15];
		
		return r;
	}
	
	/**
	 * Print out contents of the matrix
	 */
	
	public void transpose() {
		double t;
		
		t = m[ 4]; m[ 4] = m[ 1]; m[ 1] = t;
		t = m[ 8]; m[ 8] = m[ 2]; m[ 2] = t;
		t = m[12]; m[12] = m[ 3]; m[ 3] = t;
		t = m[ 9]; m[ 9] = m[ 6]; m[ 6] = t;
		t = m[13]; m[13] = m[ 7]; m[ 7] = t;
		t = m[14]; m[14] = m[11]; m[11] = t;
	}
	
	public void addTranslation(Vector v) {
		m[12] += v.x;
		m[13] += v.y;
		m[14] += v.z;
		m[15] = 1;
	}
	
	public void setTranslation(Vector v) {
		m[12] = v.x;
		m[13] = v.y;
		m[14] = v.z;
		m[15] = 1;
	}
	
	public Vector mul(Vector v) {
		return new Vector(
				m[0]*v.x + m[4]*v.y + m[ 8]*v.z + m[12],
				m[1]*v.x + m[5]*v.y + m[ 9]*v.z + m[13],
				m[2]*v.x + m[6]*v.y + m[10]*v.z + m[14]);
	}

	public Vector rotate(Vector v) {
		return new Vector(
				m[0]*v.x + m[4]*v.y + m[ 8]*v.z,
				m[1]*v.x + m[5]*v.y + m[ 9]*v.z,
				m[2]*v.x + m[6]*v.y + m[10]*v.z);
	}

        public Vector unrotate(Vector v) {
          return new Vector(
				m[0]*v.x + m[1]*v.y + m[ 2]*v.z,
				m[4]*v.x + m[5]*v.y + m[ 6]*v.z,
				m[8]*v.x + m[9]*v.y + m[10]*v.z);
        }
	
	/*
	
	public Vector mul(Vector v) {
		return new Vector(
				m[0]*v.x + m[4]*v.y + m[ 8]*v.z + m[12],
				m[1]*v.x + m[5]*v.y + m[ 9]*v.z + m[13],
				m[2]*v.x + m[6]*v.y + m[10]*v.z + m[14]);
	}

	public Vector rotate(Vector v) {
		return new Vector(
				m[0]*v.x + m[4]*v.y + m[ 8]*v.z,
				m[1]*v.x + m[5]*v.y + m[ 9]*v.z,
				m[2]*v.x + m[6]*v.y + m[10]*v.z);
	}
	
	*/
	public Vector translate(Vector v) {
		return new Vector(v.x + m[3],
						  v.y + m[7],
						  v.y + m[11]);
	}

	public Vector transform(Vector v) {
		return mul(v);
	}
	
	public Vector untransform(Vector v) {
//TODO: Create this feature
		return Vector.zero;
	}

	public void set(Vector _position, Quaternion quat) {
		setRotation(quat);
		setTranslation(_position);
	}

	public Vector velocityAt(Vector v) {
		return Vector.zero;
	}
	
	public Vector xAxis() {
		return new Vector(m[0],m[4],m[8]);
	}
	
	public Vector yAxis() {
		return new Vector(m[1],m[5],m[9]);
	}
	
	public Vector zAxis() {
		return new Vector(m[2],m[6],m[10]);
	}
}