module dlib.math;

import dlib.aliases;
import dlib.util;

import includes;

import core.stdc.math : tanf, cosf, sinf, sqrtf, fabsf;

import std.math;
import std.math.algebraic;
import std.traits;
import std.meta;
import std.format;
import std.stdio;

T
RoundUp(T)(T a, T b)
{
	a += b - 1;
	a -= a%b;
	return a;
}

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

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

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

	enum _N = N;
	alias T _T;

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

			T y;
			alias y g;

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

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

	this(Vec3, f32)(Vec3 v3, f32 f) if (N == 4 && is(T: f32))
	{
		x = v3.x;
		y = v3.y;
		z = v3.z;
		w = f;
	}

	this(Arr)(Arr arr) if (is(Arr: typeof(v)))
	{
		this.v = arr;
	}

	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 (is(U: T))
	{
		mixin(GenerateLoop!("v[@] = x;", N)());
		return this;
	}

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

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

	bool opEquals(U)(U other) if (is(U: Vector!(T, N)))
	{
		bool result = true;

		foreach(i; 0 .. N)
		{
			if (fabsf(v[i] - other.v[i]) > 0.0000009)
			{
				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 
			{
				mov R8, l;
				mov R9, r;
				mov R10, res;
				movups XMM0, x.offsetof[R8];
				movups XMM1, operand.x.offsetof[R9];
			}
			static if (op == "*")      asm  { mulps XMM0, XMM1; }
			else static if (op == "-") asm  { subps XMM0, XMM1; }
			else static if (op == "+") asm  { addps XMM0, XMM1; }
			else static if (op == "/") asm  { divps XMM0, XMM1; }
			asm 
			{
				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 
			{
				mov R8, l;
				mov R9, r;
				mov R10, res;
				movups XMM0, x.offsetof[R8];
				movups XMM1, other.x.offsetof[R9];
			}
			static if (op == "*")      asm  { mulps XMM0, XMM1; }
			else static if (op == "-") asm  { subps XMM0, XMM1; }
			else static if (op == "+") asm  { addps XMM0, XMM1; }
			else static if (op == "/") asm  { divps XMM0, XMM1; }
			asm 
			{
				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;
		}
	}
}

// TODO: fix alignment for non mat4s
align(16) struct Matrix(T, int D)
{
	static assert(D > 0 && D <= 4);

	alias Vector!(T, D) MatrixVec;
	alias T _T;
	alias T[4] Row;

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

	union
	{
		T[N] v;
		Row[D] rows;
		MatrixVec[D] vec;
		static if (D == 4) mat4 glm_mat;
		static if (D == 3) mat3 glm_mat;
		static if (D == 2) mat2 glm_mat;
	}

	// TODO: setup 

	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;
	}

	bool opEquals(U)(U other) if (is(U: Matrix!(T, D)))
	{
		bool result = true;

		static foreach(i; 0 .. N)
		{
			if (fabsf(this.v[i] - other.v[i]) > 0.0000009)
			{
				result = false;
			}
		}

		return result;
	}

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

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

		return result;
	}

	static if (D == 4)
	{
		Vec4 opBinary(string op, U)(U x) if (is(U: Vec4) && is(T: f32) && (op == "*"))
		{
			Vec4 result = 0.0;
			glm_mat4_mulv(glm_mat.ptr, x.v.ptr, result.v.ptr);
			return result;
		}

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

			glm_mat4_mul(glm_mat.ptr, x.glm_mat.ptr, result.glm_mat.ptr);

			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))
	{
		Mat4 result;
		glm_quat_mat4(vec.ptr, result.glm_mat.ptr);
		return result;
	}

	Quat opBinary(string op, U)(U r) if (op == "*" && is(U: Quat))
	{
		Quat q;

		q.x = this.w * r.x + this.x * r.w + this.y * r.z - this.z * r.y;
		q.y = this.w * r.y - this.x * r.z + this.y * r.w + this.z * r.x;
		q.z = this.w * r.z + this.x * r.y - this.y * r.x + this.z * r.w;
		q.w = this.w * r.w - this.x * r.x - this.y * r.y - this.z * r.z;

		return q;
	}
}

Mat4
Mat4MulASM(Mat4 l, Mat4 r)
{
	Mat4 result;

	auto lp = &l;
	auto rp = &r;
	auto res = &result;

	// TODO: fix this
	asm @trusted 
	{
		mov R8, lp;
		mov R9, rp;
		mov R10, res;

		movups XMM0, [R8];
		movups XMM1, [R9+00];
		movups XMM2, [R9+16];
		movups XMM3, [R9+32];
		movups XMM4, [R9+48];

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

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

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

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

		movups XMM0, [R8+16];

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

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

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

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

		movups XMM0, [R8+32];

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

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

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

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

		movups XMM0, [R8+48];

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

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

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

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

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

	return result;
}

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;
}

 pragma(inline) Mat3 
