#include "matrix-math.h"
#include <math.h>
#include <stdio.h>
#include <string.h>

/**
 * overwrites a 4x4 matrix with the identity matrix
 */
void mat4Identity(mat4 mat) {
	for (int i = 0; i < 16; i++) {
		mat[i] = (i % 4 == i / 4) ? 1 : 0;
	}
}

/**
 * copies a mat4 from src to dst
 */
void mat4Copy(mat4 src, mat4 dst) {
	for (int i = 0; i < 16; i++) {
		dst[i] = src[i];
	}
}

/**
 * sets all the values in a mat4 to zero
 */
void mat4Empty(mat4 mat) {
	for (int i = 0; i < 16; i++) {
		mat[i] = 0;
	}
}

/**
 * mutliplies A with B and stores the result in result
 */
void mat4Multiply(mat4 result, mat4 A, mat4 B) {
	// if result is one of the arguments, modify copy instead of result directly
	if (result == A || result == B) {
		mat4 tempResult;
		mat4Multiply(tempResult, A, B);
		mat4Copy(tempResult, result);
		return;
	}

	// loops over cells of the result matrix
	for (int i = 0; i < 16; i++) {
		int col = i / 4;
		int row = i % 4;

		// pointer to the current cell
		GLfloat* curr = &(result[i]);

		// initialise current cell with 0
		*curr = 0;

		// loop over the row of A and column of B
		// continuously adding the multiplication of the two values to the result cell
		for (int j = 0; j < 4; j++) {
			*curr += A[row + j * 4] * B[j + col * 4];
		}
	}
}

/**
 * prints a mat4 to the screen
 */
void mat4Print(mat4 m) {
	for (int i = 0; i < 4; i++) {
		for (int j = 0; j < 4; j++) {
			printf("%.2f ", m[j * 4 + i]);
		}
		printf("\n");
	}
}

/**
 * translates by the vector v
 */
void mat4Translate(mat4 out, mat4 in, vec3 v) {
	mat4 T;
	mat4Identity(T);
	
	T[12] = v[0];
	T[13] = v[1];
	T[14] = v[2];

	mat4Multiply(out, T, in);
}

/**
 * scales by the vector v
 */
void mat4Scale(mat4 out, mat4 in, vec3 v) {
	mat4 T;
	mat4Identity(T);

	T[0] = v[0];
	T[5] = v[1];
	T[10] = v[2];

	mat4Multiply(out, T, in);
}


/**
 * rotates around the X axis by the angle a (in radians)
 */
void mat4RotateX(mat4 out, mat4 in, GLfloat a) {
	mat4 T;
	mat4Identity(T);

	T[5] = cos(a);
	T[6] = sin(a);
	T[9] = -sin(a);
	T[10] = cos(a);
	
	mat4Multiply(out, T, in);
}

/**
 * rotates around the Y axis by the angle a (in radians)
 */
void mat4RotateY(mat4 out, mat4 in, GLfloat a) {
	mat4 T;
	mat4Identity(T);

	T[0] = cos(a);
	T[2] = -sin(a);
	T[8] = sin(a);
	T[10] = cos(a);
	
	mat4Multiply(out, T, in);
}

/**
 * rotates around the Z axis by the angle a (in radians)
 */
void mat4RotateZ(mat4 out, mat4 in, GLfloat a) {
	mat4 T;
	mat4Identity(T);

	T[0] = cos(a);
	T[1] = sin(a);
	T[4] = -sin(a);
	T[5] = cos(a);

	mat4Multiply(out, T, in);
}

/**
 * builds a look-at matrix, overwriting out
 * eye is the position of the camera
 * look is the position of the target to be looked at
 * up is the up vector
 */
