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

#include "matrixMath.h"
#include "transformation.h"


#define KRED  "\x1B[31m"
#define KGRN  "\x1B[32m"
#define KNRM  "\x1B[0m"

#define EPSILON 0.001f

int exitCode = 0;

void printTest(char* name, bool result) {
	if (result) {
		printf("%s    PASSED%s - %s\n", KGRN, KNRM, name);
	} else {
		printf("%s!!! FAILED%s - %s\n", KRED, KNRM, name);
		exitCode = 1;
	}
}

bool vec3Equals(vec3* a, vec3* b) {
    return (fabs(a->x - b->x) < EPSILON) &&
           (fabs(a->y - b->y) < EPSILON) &&
           (fabs(a->z - b->z) < EPSILON);
}

bool vec2Equals(vec2* a, vec2* b) {
    return (fabs(a->x - b->x) < EPSILON) &&
           (fabs(a->y - b->y) < EPSILON);
}


void testMat3SumDiff() {
	mat3 A = {1, 0, 0, -1, 0, -1, 1, -1, 9};
	mat3 B = {0, 1, -1, 0, 0, 1, 1, -1, 8};
	GLfloat target = 7;

	GLfloat value = mat3SumDiff(&A, &B);

	bool result = fabs(value - target) < EPSILON;

	if (!result) {
		printf("\nA:\n");
		mat3Print(&A);
		printf("\nB:\n");
		mat3Print(&B);
		printf("target: %f\n", target);
		printf("value: %f\n", value);
	}

	printTest("mat3SumDiff", result);
}

void testMat3Minor() {
	mat3 M = {2, 0, 1, -1, 5, -1, 3, 2, -2};
	mat3 target = {-8, 5, -17, -2, -7, 4, -5, -1, 10};

	mat3Minor(&M, &M);

	printTest("mat3Minor", mat3SumDiff(&M, &target) < EPSILON);
}

void testMat3Cofactor() {
	mat3 M = {2, 0, 1, -1, 5, -1, 3, 2, -2};
	mat3 target = {-8, -5, -17, 2, -7, -4, -5, 1, 10};

	mat3Cofactor(&M, &M);

	printTest("mat3Cofactor", mat3SumDiff(&M, &target) < EPSILON);
}

void testMat3Adjoint() {
	mat3 M = {1, 2, -1, 2, 1, 2, -1, 2, 1};
	mat3 target = {-3, -4, 5, -4, 0, -4, 5, -4, -3};


	mat3Adjoint(&M, &M);

	printTest("mat3Adjoint", mat3SumDiff(&M, &target) < EPSILON);
}

void testMat3MultiplyScalar() {
	mat3 M = {1, 2, 3, 4, 0, -1.5f, -2.5f, -3.5f, -4.5f};
	GLfloat x = 0.9f;
	mat3 target = {0.9f, 1.8f, 2.7f, 3.6f, 0, -1.35f, -2.25f, -3.15f, -4.05f};

	mat3MultiplyScalar(&M, &M, x);

	printTest("mat3MultiplyScalar", mat3SumDiff(&M, &target) < EPSILON);
}

void testMat3Determinant() {
	mat3 M = {1, -3, 2, 3, -1, 3, 2, -3, 1};
	GLfloat target = -15;

	printTest("mat3Determinant", fabs(mat3Determinant(&M) - target) < EPSILON);
}

void testMat3Inverse() {
	mat3 M = {1, 2, -1, 2, 1, 2, -1, 2, 1};
	mat3 target = {0.1875f, 0.25f, -0.3125f, 0.25f, 0, 0.25f, -0.3125f, 0.25f, 0.1875f};

	mat3Inverse(&M, &M);

	bool result = mat3SumDiff(&M, &target) < EPSILON;


	printTest("mat3Inverse", result);
}

void testMat3Transpose() {
	mat3 M = {1, 2, 3, 4, 5, 6, 7, 8, 9};
	mat3 target = {1, 4, 7, 2, 5, 8, 3, 6, 9};

	mat3Transpose(&M, &M);

	printTest("mat3Transpose", mat3SumDiff(&M, &target) < EPSILON);
}