Mat3Identity()
{
	return Mat3(
		1.0, 0.0, 0.0,
		0.0, 1.0, 0.0,
		0.0, 0.0, 1.0
	);
}

 pragma(inline) Mat2
Mat2Identity()
{
	return Mat2(
		1.0, 0.0,
		0.0, 1.0
	);
}

pragma(inline) Quat
QuatFromAxis(f32 angle, Vec3 axis)
{
	Quat q;
	glm_quatv(q.vec.ptr, angle, axis.v.ptr);
	return q;
}

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);

	if (length < f32.epsilon)
	{
		mixin(GenerateLoop!("vec.v[@] = 0.0;", vec._N)());
	}
	else
	{
		mixin(GenerateLoop!("vec.v[@] *= (1.0 / length);", vec._N)());
	}
}

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

pragma(inline) Quat
Normalize(Quat q)
{
	f32 dot = Norm(&q.vec);
 
	if (dot <= 0.0)
	{
		q = Quat(1.0, 0.0, 0.0, 0.0);
	}

	q.vec *= 1.0 / sqrtf(dot);

	return q;
}

pragma(inline) Mat4
Perspective(f32 fov, f32 aspect, f32 near, f32 far)
{
	Mat4 res;
	MatZero(&res);
	glm_perspective(fov, aspect, near, far, res.glm_mat.ptr);
	res[1, 1] *= -1.0;
	return res;
}

void
Ortho(Mat4* mat, f32 left, f32 bottom, f32 right, f32 top, f32 near, f32 far)
{
	MatZero(mat);
	glm_ortho(left, right, bottom, top, near, far, mat.glm_mat.ptr);
}

Mat4
Ortho(f32 left, f32 bottom, f32 right, f32 top, f32 near, f32 far)
{
	Mat4 mat;
	MatZero(&mat);
	glm_ortho(left, right, bottom, top, near, far, mat.glm_mat.ptr);
	return mat;
}

pragma(inline) Vec3
Rotate(Quat q, Vec3 vec)
{
	Quat p = Normalize(q);

	Vec3 i = p.vec.xyz;
	f32 r = p.vec.w;

	Vec3 v1 = i * (2.0 * Dot(&i, &vec));
	Vec3 v2 = i * (r * r - Dot(&i, &i));
	v1 += v2;

	v2 = i * vec;
	v2 = v2 * (2.0 * r);

	return v1 + v2;
}

pragma(inline) Mat4
LookAt(Vec3 eye, Vec3 center, Vec3 up)
{
	Mat4 result;
	MatZero(&result);
	glm_lookat(eye.v.ptr, center.v.ptr, up.v.ptr, result.glm_mat.ptr);
	return result;
}

pragma(inline) Mat4
Look(Vec3 eye, Vec3 dir, Vec3 up)
{
	return LookAt(eye, eye + dir, up);
}

pragma(inline) Vec3
Cross(Vec3 a, Vec3 b)
{
	Vec3 c;
	Cross(a, b, &c);
	return c;
}

pragma(inline) Vec3
CrossN(Vec3 a, Vec3 b)
{
	Vec3 c;
	Cross(a, b, &c);
	Normalize(&c);
	return c;
}

pragma(inline) void
CrossN(Vec3 a, Vec3 b, Vec3* dst)
{
	Cross(a, b, dst);
	glm_vec3_normalize(dst.v.ptr);
}

pragma(inline) void
Cross(Vec3 a, Vec3 b, Vec3* dst)
{
	glm_vec3_cross(a.v.ptr, b.v.ptr, dst.v.ptr);
}