void mat4BuildLookAt(mat4 out, vec3 eye, vec3 look, vec3 up) {
	vec3 n;
	vec3 u;
	vec3 v;
	vec3 t;

	vec3Subtract(n, eye, look);

	vec3CrossProduct(u, up, n);

	vec3CrossProduct(v, n, u);

	vec3Normalise(n, n);
	vec3Normalise(u, u);
	vec3Normalise(v, v);

	t[0] = - vec3DotProduct(u, eye);
	t[1] = - vec3DotProduct(v, eye);
	t[2] = - vec3DotProduct(n, eye);

	out[0] = u[0]; out[4] = u[1]; out[8]  = u[2]; out[12] = t[0];
	out[1] = v[0]; out[5] = v[1]; out[9]  = v[2]; out[13] = t[1];
	out[2] = n[0]; out[6] = n[1]; out[10] = n[2]; out[14] = t[2];
	out[3] = 0;    out[7] = 0;    out[11] = 0;    out[15] = 1;
}

/**
 * builds a projection matrix, overwriting out
 * r, l, t, b are the right, left, top and bottom of the frustum
 * n and f are the distance of the near and far planes from the camera
 */
void mat4BuildProjection(mat4 out, GLfloat r, GLfloat l, GLfloat t, GLfloat b, GLfloat n, GLfloat f) {
	mat4Identity(out);

	out[0] = 2.0f / (r - l);
	out[5] = 2.0f / (t - b);

	out[8] = 1.0f / n * (r + l) / (r - l);
	out[9] = 1.0f / n * (t + b) / (t - b);
	out[10] = -1.0f / n * (f + n) / (f - n);
	out[11] = -1.0f / n;

	out[14] = - 2.0f * f / (f - n);
	out[15] = 0.0f;
}

/**
 * builds a perspective projection matrix, overwriting out
 * fovy is the field of view in the y direction
 * aspect is the aspect ratio
 * n and f are the distance of the near and far planes from the camera
 */
void mat4BuildPerspective(mat4 out, GLfloat fovy, GLfloat aspect, GLfloat n, GLfloat f) {
	GLfloat t = n * tan(0.5f * fovy);
	GLfloat b = -t;

	GLfloat r = aspect * t;
	GLfloat l = -r;

	mat4BuildProjection(out, r, l, t, b, n, f);
}

/**
 * linearly interpolates between a and b, storing the result in out
 */
void mat4Interpolate(mat4 out, mat4 a, mat4 b, GLfloat f) {
	for (int i = 0; i < 16; i++) {
		out[i] = (1 - f) * a[i] + f * b[i];
	}
}

void mat4From3(mat4 out, mat3 in) {
	mat4Identity(out);
	memcpy(&out[0], &in[0], sizeof(GLfloat) * 3);
	memcpy(&out[4], &in[3], sizeof(GLfloat) * 3);
	memcpy(&out[8], &in[6], sizeof(GLfloat) * 3);
}

/**
 * https://stackoverflow.com/questions/1148309/inverting-a-4x4-matrix
 */
