package speiger.src.coreengine.math.vector.matrix;

import java.nio.ByteBuffer;
import java.nio.FloatBuffer;
import java.util.Arrays;

import speiger.src.coreengine.math.MathUtils;
import speiger.src.coreengine.math.vector.floats.Vec3f;
import speiger.src.coreengine.math.vector.floats.Vec4f;
import speiger.src.coreengine.math.vector.quaternion.Quaternion;

public class Matrix4f {
	public static final int PROPERTY_UNKNOWN = 0;
	public static final int PROPERTY_IDENTITY = 1 << 0;
	public static final int PROPERTY_AFFINE = 1 << 1;
	public static final int PROPERTY_TRANSLATION = 1 << 2;
	public static final int PROPERTY_SCALE = 1 << 3;
	public static final int PROPERTY_ROTATION = 1 << 4;
	
	public static final int M00 = 0;
	public static final int M01 = 1;
	public static final int M02 = 2;
	public static final int M03 = 3;
	public static final int M10 = 4;
	public static final int M11 = 5;
	public static final int M12 = 6;
	public static final int M13 = 7;
	public static final int M20 = 8;
	public static final int M21 = 9;
	public static final int M22 = 10;
	public static final int M23 = 11;
	public static final int M30 = 12;
	public static final int M31 = 13;
	public static final int M32 = 14;
	public static final int M33 = 15;
	
	int properties;
	float[] data = new float[16];
	
	public Matrix4f() {
		setIdentity();
	}
	public Matrix4f(float[] data) {
		System.arraycopy(data, 0, this.data, 0, 16);
		evaluateProps();
	}
	public Matrix4f(Matrix4f other) {
		this(other.data);
	}
	
	public Matrix4f store(FloatBuffer buffer) {
		buffer.put(data);
		return this;
	}
	
	public Matrix4f store(ByteBuffer buffer) {
		for(int i = 0;i < 16;i++)
			buffer.putFloat(data[i]);
		return this;
	}
	
	public Matrix4f store(Matrix4f other) {
		System.arraycopy(data, 0, other.data, 0, 16);
		return this;
	}
	
	public Matrix4f load(FloatBuffer buffer) {
		buffer.get(data);
		return evaluateProps();
	}
	
	public Matrix4f load(ByteBuffer buffer) {
		for(int i = 0;i < 16;i++)
			data[i] = buffer.getFloat();
		return evaluateProps();
	}
	
	public Matrix4f load(Matrix4f other) {
		System.arraycopy(other.data, 0, data, 0, 16);
		return evaluateProps();
	}
	
	@Override
	public String toString() {
		StringBuilder buf = new StringBuilder().append("\n");
		buf.append("x0=").append(data[M00]).append(' ').append("y0=").append(data[M10]).append(' ').append("z0=").append(data[M20]).append(' ').append("w0=").append(data[M30]).append('\n');
		buf.append("x1=").append(data[M01]).append(' ').append("y1=").append(data[M11]).append(' ').append("z1=").append(data[M21]).append(' ').append("w1=").append(data[M31]).append('\n');
		buf.append("x2=").append(data[M02]).append(' ').append("y2=").append(data[M12]).append(' ').append("z2=").append(data[M22]).append(' ').append("w2=").append(data[M32]).append('\n');
		buf.append("x3=").append(data[M03]).append(' ').append("y3=").append(data[M13]).append(' ').append("z3=").append(data[M23]).append(' ').append("w3=").append(data[M33]).append('\n');
		return buf.toString();
	}
	
	@Override
	public boolean equals(Object obj) {
		return obj instanceof Matrix4f matrix && Arrays.equals(data, matrix.data);
	}
	
	public float[] getData() { return data; }
	public float get(int index) { return data[index]; }
	public float get(int column, int row) { return data[(column * 4) + row]; }
	public int properties() { return properties; }
	
	public Matrix4f set(int column, int row, float value) {
		data[(column * 4) + row] = value;
		properties = PROPERTY_UNKNOWN;
		return this;
	}
	
	public Matrix4f set(int index, float value) {
		data[index] = value;
		properties = PROPERTY_UNKNOWN;
		return this;
	}
	
	public Matrix4f evaluateProps() {
		int properties = PROPERTY_UNKNOWN;
		if(data[M03] == 0F && data[M13] == 0F && data[M23] == 0F && data[M33] == 1F) {
			properties |= PROPERTY_AFFINE;
			if(data[M01] == 0F && data[M02] == 0F && data[M10] == 0F && data[M12] == 0F && data[M20] == 0F && data[M21] == 0F) {
				properties |= PROPERTY_TRANSLATION;
				if(data[M00] == 1F && data[M11] == 1F && data[M22] == 1F) {
					if(data[M30] == 0F && data[M31] == 0F && data[M32] == 0F) {
						properties |= PROPERTY_IDENTITY;
					}
				}
				else {
					properties |= PROPERTY_SCALE;
				}
			}
			else {
				properties |= PROPERTY_ROTATION;
			}
		}
		this.properties = properties;
		return this;
	}
	
