#include "mons_math/mat4.h"
#include "mons_math/mat3.h"
#include "mons_math/vec4.h"
#include <stdio.h>

mons_mat4 mons_mat4_add(mons_mat4 a, mons_mat4 b) {
    mons_mat4 result = {
        mons_vec4_add(a.m1, b.m1),
        mons_vec4_add(a.m2, b.m2),
        mons_vec4_add(a.m3, b.m3),
        mons_vec4_add(a.m4, b.m4),
    };
    return result;
}

void mons_mat4_add_inplace(mons_mat4 *a, mons_mat4 b) {
    mons_vec4_add_inplace(&a->m1, b.m1);
    mons_vec4_add_inplace(&a->m2, b.m2);
    mons_vec4_add_inplace(&a->m3, b.m3);
    mons_vec4_add_inplace(&a->m4, b.m4);
}

mons_mat4 mons_mat4_mul_f(mons_mat4 a, float b) {
    mons_mat4 result = {
        mons_vec4_mul_f(a.m1, b),
        mons_vec4_mul_f(a.m2, b),
        mons_vec4_mul_f(a.m3, b),
        mons_vec4_mul_f(a.m4, b),
    };
    return result;
}

mons_mat4 mons_mat4_mul_i(mons_mat4 a, int b) {
    mons_mat4 result = {
        mons_vec4_mul_i(a.m1, b),
        mons_vec4_mul_i(a.m2, b),
        mons_vec4_mul_i(a.m3, b),
        mons_vec4_mul_i(a.m4, b),
    };
    return result;
}

void mons_mat4_mul_f_inplace(mons_mat4 *a, float b) {
    mons_vec4_mul_f_inplace(&a->m1, b);
    mons_vec4_mul_f_inplace(&a->m2, b);
    mons_vec4_mul_f_inplace(&a->m3, b);
    mons_vec4_mul_f_inplace(&a->m4, b);
}

void mons_mat4_mul_i_inplace(mons_mat4 *a, int b) {
    mons_vec4_mul_i_inplace(&a->m1, b);
    mons_vec4_mul_i_inplace(&a->m2, b);
    mons_vec4_mul_i_inplace(&a->m3, b);
    mons_vec4_mul_i_inplace(&a->m4, b);
}

mons_mat4 mons_mat4_mul(mons_mat4 a, mons_mat4 b) {
    mons_vec4 b_n1 = mons_mat4_n1(b);
    mons_vec4 b_n2 = mons_mat4_n2(b);
    mons_vec4 b_n3 = mons_mat4_n3(b);
    mons_vec4 b_n4 = mons_mat4_n4(b);

    mons_mat4 result = {
        {
            mons_vec4_dot(a.m1, b_n1),
            mons_vec4_dot(a.m1, b_n2),
            mons_vec4_dot(a.m1, b_n3),
            mons_vec4_dot(a.m1, b_n4),
        },
        {
            mons_vec4_dot(a.m2, b_n1),
            mons_vec4_dot(a.m2, b_n2),
            mons_vec4_dot(a.m2, b_n3),
            mons_vec4_dot(a.m2, b_n4),
        },
        {
            mons_vec4_dot(a.m3, b_n1),
            mons_vec4_dot(a.m3, b_n2),
            mons_vec4_dot(a.m3, b_n3),
            mons_vec4_dot(a.m3, b_n4),
        },
        {
            mons_vec4_dot(a.m4, b_n1),
            mons_vec4_dot(a.m4, b_n2),
            mons_vec4_dot(a.m4, b_n3),
            mons_vec4_dot(a.m4, b_n4),
        },
    };
    return result;
}

mons_mat4 mons_mat4_transpose(mons_mat4 a) {
    mons_mat4 result = {
        mons_mat4_n1(a),
        mons_mat4_n2(a),
        mons_mat4_n3(a),
        mons_mat4_n4(a),
    };
    return result;
}

void mons_mat4_transpose_inplace(mons_mat4 *a) { *a = mons_mat4_transpose(*a); }

mons_vec4 mons_mat4_n1(mons_mat4 a) {
    mons_vec4 result = {
        a.m1.x,
        a.m2.x,
        a.m3.x,
        a.m4.x,
    };
    return result;
}

mons_vec4 mons_mat4_n2(mons_mat4 a) {
    mons_vec4 result = {
        a.m1.y,
        a.m2.y,
        a.m3.y,
        a.m4.y,
    };
    return result;
}