void mat4Inverse(mat4 out, mat4 m) {
	GLfloat inv[16], det;
	int i;

	inv[0] = m[5]  * m[10] * m[15] - 
	         m[5]  * m[11] * m[14] - 
	         m[9]  * m[6]  * m[15] + 
	         m[9]  * m[7]  * m[14] +
	         m[13] * m[6]  * m[11] - 
	         m[13] * m[7]  * m[10];

	inv[4] = -m[4]  * m[10] * m[15] + 
	          m[4]  * m[11] * m[14] + 
	          m[8]  * m[6]  * m[15] - 
	          m[8]  * m[7]  * m[14] - 
	          m[12] * m[6]  * m[11] + 
	          m[12] * m[7]  * m[10];

	inv[8] = m[4]  * m[9] * m[15] - 
	         m[4]  * m[11] * m[13] - 
	         m[8]  * m[5] * m[15] + 
	         m[8]  * m[7] * m[13] + 
	         m[12] * m[5] * m[11] - 
	         m[12] * m[7] * m[9];

	inv[12] = -m[4]  * m[9] * m[14] + 
	           m[4]  * m[10] * m[13] +
	           m[8]  * m[5] * m[14] - 
	           m[8]  * m[6] * m[13] - 
	           m[12] * m[5] * m[10] + 
	           m[12] * m[6] * m[9];

	inv[1] = -m[1]  * m[10] * m[15] + 
	          m[1]  * m[11] * m[14] + 
	          m[9]  * m[2] * m[15] - 
	          m[9]  * m[3] * m[14] - 
	          m[13] * m[2] * m[11] + 
	          m[13] * m[3] * m[10];

	inv[5] = m[0]  * m[10] * m[15] - 
	         m[0]  * m[11] * m[14] - 
	         m[8]  * m[2] * m[15] + 
	         m[8]  * m[3] * m[14] + 
	         m[12] * m[2] * m[11] - 
	         m[12] * m[3] * m[10];

	inv[9] = -m[0]  * m[9] * m[15] + 
	          m[0]  * m[11] * m[13] + 
	          m[8]  * m[1] * m[15] - 
	          m[8]  * m[3] * m[13] - 
	          m[12] * m[1] * m[11] + 
	          m[12] * m[3] * m[9];

	inv[13] = m[0]  * m[9] * m[14] - 
	          m[0]  * m[10] * m[13] - 
	          m[8]  * m[1] * m[14] + 
	          m[8]  * m[2] * m[13] + 
	          m[12] * m[1] * m[10] - 
	          m[12] * m[2] * m[9];

	inv[2] = m[1]  * m[6] * m[15] - 
	         m[1]  * m[7] * m[14] - 
	         m[5]  * m[2] * m[15] + 
	         m[5]  * m[3] * m[14] + 
	         m[13] * m[2] * m[7] - 
	         m[13] * m[3] * m[6];

	inv[6] = -m[0]  * m[6] * m[15] + 
	          m[0]  * m[7] * m[14] + 
	          m[4]  * m[2] * m[15] - 
	          m[4]  * m[3] * m[14] - 
	          m[12] * m[2] * m[7] + 
	          m[12] * m[3] * m[6];

	inv[10] = m[0]  * m[5] * m[15] - 
	          m[0]  * m[7] * m[13] - 
	          m[4]  * m[1] * m[15] + 
	          m[4]  * m[3] * m[13] + 
	          m[12] * m[1] * m[7] - 
	          m[12] * m[3] * m[5];

	inv[14] = -m[0]  * m[5] * m[14] + 
	           m[0]  * m[6] * m[13] + 
	           m[4]  * m[1] * m[14] - 
	           m[4]  * m[2] * m[13] - 
	           m[12] * m[1] * m[6] + 
	           m[12] * m[2] * m[5];

	inv[3] = -m[1] * m[6] * m[11] + 
	          m[1] * m[7] * m[10] + 
	          m[5] * m[2] * m[11] - 
	          m[5] * m[3] * m[10] - 
	          m[9] * m[2] * m[7] + 
	          m[9] * m[3] * m[6];

	inv[7] = m[0] * m[6] * m[11] - 
	         m[0] * m[7] * m[10] - 
	         m[4] * m[2] * m[11] + 
	         m[4] * m[3] * m[10] + 
	         m[8] * m[2] * m[7] - 
	         m[8] * m[3] * m[6];

	inv[11] = -m[0] * m[5] * m[11] + 
	           m[0] * m[7] * m[9] + 
	           m[4] * m[1] * m[11] - 
	           m[4] * m[3] * m[9] - 
	           m[8] * m[1] * m[7] + 
	           m[8] * m[3] * m[5];

	inv[15] = m[0] * m[5] * m[10] - 
	          m[0] * m[6] * m[9] - 
	          m[4] * m[1] * m[10] + 
	          m[4] * m[2] * m[9] + 
	          m[8] * m[1] * m[6] - 
	          m[8] * m[2] * m[5];

	det = m[0] * inv[0] + m[1] * inv[4] + m[2] * inv[8] + m[3] * inv[12];

	if (det == 0) {
		mat4Copy(m, out); // singular matrix, can't invert
		return;
	}

	det = 1.0 / det;

	for (i = 0; i < 16; i++)
		out[i] = inv[i] * det;

	return;
}


/**
 * subtracts b from a, storing the result in out
 */
void vec3Subtract(vec3 out, vec3 a, vec3 b) {
	out[0] = a[0] - b[0];
	out[1] = a[1] - b[1];
	out[2] = a[2] - b[2];
}

/**
 * calculates the cross product of a and b, storing the result in out
 */