	public Matrix4f setIdentity() {
		if((properties & PROPERTY_IDENTITY) == PROPERTY_IDENTITY) return this;
		Arrays.fill(data, 0F);
		data[M00] = 1F;
		data[M11] = 1F;
		data[M22] = 1F;
		data[M33] = 1F;
		properties = PROPERTY_IDENTITY | PROPERTY_TRANSLATION | PROPERTY_AFFINE;
		return this;
	}
	
	public Matrix4f invert() {
		float determinant = determinant();
		if(determinant != 0F) {
			float determinant_inv = 1F / determinant;
			float t00 = determinant3x3(data[M11], data[M12], data[M13], data[M21], data[M22], data[M23], data[M31], data[M32], data[M33]);
			float t01 = -determinant3x3(data[M10], data[M12], data[M13], data[M20], data[M22], data[M23], data[M30], data[M32], data[M33]);
			float t02 = determinant3x3(data[M10], data[M11], data[M13], data[M20], data[M21], data[M23], data[M30], data[M31], data[M33]);
			float t03 = -determinant3x3(data[M10], data[M11], data[M12], data[M20], data[M21], data[M22], data[M30], data[M31], data[M32]);
			float t10 = -determinant3x3(data[M01], data[M02], data[M03], data[M21], data[M22], data[M23], data[M31], data[M32], data[M33]);
			float t11 = determinant3x3(data[M00], data[M02], data[M03], data[M20], data[M22], data[M23], data[M30], data[M32], data[M33]);
			float t12 = -determinant3x3(data[M00], data[M01], data[M03], data[M20], data[M21], data[M23], data[M30], data[M31], data[M33]);
			float t13 = determinant3x3(data[M00], data[M01], data[M02], data[M20], data[M21], data[M22], data[M30], data[M31], data[M32]);
			float t20 = determinant3x3(data[M01], data[M02], data[M03], data[M11], data[M12], data[M13], data[M31], data[M32], data[M33]);
			float t21 = -determinant3x3(data[M00], data[M02], data[M03], data[M10], data[M12], data[M13], data[M30], data[M32], data[M33]);
			float t22 = determinant3x3(data[M00], data[M01], data[M03], data[M10], data[M11], data[M13], data[M30], data[M31], data[M33]);
			float t23 = -determinant3x3(data[M00], data[M01], data[M02], data[M10], data[M11], data[M12], data[M30], data[M31], data[M32]);
			float t30 = -determinant3x3(data[M01], data[M02], data[M03], data[M11], data[M12], data[M13], data[M21], data[M22], data[M23]);
			float t31 = determinant3x3(data[M00], data[M02], data[M03], data[M10], data[M12], data[M13], data[M20], data[M22], data[M23]);
			float t32 = -determinant3x3(data[M00], data[M01], data[M03], data[M10], data[M11], data[M13], data[M20], data[M21], data[M23]);
			float t33 = determinant3x3(data[M00], data[M01], data[M02], data[M10], data[M11], data[M12], data[M20], data[M21], data[M22]);
			
			data[M00] = t00 * determinant_inv;
			data[M11] = t11 * determinant_inv;
			data[M22] = t22 * determinant_inv;
			data[M33] = t33 * determinant_inv;
			data[M01] = t10 * determinant_inv;
			data[M10] = t01 * determinant_inv;
			data[M20] = t02 * determinant_inv;
			data[M02] = t20 * determinant_inv;
			data[M12] = t21 * determinant_inv;
			data[M21] = t12 * determinant_inv;
			data[M03] = t30 * determinant_inv;
			data[M30] = t03 * determinant_inv;
			data[M13] = t31 * determinant_inv;
			data[M31] = t13 * determinant_inv;
			data[M32] = t23 * determinant_inv;
			data[M23] = t32 * determinant_inv;
			evaluateProps();
		}
		return this;
	}
	
	public Matrix4f flip() {
		for(int i = 0;i<16;i++) {
			data[i] = -data[i];			
		}
		return evaluateProps();
	}
	
	public Matrix4f negate() {
		Arrays.fill(data, 0F);
		return evaluateProps();
	}
	
