import aliases;
import util;
import std.math;
import core.stdc.math : tanf, cosf, sinf, sqrtf;
import std.traits;
import inteli;
import std.meta;
import std.format;
import std.stdio;

T AlignPow2(T)(T v, T a)
{
	return (v + a - 1) & ~(a - 1);
}

f32 Radians(f32 deg)
{
	return deg * (PI / 180.0);
}

// TODO: Clean this mess up

enum IsVector(T) = is(T : Vector!U, U...);

struct Vector(T, int N)
{
	static assert(N > 0 && N <= 4);

	enum _N = N;
	alias T _T;

	union
	{
		T[N] v;
		struct
		{
			T x;
			alias x r;

			static if (N > 1)
			{
				T y;
				alias y g;
			}

			static if (N > 2)
			{
				T z;
				alias z b;
			}

			static if (N > 3)
			{
				T w;
				alias w a;
			}
		}
	}

	nothrow @nogc pure:

	this(Vec3, f32)(Vec3 v3, f32 f)
	{
		x = v3.x;
		y = v3.y;
		z = v3.z;
		w = f;
	}

	this(Args...)(Args args) 
	{
		static if (args.length == 1)
		{
			opAssign!(Args[0])(args[0]);
		}
		else static if (args.length == N)
		{
			mixin(GenerateLoop!("v[@] = args[@];", N)());
		}
		else static if (args.length == 2 && N == 4)
		{
			v[0] = args[0];
			v[1] = args[0];
			v[2] = args[0];
			v[3] = args[1];
		}
		else
		{
			static assert(false, "Invalid Vector constructor");
		}
	}

	ref Vector opAssign(U)(U x) if (isAssignable!(T, U))
	{
		mixin(GenerateLoop!("v[@] = x;", N)());
		return this;
	}

	ref Vector opAssign(U)(U arr) if (isStaticArray!(U) && isAssignable!(T, typeof(arr[0])) && arr.length == N)
	{
		mixin(GenerateLoop!("v[@] = arr[@];", N)());
		return this;
	}

	ref Vector opAssign(U)(U arr) if (isDynamicArray!(U) && isAssignable!(T, typeof(arr[0])))
	{
		mixin(GenerateLoop!("v[@] = arr[@];", N)());
		return this;
	}

	ref Vector opAssign(U)(U u) if (is(U : Vector))
	{
		v[] = u.v[];
		return this;
	}

	ref Vector opAssign(U)(U x) if (IsVector!(U) && isAssignable!(T, U._T) && (!is(U: Vector)) && (U._N == _N))
	{
		mixin(GenerateLoop!("v[@] = x.v[@];", N)());
		return this;
	}

	inout(T)* ptr() inout @property
	{
		return v.ptr;
	}

	bool opEquals(U)(U other) if (IsConvertible!(U))
	{
		Vector conv = other;
		return opEquals(conv);
	}

	Vector opEquals(U)(U other) if (is(U: Vector))
	{
		bool result = true;
		
		foreach(i; 0 .. N)
		{
			if (v[i] != other.v[i])
			{
				result = false;
				break;
			}
		}

		return result;
	}

	int opDollar()
	{
		return N;
	}

	T[] opSlice()
	{
		return v[];
	}

	Vector opUnary(string op)() if (op == "+" || op == "-" || op == "~" || op == "!")
	{
		Vector result;
		mixin(GenerateLoop!("res.v[@] = " ~ op ~ " v[@];", N)());
		return res;
	}

	ref Vector opOpAssign(string op, U)(U value) if (is(U: Vector))
	{
		mixin(GenerateLoop!("v[@] " ~ op ~ "= value.v[@];", N)());
		return this;
	}

	ref Vector opOpAssign(string op, U)(U value) if (IsConvertible!(U))
	{
		Vector conv = value;
		return opOpAssign!(op)(conv);
	}