mons_vec4 mons_mat4_n3(mons_mat4 a) {
    mons_vec4 result = {
        a.m1.z,
        a.m2.z,
        a.m3.z,
        a.m4.z,
    };
    return result;
}

mons_vec4 mons_mat4_n4(mons_mat4 a) {
    mons_vec4 result = {
        a.m1.w,
        a.m2.w,
        a.m3.w,
        a.m4.w,
    };
    return result;
}

float mons_mat4_determinant(mons_mat4 a) {
    mons_mat3 minor_1_1 = {
        {a.m2.y, a.m2.z, a.m2.w},
        {a.m3.y, a.m3.z, a.m3.w},
        {a.m4.y, a.m4.z, a.m4.w},
    };
    mons_mat3 minor_1_2 = {
        {a.m2.x, a.m2.z, a.m2.w},
        {a.m3.x, a.m3.z, a.m3.w},
        {a.m4.x, a.m4.z, a.m4.w},
    };
    mons_mat3 minor_1_3 = {
        {a.m2.x, a.m2.y, a.m2.w},
        {a.m3.x, a.m3.y, a.m3.w},
        {a.m4.x, a.m4.y, a.m4.w},
    };
    mons_mat3 minor_1_4 = {
        {a.m2.x, a.m2.y, a.m2.z},
        {a.m3.x, a.m3.y, a.m3.z},
        {a.m4.x, a.m4.y, a.m4.z},
    };

    return (a.m1.x * mons_mat3_determinant(minor_1_1)) -
           (a.m1.y * mons_mat3_determinant(minor_1_2)) +
           (a.m1.z * mons_mat3_determinant(minor_1_3)) -
           (a.m1.w * mons_mat3_determinant(minor_1_4));
}

mons_mat4 mons_mat4_minor(mons_mat4 a) {
    // First Row
    mons_mat3 minor_1_1 = {
        {a.m2.y, a.m2.z, a.m2.w},
        {a.m3.y, a.m3.z, a.m3.w},
        {a.m4.y, a.m4.z, a.m4.w},
    };
    float det_1_1 = mons_mat3_determinant(minor_1_1);
    mons_mat3 minor_1_2 = {
        {a.m2.x, a.m2.z, a.m2.w},
        {a.m3.x, a.m3.z, a.m3.w},
        {a.m4.x, a.m4.z, a.m4.w},
    };
    float det_1_2 = mons_mat3_determinant(minor_1_2);
    mons_mat3 minor_1_3 = {
        {a.m2.x, a.m2.y, a.m2.w},
        {a.m3.x, a.m3.y, a.m3.w},
        {a.m4.x, a.m4.y, a.m4.w},
    };
    float det_1_3 = mons_mat3_determinant(minor_1_3);
    mons_mat3 minor_1_4 = {
        {a.m2.x, a.m2.y, a.m2.z},
        {a.m3.x, a.m3.y, a.m3.z},
        {a.m4.x, a.m4.y, a.m4.z},
    };
    float det_1_4 = mons_mat3_determinant(minor_1_4);
    // Second Row
    mons_mat3 minor_2_1 = {
        {a.m1.y, a.m1.z, a.m1.w},
        {a.m3.y, a.m3.z, a.m3.w},
        {a.m4.y, a.m4.z, a.m4.w},
    };
    float det_2_1 = mons_mat3_determinant(minor_2_1);
    mons_mat3 minor_2_2 = {
        {a.m1.x, a.m1.z, a.m1.w},
        {a.m3.x, a.m3.z, a.m3.w},
        {a.m4.x, a.m4.z, a.m4.w},
    };
    float det_2_2 = mons_mat3_determinant(minor_2_2);
    mons_mat3 minor_2_3 = {
        {a.m1.x, a.m1.y, a.m1.w},
        {a.m3.x, a.m3.y, a.m3.w},
        {a.m4.x, a.m4.y, a.m4.w},
    };
    float det_2_3 = mons_mat3_determinant(minor_2_3);
    mons_mat3 minor_2_4 = {
        {a.m1.x, a.m1.y, a.m1.z},
        {a.m3.x, a.m3.y, a.m3.z},
        {a.m4.x, a.m4.y, a.m4.z},
    };
    float det_2_4 = mons_mat3_determinant(minor_2_4);
    // Third Row
    mons_mat3 minor_3_1 = {
        {a.m1.y, a.m1.z, a.m1.w},
        {a.m2.y, a.m2.z, a.m2.w},
        {a.m4.y, a.m4.z, a.m4.w},
    };
    float det_3_1 = mons_mat3_determinant(minor_3_1);
    mons_mat3 minor_3_2 = {
        {a.m1.x, a.m1.z, a.m1.w},
        {a.m2.x, a.m2.z, a.m2.w},
        {a.m4.x, a.m4.z, a.m4.w},
    };
    float det_3_2 = mons_mat3_determinant(minor_3_2);
    mons_mat3 minor_3_3 = {
        {a.m1.x, a.m1.y, a.m1.w},
        {a.m2.x, a.m2.y, a.m2.w},
        {a.m4.x, a.m4.y, a.m4.w},
    };
    float det_3_3 = mons_mat3_determinant(minor_3_3);
    mons_mat3 minor_3_4 = {
        {a.m1.x, a.m1.y, a.m1.z},
        {a.m2.x, a.m2.y, a.m2.z},
        {a.m4.x, a.m4.y, a.m4.z},
    };
    float det_3_4 = mons_mat3_determinant(minor_3_4);
    // Fourth Row
    mons_mat3 minor_4_1 = {
        {a.m1.y, a.m1.z, a.m1.w},
        {a.m2.y, a.m2.z, a.m2.w},
        {a.m3.y, a.m3.z, a.m3.w},
    };
    float det_4_1 = mons_mat3_determinant(minor_4_1);
    mons_mat3 minor_4_2 = {
        {a.m1.x, a.m1.z, a.m1.w},
        {a.m2.x, a.m2.z, a.m2.w},
        {a.m3.x, a.m3.z, a.m3.w},
    };
    float det_4_2 = mons_mat3_determinant(minor_4_2);
    mons_mat3 minor_4_3 = {
        {a.m1.x, a.m1.y, a.m1.w},
        {a.m2.x, a.m2.y, a.m2.w},
        {a.m3.x, a.m3.y, a.m3.w},
    };
    float det_4_3 = mons_mat3_determinant(minor_4_3);
    mons_mat3 minor_4_4 = {
        {a.m1.x, a.m1.y, a.m1.z},
        {a.m2.x, a.m2.y, a.m2.z},
        {a.m3.x, a.m3.y, a.m3.z},
    };
    float det_4_4 = mons_mat3_determinant(minor_4_4);

    mons_mat4 result = {
        {det_1_1, det_1_2, det_1_3, det_1_4},
        {det_2_1, det_2_2, det_2_3, det_2_4},
        {det_3_1, det_3_2, det_3_3, det_3_4},
        {det_4_1, det_4_2, det_4_3, det_4_4},
    };
    return result;
}