	public Matrix4f add(Matrix4f other) {
		for(int i = 0;i<16;i++) {
			data[i] += other.data[i];			
		}
		return evaluateProps();
	}
	
	public Matrix4f sub(Matrix4f other) {
		for(int i = 0;i<16;i++) {
			data[i] -= other.data[i];			
		}
		return evaluateProps();
	}
	
	public Matrix4f multiply(Matrix4f other) {
		float data0 = data[M00] * other.data[M00] + data[M10] * other.data[M01] + data[M20] * other.data[M02] + data[M30] * other.data[M03];
		float data1 = data[M01] * other.data[M00] + data[M11] * other.data[M01] + data[M21] * other.data[M02] + data[M31] * other.data[M03];
		float data2 = data[M02] * other.data[M00] + data[M12] * other.data[M01] + data[M22] * other.data[M02] + data[M32] * other.data[M03];
		float data3 = data[M03] * other.data[M00] + data[M13] * other.data[M01] + data[M23] * other.data[M02] + data[M33] * other.data[M03];
		float data4 = data[M00] * other.data[M10] + data[M10] * other.data[M11] + data[M20] * other.data[M12] + data[M30] * other.data[M13];
		float data5 = data[M01] * other.data[M10] + data[M11] * other.data[M11] + data[M21] * other.data[M12] + data[M31] * other.data[M13];
		float data6 = data[M02] * other.data[M10] + data[M12] * other.data[M11] + data[M22] * other.data[M12] + data[M32] * other.data[M13];
		float data7 = data[M03] * other.data[M10] + data[M13] * other.data[M11] + data[M23] * other.data[M12] + data[M33] * other.data[M13];
		float data8 = data[M00] * other.data[M20] + data[M10] * other.data[M21] + data[M20] * other.data[M22] + data[M30] * other.data[M23];
		float data9 = data[M01] * other.data[M20] + data[M11] * other.data[M21] + data[M21] * other.data[M22] + data[M31] * other.data[M23];
		float data10 = data[M02] * other.data[M20] + data[M12] * other.data[M21] + data[M22] * other.data[M22] + data[M32] * other.data[M23];
		float data11 = data[M03] * other.data[M20] + data[M13] * other.data[M21] + data[M23] * other.data[M22] + data[M33] * other.data[M23];
		float data12 = data[M00] * other.data[M30] + data[M10] * other.data[M31] + data[M20] * other.data[M32] + data[M30] * other.data[M33];
		float data13 = data[M01] * other.data[M30] + data[M11] * other.data[M31] + data[M21] * other.data[M32] + data[M31] * other.data[M33];
		float data14 = data[M02] * other.data[M30] + data[M12] * other.data[M31] + data[M22] * other.data[M32] + data[M32] * other.data[M33];
		float data15 = data[M03] * other.data[M30] + data[M13] * other.data[M31] + data[M23] * other.data[M32] + data[M33] * other.data[M33];
		data[M00] = data0;
		data[M01] = data1;
		data[M02] = data2;
		data[M03] = data3;
		data[M10] = data4;
		data[M11] = data5;
		data[M12] = data6;
		data[M13] = data7;
		data[M20] = data8;
		data[M21] = data9;
		data[M22] = data10;
		data[M23] = data11;
		data[M30] = data12;
		data[M31] = data13;
		data[M32] = data14;
		data[M33] = data15;
		return evaluateProps();
	}
	
	public Matrix4f transpose() {
		float data0 = data[M00];
		float data1 = data[M10];
		float data2 = data[M20];
		float data3 = data[M30];
		float data4 = data[M01];
		float data5 = data[M11];
		float data6 = data[M21];
		float data7 = data[M31];
		float data8 = data[M02];
		float data9 = data[M12];
		float data10 = data[M22];
		float data11 = data[M32];
		float data12 = data[M03];
		float data13 = data[M13];
		float data14 = data[M23];
		float data15 = data[M33];
		
		data[M00] = data0;
		data[M01] = data1;
		data[M02] = data2;
		data[M03] = data3;
		data[M10] = data4;
		data[M11] = data5;
		data[M12] = data6;
		data[M13] = data7;
		data[M20] = data8;
		data[M21] = data9;
		data[M22] = data10;
		data[M23] = data11;
		data[M30] = data12;
		data[M31] = data13;
		data[M32] = data14;
		data[M33] = data15;
		return evaluateProps();
	}
	