	@property auto opDispatch(string op, U = void)() if (ValidSwizzle!(op) && op.length <= 4)
	{
		Vector!(T, op.length) result;
		enum index_tuple = SwizzleTuple!(op);
		static foreach(i, index; index_tuple)
		{
			result.v[i] = v[index];
		}
		return result;
	}

	@property void opDispatch(string op, U)(U x) if ((op.length > 1) && ValidUniqueSwizzle!(op) && is(typeof(Vector!(T, op.length)(x))))
	{
		Vector!(T, op.length) conv = x;
		enum index_tuple = SwizzleTuple!(op);
		static foreach(i, index; index_tuple)
		{
			v[index] = conv[i];
		}
	}

	static if (N == 4)
	{
		Vector opBinary(string op, U)(U operand) if ((is(U: Vector!(f32, 4)) && is(T: f32)) && (op == "*" || op == "+" || op == "-" || op == "/"))
		{
			Vector result;
			f32* l = &x;
			f32* r = &operand.x;
			f32* res = &result.x;
			
			asm pure @nogc nothrow
			{
				mov R8, l;
				mov R9, r;
				mov R10, res;
				movups XMM0, x.offsetof[R8];
				movups XMM1, operand.x.offsetof[R9];
			}
			static if (op == "*")      asm pure @nogc nothrow { mulps XMM0, XMM1; }
			else static if (op == "-") asm pure @nogc nothrow { subps XMM0, XMM1; }
			else static if (op == "+") asm pure @nogc nothrow { addps XMM0, XMM1; }
			else static if (op == "/") asm pure @nogc nothrow { divps XMM0, XMM1; }
			asm pure @nogc nothrow
			{
				movups result.x.offsetof[R10], XMM0;
			}

			return result;
		}

		Vector opBinary(string op, U)(U operand) if (IsConvertible!(U) && (op == "*" || op == "+" || op == "-" || op == "/")) 
		{
			Vector result;
			Vector other = operand;
			f32* l = &x;
			f32* r = &other.x;
			f32* res = &result.x;
			
			asm pure @nogc nothrow
			{
				mov R8, l;
				mov R9, r;
				mov R10, res;
				movups XMM0, x.offsetof[R8];
				movups XMM1, other.x.offsetof[R9];
			}
			static if (op == "*")      asm pure @nogc nothrow { mulps XMM0, XMM1; }
			else static if (op == "-") asm pure @nogc nothrow { subps XMM0, XMM1; }
			else static if (op == "+") asm pure @nogc nothrow { addps XMM0, XMM1; }
			else static if (op == "/") asm pure @nogc nothrow { divps XMM0, XMM1; }
			asm pure @nogc nothrow
			{
				movups result.x.offsetof[R8], XMM0;
			}

			return result;
		}
	}
	else
	{
		Vector opBinary(string op, U)(U operand) if (is(U: Vector) && U._N == N && (op == "*" || op == "+" || op == "-" || op == "/"))
		{
			Vector res;
			mixin(GenerateLoop!("res.v[@] = v[@] " ~ op ~ " operand.v[@];", N)());
			return res;
		}

		Vector opBinary(string op, U)(U operand) if (IsConvertible!(U) && (op == "*" || op == "+" || op == "-" || op == "/"))
		{
			Vector res;
			Vector other = operand;
			mixin(GenerateLoop!("res.v[@] = v[@] " ~ op ~ " other.v[@];", N)());
			return res;
		}
	}

	

	ref T opIndex(size_t i)
	{
		return v[i];
	}

	T opIndexAssign(U : T)(U x, size_t i)
	{
		return v[i] = x;
	}

	U opCast(U)() if (IsVector!(U) && (U._N == _N))
	{
		U result;
		mixin(GenerateLoop!("res.v[@] = cast(U._T)v[@];", N)());
		return result;
	}

	template IsConvertible(T)
	{
		enum bool IsConvertible = (!is(T : Vector)) && is(typeof({ T x; Vector v = x; }()));
	}

