#include "svk_vector_utils.hpp"
#include <iomanip>
#include <cstdint> // For width when accessing a matrix like m[{1, 1}]


static bool _automaticTranspose{true};

inline void hash_combine(size_t &seed, size_t hash)
{
	hash += 0x9e3779b9 + (seed << 6) + (seed >> 2);
	seed ^= hash;
}


vec2::vec2()
: x{0.0f}, y{0.0f} {}

vec2::vec2(float const& value)
: x{value}, y{value} {}

vec2::vec2(float const& pX, float const& pY)
: x{pX}, y{pY} {}

vec2::vec2(vec3 const& v)
: x{v.x}, y{v.y} {}

vec2::vec2(vec4 const& v)
: x{v.x}, y{v.y} {}

vec2& vec2::operator=(vec3 const& rhs)
{
	return (*this = vec2{rhs});
}

vec2& vec2::operator=(vec4 const& rhs)
{
	return (*this = vec2{rhs});
}

bool vec2::operator==(vec2 const& rhs) const
{
	return x == rhs.x && y == rhs.y;
}

bool vec2::operator!=(vec2 const& rhs) const
{
	return !(*this == rhs);
}

vec2 vec2::operator+(vec2 const& rhs) const
{
	return vec2{x + rhs.x, y + rhs.y};
}

vec2 vec2::operator-(vec2 const& rhs) const
{
	return vec2{x - rhs.x, y - rhs.y};
}

vec2 vec2::operator*(vec2 const& rhs) const
{
	return vec2{x * rhs.x, y * rhs.y};
}

vec2 vec2::operator+(float const& rhs) const
{
	return vec2{x + rhs, y + rhs};
}

vec2 vec2::operator-(float const& rhs) const
{
	return vec2{x - rhs, y - rhs};
}

vec2 vec2::operator*(float const& rhs) const
{
	return vec2{x * rhs, y * rhs};
}

vec2 vec2::operator/(float const& rhs) const
{
	return vec2{x / rhs, y / rhs};
}

vec2& vec2::operator+=(vec2 const& rhs)
{
	x += rhs.x;
	y += rhs.y;
	return *this;
}

vec2& vec2::operator-=(vec2 const& rhs)
{
	x -= rhs.x;
	y -= rhs.y;
	return *this;
}

vec2& vec2::operator*=(vec2 const& rhs)
{
	x *= rhs.x;
	y *= rhs.y;
	return *this;
}

vec2& vec2::operator+=(float const& rhs)
{
	x += rhs;
	y += rhs;
	return *this;
}
vec2& vec2::operator-=(float const& rhs)
{
	x -= rhs;
	y -= rhs;
	return *this;
}

vec2& vec2::operator*=(float const& rhs)
{
	x *= rhs;
	y *= rhs;
	return *this;
}

vec2& vec2::operator/=(float const& rhs)
{
	x /= rhs;
	y /= rhs;
	return *this;
}

vec2 vec2::operator-()
{
	return vec2{-x, -y};
}

float vec2::dot(vec2 const& rhs) const
{
	return x * rhs.x + y * rhs.y;
}

float vec2::length() const
{
	return std::sqrt(std::pow(x, 2) + std::pow(y, 2));
}

vec2 vec2::normalized() const
{
	return (*this / length());
}

vec2 operator*(float const& lhs, vec2 const& rhs)
{
	return rhs*lhs;
}

std::ostream& operator<<(std::ostream& os, vec2 const& v)
{
    os << "(" << v.x << ", " << v.y << ")";
	return os;
}

std::size_t std::hash<vec2>::operator()(vec2 const& v) const noexcept
{
	size_t seed = 0;
	hash<float> hasher;
	hash_combine(seed, hasher(v.x));
	hash_combine(seed, hasher(v.y));
	return seed;
}


vec3::vec3()
: x{0.0f}, y{0.0f}, z{0.0f} {}

vec3::vec3(float const& value)
: x{value}, y{value}, z{value} {}

vec3::vec3(float const& pX, float const& pY, float const& pZ)
: x{pX}, y{pY}, z{pZ} {}

vec3::vec3(vec2 const& v)
: x{v.x}, y{v.y}, z{0.0f} {}

vec3::vec3(vec2 const& v, float const& pZ)
: x{v.x}, y{v.y}, z{pZ} {}

vec3::vec3(vec4 const& v)
: x{v.x}, y{v.y}, z{v.z} {}

vec3& vec3::operator=(vec2 const& rhs)
{
	return (*this = vec3{rhs});
}

vec3& vec3::operator=(vec4 const& rhs)
{
	return (*this = vec3{rhs});
}

bool vec3::operator==(vec3 const& rhs) const
{
	return x == rhs.x && y == rhs.y && z == rhs.z;
}

bool vec3::operator!=(vec3 const& rhs) const
{
	return !(*this == rhs);
}