	public Matrix4f transpose3x3() {
		float nm00 = data[M00];
		float nm01 = data[M10];
		float nm02 = data[M20];
		float nm10 = data[M01];
		float nm11 = data[M11];
		float nm12 = data[M21];
		float nm20 = data[M02];
		float nm21 = data[M12];
		float nm22 = data[M22];
		data[M00] = nm00;
		data[M01] = nm01;
		data[M02] = nm02;
		data[M10] = nm10;
		data[M11] = nm11;
		data[M12] = nm12;
		data[M20] = nm20;
		data[M21] = nm21;
		data[M22] = nm22;
		return evaluateProps();
	}
	
	public Matrix4f decompose(Vec3f position, Quaternion rotation, Vec3f scale) {
		position.set(get(3, 0), get(3, 1), get(3, 2));
		rotation.set(this);
		scale.set(get(0, 0), get(1, 1), get(2, 2));
		return this;
	}
	
	public Matrix4f setTranslation(float x, float y, float z) {
		data[M30] = x;
		data[M31] = y;
		data[M32] = z;
		if(x != 0F || y != 0F || z != 0F) properties |= PROPERTY_TRANSLATION;
		else properties &= PROPERTY_TRANSLATION;
		return this;
	}
	
	public Matrix4f translateZ(float z) {
		if(z == 0F) return this;
		if((properties & (PROPERTY_ROTATION | PROPERTY_SCALE)) == 0) {
			data[M32] += z;
			return evaluateProps();
		}
		if((properties & PROPERTY_ROTATION) == 0) {
			data[M32] += data[M22] * z;
			evaluateProps();
		}
		data[M30] += data[M20] * z;
		data[M31] += data[M21] * z;
		data[M32] += data[M22] * z;
		data[M33] += data[M23] * z;
		return evaluateProps();
	}
	
	public Matrix4f translate(float x, float y) {
		if(x == 0F && y == 0F) return this;
		if((properties & (PROPERTY_ROTATION | PROPERTY_SCALE)) == 0) {
			data[M30] += x;
			data[M31] += y;
			return evaluateProps();
		}
		if((properties & PROPERTY_ROTATION) == 0) {
			data[M30] += data[M00] * x;
			data[M31] += data[M11] * y;
			evaluateProps();
		}
		data[M30] += data[M00] * x + data[M10] * y;
		data[M31] += data[M01] * x + data[M11] * y;
		data[M32] += data[M02] * x + data[M12] * y;
		data[M33] += data[M03] * x + data[M13] * y;
		return evaluateProps();
	}
	
	public Matrix4f translate(float x, float y, float z) {
		if(x == 0F && y == 0F && z == 0F) return this;
		if((properties & (PROPERTY_ROTATION | PROPERTY_SCALE)) == 0) {
			data[M30] += x;
			data[M31] += y;
			data[M32] += z;
			properties |= PROPERTY_TRANSLATION;
			return evaluateProps();
		}
		if((properties & PROPERTY_ROTATION) == 0) {
			data[M30] += data[M00] * x;
			data[M31] += data[M11] * y;
			data[M32] += data[M22] * z;
			evaluateProps();
		}
		data[M30] += data[M00] * x + data[M10] * y + data[M20] * z;
		data[M31] += data[M01] * x + data[M11] * y + data[M21] * z;
		data[M32] += data[M02] * x + data[M12] * y + data[M22] * z;
		data[M33] += data[M03] * x + data[M13] * y + data[M23] * z;
		return evaluateProps();
	}
	
	public Matrix4f rotateX(float angle) { return rotate((float)Math.toRadians(angle), 1F, 0F, 0F); }
	public Matrix4f rotateY(float angle) { return rotate((float)Math.toRadians(angle), 0F, 1F, 0F); }
	public Matrix4f rotateZ(float angle) { return rotate((float)Math.toRadians(angle), 0F, 0F, 1F); }
	public Matrix4f rotateRadX(float angle) { return rotate(angle, 1F, 0F, 0F); }
	public Matrix4f rotateRadY(float angle) { return rotate(angle, 0F, 1F, 0F); }
	public Matrix4f rotateRadZ(float angle) { return rotate(angle, 0F, 0F, 1F); }
	