pragma(inline) void
MatZero(Mat4* mat)
{
	auto v = &mat.vec;
	asm 
	{
		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)
{
	glm_translate(mat.glm_mat.ptr, vec.v.ptr);
}

pragma(inline) Mat4
Inverse(Mat4 mat)
{
	Mat4 res;
	MatZero(&res);
	glm_mat4_inv(mat.glm_mat.ptr, res.glm_mat.ptr);
	return res;
}

pragma(inline) f32
Mix(f32 x, f32 y, f32 a)
{
	return x * (1 - a) + y * a;
}

pragma(inline) f32
InverseLerp(f32 v, f32 min, f32 max)
{
	return (v - min) / (max - min);
}

pragma(inline) f32
Remap(f32 v, f32 in_min, f32 in_max, f32 out_min, f32 out_max)
{
	f32 t = InverseLerp(v, in_min, in_max);
	return Mix(out_min, out_max, t);
}

unittest
{
	enum FLOAT_MAX = f32.max;
	enum FLOAT_MIN = -f32.max;

	/*
	import core.stdc.stdio;
	import core.stdc.stdlib;
	import core.stdc.time;
	import std.range : take;
	import std.algorithm.iteration : sum;

	void PrintMatrix(Mat4 mat)
	{
		foreach(i; 0 .. mat.N)
		{
			if (i % 4 == 0)
			{
				printf("\n");
			}
			printf("%.08f ", mat.v[i]);
		}
		printf("\n");
	}

	srand(cast(u32)time(null));

	f32 RandomFloat()
	{
		return cast(f32)(rand())/cast(f32)(RAND_MAX + 1.0);	
	}

	{ // Vec2 arithmetic
		Vec2 v1 = Vec2(5.0, 10.0);
		Vec2 v2 = Vec2(2.5, 5.0);

		Vec2 result = v1 * v2;

		assert(result == Vec2(12.5, 50.0), "Vec2 mul failure");

		result = v1 + v2;

		assert(result == Vec2(7.5, 15.0), "Vec2 add failure");

		result = v1 - v2;

		assert(result == Vec2(2.5, 5.0), "Vec2 sub failure");

		result = v1 / v2;

		assert(result == Vec2(2.0), "Vec2 div failure");
	}

	{ // Vec3 Arithmetic
		Vec3 v1 = Vec3(5.0, 10.0, 15.0);
		Vec3 v2 = Vec3(2.5, 5.0, 7.5);

		Vec3 result = v1 * v2;

		assert(result == Vec3(12.5, 50.0, 112.5), "Vec3 mul failure");

		result = v1 + v2;

		assert(result == Vec3(7.5, 15.0, 22.5), "Vec3 add failure");

		result = v1 - v2;

		assert(result == Vec3(2.5, 5.0, 7.5), "Vec3 sub failure");

		result = v1 / v2;

		assert(result == Vec3(2.0), "Vec3 div failure");
	}

	{ // Vec3 Arithmetic
		Vec4 v1 = Vec4(5.0, 10.0, 15.0, 20.0);
		Vec4 v2 = Vec4(2.5, 5.0, 7.5, 10.0);

		Vec4 result = v1 * v2;

		assert(result == Vec4(12.5, 50.0, 112.5, 200.0), "Vec4 mul failure");

		result = v1 + v2;

		assert(result == Vec4(7.5, 15.0, 22.5, 30.0), "Vec4 add failure");

		result = v1 - v2;

		assert(result == Vec4(2.5, 5.0, 7.5, 10.0), "Vec4 sub failure");

		result = v1 / v2;

		assert(result == Vec4(2.0), "Vec4 div failure");
	}

	{ // Mat4 Arithmetic
		Mat4 m1 = RandomMat4();
		Mat4 m2 = RandomMat4();
		Mat4 m3 = m1 * m2;
		Mat4 m4;
		
		MatZero(&m4);

		for(u32 i = 0; i < 4; i += 1)
		{
			for(u32 j = 0; j < 4; j += 1)
			{
				for(u32 k = 0; k < 4; k += 1)
				{
					m4.rows[i][j] += m1.rows[k][j] * m2.rows[i][k];
				}
			}
		}

		assert(m3 == m4, "Mat4 mul failure");
	}

	{ // Translate
		Mat4 mat = Mat4Identity();
		Vec4 vec = Vec4(1.0, 2.0, 3.0, 1.0);

		Translate(&mat, Vec3(13.0, 11.0, 7.0));
		Vec4 result = mat * vec;

		assert(result == Vec4(14.0, 13.0, 10.0, 1.0));

		mat = Mat4Identity();
		Translate(&mat, Vec3(1.0, -1.0, -5.0));
		result = mat * result;

		assert(result == Vec4(15.0, 12.0, 5.0, 1.0));
	}

	{ // Identity
		Mat4 identity = 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
				);
		Mat4 mat = Mat4Identity();

		assert(identity == mat);
	}


	{ // Inverse
		foreach(i; 0 .. 1000)
		{
			Mat4 m1 = RandomMat4();

			Mat4 m1_inv = Inverse(m1);
			Mat4 m1_reinv = Inverse(m1_inv);

			assert(m1 == m1_reinv, "Inverse test failed");
		}
	}

	{ // Cross
		Vec3 v1 = Vec3(2.0, -3.0, 4.0);
		Vec3 v2 = Vec3(12.0, -31.0, 43.0);

		Vec3 v3 = Cross(v1, v2);

		Vec3 v4 = Vec3(
			v1.y * v2.z - v1.z * v2.y,
			v1.z * v2.x - v1.x * v2.z,
			v1.x * v2.y - v1.y * v2.x
		);

		assert(v3 == v4, "Vec3 Cross failure");

		v3 = CrossN(v1, v2);

		Normalize(&v4);

		assert(v3 == v4, "Vec3 CrossN failure");
	}
	*/
}