//
// Created by Tristan on 12/10/2025.
//

#include "matrix.h"
#include <math.h>

mat4_t mat4_identity(void) {
    mat4_t m = {
        {
            1.0f, 0.0f, 0.0f, 0.0f,
            0.0f, 1.0f, 0.0f, 0.0f,
            0.0f, 0.0f, 1.0f, 0.0f,
            0.0f, 0.0f, 0.0f, 1.0f
        }
    };
    return m;
}

mat4_t mat4_make_scale(GLfloat sx, GLfloat sy, GLfloat sz) {
    mat4_t m = mat4_identity();
    m.m[0] = sx;
    m.m[5] = sy;
    m.m[10] = sz;
    return m;
};

mat4_t mat4_make_rotation_x(GLfloat angle) {
    GLfloat c = cos(angle);
    GLfloat s  = sin(angle);
    mat4_t m = mat4_identity();

    m.m[5] = c;
    m.m[6] = -s;
    m.m[9] = s;
    m.m[10] = c;
    return m;
}

mat4_t mat4_make_rotation_y(GLfloat angle) {
    GLfloat c = cos(angle);
    GLfloat s  = sin(angle);
    mat4_t m = mat4_identity();

    m.m[0] = c;
    m.m[2] = s;
    m.m[8] = -s;
    m.m[10] = c;
    return m;
}

mat4_t mat4_make_rotation_z(GLfloat angle) {
    GLfloat c = cos(angle);
    GLfloat s  = sin(angle);
    mat4_t m = mat4_identity();

    m.m[0] = c;
    m.m[1] = -s;
    m.m[4] = s;
    m.m[5] = c;
    return m;
}

mat4_t mat4_make_translation(GLfloat tx, GLfloat ty, GLfloat tz) {
    mat4_t m = mat4_identity();
    m.m[12] = tx;
    m.m[13] = ty;
    m.m[14] = tz;
    return m;
}

mat4_t mat4_mul_mat4(mat4_t a, mat4_t b) {
    mat4_t m;
    for (int j = 0; j < 4; j++) { // cols
        // m.m[j*4 + i]
        for (int i = 0; i < 4; i++) { // rows
            GLfloat sum = 0.0f;
            for (int k = 0; k < 4; k++) {
                GLfloat element_a = a.m[k * 4 + i];
                GLfloat element_b = b.m[j * 4 + k];

                sum += element_a * element_b;
            }
            m.m[j * 4 + i] = sum;
        }
    }
    return m;
}

mat4_t mat4_make_perspective(float fov_rad, float aspect, float znear, float zfar) {
    /*
    Sx, 0.0f, 0.0f, 0.0f,
    0.0f, Sy, 0.0f, 0.0f,
    0.0f, 0.0f, Sz, -1.0f,
    0.0f, 0.0f, Pz, 0.0f
    */
    float range = tan(fov_rad * 0.5f) * znear;
    float Sy = znear / range;
    float Sx = Sy / aspect;
    float Sz = -(zfar + znear) / (zfar - znear);
    float Pz = -(2.0f * zfar * znear) / (zfar - znear);

    mat4_t m = { 0 }; // Initialize all to 0.0f

    // Diagonal scaling
    m.m[0] = Sx;
    m.m[5] = Sy;
    m.m[10] = Sz;

    // perspective divide (w-axis)
    m.m[11] = -1.0f;

    // depth translation (Pz)
    m.m[14] = Pz;

    return m;
}

mat4_t mat4_make_quaternion(const GLfloat* q) {
    mat4_t m;
    float w = q[0];
    float x = q[1];
    float y = q[2];
    float z = q[3];

    float x2 = x * x;
    float y2 = y * y;
    float z2 = z * z;

    // col 1
    m.m[0] = 1.0f - (2.0f * y2) - (2.0f * z2);
    m.m[1] = (2.0f * x * y) + (2.0f * w * z);
    m.m[2] = (2 * x * z) - (2 * w * y);
    m.m[3] = 0.0f;

    // col 2
    m.m[4] = (2.0f * x * y) - (2.0f * w * z);
    m.m[5] = 1.0f - (2 * x2) - (2 * z2);
    m.m[6] = (2.0f * y * z) + (2.0f * w * x);
    m.m[7] = 0.0f;

    // col 3
    m.m[8] = (2.0f * x * z) + (2.0f * w * y);
    m.m[9] = (2.0f * y * z) - (2.0f * w * x);
    m.m[10] = 1.0f - (2.0f * x2) - (2.0 * y2);
    m.m[11] = 0.0f;

    // col 4
    m.m[12] = 0.0f;
    m.m[13] = 0.0f;
    m.m[14] = 0.0f;
    m.m[15] = 1.0f;

    return m;
}

mat4_t mat4_look_at(float eyex, float eyey, float eyez,
                    float targetx, float targety, float targetz,
                    float upx, float upy, float upz) {
    // calculate the forward vector (direction from Eye to Target)
    float forward_x = targetx - eyex;
    float forward_y = targety - eyey;
    float forward_z = targetz - eyez;

    // normalize forward vector
    float reciprocal_length_forward = 1.0f / sqrt(forward_x * forward_x + forward_y * forward_y + forward_z * forward_z);
    forward_x *= reciprocal_length_forward;
    forward_y *= reciprocal_length_forward;
    forward_z *= reciprocal_length_forward;

    // calculate right vector (Forward x Up)
    // use the "world up" passed in (usually 0, 1, 0) to determine "right"
    float rightx = forward_z * upy - forward_y * upz;
    float righty = forward_x * upz - forward_z * upx;
    float rightz = forward_y * upx - forward_x * upy;

    // normalize right vector
    float reciprocal_length_right = 1.0f / sqrt(rightx * rightx + righty * righty + rightz * rightz);
    rightx *= reciprocal_length_right;
    righty *= reciprocal_length_right;
    rightz *= reciprocal_length_right;

    // re-calculate Up Vector (Right x Forward)
    // calculate the "True Up" perpendicular to the new Forward and Right vector
    upx = righty * forward_z - rightz * forward_y;
    upy = rightz * forward_x - rightx * forward_z;
    upz = rightx * forward_y - righty * forward_x;

    // create the matrix
    mat4_t m = mat4_identity();

    // row 0 (right vector)
    m.m[0] = rightx;
    m.m[4] = righty;
    m.m[8] = rightz;

    // row 1 (up vector)
    m.m[1] = upx;
    m.m[5] = upy;
    m.m[9] = upz;

    // row 2 (forward vector inverted for openGL camera)
    // invert Forward since we're looking down the negative Z axis with openGL
    m.m[2] = -forward_x;
    m.m[6] = -forward_y;
    m.m[10] = -forward_z;

    // Dot product of rotation and negative eye position
    m.m[12] = -(rightx * eyex + righty * eyey + rightz * eyez);
    m.m[13] = -(upx * eyex + upy * eyey + upz * eyez);
    m.m[14] = -(-forward_x * eyex - forward_y * eyey - forward_z * eyez);

    // row 3 (0, 0, 0, 1)

    return m;
}