	public Matrix4f rotate(float angle, float x, float y, float z) {
		float c = MathUtils.cos(angle);
		float s = MathUtils.sin(angle);
		float oneminusc = 1.0F - c;
		float xy = x * y;
		float yz = y * z;
		float xz = x * z;
		float xs = x * s;
		float ys = y * s;
		float zs = z * s;
		
		float f00 = x * x * oneminusc + c;
		float f01 = xy * oneminusc + zs;
		float f02 = xz * oneminusc - ys;
		
		float f10 = xy * oneminusc - zs;
		float f11 = y * y * oneminusc + c;
		float f12 = yz * oneminusc + xs;
		
		float f20 = xz * oneminusc + ys;
		float f21 = yz * oneminusc - xs;
		float f22 = z * z * oneminusc + c;
		
		float t00 = data[M00] * f00 + data[M10] * f01 + data[M20] * f02;
		float t01 = data[M01] * f00 + data[M11] * f01 + data[M21] * f02;
		float t02 = data[M02] * f00 + data[M12] * f01 + data[M22] * f02;
		float t03 = data[M03] * f00 + data[M13] * f01 + data[M23] * f02;
		float t10 = data[M00] * f10 + data[M10] * f11 + data[M20] * f12;
		float t11 = data[M01] * f10 + data[M11] * f11 + data[M21] * f12;
		float t12 = data[M02] * f10 + data[M12] * f11 + data[M22] * f12;
		float t13 = data[M03] * f10 + data[M13] * f11 + data[M23] * f12;
		data[M20] = data[M00] * f20 + data[M10] * f21 + data[M20] * f22;
		data[M21] = data[M01] * f20 + data[M11] * f21 + data[M21] * f22;
		data[M22] = data[M02] * f20 + data[M12] * f21 + data[M22] * f22;
		data[M23] = data[M03] * f20 + data[M13] * f21 + data[M23] * f22;
		data[M00] = t00;
		data[M01] = t01;
		data[M02] = t02;
		data[M03] = t03;
		data[M10] = t10;
		data[M11] = t11;
		data[M12] = t12;
		data[M13] = t13;
		return evaluateProps();
	}
	
	public Matrix4f rotate(Quaternion rotation) {
		float w2 = rotation.getW() * rotation.getW();
		float x2 = rotation.getX() * rotation.getX();
		float y2 = rotation.getY() * rotation.getY();
		float z2 = rotation.getZ() * rotation.getZ();
		float zw = rotation.getZ() * rotation.getW();
		float dzw = zw + zw;
		float xy = rotation.getX() * rotation.getY();
		float dxy = xy + xy;
		float xz = rotation.getX() * rotation.getZ();
		float dxz = xz + xz;
		float yw = rotation.getY() * rotation.getW();
		float dyw = yw + yw;
		float yz = rotation.getY() * rotation.getZ();
		float dyz = yz + yz;
		float xw = rotation.getX() * rotation.getW();
		float dxw = xw + xw;
		float rm00 = w2 + x2 - z2 - y2;
		float rm01 = dxy + dzw;
		float rm02 = dxz - dyw;
		float rm10 = -dzw + dxy;
		float rm11 = y2 - z2 + w2 - x2;
		float rm12 = dyz + dxw;
		float rm20 = dyw + dxz;
		float rm21 = dyz - dxw;
		float rm22 = z2 - y2 - x2 + w2;
		float nm00 = data[M00] * rm00 + data[M10] * rm01 + data[M20] * rm02;
		float nm01 = data[M01] * rm00 + data[M11] * rm01 + data[M21] * rm02;
		float nm02 = data[M02] * rm00 + data[M12] * rm01 + data[M22] * rm02;
		float nm03 = data[M03] * rm00 + data[M13] * rm01 + data[M23] * rm02;
		float nm10 = data[M00] * rm10 + data[M10] * rm11 + data[M20] * rm12;
		float nm11 = data[M01] * rm10 + data[M11] * rm11 + data[M21] * rm12;
		float nm12 = data[M02] * rm10 + data[M12] * rm11 + data[M22] * rm12;
		float nm13 = data[M03] * rm10 + data[M13] * rm11 + data[M23] * rm12;
		data[M20] = data[M00] * rm20 + data[M10] * rm21 + data[M20] * rm22;
		data[M21] = data[M01] * rm20 + data[M11] * rm21 + data[M21] * rm22;
		data[M22] = data[M02] * rm20 + data[M12] * rm21 + data[M22] * rm22;
		data[M23] = data[M03] * rm20 + data[M13] * rm21 + data[M23] * rm22;
		data[M00] = nm00;
		data[M01] = nm01;
		data[M02] = nm02;
		data[M03] = nm03;
		data[M10] = nm10;
		data[M11] = nm11;
		data[M12] = nm12;
		data[M13] = nm13;
		return evaluateProps();
	}
	
	public Matrix4f setRotation(Matrix4f source) {
		data[M00] = source.data[M00];
		data[M01] = source.data[M01];
		data[M02] = source.data[M02];
		data[M10] = source.data[M10];
		data[M11] = source.data[M11];
		data[M12] = source.data[M12];
		data[M20] = source.data[M20];
		data[M21] = source.data[M21];
		data[M22] = source.data[M22];
		return evaluateProps();
	}
	