vec3 vec3::operator+(vec3 const& rhs) const
{
	return vec3{x + rhs.x, y + rhs.y, z + rhs.z};
}

vec3 vec3::operator-(vec3 const& rhs) const
{
	return vec3{x - rhs.x, y - rhs.y, z - rhs.z};
}

vec3 vec3::operator*(vec3 const& rhs) const
{
	return vec3{x * rhs.x, y * rhs.y, z * rhs.z};
}

vec3 vec3::operator+(float const& rhs) const
{
	return vec3{x + rhs, y + rhs, z + rhs};
}

vec3 vec3::operator-(float const& rhs) const
{
	return vec3{x - rhs, y - rhs, z - rhs};
}

vec3 vec3::operator*(float const& rhs) const
{
	return vec3{x * rhs, y * rhs, z * rhs};
}

vec3 vec3::operator/(float const& rhs) const
{
	return vec3{x / rhs, y / rhs, z / rhs};
}

vec3& vec3::operator+=(vec3 const& rhs)
{
	x += rhs.x;
	y += rhs.y;
	z += rhs.z;
	return *this;
}

vec3& vec3::operator-=(vec3 const& rhs)
{
	x -= rhs.x;
	y -= rhs.y;
	z -= rhs.z;
	return *this;
}

vec3& vec3::operator*=(vec3 const& rhs)
{
	x *= rhs.x;
	y *= rhs.y;
	z *= rhs.z;
	return *this;
}

vec3& vec3::operator+=(float const& rhs)
{
	x += rhs;
	y += rhs;
	z += rhs;
	return *this;
}
vec3& vec3::operator-=(float const& rhs)
{
	x -= rhs;
	y -= rhs;
	z -= rhs;
	return *this;
}

vec3& vec3::operator*=(float const& rhs)
{
	x *= rhs;
	y *= rhs;
	z *= rhs;
	return *this;
}

vec3& vec3::operator/=(float const& rhs)
{
	x /= rhs;
	y /= rhs;
	z /= rhs;
	return *this;
}

vec3 vec3::operator-()
{
	return vec3{-x, -y, -z};
}

float vec3::dot(vec3 const& rhs) const
{
	return x * rhs.x + y * rhs.y + z * rhs.z;
}

vec3 vec3::cross(vec3 const& rhs) const
{
	return vec3{y*rhs.z - z*rhs.y,
		 		z*rhs.x - x*rhs.z,
		 		x*rhs.y - y*rhs.x};
}

float vec3::length() const
{
	return std::sqrt(std::pow(x, 2) + std::pow(y, 2) + std::pow(z, 2));
}

vec3 vec3::normalized() const
{
	return (*this / length());
}

vec3 operator*(float const& lhs, vec3 const& rhs)
{
	return rhs*lhs;
}

std::ostream& operator<<(std::ostream& os, vec3 const& v)
{
	os << "(" << v.x << ", " << v.y << ", " << v.z << ")";
	return os;
}

std::size_t std::hash<vec3>::operator()(vec3 const& v) const noexcept
{
	size_t seed = 0;
	hash<float> hasher;
	hash_combine(seed, hasher(v.x));
	hash_combine(seed, hasher(v.y));
	hash_combine(seed, hasher(v.z));
	return seed;
}


vec4::vec4()
: x{0.0f}, y{0.0f}, z{0.0f} {}

vec4::vec4(float const& value)
: x{value}, y{value}, z{value}, w{value} {}

vec4::vec4(float const& pX, float const& pY, float const& pZ, float const& pW)
: x{pX}, y{pY}, z{pZ}, w{pW} {}

vec4::vec4(vec2 const& v)
: x{v.x}, y{v.y}, z{0.0f}, w{0.0f} {}

vec4::vec4(vec2 const& v, float const& pZ)
: x{v.x}, y{v.y}, z{pZ}, w{0.0f} {}

vec4::vec4(vec2 const& v, float const& pZ, float const& pW)
: x{v.x}, y{v.y}, z{pZ}, w{pW} {}

vec4::vec4(vec3 const& v)
: x{v.x}, y{v.y}, z{v.z}, w{1.0f} {}

vec4::vec4(vec3 const& v, float const& pW)
: x{v.x}, y{v.y}, z{v.z}, w{pW} {}

vec4& vec4::operator=(vec2 const& rhs)
{
	return (*this = vec4{rhs});
}

vec4& vec4::operator=(vec3 const& rhs)
{
	return (*this = vec4{rhs});
}

bool vec4::operator==(vec4 const& rhs) const
{
	return x == rhs.x && y == rhs.y && z == rhs.z && w == rhs.w;
}

bool vec4::operator!=(vec4 const& rhs) const
{
	return !(*this == rhs);
}

vec4 vec4::operator+(vec4 const& rhs) const
{
	return vec4{x + rhs.x, y + rhs.y, z + rhs.z, w + rhs.w};
}

vec4 vec4::operator-(vec4 const& rhs) const
{
	return vec4{x - rhs.x, y - rhs.y, z - rhs.z, w - rhs.w};
}