	template SwizzleIndex(char c)
	{
		static if ((c == 'x' || c == 'r') && N > 0)
			enum SwizzleIndex = 0;
		else static if ((c == 'y' || c == 'g') && N > 1)
			enum SwizzleIndex = 1;
		else static if ((c == 'z' || c == 'b') && N > 2)
			enum SwizzleIndex = 2;
		else static if ((c == 'w' || c == 'a') && N > 3)
			enum SwizzleIndex = 3;
		else
			enum SwizzleIndex = -1;
	}

	template SwizzleSet(char c)
	{
		static if (c == 'x' || c == 'y' || c == 'z' || c == 'w')
			enum SwizzleSet = 0;
		else static if (c == 'r' || c == 'g' || c == 'b' || c == 'a')
			enum SwizzleSet = 1;
		else
			enum SwizzleSet = -1;
	}

	template SwizzleTuple(string op)
	{
		enum op_length = op.length;
		static if (op.length == 0)
			enum SwizzleTuple = [];
		else
			enum SwizzleTuple = [ SwizzleIndex!(op[0])] ~ SwizzleTuple!(op[1 .. op.length]);
	}

	template SearchString(char c, string s)
	{
		static if (s.length == 0)
		{
			enum bool result = false;
		}
		else
		{
			enum string tail = s[1 .. s.length];
			enum bool result = (s[0] == c) || SearchString!(c, tail).result;
		}
	}

	template UniqueChars(string s)
	{
		static if (s.length == 1)
		{
			enum bool result = true;
		}
		else
		{
			enum tail = s[1 .. s.length];
			enum bool result = !(SearchString!(s[0], tail).result) && UniqueChars!(tail).result;
		}
	}

	template ValidSwizzle(string op, int last_swizzle = -1)
	{
		static if (op.length == 0)
		{
			enum bool ValidSwizzle = true;
		}
		else
		{
			enum length = op.length;
			enum int swizzle_set = SwizzleSet!(op[0]);
			enum bool valid_swizzle_set = (last_swizzle == -1 || (swizzle_set == last_swizzle));
			enum bool ValidSwizzle = (SwizzleIndex!(op[0]) != -1) && valid_swizzle_set && ValidSwizzle!(op[1 .. length], swizzle_set);
		}
	}

	template ValidUniqueSwizzle(string op)
	{
		static if (ValidSwizzle!(op))
		{
			enum ValidUniqueSwizzle = UniqueChars!(op).result;
		}
		else
		{
			enum ValidUniqueSwizzle = false;
		}
	}
}

struct Matrix(T, int D)
{
	static assert(D > 0 && D <= 4);

	alias Vector!(T, D) MatrixVec;
	alias T _T;

	enum N = D*D;
	enum _D = D;

	union
	{
		T[N] v;
		MatrixVec[D] vec;
	}

	// TODO: setup @nogc nothrow

	this(U...)(U values)
	{
		static if ((U.length == N) && allSatisfy!(IsTypeAssignable, U))
		{
			static foreach(i, x; values)
			{
				v[i] = x;
			}
		}
		else static if ((U.length == 1) && (isAssignable!(U[0])) && (!is(U[0] : Matrix)))
		{
			v[] = values[0];
		}
		else static assert(false, "Cannot construct matrix with provided parameters");
	}

	this(U)(T x)
	{
		static foreach(i; 0 .. N)
		{
			v[i] = x;
		}
	}

	@property inout(T)* ptr() inout
	{
		return v.ptr;
	}

	ref Matrix opAssign(U : T)(U x)
	{
		static foreach(i; 0 .. N)
		{
			v[i] = x;
		}

		return this;
	}

	ref Matrix opAssign(U : Matrix)(U x)
	{
		static foreach(i; 0 .. N)
		{
			v[i] = x.v[i];
		}

		return this;
	}