void testVec3Zero() {
	vec3 result = {0, 0, 0};
	vec3 test = {1, 2, 3};

	vec3Zero(&test);

	printTest("vec3Zero", vec3Equals(&result, &test));
}

void testVec3Add() {
	vec3 first = {1, 2, 3};
	vec3 second = {4, 5, 6};

	vec3 expectedResult = {5, 7, 9};

	vec3 result = {0, 0, 0};

	vec3Add(&result, &first, &second);

	printTest("vec3Add", vec3Equals(&result, &expectedResult));
}

void testVec3Multiply() {
	vec3 test = {1, 2, 3};
	GLfloat multiplyBy = 2;
	
	vec3 result = {0, 0, 0};

	vec3 expectedResult = {2, 4, 6};

	vec3Multiply(&result, &test, multiplyBy);

	printTest("vec3Multiply", vec3Equals(&result, &expectedResult));
}

void testVec3Subtract() {
	vec3 first = {1, 2, 3};
	vec3 second = {4, 5, 6};

	vec3 expectedResult = {-3, -3, -3};

	vec3 result = {0, 0, 0};

	vec3Subtract(&result, &first, &second);

	printTest("vec3Substract", vec3Equals(&result, &expectedResult));
}

void testVec3Cross() {
	vec3 first = {1, 2, 3};
	vec3 second = {4, 5, 6};

	vec3 expectedResult = {-3, 6, -3};

	vec3 result = {0, 0, 0};

	vec3Cross(&result, &first, &second);

	printTest("vec3Cross", vec3Equals(&result, &expectedResult));
}

void testVec3Length() {
	vec3 test = {3, 4, 0};

	GLfloat expectedResult = 5;

	GLfloat result = vec3Length(&test);

	printTest("vec3Length", fabs(result - expectedResult) < EPSILON);
}

void testVec3Dot() {
	vec3 first = {1, 2, 3};
	vec3 second = {4, 5, 6};

	GLfloat expectedResult = 32;

	GLfloat result = vec3Dot(&first, &second);

	printTest("vec3Dot", fabs(result - expectedResult) < EPSILON);
}

void testVec3Normalise() {
	vec3 first = {1, 2, 3};

	vec3 expectedResult = {sqrt(14)/14, sqrt(14)*2/14, sqrt(14)*3/14};
	vec3 result = {0, 0, 0};

	vec3Normalise(&result, &first);

	printTest("vec3Normalise", vec3Equals(&result, &expectedResult));
}

void testVec2Subtract() {
	vec2 first = {1, 2};
	vec2 second = {4, 5};

	vec2 expectedResult = {-3, -3};

	vec2 result = {0, 0};

	vec2Subtract(&result, &first, &second);

	printTest("vec2Substract", vec2Equals(&result, &expectedResult));
}

void testMat4Identity() {
	mat4 expectedResult = {1, 0, 0, 0,
						   0, 1, 0, 0,
						   0, 0, 1, 0,
						   0, 0, 0, 1};

	mat4 randomMat4 = {0, 0, 0, 0,
					   0, 0, 0, 0,
					   0, 0, 0, 0,
					   0, 0, 0, 0};

	identity(&randomMat4);

	printTest("Mat4Identity", sumDiffAny((GLfloat*) &expectedResult, (GLfloat*) &randomMat4, 4, 4) < EPSILON);

}

void testMat4Multiply() {
	mat4 matrix1 = {2, 9, 4, 4,
					8, 8, 5, 2,
					6, 7, 4, 7,
					8, 6, 2, 6};

	mat4 matrix2 = {1, 1, 3, 3,
					8, 8, 5, 3,
					9, 5, 7, 7,
					2, 1, 5, 6};

	mat4 result = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};

	mat4 expectedResult = {
    	52, 56, 27, 45,
    	134, 189, 98, 101,
    	156, 212, 103, 137,
    	90, 97, 45, 81
	};



	multiply(&result, &matrix1, &matrix2);
	
	printTest("Mat4Multiply", sumDiffAny((GLfloat*) &expectedResult, (GLfloat*) &result, 4, 4) < EPSILON);
}