vec4 vec4::operator*(vec4 const& rhs) const
{
	return vec4{x * rhs.x, y * rhs.y, z * rhs.z, w * rhs.w};
}

vec4 vec4::operator+(float const& rhs) const
{
	return vec4{x + rhs, y + rhs, z + rhs, w + rhs};
}

vec4 vec4::operator-(float const& rhs) const
{
	return vec4{x - rhs, y - rhs, z - rhs, w - rhs};
}

vec4 vec4::operator*(float const& rhs) const
{
	return vec4{x * rhs, y * rhs, z * rhs, w * rhs};
}

vec4 vec4::operator/(float const& rhs) const
{
	return vec4{x / rhs, y / rhs, z / rhs, w / rhs};
}

vec4& vec4::operator+=(vec4 const& rhs)
{
	x += rhs.x;
	y += rhs.y;
	z += rhs.z;
	w += rhs.w;
	return *this;
}

vec4& vec4::operator-=(vec4 const& rhs)
{
	x -= rhs.x;
	y -= rhs.y;
	z -= rhs.z;
	w += rhs.w;
	return *this;
}

vec4& vec4::operator*=(vec4 const& rhs)
{
	x *= rhs.x;
	y *= rhs.y;
	z *= rhs.z;
	w *= rhs.w;
	return *this;
}

vec4& vec4::operator+=(float const& rhs)
{
	x += rhs;
	y += rhs;
	z += rhs;
	w += rhs;
	return *this;
}
vec4& vec4::operator-=(float const& rhs)
{
	x -= rhs;
	y -= rhs;
	z -= rhs;
	w -= rhs;
	return *this;
}

vec4& vec4::operator*=(float const& rhs)
{
	x *= rhs;
	y *= rhs;
	z *= rhs;
	w *= rhs;
	return *this;
}

vec4& vec4::operator/=(float const& rhs)
{
	x /= rhs;
	y /= rhs;
	z /= rhs;
	w /= rhs;
	return *this;
}

vec4 vec4::operator-()
{
	return vec4{-x, -y, -z, -w};
}

float vec4::dot(vec4 const& rhs) const
{
	return x * rhs.x + y * rhs.y + z * rhs.z + w * rhs.w;
}

float vec4::length() const
{
	return std::sqrt(std::pow(x, 2) +
					 std::pow(y, 2) +
					 std::pow(z, 2) +
					 std::pow(w, 2));
}

vec4 vec4::normalized() const
{
	return (*this / length());
}

vec4 operator*(float const& lhs, vec4 const& rhs)
{
	return rhs*lhs;
}

std::ostream& operator<<(std::ostream& os, vec4 const& v)
{
	os << "(" << v.x << ", " << v.y << ", " << v.z << ", " << v.w << ")";
	return os;
}

std::size_t std::hash<vec4>::operator()(vec4 const& v) const noexcept
{
	size_t seed = 0;
	hash<float> hasher;
	hash_combine(seed, hasher(v.x));
	hash_combine(seed, hasher(v.y));
	hash_combine(seed, hasher(v.z));
	hash_combine(seed, hasher(v.w));
	return seed;
}

mat3::mat3()
: m{1.0f, 0.0f, 0.0f,
	0.0f, 1.0f, 0.0f,
	0.0f, 0.0f, 1.0f} {}

mat3::mat3(float const& value)
: m{value, 0.0f, 0.0f,
	0.0f, value, 0.0f,
	0.0f, 0.0f, value} {}


mat3::mat3(float const& p00, float const& p11, float const& p22)
: m{p00,  0.0f, 0.0f,
    0.0f, p11,  0.0f,
    0.0f, 0.0f, p22} {}

mat3::mat3(float const& p00, float const& p01, float const& p02,
		   float const& p10, float const& p11, float const& p12,
		   float const& p20, float const& p21, float const& p22)
: m{p00, p01, p02,
	p10, p11, p12,
	p20, p21, p22} {}

mat3::mat3(vec3 const& v1, vec3 const& v2, vec3 const& v3)
: m{v1.x, v1.y, v1.z,
	v2.x, v2.y, v2.z,
	v3.x, v3.y, v3.z}
{}

mat3::mat3(mat4 const& mat)
: m{mat[0], mat[1], mat[2],
	mat[4], mat[5], mat[6],
	mat[8], mat[9], mat[10]} {}

float const& mat3::operator[](size_t const& index) const
{
	if (index > 8)
		throw std::out_of_range("Index must be between [0, 8] when accesing mat3");
	return m[index];
}

float& mat3::operator[](size_t const& index)
{
	if (index > 8)
		throw std::out_of_range("Index must be between [0, 8] when accesing mat3");
	return m[index];
}