	ref Matrix opAssign(U)(U x) if (IsMatrixInstantiation!(U) && is(U._T : _T) && (!is(U: Matrix) && (U.N != N)))
	{
		static foreach(i; 0 .. N)
		{
			v[i] = x.v[i];
		}

		return this;
	}

	ref T opIndex(size_t i, size_t j)
	{
		return v[(i * D) + j];
	}

	T opIndexAssign(U: T)(U x, size_t i, size_t j)
	{
		return v[(i * D) + j] = x;
	}

	Matrix opBinary(string op)(T scalar) if (op == "*")
	{
		Matrix result;

		static foreach(i; 0 .. N)
		{
			result.v[i] = v[i] * scalar;
		}

		return result;
	}

	MatrixVec opBinary(string op)(T factor) if (op == "*")
	{
		Matrix result;

		return result;
	}

	static if (D == 4)
	{
		Vec4 opBinary(string op, U)(U x) if (is(U: Vec4) && is(T: f32) && (op == "*"))
		{
			Vec4 result;

			auto res = &result;
			auto m = &this;
			auto v = &x;

			asm pure @trusted @nogc nothrow
			{
				mov R8, m;
				mov R9, v;
				mov R10, res;

				movups XMM0, vec.offsetof[R8]+00;
				movups XMM1, vec.offsetof[R8]+16;
				movups XMM2, vec.offsetof[R8]+32;
				movups XMM3, vec.offsetof[R8]+48;

				movups XMM4, x.x.offsetof[R9];
				movups XMM5, XMM4;
				shufps XMM5, XMM5, 0;
				movups XMM6, XMM4;
				shufps XMM6, XMM6, 85;
				movups XMM7, XMM4;
				shufps XMM7, XMM7, 170;
				movups XMM8, XMM4;
				shufps XMM8, XMM8, 255;

				mulps XMM3, XMM8;
				mulps XMM2, XMM7;
				addps XMM3, XMM2;
				mulps XMM1, XMM6;
				addps XMM3, XMM1;
				mulps XMM0, XMM5;
				addps XMM3, XMM0;

				movups result.x.offsetof[R10], XMM3;
			}

			return result;
		}

		Matrix opBinary(string op, U)(U x) if (is(U: Matrix!(T, D)) && is(T: f32) && (op == "*")) 
		{
			Matrix result;

			auto res = &result;
			auto l = &this;
			auto r = &x;


			// TODO: Test assembly on windows
			asm pure @trusted @nogc nothrow
			{
				mov R8, l;
				mov R9, r;
				mov R10, res;

				movups XMM0, vec.offsetof[R8];
				movups XMM1, x.vec.offsetof[R9];
				movups XMM2, x.vec.offsetof[R9]+16;
				movups XMM3, x.vec.offsetof[R9]+32;
				movups XMM4, x.vec.offsetof[R9]+48;

				movups XMM5, XMM1;
				shufps XMM5, XMM5, 0; // XMM5 = vec.xxxx;
				mulps XMM5, XMM0;
				movups XMM6, XMM5; // XMM6 = col1;

				movups XMM5, XMM2;
				shufps XMM5, XMM5, 0;
				mulps XMM5, XMM0;
				movups XMM7, XMM5; // XMM7 = col2;

				movups XMM5, XMM3;
				shufps XMM5, XMM5, 0;
				mulps XMM5, XMM0;
				movups XMM8, XMM5; // XMM8 = col3;

				movups XMM5, XMM3;
				shufps XMM5, XMM5, 0;
				mulps XMM5, XMM0;
				movups XMM9, XMM5; // XMM9 = col4;

				movups XMM0, vec.offsetof[R8]+16;

				movups XMM5, XMM1;
				shufps XMM5, XMM5, 85; // XMM5 = vec.yyyy;
				movups XMM10, XMM0;
				mulps XMM10, XMM5;
				addps XMM6, XMM10;

				movups XMM5, XMM2;
				shufps XMM5, XMM5, 85;
				movups XMM10, XMM0;
				mulps XMM10, XMM5;
				addps XMM7, XMM10;

				movups XMM5, XMM3;
				shufps XMM5, XMM5, 85;
				movups XMM10, XMM0;
				mulps XMM10, XMM5;
				addps XMM8, XMM10;

				movups XMM5, XMM4;
				shufps XMM5, XMM5, 85;
				movups XMM10, XMM0;
				mulps XMM10, XMM5;
				addps XMM9, XMM10;

				movups XMM0, vec.offsetof[R8]+32;

				movups XMM5, XMM1;
				shufps XMM5, XMM5, 170; // XMM5 = vec.zzzz;
				movups XMM10, XMM0;
				mulps XMM10, XMM5;
				addps XMM6, XMM10;

				movups XMM5, XMM2;
				shufps XMM5, XMM5, 170;
				movups XMM10, XMM0;
				mulps XMM10, XMM5;
				addps XMM7, XMM10;

				movups XMM5, XMM3;
				shufps XMM5, XMM5, 170;
				movups XMM10, XMM0;
				mulps XMM10, XMM5;
				addps XMM8, XMM10;

				movups XMM5, XMM4;
				shufps XMM5, XMM5, 170;
				movups XMM10, XMM0;
				mulps XMM10, XMM5;
				addps XMM9, XMM10;

				movups XMM0, vec.offsetof[R8]+48;

				movups XMM5, XMM1;
				shufps XMM5, XMM5, 255; // XMM5 = vec.wwww;
				movups XMM10, XMM0;
				mulps XMM10, XMM5;
				addps XMM6, XMM10;

				movups XMM5, XMM2;
				shufps XMM5, XMM5, 255;
				movups XMM10, XMM0;
				mulps XMM10, XMM5;
				addps XMM7, XMM10;

				movups XMM5, XMM3;
				shufps XMM5, XMM5, 255;
				movups XMM10, XMM0;
				mulps XMM10, XMM5;
				addps XMM8, XMM10;

				movups XMM5, XMM4;
				shufps XMM5, XMM5, 255;
				movups XMM10, XMM0;
				mulps XMM10, XMM5;
				addps XMM9, XMM10;

				movups result.vec.offsetof[R10]+00, XMM6;
				movups result.vec.offsetof[R10]+16, XMM7;
				movups result.vec.offsetof[R10]+32, XMM8;
				movups result.vec.offsetof[R10]+48, XMM9;
			}

			return result;
		}
	}

