#include "matrix.h"

mat4_t mat4_identity(void) {
    mat4_t m = {{
        { 1, 0, 0, 0},
        { 0, 1, 0, 0},
        { 0, 0, 1, 0},
        { 0, 0, 0, 1},
    }};

    return m;
}

mat4_t mat4_make_scale(float sx, float sy, float sz) {
    mat4_t m = mat4_identity();
    m.m[0][0] *= sx;
    m.m[1][1] *= sy;
    m.m[2][2] *= sz;

    return m;
}

mat4_t mat4_make_translation(float tx, float ty, float tz) {
    /*
     * 1 0 0 tx
     * 0 1 0 ty
     * 0 0 1 tz
     * 0 0 0 1
     */

    mat4_t m = mat4_identity();
    m.m[0][3] = tx;
    m.m[1][3] = ty;
    m.m[2][3] = tz;

    return m;
}

mat4_t mat4_make_rotation_y(float ra) {
    /*
     * cos 0 -sin 0
     * 0 1 0 0
     * sin 0 cos 0
     * 0 0 0 1
     */
    const float cosine = cosf(ra);
    const float sine = sinf(ra);
    mat4_t m = mat4_identity();
    m.m[0][0] = cosine;
    m.m[0][2] = -sine;
    m.m[2][0] = sine;
    m.m[2][2] = cosine;

    return m;
}

mat4_t mat4_make_rotation_x(float ra) {
    /*
     * 1 0 0 0
     * 0 cos sin 0
     * 0 -sin cos 0
     * 0 0 0 1
     */
    const float cosine = cosf(ra);
    const float sine = sinf(ra);
    mat4_t m = mat4_identity();
    m.m[1][1] = cosine;
    m.m[1][2] = sine;
    m.m[2][1] = -sine;
    m.m[2][2] = cosine;

    return m;
}


mat4_t mat4_make_rotation_z(float ra) {
    /*
     * cos sin 0 0
     * -sin cos 0 0
     * 0 0 1 0
     * 0 0 0 1
     */
    const float cosine = cosf(ra);
    const float sine = sinf(ra);
    mat4_t m = mat4_identity();
    m.m[0][0] = cosine;
    m.m[0][1] = sine;
    m.m[1][0] = -sine;
    m.m[1][1] = cosine;

    return m;
}

vec4_t mat4_multiply_vec4(mat4_t m, vec4_t v) {
    vec4_t ret = {};

    ret.x = m.m[0][0] * v.x + m.m[0][1] * v.y + m.m[0][2] * v.z + m.m[0][3] * v.w;
    ret.y = m.m[1][0] * v.x + m.m[1][1] * v.y + m.m[1][2] * v.z + m.m[1][3] * v.w;
    ret.z = m.m[2][0] * v.x + m.m[2][1] * v.y + m.m[2][2] * v.z + m.m[2][3] * v.w;
    ret.w = m.m[3][0] * v.x + m.m[3][1] * v.y + m.m[3][2] * v.z + m.m[3][3] * v.w;

    return ret;
}

mat4_t mat4_mul_mat4(mat4_t* a, mat4_t* b) {
    mat4_t ret = {};

    for (int r = 0; r < 4; r++) {
        for (int c = 0; c < 4; c++) {
            ret.m[r][c] = (a->m[r][0] * b->m[0][c]) + (a->m[r][1] * b->m[1][c]) + (a->m[r][2] * b->m[2][c]) + (a->m[r][3] * b->m[3][c]);
        }
    }

    return ret;
}

mat4_t mat4_make_perspective(float fov, float aspect, float znear, float zfar) {
    mat4_t m = {{{ 0 }}};
    m.m[0][0] = aspect * (1 / tanf(fov / 2));
    m.m[1][1] = 1 / tanf(fov / 2);
    m.m[2][2] = zfar / (zfar - znear);
    m.m[2][3] = (-zfar * znear) / (zfar - znear);
    m.m[3][2] = 1.0f;
    return m;
}

vec4_t mat4_mul_vec4_project(mat4_t mat_proj, vec4_t v) {
    // multiply the projection matrix by our original vector
    vec4_t result = mat4_multiply_vec4(mat_proj, v);

    // perform perspective divide with original z-value
    if (result.w != 0.0) {
        result.x /= result.w;
        result.y /= result.w;
        result.z /= result.w;
    }

    return result;
}