	public Matrix4f setBillboard(Matrix4f source) {
		data[M00] = source.data[M00];
		data[M01] = source.data[M10];
		data[M02] = source.data[M20];
		data[M10] = source.data[M01];
		data[M11] = source.data[M11];
		data[M12] = source.data[M21];
		data[M20] = source.data[M02];
		data[M21] = source.data[M12];
		data[M22] = source.data[M22];
		return evaluateProps();
	}
	
	public Matrix4f scale(float value) { return scale(value, value, value); }
	
	public Matrix4f scale(float x, float y, float z) {
		if(x != 1F) {
			data[M00] *= x;
			data[M01] *= x;
			data[M02] *= x;
			data[M03] *= x;			
		}
		if(y != 1F) {
			data[M10] *= y;
			data[M11] *= y;
			data[M12] *= y;
			data[M13] *= y;			
		}
		if(z != 1F) {
			data[M20] *= z;
			data[M21] *= z;
			data[M22] *= z;
			data[M23] *= z;
		}
		return evaluateProps();
	}
	
	public Matrix4f unscale(float scale) { return unscale(scale, scale, scale); }
	
	public Matrix4f unscale(float x, float y, float z) {
		if(x != 1F) {
			data[M00] /= x;
			data[M01] /= x;
			data[M02] /= x;
			data[M03] /= x;			
		}
		if(y != 1F) {
			data[M10] /= y;
			data[M11] /= y;
			data[M12] /= y;
			data[M13] /= y;
		}
		if(z != 1F) {
			data[M20] /= z;
			data[M21] /= z;
			data[M22] /= z;
			data[M23] /= z;
		}
		return evaluateProps();
	}
	
	public Matrix4f setPerspective(float fovy, float aspect, float zNear, float zFar) {
		float h = (float)Math.tan(fovy * 0.5F);
		set(0, 0, 1F / (h * aspect)).set(1, 1, 1F / h);
		boolean farInf = zFar > 0 && Float.isInfinite(zFar);
		boolean nearInf = zNear > 0 && Float.isInfinite(zNear);
		if(farInf) {
			set(2, 2, 0.999999F).set(3, 2, 1.999999F * zNear);
		}
		else if(nearInf) {
			float e = 1E-6F;
			set(2, 2, 1F - e).set(3, 2, (2F - e) * zFar);
		}
		else {
			set(2, 2, (zFar + zNear) / (zNear - zFar)).set(3, 2, (zFar + zFar) * zNear / (zNear - zFar));
		}
		return set(2, 3, -1F).evaluateProps();
	}
	
	public Matrix4f getTranslation(Vec3f vec) {
		vec.set(data[M30], data[M31], data[M32]);
		return this;
	}
	
	public Matrix4f getScale(Vec3f vec) {
		vec.x((float)Math.sqrt(data[M00] * data[M00] + data[M01] * data[M01] + data[M02] * data[M02]));
		vec.y((float)Math.sqrt(data[M10] * data[M10] + data[M11] * data[M11] + data[M12] * data[M12]));
		vec.z((float)Math.sqrt(data[M20] * data[M20] + data[M21] * data[M21] + data[M22] * data[M22]));
		return this;
	}
	
	public void transformOffset(float x, float y, float z, float xOff, float yOff, float zOff, ITransformOutput output) {
		if((properties & PROPERTY_IDENTITY) != 0) {
			output.accept(x + xOff, y + yOff, z + zOff);
			return;
		}
		if((properties & PROPERTY_ROTATION) != 0) {
			output.accept(fma(M00, M10, M20, M30, x, y, z, 1F) + xOff, fma(M01, M11, M21, M31, x, y, z, 1F) + yOff, fma(M02, M12, M22, M32, x, y, z, 1F) + zOff);
			return;
		}
		if((properties & PROPERTY_SCALE) != 0) {
			output.accept(Math.fma(data[M00], x, data[M30]) + xOff, Math.fma(data[M01], y, data[M31]) + yOff, Math.fma(data[M02], z, data[M32]) + zOff);
			return;
		}
		output.accept(x + data[M30] + xOff, y + data[M31] + yOff, z + data[M32] + zOff);
	}
	
	public void transform(float x, float y, float z, boolean pos, ITransformOutput output) {
		if((properties & PROPERTY_IDENTITY) != 0) {
			output.accept(x, y, z);
			return;
		}
		if((properties & PROPERTY_ROTATION) != 0) {
			float w = pos ? 1F : 0F;
			output.accept(fma(M00, M10, M20, M30, x, y, z, w), fma(M01, M11, M21, M31, x, y, z, w), fma(M02, M12, M22, M32, x, y, z, w));
			return;
		}
		if((properties & PROPERTY_SCALE) != 0) {
			if(pos) output.accept(Math.fma(data[M00], x, data[M30]), Math.fma(data[M01], y, data[M31]), Math.fma(data[M02], z, data[M32]));
			else output.accept(data[M00] * x, data[M01] * y, data[M02] * z);
			return;
		}
		if(pos) {
			output.accept(x + data[M30], y + data[M31], z + data[M32]);
			return;
		}
		output.accept(x, y, z);
	}
	