void testTranslation() {
	mat4 matrix1 = {2, 9, 4, 4,
					8, 8, 5, 2,
					6, 7, 4, 7,
					8, 6, 2, 6};


	mat4 result = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};

	vec3 test = {1, 2, 3};

	mat4 expectedResult = {1, 0, 0, 0,
						   0, 1, 0, 0, 
						   0, 0, 1, 0, 
						   1, 2, 3, 1};

	translation(&result, &test);

	printTest("Translation", sumDiffAny((GLfloat*) &expectedResult, (GLfloat*) &result, 4, 4) < EPSILON);
}

void testScaling() {
	mat4 matrix1 = {2, 9, 4, 4,
					8, 8, 5, 2,
					6, 7, 4, 7,
					8, 6, 2, 6};


	mat4 result = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};

	vec3 test = {1, 2, 3};

	mat4 expectedResult = {1, 0, 0, 0,
						   0, 2, 0, 0, 
						   0, 0, 3, 0, 
						   0, 0, 0, 1};

	scaling(&result, &test);

	printTest("Scaling", (sumDiffAny((GLfloat*)&expectedResult, (GLfloat*)&result, 4, 4) < EPSILON));
}

void testRotationZ() {
	mat4 matrix1 = {2, 9, 4, 4,
					8, 8, 5, 2,
					6, 7, 4, 7,
					8, 6, 2, 6};

	GLfloat a = 4;

	mat4 result = {0, 0, 0, 0, 
				   0, 0, 0, 0, 
				   0, 0, 0, 0, 
				   0, 0, 0, 0};


	mat4 expectedResult = {cos(a), sin(a), 0, 0,
						   -sin(a), cos(a), 0, 0, 
						   0, 0, 1, 0, 
						   0, 0, 0, 1};

	rotationZ(&result, a);

	printTest("RotationZ", (sumDiffAny((GLfloat*)&expectedResult, (GLfloat*)&result, 4, 4) < EPSILON));

}

void testRotationY() {
	mat4 matrix1 = {2, 9, 4, 4,
					8, 8, 5, 2,
					6, 7, 4, 7,
					8, 6, 2, 6};

	GLfloat a = 4;

	mat4 result = {0, 0, 0, 0, 
				   0, 0, 0, 0, 
				   0, 0, 0, 0, 
				   0, 0, 0, 0};


	mat4 expectedResult = {cos(a), 0, -sin(a), 0,
						   0, 1, 0, 0, 
						   sin(a), 0, cos(a), 0, 
						   0, 0, 0, 1};

	rotationY(&result, a);

	printTest("RotationY", (sumDiffAny((GLfloat*)&expectedResult, (GLfloat*)&result, 4, 4) < EPSILON));
}

void testRotationX() {
	mat4 matrix1 = {2, 9, 4, 4,
					8, 8, 5, 2,
					6, 7, 4, 7,
					8, 6, 2, 6};

	GLfloat a = 4;

	mat4 result = {0, 0, 0, 0, 
				   0, 0, 0, 0, 
				   0, 0, 0, 0, 
				   0, 0, 0, 0};


	mat4 expectedResult = {1, 0, 0, 0,
						   0, cos(a), sin(a), 0, 
						   0, -sin(a), cos(a), 0, 
						   0, 0, 0, 1};

	rotationX(&result, a);

	printTest("RotationX", (sumDiffAny((GLfloat*)&expectedResult, (GLfloat*)&result, 4, 4) < EPSILON));
}

