#pragma once

#include <cmath>

#include "vec3.h"
#include "mat4x4.h"

namespace egm
{
	// Multiplies a matrix into out
	template <typename T>
	void multiply(const tmat4x4<T>& a, const tmat4x4<T>& b, tmat4x4<T>& out)
	{
		// for i 1..4: a[row][i] * b[i][column]
		// for i 1..4: a.i.row * b.column.i
		// out.c.r = a.a.r * b.c.x + a.b.r * b.c.y + ...
		// Something is wrong here......
		out.a.x = a.a.x * b.a.x + a.b.x * b.a.y + a.c.x * b.a.z + a.d.x * b.a.w;
		out.a.y = a.a.y * b.a.x + a.b.y * b.a.y + a.c.y * b.a.z + a.d.y * b.a.w;
		out.a.z = a.a.z * b.a.x + a.b.z * b.a.y + a.c.z * b.a.z + a.d.z * b.a.w;
		out.a.w = a.a.w * b.a.x + a.b.w * b.a.y + a.c.w * b.a.z + a.d.w * b.a.w;

		out.b.x = a.a.x * b.b.x + a.b.x * b.b.y + a.c.x * b.b.z + a.d.x * b.b.w;
		out.b.y = a.a.y * b.b.x + a.b.y * b.b.y + a.c.y * b.b.z + a.d.y * b.b.w;
		out.b.z = a.a.z * b.b.x + a.b.z * b.b.y + a.c.z * b.b.z + a.d.z * b.b.w;
		out.b.w = a.a.w * b.b.x + a.b.w * b.b.y + a.c.w * b.b.z + a.d.w * b.b.w;

		out.c.x = a.a.x * b.c.x + a.b.x * b.c.y + a.c.x * b.c.z + a.d.x * b.c.w;
		out.c.y = a.a.y * b.c.x + a.b.y * b.c.y + a.c.y * b.c.z + a.d.y * b.c.w;
		out.c.z = a.a.z * b.c.x + a.b.z * b.c.y + a.c.z * b.c.z + a.d.z * b.c.w;
		out.c.w = a.a.w * b.c.x + a.b.w * b.c.y + a.c.w * b.c.z + a.d.w * b.c.w;

		out.d.x = a.a.x * b.d.x + a.b.x * b.d.y + a.c.x * b.d.z + a.d.x * b.d.w;
		out.d.y = a.a.y * b.d.x + a.b.y * b.d.y + a.c.y * b.d.z + a.d.y * b.d.w;
		out.d.z = a.a.z * b.d.x + a.b.z * b.d.y + a.c.z * b.d.z + a.d.z * b.d.w;
		out.d.w = a.a.w * b.d.x + a.b.w * b.d.y + a.c.w * b.d.z + a.d.w * b.d.w;
	}

	// Multiplies and transposes a matrix into out.
	template <typename T>
	void multiply_and_transpose(const tmat4x4<T>& a, const tmat4x4<T>& b, tmat4x4<T>& out)
	{
		out.a.x = a.a.x * b.a.x + a.a.y * b.b.x + a.a.z * b.c.x + a.a.w * b.d.x;
		out.a.y = a.a.x * b.a.y + a.a.y * b.b.y + a.a.z * b.c.y + a.a.w * b.d.y;
		out.a.z = a.a.x * b.a.z + a.a.y * b.b.z + a.a.z * b.c.z + a.a.w * b.d.z;
		out.a.w = a.a.x * b.a.w + a.a.y * b.b.w + a.a.z * b.c.w + a.a.w * b.d.w;

		out.b.x = a.b.x * b.a.x + a.b.y * b.b.x + a.b.z * b.c.x + a.b.w * b.d.x;
		out.b.y = a.b.x * b.a.y + a.b.y * b.b.y + a.b.z * b.c.y + a.b.w * b.d.y;
		out.b.z = a.b.x * b.a.z + a.b.y * b.b.z + a.b.z * b.c.z + a.b.w * b.d.z;
		out.b.w = a.b.x * b.a.w + a.b.y * b.b.w + a.b.z * b.c.w + a.b.w * b.d.w;

		out.c.x = a.c.x * b.a.x + a.c.y * b.b.x + a.c.z * b.c.x + a.c.w * b.d.x;
		out.c.y = a.c.x * b.a.y + a.c.y * b.b.y + a.c.z * b.c.y + a.c.w * b.d.y;
		out.c.z = a.c.x * b.a.z + a.c.y * b.b.z + a.c.z * b.c.z + a.c.w * b.d.z;
		out.c.w = a.c.x * b.a.w + a.c.y * b.b.w + a.c.z * b.c.w + a.c.w * b.d.w;

		out.d.x = a.d.x * b.a.x + a.d.y * b.b.x + a.d.z * b.c.x + a.d.w * b.d.x;
		out.d.y = a.d.x * b.a.y + a.d.y * b.b.y + a.d.z * b.c.y + a.d.w * b.d.y;
		out.d.w = a.d.x * b.a.z + a.d.y * b.b.z + a.d.z * b.c.z + a.d.w * b.d.z;
		out.d.w = a.d.x * b.a.w + a.d.y * b.b.w + a.d.z * b.c.w + a.d.w * b.d.w;
	}