mons_mat4 mons_mat4_cofactor(mons_mat4 a) {
    mons_mat4 result = mons_mat4_minor(a);
    result.m1.y *= -1;
    result.m1.w *= -1;
    result.m2.x *= -1;
    result.m2.z *= -1;
    result.m3.y *= -1;
    result.m3.w *= -1;
    result.m4.x *= -1;
    result.m4.z *= -1;
    return result;
}

mons_mat4 mons_mat4_adjoint(mons_mat4 a) {
    return mons_mat4_transpose(mons_mat4_cofactor(a));
}

mons_mat4 mons_mat4_inverse(mons_mat4 a) {
    return mons_mat4_mul_f(mons_mat4_adjoint(a),
                           1.0 / mons_mat4_determinant(a));
}

int mons_mat4_equal(mons_mat4 a, mons_mat4 b) {
    return mons_vec4_equal(a.m1, b.m1)
        && mons_vec4_equal(a.m2, b.m2)
        && mons_vec4_equal(a.m3, b.m3)
        && mons_vec4_equal(a.m4, b.m4);
}

void mons_mat4_mul_inplace(mons_mat4 *a, mons_mat4 b) {
    *a = mons_mat4_mul(*a, b);
}

void mons_mat4_print(mons_mat4 mat) {
    printf("[%.3f %.3f %.3f %.3f]\n", mat.m1.x, mat.m2.x, mat.m3.x, mat.m4.x);
    printf("[%.3f %.3f %.3f %.3f]\n", mat.m1.y, mat.m2.y, mat.m3.y, mat.m4.y);
    printf("[%.3f %.3f %.3f %.3f]\n", mat.m1.z, mat.m2.z, mat.m3.z, mat.m4.z);
    printf("[%.3f %.3f %.3f %.3f]\n", mat.m1.w, mat.m2.w, mat.m3.w, mat.m4.w);
}