float const& mat3::operator[](std::pair<size_t, size_t> const& index) const
{
	const uint8_t width = 3;
	const size_t row = index.first * width;
	const size_t column = index.second;
	if (row + column > 8)
		throw std::out_of_range("Index must be between {[0, 2], [0, 2]} when accesing mat3");
	return m[row + column];
}

float& mat3::operator[](std::pair<size_t, size_t> const& index)
{
	const uint8_t width = 3;
	const size_t row = index.first * width;
	const size_t column = index.second;
	if (row + column > 8)
		throw std::out_of_range("Index must be between {[0, 2], [0, 2]} when accesing mat3");
	return m[row + column];
}


mat3 mat3::operator+(mat3 const& rhs) const
{
	mat3 dest;
	
	for (size_t i = 0; i < 9; ++i)
		dest[i] = m[i] + rhs[i];
	
	return dest;
}

mat3 mat3::operator*(mat3 const& rhs) const
{
	mat3 ret;

	int x, y;
	for (x = 0; x <= 2; ++x)
		for (y = 0; y <= 2; ++y)
			if (_automaticTranspose)
				ret.m[x*3 + y] = m[y+3*0] * rhs.m[0+3*x] +
								 m[y+3*1] * rhs.m[1+3*x] +
								 m[y+3*2] * rhs.m[2+3*x];
			else
				ret.m[y*3 + x] = m[y*3+0] * rhs.m[0*3+x] +
								 m[y*3+1] * rhs.m[1*3+x] +
								 m[y*3+2] * rhs.m[2*3+x];

	return ret;
}

vec3 mat3::operator*(vec3 const& rhs) const
{
	vec3 v;
	
	if (_automaticTranspose)
	{
		v.x = m[0]*rhs.x + m[3]*rhs.y + m[6]*rhs.z;
		v.y = m[1]*rhs.x + m[4]*rhs.y + m[7]*rhs.z;
		v.z = m[2]*rhs.x + m[5]*rhs.y + m[8]*rhs.z;
	}

	else
	{
		v.x = m[0]*rhs.x + m[1]*rhs.y + m[2]*rhs.z;
		v.y = m[3]*rhs.x + m[4]*rhs.y + m[5]*rhs.z;
		v.z = m[6]*rhs.x + m[7]*rhs.y + m[8]*rhs.z;
	}
	
	return v;
}

mat3 mat3::transposed() const
{
	return mat3{m[0], m[3], m[6],
				m[1], m[4], m[7],
				m[2], m[5], m[8]};
}

mat3 mat3::inverted() const
{
	float a11, a12, a13, a21, a22, a23, a31, a32, a33;
	mat3 out;
	float DET;
	
	// Copying to internal variables both clarify the code and
	// buffers data so the output may be identical to the input!
	a11 = m[0];
	a12 = m[1];
	a13 = m[2];
	a21 = m[3];
	a22 = m[4];
	a23 = m[5];
	a31 = m[6];
	a32 = m[7];
	a33 = m[8];
	DET = a11*(a33*a22-a32*a23)-a21*(a33*a12-a32*a13)+a31*(a23*a12-a22*a13);
	if (DET != 0)
	{
		out.m[0] = (a33*a22-a32*a23)/DET;
		out.m[1] = -(a33*a12-a32*a13)/DET;
		out.m[2] = (a23*a12-a22*a13)/DET;
		out.m[3] = -(a33*a21-a31*a23)/DET;
		out.m[4] = (a33*a11-a31*a13)/DET;
		out.m[5] = -(a23*a11-a21*a13)/DET;
		out.m[6] = (a32*a21-a31*a22)/DET;
		out.m[7] = -(a32*a11-a31*a12)/DET;
		out.m[8] = (a22*a11-a21*a12)/DET;
		return out;
	}
	else
		return mat3{NAN, NAN, NAN,
				    NAN, NAN, NAN,
				    NAN, NAN, NAN};
}

std::ostream& operator<<(std::ostream& os, mat3 const& m)
{
	os << "-------------------------------------------------" << std::endl;
	for (size_t i = 0; i < 3; ++i)
	{
		size_t n = i * 3;
		os << "|" << std::setw(11) << std::fixed << std::setprecision(2) << m[n] << "\t| "
		   << std::setw(11) << std::fixed << std::setprecision(2) << m[n + 1] << "\t| "
		   << std::setw(11) << std::fixed << std::setprecision(2) << m[n + 2] << "\t| " << std::endl;
	}
	os << "-------------------------------------------------" << std::endl;
	return os;
}


mat4::mat4()
: m{1.0f, 0.0f, 0.0f, 0.0f,
	0.0f, 1.0f, 0.0f, 0.0f,
	0.0f, 0.0f, 1.0f, 0.0f,
	0.0f, 0.0f, 0.0f, 1.0f} {}

mat4::mat4(float const& value)
: m{value, 0.0f, 0.0f, 0.0f,
	0.0f, value, 0.0f, 0.0f,
	0.0f, 0.0f, value, 0.0f,
	0.0f, 0.0f, 0.0f, value} {}

