#include "blitz/misc/Maths.h"

#include <cassert>

namespace blitz {
namespace maths {

Mat4f Perspective(float fovY, float aspectRatio, float zNear, float zFar) {
	const float tanHalfFovy = std::tan(fovY / 2.0f);

	Mat4f result{};
	result.x0 = 1.0f / (aspectRatio * tanHalfFovy);
	result.y1 = 1.0f / (tanHalfFovy);
	result.z2 = -(zFar + zNear) / (zFar - zNear);
	result.z3 = -1.0f;
	result.w2 = -(2.0f * zFar * zNear) / (zFar - zNear);

	return result;
}

Mat4f Look(const Vec3f& eye, const Vec3f& front, const Vec3f& up) {
	const Vec3f f = Normalize(front);
	const Vec3f s = Normalize(Cross(f, up));
	const Vec3f u = Cross(s, f);

	Mat4f result = Identity<float>();
	result.x0 = s.x;
	result.y0 = s.y;
	result.z0 = s.z;
	result.x1 = u.x;
	result.y1 = u.y;
	result.z1 = u.z;
	result.x2 = -f.x;
	result.y2 = -f.y;
	result.z2 = -f.z;
	result.w0 = -Dot(s, eye);
	result.w1 = -Dot(u, eye);
	result.w2 = Dot(f, eye);

	return result;
}

Mat4f Inverse(const Mat4f& mat) {
	float s0 = mat.at(0, 0) * mat.at(1, 1) - mat.at(1, 0) * mat.at(0, 1);
	float s1 = mat.at(0, 0) * mat.at(1, 2) - mat.at(1, 0) * mat.at(0, 2);
	float s2 = mat.at(0, 0) * mat.at(1, 3) - mat.at(1, 0) * mat.at(0, 3);
	float s3 = mat.at(0, 1) * mat.at(1, 2) - mat.at(1, 1) * mat.at(0, 2);
	float s4 = mat.at(0, 1) * mat.at(1, 3) - mat.at(1, 1) * mat.at(0, 3);
	float s5 = mat.at(0, 2) * mat.at(1, 3) - mat.at(1, 2) * mat.at(0, 3);

	float c5 = mat.at(2, 2) * mat.at(3, 3) - mat.at(3, 2) * mat.at(2, 3);
	float c4 = mat.at(2, 1) * mat.at(3, 3) - mat.at(3, 1) * mat.at(2, 3);
	float c3 = mat.at(2, 1) * mat.at(3, 2) - mat.at(3, 1) * mat.at(2, 2);
	float c2 = mat.at(2, 0) * mat.at(3, 3) - mat.at(3, 0) * mat.at(2, 3);
	float c1 = mat.at(2, 0) * mat.at(3, 2) - mat.at(3, 0) * mat.at(2, 2);
	float c0 = mat.at(2, 0) * mat.at(3, 1) - mat.at(3, 0) * mat.at(2, 1);

	float det = s0 * c5 - s1 * c4 + s2 * c3 + s3 * c2 - s4 * c1 + s5 * c0;

	assert(det != 0 && "Determinant equals 0 !");

	float invdet = 1.0 / det;

	Mat4f result;

	result.at(0, 0) = (mat.at(1, 1) * c5 - mat.at(1, 2) * c4 + mat.at(1, 3) * c3) * invdet;
	result.at(0, 1) = (-mat.at(0, 1) * c5 + mat.at(0, 2) * c4 - mat.at(0, 3) * c3) * invdet;
	result.at(0, 2) = (mat.at(3, 1) * s5 - mat.at(3, 2) * s4 + mat.at(3, 3) * s3) * invdet;
	result.at(0, 3) = (-mat.at(2, 1) * s5 + mat.at(2, 2) * s4 - mat.at(2, 3) * s3) * invdet;

	result.at(1, 0) = (-mat.at(1, 0) * c5 + mat.at(1, 2) * c2 - mat.at(1, 3) * c1) * invdet;
	result.at(1, 1) = (mat.at(0, 0) * c5 - mat.at(0, 2) * c2 + mat.at(0, 3) * c1) * invdet;
	result.at(1, 2) = (-mat.at(3, 0) * s5 + mat.at(3, 2) * s2 - mat.at(3, 3) * s1) * invdet;
	result.at(1, 3) = (mat.at(2, 0) * s5 - mat.at(2, 2) * s2 + mat.at(2, 3) * s1) * invdet;

	result.at(2, 0) = (mat.at(1, 0) * c4 - mat.at(1, 1) * c2 + mat.at(1, 3) * c0) * invdet;
	result.at(2, 1) = (-mat.at(0, 0) * c4 + mat.at(0, 1) * c2 - mat.at(0, 3) * c0) * invdet;
	result.at(2, 2) = (mat.at(3, 0) * s4 - mat.at(3, 1) * s2 + mat.at(3, 3) * s0) * invdet;
	result.at(2, 3) = (-mat.at(2, 0) * s4 + mat.at(2, 1) * s2 - mat.at(2, 3) * s0) * invdet;

	result.at(3, 0) = (-mat.at(1, 0) * c3 + mat.at(1, 1) * c1 - mat.at(1, 2) * c0) * invdet;
	result.at(3, 1) = (mat.at(0, 0) * c3 - mat.at(0, 1) * c1 + mat.at(0, 2) * c0) * invdet;
	result.at(3, 2) = (-mat.at(3, 0) * s3 + mat.at(3, 1) * s1 - mat.at(3, 2) * s0) * invdet;
	result.at(3, 3) = (mat.at(2, 0) * s3 - mat.at(2, 1) * s1 + mat.at(2, 2) * s0) * invdet;

	return result;
}

}  // namespace maths
}  // namespace blitz