	template IsTypeAssignable(U)
	{
		enum bool IsTypeAssignable = std.traits.isAssignable!(T, U);
	}

	template IsMatrixInstantiation(U)
	{
		static void IsMatrix(T, int D)(Matrix!(T, D) x) {}
		
		enum bool IsMatrixInstantiation = is(typeof(IsMatrix(U.init)));
	}
}

struct Quat
{
	union
	{
		f32[4] v;
		Vec4 vec;
		struct
		{
			f32 x;
			f32 y;
			f32 z;
			f32 w;
		};
	};

	this(f32 w, f32 x, f32 y, f32 z)
	{
		vec.x = x;
		vec.y = y;
		vec.z = z;
		vec.w = w;
	}

	U opCast(U)() if (is(U: Mat4))
	{
		f32 norm = Norm(&vec);
		f32 s = norm > 0.0 ? 2.0 / norm : 0.0;

		f32 _x = x;
		f32 _y = y;
		f32 _z = z;
		f32 _w = w;

		f32 xx = s * x * x; f32 xy = s * x * y; f32 wx = s * w * x;
		f32 yy = s * y * y; f32 yz = s * y * z; f32 wy = s * w * y;
		f32 zz = s * z * z; f32 xz = s * x * z; f32 wz = s * w * z;

		U mat;

		mat[0, 0] = 1.0 - yy - zz;
		mat[1, 1] = 1.0 - xx - zz;
		mat[2, 2] = 1.0 - xx - yy;

		mat[0, 1] = xy + wz;
		mat[1, 2] = yz + wx;
		mat[2, 0] = xz + wy;

		mat[1, 0] = xy - wz;
		mat[2, 1] = yz - wx;
		mat[0, 2] = xz - wy;

		mat[0, 3] = 0.0;
		mat[1, 3] = 0.0;
		mat[2, 3] = 0.0;
		mat[3, 0] = 0.0;
		mat[3, 1] = 0.0;
		mat[3, 2] = 0.0;
		mat[3, 3] = 1.0;

		return mat;
	}
}