mat4::mat4(float const& p00, float const& p11, float const& p22, float const& p33)
: m{p00,  0.0f, 0.0f, 0.0f,
	0.0f, p11,  0.0f, 0.0f,
	0.0f, 0.0f, p22,  0.0f,
	0.0f, 0.0f, 0.0f, p33} {}

mat4::mat4(float const& p00, float const& p01, float const& p02, float const& p03,
		   float const& p10, float const& p11, float const& p12, float const& p13,
		   float const& p20, float const& p21, float const& p22, float const& p23,
		   float const& p30, float const& p31, float const& p32, float const& p33)
: m{p00, p01, p02, p03,
	p10, p11, p12, p13,
	p20, p21, p22, p23,
	p30, p31, p32, p33} {}

mat4::mat4(mat3 const& mat)
: m{mat[0], mat[1], mat[2], 0.0f,
	mat[3], mat[4], mat[5], 0.0f,
	mat[6], mat[7], mat[8], 0.0f,
	0.0f, 0.0f, 0.0f, 1.0f} {}


float const& mat4::operator[](size_t const& index) const
{
	if (index > 15)
		throw std::out_of_range("Index must be between [0, 15] when accesing mat4");
	return m[index];
}

float& mat4::operator[](size_t const& index)
{
	if (index > 15)
		throw std::out_of_range("Index must be between [0, 15] when accesing mat4");
	return m[index];
}

float const& mat4::operator[](std::pair<size_t, size_t> const& index) const
{
	const uint8_t width = 4;
	const size_t row = index.first * width;
	const size_t column = index.second;
	if (row + column > 15)
		throw std::out_of_range("Index must be between {[0, 2], [0, 2]} when accesing mat3");
	return m[row + column];
}

float& mat4::operator[](std::pair<size_t, size_t> const& index)
{
	const uint8_t width = 4;
	const size_t row = index.first * width;
	const size_t column = index.second;
	if (row + column > 15)
		throw std::out_of_range("Index must be between {[0, 2], [0, 2]} when accesing mat3");
	return m[row + column];
}

mat4 mat4::operator+(mat4 const& rhs) const
{
	mat4 dest;
	
	for (size_t i = 0; i < 16; ++i)
		dest[i] = m[i] + rhs[i];
	
	return dest;
}

mat4 mat4::operator*(mat4 const& rhs) const
{
	mat4 ret;

	int x, y;
	for (x = 0; x <= 3; x++)
		for (y = 0; y <= 3; y++)
			if (_automaticTranspose)
				ret.m[x*4 + y] = m[y+4*0] * rhs.m[0+4*x] +
								 m[y+4*1] * rhs.m[1+4*x] +
								 m[y+4*2] * rhs.m[2+4*x] +
								 m[y+4*3] * rhs.m[3+4*x];
			else
				ret.m[y*4 + x] = m[y*4+0] * rhs.m[0*4+x] +
								 m[y*4+1] * rhs.m[1*4+x] +
								 m[y*4+2] * rhs.m[2*4+x] +
								 m[y*4+3] * rhs.m[3*4+x];

	return ret;
}

vec4 mat4::operator*(vec4 const& rhs) const
{
	vec4 r;

	if (_automaticTranspose)
	{
		r.x = m[0]*rhs.x + m[4]*rhs.y + m[8]*rhs.z + m[12]*rhs.w;
		r.y = m[1]*rhs.x + m[5]*rhs.y + m[9]*rhs.z + m[13]*rhs.w;
		r.z = m[2]*rhs.x + m[6]*rhs.y + m[10]*rhs.z + m[14]*rhs.w;
		r.w = m[3]*rhs.x + m[7]*rhs.y + m[11]*rhs.z + m[15]*rhs.w;
	}
	else
	{
		r.x = m[0]*rhs.x + m[1]*rhs.y + m[2]*rhs.z + m[3]*rhs.w;
		r.y = m[4]*rhs.x + m[5]*rhs.y + m[6]*rhs.z + m[7]*rhs.w;
		r.z = m[8]*rhs.x + m[9]*rhs.y + m[10]*rhs.z + m[11]*rhs.w;
		r.w = m[12]*rhs.x + m[13]*rhs.y + m[14]*rhs.z + m[15]*rhs.w;
	}
	
	return r;
}

mat4 mat4::transposed() const
{
	return mat4{m[0], m[4], m[8], m[12],
				m[1], m[5], m[9], m[13],
				m[2], m[6], m[10], m[14],
				m[3], m[7], m[11], m[15]};
}

