#include <math.h>
#include <stdlib.h>
#include <stdio.h>
#include <GL/glew.h>
#include <string.h>

void vec3Zero(GLfloat* out) {
	for (int i = 0; i < 3; i++) {
		out[i] = 0;
	}
}

void vec3Add(GLfloat* out, GLfloat* a, GLfloat* b) {
	for (int i = 0; i < 3; i++) {
		out[i] = a[i] + b[i];
	}
}

void vec3Multiply(GLfloat* out, GLfloat* a, GLfloat x) {
	for (int i = 0; i < 3; i++) {
		out[i] = a[i] * x;
	}
}

void vec3Subtract(GLfloat* out, GLfloat* a, GLfloat* b) {
	vec3Multiply(out, b, -1);
	vec3Add(out, a, out);
}

void vec3Cross(GLfloat* out, GLfloat* a, GLfloat* b) {
	GLfloat result[3];
	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];
	memcpy(out, result, sizeof(result));
}

GLfloat vec3Length(GLfloat* a) {
	return (GLfloat)sqrt(a[0]*a[0] + a[1]*a[1] + a[2]*a[2]);
}

GLfloat vec3Dot(GLfloat* a, GLfloat* b) {
	return a[0] * b[0] + a[1] * b[1] + a[2] * b[2];
}

void vec3Normalise(GLfloat* out, GLfloat* a) {
	vec3Multiply(out, a, 1 / vec3Length(a));
}
// CREATE 4x4 IDENTITY MATRIX
void identity(GLfloat* out) {
	for (int i = 0; i < 16; i++) {
		out[i] = (i % 4 == i / 4);
	}
}

// CREATE 4x4 TRANSLATION MATRIX
void translation(GLfloat* out, GLfloat* v) {
	identity(out);
	for (int i = 0; i < 3; i++) {
		out[3 * 4 + i] = v[i];
	}
}

// CREATE 4x4 SCALING MATRIX
void scaling(GLfloat* out, GLfloat* v) {
	identity(out);
	for (int i = 0; i < 3; i++) {
		out[i * 5] = v[i];
	}
}

// CREATE 4x4 ROTATION MATRIX AROUND Z AXIS
/*  cos a  -sin a   0      0
 *  sin a   cos a   0      0
 *    0       0     1      0
 *    0       0     0      1
*/
void rotationZ(GLfloat* out, GLfloat angle) {
	identity(out);
	out[0] = cos(angle);
	out[1] = sin(angle);
	out[4] = -sin(angle);
	out[5] = cos(angle);
}

// CREATE 4x4 ROTATION MATRIX AROUND Y AXIS
void rotationY(GLfloat* out, GLfloat angle) {
	identity(out);
	out[0] = cos(angle);
	out[2] = -sin(angle);
	out[8] = sin(angle);
	out[10] = cos(angle);
}

// CREATE 4x4 ROTATION MATRIX AROUND Y AXIS
void rotationX(GLfloat* out, GLfloat angle) {
	identity(out);
	out[5] = cos(angle);
	out[6] = sin(angle);
	out[9] = -sin(angle);
	out[10] = cos(angle);
}

// MULTIPLY ANY TO MATRICES
void multiplyAny(GLfloat* A, GLfloat* B, GLfloat* out, int wA, int hA, int wB) {
	int sizeOut = hA * wB;
	GLfloat* result = (GLfloat*) malloc(sizeOut * sizeof(GLfloat));
	for (int i = 0; i < sizeOut; i++) {
		result[i] = 0;
		// printf("%d: ", i);
		for (int j = 0; j < wA; j++) {
			// printf("%d : %f ", j * hA + i % hA, A[j * hA + i % hA]);
			result[i] += A[j * hA + i % hA] * B[j + i / hA * wB];
		}
		// printf("\n");
	}
	memcpy(out, result, sizeOut * sizeof(GLfloat));
	free(result);
	result = NULL;
}

// MULTIPLY TWO 4x4 MATRICES
void multiply(GLfloat* A, GLfloat* B, GLfloat* out) {
	multiplyAny(A, B, out, 4, 4, 4);
}

// MULTIPLY in WITH TRANSLATION MATRIX OF v
void translate(GLfloat* out, GLfloat* in, GLfloat* v) {
	GLfloat translationMatrix[16];
	translation(translationMatrix, v);
	multiply(translationMatrix, in, out);
}

// MULTIPLY in WITH SCALING MATRIX OF v
void scale(GLfloat* out, GLfloat* in, GLfloat* v) {
	GLfloat scalingMatrix[16];
	scaling(scalingMatrix, v);
	multiply(scalingMatrix, in, out);
}

// MULTIPLY in WITH ROTATION MATRIX OF a AROUND Z AXIS
void rotateZ(GLfloat* out, GLfloat* in, GLfloat angle) {
	GLfloat rotationMatrix[16];
	rotationZ(rotationMatrix, angle);
	multiply(rotationMatrix, in, out);
}
// MULTIPLY in WITH ROTATION MATRIX OF a AROUND Y AXIS
void rotateY(GLfloat* out, GLfloat* in, GLfloat angle) {
	GLfloat rotationMatrix[16];
	rotationY(rotationMatrix, angle);
	multiply(rotationMatrix, in, out);
}
// MULTIPLY in WITH ROTATION MATRIX OF a AROUND X AXIS
void rotateX(GLfloat* out, GLfloat* in, GLfloat angle) {
	GLfloat rotationMatrix[16];
	rotationX(rotationMatrix, angle);
	multiply(rotationMatrix, in, out);
}

void transposeAny(GLfloat* out, GLfloat* in, int w, int h) {
	int size = w * h;
	GLfloat* result = (GLfloat*) malloc(size * sizeof(GLfloat));

	for (int i = 0; i < size; i++) {
		result[i] = in[(i % w) * h + i / w];
	}

	memcpy(out, result, size * sizeof(GLfloat));
	free(result);
	result = NULL;
}


void transpose(GLfloat* out, GLfloat* in) {
	transposeAny(out, in, 4, 4);
}

void printAny(GLfloat* M, int w, int h) {
	GLfloat* transposed = (GLfloat*) malloc(w * h * sizeof(GLfloat));
	transposeAny(transposed, M, w, h);

	for (int i = 0; i < h; i++) {
		for (int j = 0; j < w; j++) {
			printf("%.4f ", transposed[i * w + j]);
		}
		printf("\n");
	}
	free(transposed);
	transposed = NULL;
}

void vec3Print(GLfloat* a) {
	printAny(a, 1, 3);
}

void mat4Print(GLfloat* m) {
	printAny(m, 4, 4);
}

void lookAt(GLfloat* out, GLfloat* eye, GLfloat* center, GLfloat* up) {


	GLfloat n[3];
	vec3Subtract(n, eye, center);

	GLfloat u[3];
	vec3Cross(u, up, n);

	GLfloat v[3];
	vec3Cross(v, n, u);


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


	GLfloat Mr[16];
	identity(Mr);

	memcpy(&Mr[0], u, sizeof(GLfloat) * 3);
	memcpy(&Mr[4], v, sizeof(GLfloat) * 3);
	memcpy(&Mr[8], n, sizeof(GLfloat) * 3);
	transpose(Mr, Mr);


	GLfloat t[3];
	vec3Multiply(u, u, -1);
	vec3Multiply(v, v, -1);
	vec3Multiply(n, n, -1);



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


	memcpy(&Mr[12], t, sizeof(GLfloat) * 3);

	memcpy(out, Mr, sizeof(GLfloat) * 16);
}