	// Multiplies a matrix by a vector into out
	// This 
	template <typename T>
	void multiply(const tmat4x4<T>& m, const tvec4<T>& v, tvec4<T>& out)
	{
		out.x = m.a.x * v.x + m.b.x * v.y + m.c.x * v.z + m.d.x * v.w;
		out.y = m.a.y * v.x + m.b.y * v.y + m.c.y * v.z + m.d.y * v.w;
		out.z = m.a.z * v.x + m.b.z * v.y + m.c.z * v.z + m.d.z * v.w;
		out.w = m.a.w * v.x + m.b.w * v.y + m.c.w * v.z + m.d.w * v.w;
	}

	// Takes the cross product of two vec3s into out
	template <typename T>
	void cross_product(const tvec3<T>& a, const tvec3<T>& b, tvec3<T>& out)
	{
		out.x = a.y * b.z - a.z * b.y;
		out.y = a.z * b.x - a.x * b.z;
		out.z = a.x * b.y - a.y * b.x;
	}

	// Creates an orthographic projection matrix
	template <typename T>
	void ortho(T left, T right, T bottom, T top, T znear, T zfar, tmat4x4<T>& out)
	{
		out.to_identity();
		out.a.x = T(2) / (right - left);
		out.b.y = T(2) / (top - bottom);
		out.c.z = -T(2) / (zfar - znear);
		out.d.x = -(right + left) / (right - left);
		out.d.y = -(top + bottom) / (top - bottom);
		out.d.z = -(zfar + znear) / (zfar - znear);
	}

	// Creates an orthographic projection matrix centered at the origin
	template <typename T>
	void ortho(T width, T height, T depth, tmat4x4<T>& out)
	{
		out.to_identity();
		out.a.x = T(2) / width;
		out.b.y = T(2) / height;
		out.c.z = T(2) / depth;
	}

	template <typename T>
	void perspective(T fov_y, T aspect_ratio, T znear, T zfar, tmat4x4<T>& out)
	{
		const T tan_half_fov_y = tan(fov_y / static_cast<T>(2));
		out.zero();
		out.a.x = static_cast<T>(1) / (aspect_ratio * tan_half_fov_y);
		out.b.y = static_cast<T>(1) / tan_half_fov_y;
		out.c.z = -(zfar + znear) / (zfar - znear);
		out.c.w = -static_cast<T>(1);
		out.d.z = -(static_cast<T>(2) * zfar * znear) / (zfar - znear);
	}

	template <typename T>
	void translation(T x, T y, T z, tmat4x4<T>& out)
	{
		out.to_identity();
		out.d.x = x;
		out.d.y = y;
		out.d.z = z;
	}

	template <typename T>
	void rotation(T angle, const tvec3<T>& rotation_axis, tmat4x4<T>& out)
	{
		const T c = cos(angle);
		const T s = sin(angle);

		tvec3<T> axis(rotation_axis.normalized());
		tvec3<T> temp(axis * (static_cast<T>(1) - c));

		out.to_identity();

		out.a.x = c + temp.x * axis.x;
		out.a.y = temp.x * axis.y + s * axis.z;
		out.a.z = temp.x * axis.z - s * axis.y;

		out.b.x = temp.y * axis.x - s * axis.z;
		out.b.y = c + temp.y * axis.y;
		out.b.z = temp.y * axis.z + s * axis.x;

		out.c.x = temp.z * axis.x + s * axis.y;
		out.c.y = temp.z * axis.y - s * axis.x;
		out.c.z = c + temp.z * axis.z;
	}
}