void testMultiplyAny() {
    GLfloat matrix1[6] = {1, 2, 3,
                          4, 5, 6};  // 2x3 matrix

    GLfloat matrix2[6] = {7, 8,
                          9, 10,
                          11, 12};  // 3x2 matrix

    GLfloat result[4] = {0, 0,
                         0, 0};  // 2x2 result matrix

    GLfloat expectedResult[4] = {76, 100,
                                 103, 136};

    multiplyAny((GLfloat*)&result, (GLfloat*)&matrix1, (GLfloat*)&matrix2, 3, 2, 2);

    printTest("Multiply Any", sumDiffAny(result, expectedResult, 2, 2) < EPSILON);
}


void testAnyTranspose() {
	mat3 M = {1, 2, 3, 4, 5, 6, 7, 8, 9};
	mat3 target = {1, 4, 7, 2, 5, 8, 3, 6, 9};

	transposeAny((GLfloat*)&M, (GLfloat*)&M, 3, 3);

	printTest("Transpose Any", mat3SumDiff((mat3*)&M, (mat3*)&target) < EPSILON);
}

void testTranslate() {
    mat4 in = {1, 0, 0, 0,
               0, 1, 0, 0,
               0, 0, 1, 0,
               0, 0, 0, 1};

    vec3 translationVec = {1, 2, 3};

    mat4 expectedResult = {1, 0, 0, 0,
                           0, 1, 0, 0,
                           0, 0, 1, 0,
                           1, 2, 3, 1};

    mat4 result;
    translate(&result, &in, &translationVec);

    printTest("translate", sumDiffAny((GLfloat*)&result, (GLfloat*)&expectedResult, 4, 4) < EPSILON);
}

void testScale() {
    mat4 in = {1, 0, 0, 0,
               0, 1, 0, 0,
               0, 0, 1, 0,
               0, 0, 0, 1};

    vec3 scaleVec = {2, 3, 4};

    mat4 expectedResult = {2, 0, 0, 0,
                           0, 3, 0, 0,
                           0, 0, 4, 0,
                           0, 0, 0, 1};

    mat4 result;
    scale(&result, &in, &scaleVec);

    printTest("scale", sumDiffAny((GLfloat*)&result, (GLfloat*)&expectedResult, 4, 4) < EPSILON);
}


void testRotateZ() {
    mat4 in = {1, 0, 0, 0,
               0, 1, 0, 0,
               0, 0, 1, 0,
               0, 0, 0, 1};

    GLfloat angle = M_PI / 4; // 45 degrees

    mat4 expectedResult = {cos(angle), sin(angle), 0, 0,
                           -sin(angle), cos(angle), 0, 0,
                           0, 0, 1, 0,
                           0, 0, 0, 1};

    mat4 result;
    rotateZ(&result, &in, angle);

    printTest("rotateZ", sumDiffAny((GLfloat*)&result, (GLfloat*)&expectedResult, 4, 4) < EPSILON);
}

void testRotateY() {
    mat4 in = {1, 0, 0, 0,
               0, 1, 0, 0,
               0, 0, 1, 0,
               0, 0, 0, 1};

    GLfloat angle = M_PI / 4; // 45 degrees

    mat4 expectedResult = {cos(angle), 0, -sin(angle), 0,
                           0, 1, 0, 0,
                           sin(angle), 0, cos(angle), 0,
                           0, 0, 0, 1};

    mat4 result;
    rotateY(&result, &in, angle);

    printTest("rotateY", sumDiffAny((GLfloat*)&result, (GLfloat*)&expectedResult, 4, 4) < EPSILON);
}

void testRotateX() {
    mat4 in = {1, 0, 0, 0,
               0, 1, 0, 0,
               0, 0, 1, 0,
               0, 0, 0, 1};

    GLfloat angle = M_PI / 4; // 45 degrees

    mat4 expectedResult = {1, 0, 0, 0,
                           0, cos(angle), sin(angle), 0,
                           0, -sin(angle), cos(angle), 0,
                           0, 0, 0, 1};

    mat4 result;
    rotateX(&result, &in, angle);

    printTest("rotateX", sumDiffAny((GLfloat*)&result, (GLfloat*)&expectedResult, 4, 4) < EPSILON);
}