nothrow @nogc pragma(inline): Mat4 
Mat4Identity()
{
	return Mat4(
		1.0, 0.0, 0.0, 0.0,
		0.0, 1.0, 0.0, 0.0,
		0.0, 0.0, 1.0, 0.0,
		0.0, 0.0, 0.0, 1.0
	);
}

pragma(inline): void
Mat4Identity(Mat4* mat)
{
	MatZero(mat);

	(*mat)[0, 0] = 1.0;
	(*mat)[1, 1] = 1.0;
	(*mat)[2, 2] = 1.0;
	(*mat)[3, 3] = 1.0;
}

nothrow @nogc pragma(inline): Mat3 
Mat3Identity()
{
	return Mat3(
		1.0, 0.0, 0.0,
		0.0, 1.0, 0.0,
		0.0, 0.0, 1.0
	);
}

nothrow @nogc pragma(inline): Mat2
Mat2Identity()
{
	return Mat2(
		1.0, 0.0,
		0.0, 1.0
	);
}

pragma(inline): void
QuatFromAxis(Quat* q, f32 angle, Vec3 axis)
{
	Vec3 k;

	f32 a = angle * 0.5f;
	f32 c = cosf(a);
	f32 s = sinf(a);

	NormalizeTo(&axis, &k);

	q.x = s * k.x;
	q.y = s * k.y;
	q.z = s * k.z;
	q.w = c;
}

pragma(inline): f32
Dot(Vec2* l, Vec2* r)
{
	return l.x * r.x + l.y * r.y;
}

pragma(inline): f32
Dot(Vec3* l, Vec3* r)
{
	return l.x * r.x + l.y * r.y + l.z * r.z;
}

pragma(inline): f32
Dot(Vec4* l, Vec4* r)
{
	// TODO: SIMD this
	return l.x * r.x + l.y * r.y + l.z * r.z + l.w * r.w;
}

pragma(inline): f32
Norm(Vec3* v)
{
	return sqrtf(Dot(v, v));
}

pragma(inline): f32
Norm(Vec4* v)
{
	// TODO: SIMD this
	return sqrtf(Dot(v, v));
}

pragma(inline): void
Normalize(T)(T* vec) if (is(T: Vec2) || is(T: Vec3) || is(T: Vec4))
{
	f32 length = Norm(vec);
	mixin(GenerateLoop!("vec.v[@] /= length;", vec._N)());
}

pragma(inline): Vector
Normalize(T)(T vec) if (is(T: Vec2) || is(T: Vec3) || is(T: Vec4))
{
	Normalize(&vec);
	return vec;
}

pragma(inline): void
NormalizeTo(Vec3* v, Vec3* dst)
{
	f32 norm = Norm(v);

	if (norm < f32.epsilon)
	{
		dst.x = dst.y = dst.z = 0.0;
	}
	else
	{
		*dst = (*v) * (1.0 / norm);
	}
}