void vec3CrossProduct(vec3 out, vec3 a, vec3 b) {
	vec3 result;
	result[0] = a[1] * b[2] - a[2] * b[1];
	result[1] = a[2] * b[0] - a[0] * b[2];
	result[2] = a[0] * b[1] - a[1] * b[0];

	out[0] = result[0];
	out[1] = result[1];
	out[2] = result[2];
}

/**
 * normalizes in storing the result in out
 */
void vec3Normalise(vec3 out, vec3 in) {
	GLfloat length = vec3Length(in);
	out[0] = in[0] / length;
	out[1] = in[1] / length;
	out[2] = in[2] / length;
}

/**
 * returns the length of the vector in
 */
GLfloat vec3Length(vec3 in) {
	return sqrt(in[0] * in[0] + in[1] * in[1] + in[2] * in[2]);
}

/**
 * returns the dot product of the vectors a and b
 */
GLfloat vec3DotProduct(vec3 a, vec3 b) {
	return a[0] * b[0] + a[1] * b[1] + a[2] * b[2];
}

/**
 * sets the values of out to x, y and z
 */
void vec3Set(vec3 out, GLfloat x, GLfloat y, GLfloat z) {
	out[0] = x;
	out[1] = y;
	out[2] = z;
}

void vec3Multiply(vec3 out, vec3 in, GLfloat x) {
	out[0] = in[0] * x;
	out[1] = in[1] * x;
	out[2] = in[2] * x;
}


void vec2Subtract(vec2 out, vec2 a, vec2 b) {
	out[0] = a[0] - b[0];
	out[1] = a[1] - b[1];
}


void mat3Copy(mat3 src, mat3 dst) {
	for (int i = 0; i < 9; i++) {
		dst[i] = src[i];
	}
}

void mat3From4(mat3 out, mat4 in) {
	memcpy(&out[0], &in[0], sizeof(GLfloat) * 3);
	memcpy(&out[3], &in[4], sizeof(GLfloat) * 3);
	memcpy(&out[6], &in[8], sizeof(GLfloat) * 3);
}

void mat3Transpose(mat3 out, mat3 in) {
	mat3 result;

	for (int i = 0; i < 3; i++) {
		for (int j = 0; j < 3; j++) {
			result[i * 3 + j] = in[j * 3 + i];
		}
	}

	mat3Copy(result, out);
}

void mat3Minor(mat3 out, mat3 in) {
	mat3 result;

	// TODO: check if this is correct
	result[0] = in[4] * in[8] - in[5] * in[7];
	result[1] = in[3] * in[8] - in[5] * in[6];
	result[2] = in[3] * in[7] - in[4] * in[6];

	result[3] = in[1] * in[8] - in[2] * in[7];
	result[4] = in[0] * in[8] - in[2] * in[6];
	result[5] = in[0] * in[7] - in[1] * in[6];

	result[6] = in[1] * in[5] - in[2] * in[4];
	result[7] = in[0] * in[5] - in[2] * in[3];
	result[8] = in[0] * in[4] - in[1] * in[3];

	mat3Copy(result, out);
}

void mat3Cofactor(mat3 out, mat3 in) {
	mat3Minor(out, in);

	out[1] *= -1;
	out[3] *= -1;
	out[5] *= -1;
	out[7] *= -1;
}

void mat3Adjoint(mat3 out, mat3 in) {
	mat3Cofactor(out, in);
	mat3Transpose(out, in);
}

void mat3MultiplyScalar(mat3 out, mat3 in, GLfloat x) {
	for (int i = 0; i < 9; i++) {
		out[i] = in[i] * x;
	}
}

GLfloat mat3Determinant(mat3 M) {
	return
		+ M[0] * M[4] * M[8]
		+ M[3] * M[7] * M[2]
		+ M[6] * M[1] * M[5]

		- M[2] * M[4] * M[6]
		- M[5] * M[7] * M[0]
		- M[8] * M[1] * M[3]
	;
}

void mat3Inverse(mat3 out, mat3 in) {
	mat3 result;
	mat3Adjoint(result, in);
	mat3MultiplyScalar(result, result, 1 / mat3Determinant(in));
	mat3Copy(result, out);
}