void testLookAt() {
    mat4 result;
    mat4 expected = {
          	1, 0, 0, 0,
			0, 1, 0, 0,
			0, 0, 1, 0,
			0, 0, -2, 1
    };

    vec3 eye = {0, 0, 2};
    vec3 look = {0, 0, 0};
    vec3 up = {0, 1, 0};

    lookAt(&result, &eye, &look, &up);

    bool isEqual = sumDiffAny((GLfloat*)&expected, (GLfloat*)&result, 4, 4) < EPSILON;
    printTest("lookAt", isEqual);
}


void testPerspectiveProjection() {
    mat4 result;
	mat4 expected = {
		1, 0, 0, 0,
		0, 1, 0, 0,
		0, 0, 4, -1,
		0, 0, 3, 0
	};


    GLfloat near = 1.0f;
    GLfloat far = 3.0f;

    perspectiveProjection(&result, near, far);

    bool isEqual = sumDiffAny((GLfloat*)&expected, (GLfloat*)&result, 4, 4) < EPSILON;
    printTest("perspectiveProjection", isEqual);
}


void testNormalisedDeviceCoordinates() {
    mat4 result;
	mat4 expected = {
		1, 0, 0, 0,
		0, 1, 0, 0,
		0, 0, -1, 0,
		-0, -0, -2, 1
	};


    GLfloat r = 1.0f;
    GLfloat l = -1.0f;
    GLfloat t = 1.0f;
    GLfloat b = -1.0f;
    GLfloat n = 1.0f;
    GLfloat f = 3.0f;

    normalisedDeviceCoordinates(&result, r, l, t, b, n, f);

    bool isEqual = sumDiffAny((GLfloat*)&expected, (GLfloat*)&result, 4, 4) < EPSILON;
    printTest("normalisedDeviceCoordinates", isEqual);
}


void testNormalisedDeviceCoordinatesFov() {
    mat4 result;
	mat4 expected = {
		1, 0, 0, 0,
		0, 1, 0, 0,
		0, 0, -1, 0,
		-0, -0, -2, 1
	};

    GLfloat fovy = 90.0f * (3.14159f / 180.0f);  // Convert degrees to radians
    GLfloat aspectRatio = 1.0f;
    GLfloat near = 1.0f;
    GLfloat far = 3.0f;

    normalisedDeviceCoordinatesFov(&result, fovy, aspectRatio, near, far);
	
    bool isEqual = sumDiffAny((GLfloat*)&expected, (GLfloat*)&result, 4, 4) < EPSILON;
    printTest("normalisedDeviceCoordinatesFov", isEqual);
}


int main(void) {

    // mat3 functions
    printf("==========\n mat3 functions\n==========\n");
    testMat3SumDiff();
    testMat3Minor();
    testMat3Cofactor();
    testMat3Adjoint();
    testMat3MultiplyScalar();
    testMat3Determinant();
    testMat3Inverse();
    testMat3Transpose();

    // mat4 functions
    printf("==========\n mat4 functions\n==========\n");
    testMat4Identity();
    testMat4Multiply();
    testTranslation();
    testScaling();
    testRotationZ();
    testRotationY();
    testRotationX();
    testMultiplyAny();
    testAnyTranspose();

    // mat4 transform functions
    printf("==========\n mat4 transform functions\n==========\n");
    testTranslate();
    testScale();
    testRotateZ();
    testRotateY();
    testRotateX();

    // vec2 functions
    printf("==========\n vec2 functions\n==========\n");
    testVec2Subtract();

    // vec3 functions
    printf("==========\n vec3 functions\n==========\n");
    testVec3Zero();
    testVec3Add();
    testVec3Multiply();
    testVec3Subtract();
    testVec3Cross();
    testVec3Length();
    testVec3Dot();
    testVec3Normalise();

 	// transformation functions
    printf("==========\n vec3 functions\n==========\n");
    testLookAt();
	testPerspectiveProjection();
	testNormalisedDeviceCoordinates();
	testNormalisedDeviceCoordinatesFov();
    return exitCode;
}