mat4 mat4::inverted() const
{
	mat4 res;
     
   	float c=m[0],d=m[1],e=m[2],g=m[3],
	f=m[4],h=m[5],i=m[6],j=m[7],
	k=m[8],l=m[9],o=m[10],mm=m[11],
	n=m[12],p=m[13],r=m[14],s=m[15],
	A=c*h-d*f,
	B=c*i-e*f,
	t=c*j-g*f,
	u=d*i-e*h,
	v=d*j-g*h,
	w=e*j-g*i,
	x=k*p-l*n,
	y=k*r-o*n,
	z=k*s-mm*n,
	C=l*r-o*p,
	D=l*s-mm*p,
	E=o*s-mm*r,
	q=1/(A*E-B*D+t*C+u*z-v*y+w*x);
	res[0]=(h*E-i*D+j*C)*q;
	res[1]=(-d*E+e*D-g*C)*q;
	res[2]=(p*w-r*v+s*u)*q;
	res[3]=(-l*w+o*v-mm*u)*q;
	res[4]=(-f*E+i*z-j*y)*q;
	res[5]=(c*E-e*z+g*y)*q;
	res[6]=(-n*w+r*t-s*B)*q;
	res[7]=(k*w-o*t+mm*B)*q;
	res[8]=(f*D-h*z+j*x)*q;
	res[9]=(-c*D+d*z-g*x)*q;
	res[10]=(n*v-p*t+s*A)*q;
	res[11]=(-k*v+l*t-mm*A)*q;
	res[12]=(-f*C+h*y-i*x)*q;
	res[13]=(c*C-d*y+e*x)*q;
	res[14]=(-n*u+p*B-r*A)*q;
	res[15]=(k*u-l*B+o*A)*q;
	return res;
}

mat3 mat4::inverseTransposed() const
{
	float a11, a12, a13, a21, a22, a23, a31, a32, a33;
	mat3 out, nanout;
	float DET;
	
	// Copying to internal variables
	a11 = m[0];
	a12 = m[1];
	a13 = m[2];
	a21 = m[4];
	a22 = m[5];
	a23 = m[6];
	a31 = m[8];
	a32 = m[9];
	a33 = m[10];
	DET = a11*(a33*a22-a32*a23)-a21*(a33*a12-a32*a13)+a31*(a23*a12-a22*a13);
	if (DET != 0)
	{
		out[0] = (a33*a22-a32*a23)/DET;
		out[3] = -(a33*a12-a32*a13)/DET;
		out[6] = (a23*a12-a22*a13)/DET;
		out[1] = -(a33*a21-a31*a23)/DET;
		out[4] = (a33*a11-a31*a13)/DET;
		out[7] = -(a23*a11-a21*a13)/DET;
		out[2] = (a32*a21-a31*a22)/DET;
		out[5] = -(a32*a11-a31*a12)/DET;
		out[8] = (a22*a11-a21*a12)/DET;
		return out;
	}
	else
		return mat3(NAN, NAN, NAN,
					NAN, NAN, NAN,
					NAN, NAN, NAN);
}

std::ostream& operator<<(std::ostream& os, mat4 const& m)
{
	os << "-----------------------------------------------------------------" << std::endl;
	for (size_t i = 0; i < 4; ++i)
	{
		size_t n = i * 4;
		os << "|" << std::setw(11) << std::fixed << std::setprecision(2) << m[n] << "\t| "
		   << std::setw(11) << std::fixed << std::setprecision(2) << m[n + 1] << "\t| "
		   << std::setw(11) << std::fixed << std::setprecision(2) << m[n + 2] << "\t| "
		   << std::setw(11) << std::fixed << std::setprecision(2) << m[n + 3] << "\t| " << std::endl;
	}
	os << "-----------------------------------------------------------------" << std::endl;
	return os;
}


vec3 calculateNormalVector(vec3 const& a, vec3 const& b, vec3 const& c)
{
	vec3 n;

	n = (a - b).cross(a - c);
	n /= n.length();
	
	return n;
}

// Splits v into vn (parallell to n) and vp (perpendicular). Does not demand n to be normalized.
void splitVector(vec3 const& v, vec3 const& n, vec3* vn, vec3* vp)
{
	float nlen;
	float nlen2;

	nlen = v.dot(n);
	nlen2 = std::pow(n.x, 2) + std::pow(n.y, 2) + std::pow(n.y, 2);
	if (nlen2 == 0)
	{
		*vp = v;
		*vn = vec3{0, 0, 0};
	}
	else
	{
		*vn = n * nlen/nlen2;
		*vp = v - *vn;
	}
}

// Matrix operations primarily on 4x4 matrixes!
// Row-wise by default but can be configured to column-wise (see SetTransposed)

mat4 identityMatrix()
{
	return mat4{};
}

mat4 Rx(float const& angle)
{
	mat4 m{};
	m[5] = std::cos(angle);
	if (_automaticTranspose)
		m[9] = -std::sin(angle);
	else
		m[9] = std::sin(angle);
	m[6] = -m[9]; //sin(a);
	m[10] = m[5]; //cos(a);
	return m;
}

mat4 Ry(float const& angle)
{
	mat4 m{};
	m[0] = std::cos(angle);
	if (_automaticTranspose)
		m[8] = std::sin(angle);
	else
		m[8] = -std::sin(angle);
	m[2] = -m[8]; //sin(a);
	m[10] = m[0]; //cos(a);
	return m;
}

