#include <voxula/internals/utility/math.h>
#include <math.h>
 
vx_mat2f_s vx_mat2f_identity()
{
    vx_mat2f_s matrix;
    matrix.val[0] =   1.0f;
    matrix.val[1] = 0.0f;
    matrix.val[2] = 0.0f;
    matrix.val[3] =   1.0f;
    return matrix;
}

vx_mat3f_s vx_mat3f_identity()
{
    vx_mat3f_s matrix;
    matrix.val[0] =   1.0f;
    matrix.val[1] = 0.0f;
    matrix.val[2] = 0.0f;
    matrix.val[3] = 0.0f;
    matrix.val[4] =   1.0f;
    matrix.val[5] = 0.0f;
    matrix.val[6] = 0.0f;
    matrix.val[7] = 0.0f;
    matrix.val[8] =   1.0f;
    return matrix;
}

vx_mat4f_s vx_mat4f_identity()
{
    vx_mat4f_s matrix;
    matrix.val[0]  =   1.0f;
    matrix.val[1]  = 0.0f;
    matrix.val[2]  = 0.0f;
    matrix.val[3]  = 0.0f;
    matrix.val[4]  = 0.0f;
    matrix.val[5]  =   1.0f;
    matrix.val[6]  = 0.0f;
    matrix.val[7]  = 0.0f;
    matrix.val[8]  = 0.0f;
    matrix.val[9]  = 0.0f;
    matrix.val[10] =   1.0f;
    matrix.val[11] = 0.0f;
    matrix.val[12] = 0.0f;
    matrix.val[13] = 0.0f;
    matrix.val[14] = 0.0f;
    matrix.val[15] =   1.0f;
    return matrix;
}

vx_mat4f_s vx_mat4f_translate(
    vx_mat4f_s matrix,
    vx_vec3f_s translation
) {
    matrix.val[12]  += translation.x;
    matrix.val[13]  += translation.y;
    matrix.val[14]  += translation.z;
    return matrix;
}

vx_mat4f_s vx_mat4f_multiply(
    vx_mat4f_s first,
    vx_mat4f_s second
) {
    vx_mat4f_s result;

    uint8_t x_index = 0;
    uint8_t y_index = 0;
    uint8_t level = 0;
    while(x_index < 4)
    {
        y_index = 0;
        while(y_index < 4)
        {
            result.val[y_index * 4 + x_index] = 0.0f;
            level = 0;
            while(level < 4)
            {
                result.val[y_index * 4 + x_index] +=
                    first.val[level * 4 + x_index]
                  * second.val[y_index * 4 + level];
                ++level;
            }
            ++y_index;
        }
        ++x_index;
    }
    return result;
}

vx_mat4f_s vx_mat4f_rotate_x(
    vx_mat4f_s matrix,
    float rotation
) {
    float cosine = cos(rotation);
    float sine = sin(rotation);
    vx_mat4f_s rotation_matrix;
    rotation_matrix.val[0]  = 1.0f;
    rotation_matrix.val[5]  = cosine;
    rotation_matrix.val[6]  = -sine;
    rotation_matrix.val[9]  = sine;
    rotation_matrix.val[10] = cosine;
    rotation_matrix.val[15]  = 1.0f;
    vx_mat4f_multiply(matrix, rotation_matrix);
    return rotation_matrix;
}

vx_mat4f_s vx_mat4f_rotate_y(
    vx_mat4f_s matrix,
    float rotation
) {
    float cosine = cos(rotation);
    float sine = sin(rotation);
    vx_mat4f_s rotation_matrix;
    rotation_matrix.val[0]  = cosine;
    rotation_matrix.val[2]  = sine;
    rotation_matrix.val[5]  = 1.0f;
    rotation_matrix.val[8]  = -sine;
    rotation_matrix.val[10] = cosine;
    rotation_matrix.val[15]  = 1.0f;
    vx_mat4f_multiply(matrix, rotation_matrix);
    return rotation_matrix;
}

vx_mat4f_s vx_mat4f_rotate_z(
    vx_mat4f_s matrix,
    float rotation
) {
    float cosine = cos(rotation);
    float sine = sin(rotation);
    vx_mat4f_s rotation_matrix;
    rotation_matrix.val[0]  = cosine;
    rotation_matrix.val[1]  = -sine;
    rotation_matrix.val[4]  = sine;
    rotation_matrix.val[5] = cosine;
    rotation_matrix.val[10]  = 1.0f;
    vx_mat4f_multiply(matrix, rotation_matrix);
    return rotation_matrix;
}
vx_mat4f_s vx_mat4f_scale(
    vx_mat4f_s matrix,
    vx_vec3f_s scaling
) {
    matrix.val[0]  *= scaling.x;
    matrix.val[5]  *= scaling.y;
    matrix.val[10] *= scaling.z;
    return matrix;
}

vx_mat4f_s vx_mat4f_perspective(
    float aspect_ratio,
    float fov,
    float near_plane,
    float far_plane
) {
    float plane_range = tanf(fov / 2.0f) * near_plane;
    float scale_x = (2.0f * near_plane) / (plane_range * aspect_ratio * 2.0f);
    float scale_y = near_plane / plane_range;
    float scale_z = (far_plane + near_plane) / (far_plane - near_plane);
    float position_z = (2.0f * far_plane * near_plane) / (far_plane - near_plane);

    vx_mat4f_s matrix;
    matrix.val[0] = scale_x;
    matrix.val[1]  = 0.0f;
    matrix.val[2]  = 0.0f;
    matrix.val[3]  = 0.0f;
    matrix.val[4]  = 0.0f;
    matrix.val[5]  = scale_y;
    matrix.val[6]  = 0.0f;
    matrix.val[7]  = 0.0f;
    matrix.val[8]  = 0.0f;
    matrix.val[9]  = 0.0f;
    matrix.val[10] = scale_z;
    matrix.val[11] = - 1.0f;
    matrix.val[12] = 0.0f;
    matrix.val[13] = 0.0f;
    matrix.val[14] = position_z;
    matrix.val[15] = 0.0f;
    return matrix;
}

void vx_mat4f_stringify(FILE *file, vx_mat4f_s matrix)
{
    fprintf(file, "ROW 1: %f %f %f %f\n", matrix.val[0],  matrix.val[1],  matrix.val[2],  matrix.val[3]);
    fprintf(file, "ROW 2: %f %f %f %f\n", matrix.val[4],  matrix.val[5],  matrix.val[6],  matrix.val[7]);
    fprintf(file, "ROW 3: %f %f %f %f\n", matrix.val[8],  matrix.val[9],  matrix.val[10], matrix.val[11]);
    fprintf(file, "ROW 4: %f %f %f %f\n", matrix.val[12], matrix.val[13], matrix.val[14], matrix.val[15]);
}