pragma(inline): void
Perspective(Mat4* mat, f32 fov, f32 width, f32 height, f32 near, f32 far)
{
	MatZero(mat);

	f32 f = 1.0 / tanf(fov * 0.5);
	f32 fn = 1.0 / (near - far);

	(*mat)[0, 0] = f / (width / height);
	(*mat)[1, 1] = f;
	(*mat)[2, 2] = far * fn;
	(*mat)[2, 3] = -1.0;
	(*mat)[3, 2] = near * far * fn;
}

pragma(inline): void
MatZero(Mat4* mat)
{
	auto v = &mat.vec;
	asm @nogc nothrow
	{
		mov R8, v;
		xorps XMM0, XMM0;
		movups mat.vec.offsetof[R8]+00, XMM0;
		movups mat.vec.offsetof[R8]+16, XMM0;
		movups mat.vec.offsetof[R8]+32, XMM0;
		movups mat.vec.offsetof[R8]+48, XMM0;
	}
}

pragma(inline): void
Translate(Mat4* mat, Vec3 vec)
{
	Mat4Identity(mat);
	(*mat)[3, 0] = vec[0];
	(*mat)[3, 1] = vec[1];
	(*mat)[3, 2] = vec[2];
}

pragma(inline): Mat4
Inverse(Mat4 mat)
{
	// TODO: SIMD this
	f32 a = mat[0, 0], b = mat[0, 1], c = mat[0, 2], d = mat[0, 3],
	    e = mat[1, 0], f = mat[1, 1], g = mat[1, 2], h = mat[1, 3],
			i = mat[2, 0], j = mat[2, 1], k = mat[2, 2], l = mat[2, 3],
			m = mat[3, 0], n = mat[3, 1], o = mat[3, 2], p = mat[3, 3];

	f32 c1 = k * p - l * o, c2 = c * h - d * g, c3  = i * p - l * m,
      c4 = a * h - d * e, c5 = j * p - l * n, c6  = b * h - d * f, 
      c7 = i * n - j * m, c8 = a * f - b * e, c9  = j * o - k * n,
      c10 = b * g - c * f, c11 = i * o - k * m, c12 = a * g - c * e;

	f32 idt = 1.0/(c8*c1+c4*c9+c10*c3+c2*c7-c12*c5-c6*c11), ndt = -idt;

	Mat4 res;

	res[0, 0] = (f * c1  - g * c5  + h * c9)  * idt;
  res[0, 1] = (b * c1  - c * c5  + d * c9)  * ndt;
  res[0, 2] = (n * c2  - o * c6  + p * c10) * idt;
  res[0, 3] = (j * c2  - k * c6  + l * c10) * ndt;

  res[1, 0] = (e * c1  - g * c3  + h * c11) * ndt;
  res[1, 1] = (a * c1  - c * c3  + d * c11) * idt;
  res[1, 2] = (m * c2  - o * c4  + p * c12) * ndt;
  res[1, 3] = (i * c2  - k * c4  + l * c12) * idt;

  res[2, 0] = (e * c5  - f * c3  + h * c7)  * idt;
  res[2, 1] = (a * c5  - b * c3  + d * c7)  * ndt;
  res[2, 2] = (m * c6  - n * c4  + p * c8)  * idt;
  res[2, 3] = (i * c6  - j * c4  + l * c8)  * ndt;

  res[3, 0] = (e * c9  - f * c11 + g * c7)  * ndt;
  res[3, 1] = (a * c9  - b * c11 + c * c7)  * idt;
  res[3, 2] = (m * c10 - n * c12 + o * c8)  * ndt;
  res[3, 3] = (i * c10 - j * c12 + k * c8)  * idt;

	return res;
}

pragma(inline): T 
SquaredMagnitude(T)(T v) if (IsVector!(T))
{
	T sum_squares = 0;
	mixin(GenerateLoop!("sum_squares += v.v[@] * v.v[@];", v._N)());
	return sum_squares;
}

pragma(inline): T
SquaredDistTo(T)(T l, T r) if (IsVector!(T))
{
	return SquaredMagnitude(r - l);
}