mat4 Rz(float const& angle)
{
	mat4 m{};
	m[0] = std::cos(angle);
	if (_automaticTranspose)
		m[4] = -std::sin(angle);
	else
		m[4] = std::sin(angle);
	m[1] = -m[4]; //sin(a);
	m[5] = m[0]; //cos(a);
	return m;
}

mat4 T(float const& tx, float const& ty, float const& tz)
{
	mat4 m{};
	if (_automaticTranspose)
	{
		m[12] = tx;
		m[13] = ty;
		m[14] = tz;
	}
	else
	{
		m[3] = tx;
		m[7] = ty;
		m[11] = tz;
	}
	return m;
}

mat4 S(float const& sx, float const& sy, float const& sz)
{
	return mat4{sx, sy, sz, 1.0f};
}

mat4 S(float const& value)
{
	return mat4{value, value, value, 1.0f};
}

mat4 Sx(float const& value)
{
	return mat4{value, 1.0f, 1.0f, 1.0f};
}

mat4 Sy(float const& value)
{
	return mat4{1.0f, value, 1.0f, 1.0f};
}

mat4 Sz(float const& value)
{
	return mat4{1.0f, 1.0f, value, 1.0f};
}


// Added for lab 3 (TSBK03)

// Orthonormalization of Matrix4D. Assumes rotation only, translation/projection ignored
void orthoNormalizeMatrix(mat4& R)
{
	vec3 x, y, z;

	if (_automaticTranspose)
	{
		x = vec3(R[0], R[1], R[2]);
		y = vec3(R[4], R[5], R[6]);
		// Get the cross product from each other
		// Normalize
		z = x.cross(y);
		z = z.normalized();
		x = x.normalized();
		y = z.cross(x);
		R[0] = x.x;
		R[1] = x.y;
		R[2] = x.z;
		R[4] = y.x;
		R[5] = y.y;
		R[6] = y.z;
		R[8] = z.x;
		R[9] = z.y;
		R[10] = z.z;

		R[3] = 0.0;
		R[7] = 0.0;
		R[11] = 0.0;
		R[12] = 0.0;
		R[13] = 0.0;
		R[14] = 0.0;
		R[15] = 1.0;
	}
	else
	{
	// NOT TESTED
		x = vec3(R[0], R[4], R[8]);
		y = vec3(R[1], R[5], R[9]);
		// Get the cross product from each other
		// Normalize
		z = x.cross(y);
		z = z.normalized();
		x = x.normalized();
		y = z.cross(x);
		R[0] = x.x;
		R[4] = x.y;
		R[8] = x.z;
		R[1] = y.x;
		R[5] = y.y;
		R[9] = y.z;
		R[2] = z.x;
		R[6] = z.y;
		R[10] = z.z;

		R[3] = 0.0;
		R[7] = 0.0;
		R[11] = 0.0;
		R[12] = 0.0;
		R[13] = 0.0;
		R[14] = 0.0;
		R[15] = 1.0;
	}
}

// Rotation around arbitrary axis (rotation only)
mat4 arbRotate(vec3 const& axis, float const& fi)
{
	vec3 x, y, z;
	mat4 R, Rt, Raxel;

// Check if parallel to Z
	if (axis.x < 0.0000001 && // Below some small value
	    axis.x > -0.0000001 &&
		axis.y < 0.0000001 &&
		axis.y > -0.0000001)
	{
		if (axis.z > 0)
			return Rz(fi);
		else
			return Rz(-fi);
	}

	x = axis.normalized();
	z = vec3(0, 0, 1); // Temp z
	y = z.cross(x).normalized(); // y' = z^ x x'
	z = x.cross(y); // z' = x x y

	if (_automaticTranspose)
	{
		R[0] = x.x; R[4] = x.y; R[8] =  x.z;  R[12] =  0.0;
		R[1] = y.x; R[5] = y.y; R[9] =  y.z;  R[13] =  0.0;
		R[2] = z.x; R[6] = z.y; R[10] = z.z;  R[14] = 0.0;
		R[3] = 0.0; R[7] = 0.0; R[11] = 0.0;  R[15] = 1.0;
	}
	else
	{
		R[0] =  x.x; R[1] =  x.y; R[2] =  x.z;  R[3] =  0.0;
		R[4] =  y.x; R[5] =  y.y; R[6] =  y.z;  R[7] =  0.0;
		R[8] =  z.x; R[9] =  z.y; R[10] = z.z;  R[11] = 0.0;
		R[12] = 0.0; R[13] = 0.0; R[14] = 0.0;  R[15] = 1.0;
	}

	Rt = R.transposed(); // Transpose = Invert -> felet ej i Transpose, och det \8Ar en ortonormal matris

	Raxel = Rx(fi); // Rotate around x axis

	// m := Rt * Rx * R
	return Rt * Raxel * R;
}


