#pragma once

class Matrix4x4
{
public:
	union
	{
		double m[16];
		double _m[4][4];
	};

	Matrix4x4() { LoadIdentity(); }

	void LoadNull()
	{
		for (int i = 0; i < 16; i++)
			m[i] = 0;
	}

	void LoadIdentity()
	{
		m[0] = m[5] = m[10] = m[15] = 1;
		m[1] = m[2] = m[3] = m[4] =
		m[6] = m[7] = m[8] = m[9] =
		m[11] = m[12] = m[13] = m[14] = 0;
	}

	void SetAxis(int axis, const Vec3& value)
	{
		m[0 + axis] = value.x;
		m[4 + axis] = value.y;
		m[8 + axis] = value.z;
	}

	void SetTranslation(const Vec3& value)
	{
		m[12] = value.x;
		m[13] = value.y;
		m[14] = value.z;
	}

	Vec3 TransformPoint(const Vec3& point)
	{
		return Vec3(
			(double)point.x * m[0] +
			(double)point.y * m[4] +
			(double)point.z * m[8] +
			m[12],

			(double)point.x * m[1] +
			(double)point.y * m[5] +
			(double)point.z * m[9] +
			m[13],

			(double)point.x * m[2] +
			(double)point.y * m[6] +
			(double)point.z * m[10] +
			m[14]);
	}

	Vec3 TransformDirection(const Vec3& dir)
	{
		return Vec3(
			(double)dir.x * m[0] +
			(double)dir.y * m[4] +
			(double)dir.z * m[8],
		
			(double)dir.x * m[1] +
			(double)dir.y * m[5] +
			(double)dir.z * m[9],

			(double)dir.x * m[2] +
			(double)dir.y * m[6] +
			(double)dir.z * m[10]);
	}

	bool Invert()
	{
		double tmp[12];
		double src[16];
		double dst[16];

		// Transpose matrix
		for (int i = 0; i < 4; i++) {
			src[i + 0] = m[i * 4 + 0];
			src[i + 4] = m[i * 4 + 1];
			src[i + 8] = m[i * 4 + 2];
			src[i + 12] = m[i * 4 + 3];
		}

		// Calculate pairs for first 8 elements (cofactors) 
		tmp[0] = src[10] * src[15];
		tmp[1] = src[11] * src[14];
		tmp[2] = src[9] * src[15];
		tmp[3] = src[11] * src[13];
		tmp[4] = src[9] * src[14];
		tmp[5] = src[10] * src[13];
		tmp[6] = src[8] * src[15];
		tmp[7] = src[11] * src[12];
		tmp[8] = src[8] * src[14];
		tmp[9] = src[10] * src[12];
		tmp[10] = src[8] * src[13];
		tmp[11] = src[9] * src[12];

		// Calculate first 8 elements (cofactors)
		dst[0] = tmp[0] * src[5] + tmp[3] * src[6] + tmp[4] * src[7];
		dst[0] -= tmp[1] * src[5] + tmp[2] * src[6] + tmp[5] * src[7];
		dst[1] = tmp[1] * src[4] + tmp[6] * src[6] + tmp[9] * src[7];
		dst[1] -= tmp[0] * src[4] + tmp[7] * src[6] + tmp[8] * src[7];
		dst[2] = tmp[2] * src[4] + tmp[7] * src[5] + tmp[10] * src[7];
		dst[2] -= tmp[3] * src[4] + tmp[6] * src[5] + tmp[11] * src[7];
		dst[3] = tmp[5] * src[4] + tmp[8] * src[5] + tmp[11] * src[6];
		dst[3] -= tmp[4] * src[4] + tmp[9] * src[5] + tmp[10] * src[6];
		dst[4] = tmp[1] * src[1] + tmp[2] * src[2] + tmp[5] * src[3];
		dst[4] -= tmp[0] * src[1] + tmp[3] * src[2] + tmp[4] * src[3];
		dst[5] = tmp[0] * src[0] + tmp[7] * src[2] + tmp[8] * src[3];
		dst[5] -= tmp[1] * src[0] + tmp[6] * src[2] + tmp[9] * src[3];
		dst[6] = tmp[3] * src[0] + tmp[6] * src[1] + tmp[11] * src[3];
		dst[6] -= tmp[2] * src[0] + tmp[7] * src[1] + tmp[10] * src[3];
		dst[7] = tmp[4] * src[0] + tmp[9] * src[1] + tmp[10] * src[2];
		dst[7] -= tmp[5] * src[0] + tmp[8] * src[1] + tmp[11] * src[2];

		// Calculate pairs for second 8 elements (cofactors)
		tmp[0] = src[2] * src[7];
		tmp[1] = src[3] * src[6];
		tmp[2] = src[1] * src[7];
		tmp[3] = src[3] * src[5];
		tmp[4] = src[1] * src[6];
		tmp[5] = src[2] * src[5];
		tmp[6] = src[0] * src[7];
		tmp[7] = src[3] * src[4];
		tmp[8] = src[0] * src[6];
		tmp[9] = src[2] * src[4];
		tmp[10] = src[0] * src[5];
		tmp[11] = src[1] * src[4];

		// Calculate second 8 elements (cofactors)
		dst[8] = tmp[0] * src[13] + tmp[3] * src[14] + tmp[4] * src[15];
		dst[8] -= tmp[1] * src[13] + tmp[2] * src[14] + tmp[5] * src[15];
		dst[9] = tmp[1] * src[12] + tmp[6] * src[14] + tmp[9] * src[15];
		dst[9] -= tmp[0] * src[12] + tmp[7] * src[14] + tmp[8] * src[15];
		dst[10] = tmp[2] * src[12] + tmp[7] * src[13] + tmp[10] * src[15];
		dst[10] -= tmp[3] * src[12] + tmp[6] * src[13] + tmp[11] * src[15];
		dst[11] = tmp[5] * src[12] + tmp[8] * src[13] + tmp[11] * src[14];
		dst[11] -= tmp[4] * src[12] + tmp[9] * src[13] + tmp[10] * src[14];
		dst[12] = tmp[2] * src[10] + tmp[5] * src[11] + tmp[1] * src[9];
		dst[12] -= tmp[4] * src[11] + tmp[0] * src[9] + tmp[3] * src[10];
		dst[13] = tmp[8] * src[11] + tmp[0] * src[8] + tmp[7] * src[10];
		dst[13] -= tmp[6] * src[10] + tmp[9] * src[11] + tmp[1] * src[8];
		dst[14] = tmp[6] * src[9] + tmp[11] * src[11] + tmp[3] * src[8];
		dst[14] -= tmp[10] * src[11] + tmp[2] * src[8] + tmp[7] * src[9];
		dst[15] = tmp[10] * src[10] + tmp[4] * src[8] + tmp[9] * src[9];
		dst[15] -= tmp[8] * src[9] + tmp[11] * src[10] + tmp[5] * src[8];

		// Calculate determinant
		double det = src[0] * dst[0] + src[1] * dst[1] + src[2] * dst[2] + src[3] * dst[3];

		if (fabs(det) < FLT_EPSILON)
		{
			return false;
		}
		else
		{
			// Calculate matrix inverse
			det = 1.0 / det;
			for (int i = 0; i < 16; i++)
				m[i] = dst[i] * det;
		}

		return true;
	}
};