	public float fma(int xPos, int yPos, int zPos, int wPos, float x, float y, float z, float w) {
		return Math.fma(data[xPos], x, Math.fma(data[yPos], y, Math.fma(data[zPos], z, data[wPos] * w)));
	}
	
	public float determinant() {
		float f = data[M00] * ((data[M11] * data[M22] * data[M33] + data[M12] * data[M23] * data[M31] + data[M13] * data[M21] * data[M32]) - data[M13] * data[M22] * data[M31] - data[M11] * data[M23] * data[M32] - data[M12] * data[M21] * data[M33]);
		f -= data[M01] * ((data[M10] * data[M22] * data[M33] + data[M12] * data[M23] * data[M30] + data[M13] * data[M20] * data[M32]) - data[M13] * data[M22] * data[M30] - data[M10] * data[M23] * data[M32] - data[M12] * data[M20] * data[M33]);
		f += data[M02] * ((data[M10] * data[M21] * data[M33] + data[M11] * data[M23] * data[M30] + data[M13] * data[M20] * data[M31]) - data[M13] * data[M21] * data[M30] - data[M10] * data[M23] * data[M31] - data[M11] * data[M20] * data[M33]);
		f -= data[M03] * ((data[M10] * data[M21] * data[M32] + data[M11] * data[M22] * data[M30] + data[M12] * data[M20] * data[M31]) - data[M12] * data[M21] * data[M30] - data[M10] * data[M22] * data[M31] - data[M11] * data[M20] * data[M32]);
		return f;
	}
	
	public float determinant3x3(float t00, float t01, float t02, float t10, float t11, float t12, float t20, float t21, float t22) {
		return t00 * (t11 * t22 - t12 * t21) + t01 * (t12 * t20 - t10 * t22) + t02 * (t10 * t21 - t11 * t20);
	}
	
	public Vec3f project(float x, float y, float z, int[] viewport, Vec3f winCoordsDest) {
		float invW = 1F / Math.fma(data[M03], x, Math.fma(data[M13], y, Math.fma(data[M23], z, data[M33])));
		float nx = Math.fma(data[M00], x, Math.fma(data[M10], y, Math.fma(data[M20], z, data[M30]))) * invW;
		float ny = Math.fma(data[M01], x, Math.fma(data[M11], y, Math.fma(data[M21], z, data[M31]))) * invW;
		float nz = Math.fma(data[M02], x, Math.fma(data[M12], y, Math.fma(data[M22], z, data[M32]))) * invW;
		return winCoordsDest.set(Math.fma(Math.fma(nx, 0.5F, 0.5F), viewport[2], viewport[0]), Math.fma(Math.fma(ny, 0.5F, 0.5F), viewport[3], viewport[1]), Math.fma(0.5F, nz, 0.5F));
	}
	