// Not tested much
mat4 crossMatrix(vec3 const& a) // Matrix for cross product
{
	mat4 res;
	
	if (_automaticTranspose)
	{
		res[0] = 0.0; res[4] =-a.z; res[8] =  a.y; res[12] = 0.0;
		res[1] = a.z; res[5] = 0.0; res[9] = -a.x; res[13] = 0.0;
		res[2] =-a.y; res[6] = a.x; res[10] = 0.0; res[14] = 0.0;
		res[3] = 0.0; res[7] = 0.0; res[11] = 0.0; res[15] = 0.0;
		// NOTE! 0.0 in the homogous coordinate. Thus, the matrix can
		// not be generally used, but is fine for matrix differentials
	}
	else
	{
		res[0] =  0.0; res[1] = -a.z; res[2] =  a.y; res[3] =  0.0;
		res[4] =  a.z; res[5] =  0.0; res[6] = -a.x; res[7] =  0.0;
		res[8] = -a.y; res[9] =  a.x; res[10] = 0.0; res[11] = 0.0;
		res[12] = 0.0; res[13] = 0.0; res[14] = 0.0; res[15] = 0.0;
		// NOTE! 0.0 in the homogous coordinate. Thus, the matrix can
		// not be generally used, but is fine for matrix differentials
	}
	
	return res;
}

// false = row-wise defined matrices
// true = column-wise

void setAutomaticTranspose(bool const& value)
{
	_automaticTranspose = value;
}


// Build standard matrices

mat4 lookAt(vec3 const& position, vec3 const& target, vec3 up)
{
	vec3 n, u;
	mat4 rot, trans;

	n = (position - target).normalized();
	u = up.cross(n).normalized();
	up = n.cross(u);

	if (_automaticTranspose)
		rot = mat4(u.x, up.x, n.x, 0.0,
				   u.y, up.y, n.y, 0.0,
				   u.z, up.z, n.z, 0.0,
				   0.0, 0.0, 0.0, 1.0);
	else
		rot = mat4(u.x, u.y, u.z, 0.0,
				   up.x, up.y, up.z, 0.0,
				   n.x, n.y, n.z, 0.0,
				   0.0, 0.0, 0.0, 1.0);
	trans = T(-position.x, -position.y, -position.z);
	return rot * trans;
}

mat4 lookAt(float const& positionX, float const& positionY, float const& positionZ, 
			float const& targetX, float const& targetY, float const& targetZ,
			float const& upX, float const& upY, float const& upZ)
{
	vec3 position{positionX, positionY, positionZ};
	vec3 target{targetX, targetY, targetZ};
	vec3 up{upX, upY, upZ};
	
	return lookAt(position, target, up);
}

// From http://www.opengl.org/wiki/GluPerspective_code
// Changed names and parameter order to conform with VU style
// Rewritten 120913 because it was all wrong...

// Creates a projection matrix like gluPerspective or glFrustum.
// Upload to your shader as usual.
// Error fixed 20220525: 180, not 360!
// Also made it conform to the reference manual.
mat4 perspective(float const& fovyInDegrees, float const& aspectRatio,
                 float const& znear, float const& zfar)
{
	float f = 1/std::tan(fovyInDegrees / 2);
	mat4 m{f/aspectRatio, 0, 0, 0,
		   0, f, 0, 0,
		   0, 0, (zfar+znear)/(znear-zfar), 2*zfar*znear/(znear-zfar),
		   0, 0, -1, 0};
	if (_automaticTranspose)
		return m.transposed();
	return m;
}

mat4 frustum(float const& left, float const& right,
			 float const& bottom, float const& top,
             float const& znear, float const& zfar)
{
    float temp, temp2, temp3, temp4;
    
    temp = 2.0f * znear;
    temp2 = right - left;
    temp3 = top - bottom;
    temp4 = zfar - znear;

	mat4 res{temp/temp2, 0.0, (right+left)/temp2, 0.0,
			 0.0, temp/temp3, (top+bottom)/temp3, 0.0,
			 0.0, 0.0, (-zfar+znear)/temp4, (-temp*zfar)/temp4,
			 0.0, 0.0, -1.0, 0.0};
    
    if (_automaticTranspose)
    	return res.transposed();
    return res;
}

// Not tested!
mat4 ortho(float const& left, float const& right,
		   float const& bottom, float const& top,
		   float const& near, float const& far)
{
        float a = 2.0f / (right - left);
        float b = 2.0f / (top - bottom);
        float c = -2.0f / (far - near);

        float tx = - (right + left)/(right - left);
        float ty = - (top + bottom)/(top - bottom);
        float tz = - (far + near)/(far - near);

        mat4 o{a, 0, 0, tx,
               0, b, 0, ty,
               0, 0, c, tz,
               0, 0, 0, 1};

        if (_automaticTranspose)
   			return o.transposed();
        return o;
}