	public Vec3f unproject(float winX, float winY, float winZ, int[] viewport, Vec3f dest) {
		float a = data[M00] * data[M11] - data[M01] * data[M10];
		float b = data[M00] * data[M12] - data[M02] * data[M10];
		float c = data[M00] * data[M13] - data[M03] * data[M10];
		float d = data[M01] * data[M12] - data[M02] * data[M11];
		float e = data[M01] * data[M13] - data[M03] * data[M11];
		float f = data[M02] * data[M13] - data[M03] * data[M12];
		float g = data[M20] * data[M31] - data[M21] * data[M30];
		float h = data[M20] * data[M32] - data[M22] * data[M30];
		float i = data[M20] * data[M33] - data[M23] * data[M30];
		float j = data[M21] * data[M32] - data[M22] * data[M31];
		float k = data[M21] * data[M33] - data[M23] * data[M31];
		float l = data[M22] * data[M33] - data[M23] * data[M32];
		float det = a * l - b * k + c * j + d * i - e * h + f * g;
		det = 1F / det;
		float im00 = (data[M11] * l - data[M12] * k + data[M13] * j) * det;
		float im01 = (-data[M01] * l + data[M02] * k - data[M03] * j) * det;
		float im02 = (data[M31] * f - data[M32] * e + data[M33] * d) * det;
		float im03 = (-data[M21] * f + data[M22] * e - data[M23] * d) * det;
		float im10 = (-data[M10] * l + data[M12] * i - data[M13] * h) * det;
		float im11 = (data[M00] * l - data[M02] * i + data[M03] * h) * det;
		float im12 = (-data[M30] * f + data[M32] * c - data[M33] * b) * det;
		float im13 = (data[M20] * f - data[M22] * c + data[M23] * b) * det;
		float im20 = (data[M10] * k - data[M11] * i + data[M13] * g) * det;
		float im21 = (-data[M00] * k + data[M01] * i - data[M03] * g) * det;
		float im22 = (data[M30] * e - data[M31] * c + data[M33] * a) * det;
		float im23 = (-data[M20] * e + data[M21] * c - data[M23] * a) * det;
		float im30 = (-data[M10] * j + data[M11] * h - data[M12] * g) * det;
		float im31 = (data[M00] * j - data[M01] * h + data[M02] * g) * det;
		float im32 = (-data[M30] * d + data[M31] * b - data[M32] * a) * det;
		float im33 = (data[M20] * d - data[M21] * b + data[M22] * a) * det;
		float ndcX = (winX - viewport[0]) / viewport[2] * 2F - 1F;
		float ndcY = (winY - viewport[1]) / viewport[3] * 2F - 1F;
		float ndcZ = winZ + winZ - 1F;
		float invW = 1F / (im03 * ndcX + im13 * ndcY + im23 * ndcZ + im33);
		return dest.set((im00 * ndcX + im10 * ndcY + im20 * ndcZ + im30) * invW, (im01 * ndcX + im11 * ndcY + im21 * ndcZ + im31) * invW, (im02 * ndcX + im12 * ndcY + im22 * ndcZ + im32) * invW);
	}
	
	public Matrix4f ortho(float x, float y, float width, float height, float zNear, float zFar) {
		return ortho(x, x + width, y + height, y, zNear, zFar, false);
	}
	
	public Matrix4f ortho(float left, float right, float bottom, float top, float zNear, float zFar, boolean zZeroToOne) {
		float rm00 = 2F / (right - left);
		float rm11 = 2F / (top - bottom);
		float rm22 = (zZeroToOne ? 1F : 2F) / (zFar - zNear);
		float rm30 = (left + right) / (left - right);
		float rm31 = (top + bottom) / (bottom - top);
		float rm32 = (zZeroToOne ? zNear : (zFar + zNear)) / (zNear - zFar);
		
		data[M30] = data[M00] * rm30 + data[M10] * rm31 + data[M20] * rm32 + data[M30];
		data[M31] = data[M01] * rm30 + data[M11] * rm31 + data[M21] * rm32 + data[M31];
		data[M32] = data[M02] * rm30 + data[M12] * rm31 + data[M22] * rm32 + data[M32];
		data[M33] = data[M03] * rm30 + data[M13] * rm31 + data[M23] * rm32 + data[M33];
		data[M00] = data[M00] * rm00;
		data[M01] = data[M01] * rm00;
		data[M02] = data[M02] * rm00;
		data[M03] = data[M03] * rm00;
		data[M10] = data[M10] * rm11;
		data[M11] = data[M11] * rm11;
		data[M12] = data[M12] * rm11;
		data[M13] = data[M13] * rm11;
		data[M20] = data[M20] * rm22;
		data[M21] = data[M21] * rm22;
		data[M22] = data[M22] * rm22;
		data[M23] = data[M23] * rm22;
		return evaluateProps();
	}
	
	public Vec4f storeFrustrumPlane(int plane, Vec4f toStore) {
		return switch(plane) {
			case 0 -> toStore.set(data[M03] + data[M00], data[M13] + data[M10], data[M23] + data[M20], data[M33] + data[M30]).normalize3D();
			case 1 -> toStore.set(data[M03] - data[M00], data[M13] - data[M10], data[M23] - data[M20], data[M33] - data[M30]).normalize3D();
			case 2 -> toStore.set(data[M03] + data[M01], data[M13] + data[M11], data[M23] + data[M21], data[M33] + data[M31]).normalize3D();
			case 3 -> toStore.set(data[M03] - data[M01], data[M13] - data[M11], data[M23] - data[M21], data[M33] - data[M31]).normalize3D();
			case 4 -> toStore.set(data[M03] + data[M02], data[M13] + data[M12], data[M23] + data[M22], data[M33] + data[M32]).normalize3D();
			case 5 -> toStore.set(data[M03] - data[M02], data[M13] - data[M12], data[M23] - data[M22], data[M33] - data[M32]).normalize3D();
			default -> toStore;
		};
	}
}