#include <librr/linear_algebra.h>
#include <math.h>

rr_mat2f_s rr_identity_mat2f()
{
    rr_mat2f_s matrix;
    matrix.values[0][0] = 1.0f;
    matrix.values[0][1] = 0.0f;

    matrix.values[1][0] = 0.0f;
    matrix.values[1][1] = 1.0f;
    return matrix;
}

rr_mat3f_s rr_identity_mat3f()
{
    rr_mat3f_s matrix;
    matrix.values[0][0] = 1.0f;
    matrix.values[0][1] = 0.0f;
    matrix.values[0][2] = 0.0f;

    matrix.values[1][0] = 0.0f;
    matrix.values[1][1] = 1.0f;
    matrix.values[1][2] = 0.0f;

    matrix.values[2][0] = 0.0f;
    matrix.values[2][1] = 0.0f;
    matrix.values[2][2] = 1.0f;
    return matrix;
}

rr_mat4f_s rr_identity_mat4f()
{
    rr_mat4f_s matrix;
    matrix.values[0][0] = 1.0f;
    matrix.values[0][1] = 0.0f;
    matrix.values[0][2] = 0.0f;
    matrix.values[0][3] = 0.0f;

    matrix.values[1][0] = 0.0f;
    matrix.values[1][1] = 1.0f;
    matrix.values[1][2] = 0.0f;
    matrix.values[1][3] = 0.0f;

    matrix.values[2][0] = 0.0f;
    matrix.values[2][1] = 0.0f;
    matrix.values[2][2] = 1.0f;
    matrix.values[2][3] = 0.0f;

    matrix.values[3][0] = 0.0f;
    matrix.values[3][1] = 0.0f;
    matrix.values[3][2] = 0.0f;
    matrix.values[3][3] = 1.0f;
    return matrix;
}

rr_mat4f_s rr_mat4f_multiply(rr_mat4f_s first, rr_mat4f_s second)
{
    rr_mat4f_s result;
    usz_t row = 0;
    usz_t column = 0;
    usz_t index = 0;
    while(row < 4)
    {
        column = 0;
        while(column < 4)
        {
            result.values[column][row] = 0.0f;
            index = 0;
            while(index < 4)
            {
                result.values[column][row] += first.values[column][index] * second.values[index][row];
                ++index;
            }
            ++column;
        }
        ++row;
    }
    return result;
}

rr_vec4f_s rr_mat4f_multiply_vec4f(rr_mat4f_s matrix, rr_vec4f_s vector)
{
    rr_vec4f_s result;
    result.x  = matrix.values[0][0] * vector.x;
    result.x += matrix.values[0][1] * vector.y;
    result.x += matrix.values[0][2] * vector.z;
    result.x += matrix.values[0][3] * vector.w;

    result.y  = matrix.values[1][0] * vector.x;
    result.y += matrix.values[1][1] * vector.y;
    result.y += matrix.values[1][2] * vector.z;
    result.y += matrix.values[1][3] * vector.w;

    result.z  = matrix.values[2][0] * vector.x;
    result.z += matrix.values[2][1] * vector.y;
    result.z += matrix.values[2][2] * vector.z;
    result.z += matrix.values[2][3] * vector.w;

    result.w  = matrix.values[3][0] * vector.x;
    result.w += matrix.values[3][1] * vector.y;
    result.w += matrix.values[3][2] * vector.z;
    result.w += matrix.values[3][3] * vector.w;

    return result;
}



void rr_mat4f_translate(rr_mat4f_s *matrix, rr_vec3f_s xyz)
{
    matrix->values[0][3] = xyz.x;
    matrix->values[1][3] = xyz.y;
    matrix->values[2][3] = xyz.z;
}

rr_mat4f_s rr_mat4f_rotate_x(float radians)
{
    float sine = sinf(radians);
    float cosine = cosf(radians);

    rr_mat4f_s matrix;
    matrix.values[1][1] = cosine;
    matrix.values[1][2] = -sine;
    matrix.values[2][1] = sine;
    matrix.values[2][2] = -cosine;

    return matrix;
}

rr_mat4f_s rr_mat4f_rotate_y(float radians)
{
    float sine = sinf(radians);
    float cosine = cosf(radians);

    rr_mat4f_s matrix;
    matrix.values[0][0] = cosine;
    matrix.values[0][2] = sine;
    matrix.values[2][0] = -sine;
    matrix.values[2][2] = cosine;

    return matrix;
}

rr_mat4f_s rr_mat4f_rotate_z(float radians)
{
    float sine = sinf(radians);
    float cosine = cosf(radians);

    rr_mat4f_s matrix;
    matrix.values[0][0] = cosine;
    matrix.values[0][1] = -sine;
    matrix.values[1][0] = sine;
    matrix.values[1][1] = cosine;

    return matrix;
}

rr_mat4f_s rr_mat4f_rotate_xyz(float x_rad, float y_rad, float z_rad)
{
    rr_mat4f_s matrix = rr_mat4f_rotate_z(z_rad);
    matrix = rr_mat4f_multiply(matrix, rr_mat4f_rotate_y(y_rad));
    matrix = rr_mat4f_multiply(matrix, rr_mat4f_rotate_x(x_rad));
    return matrix;
}

rr_mat4f_s rr_mat4f_perspective(float fov, float near_plane, float far_plane, float aspect_ratio)
{
    float plane_range = far_plane - near_plane;
    float scale_x     = (2 * near_plane) / (plane_range * aspect_ratio * 2);
    float scale_y     = near_plane / plane_range;
    float scale_z     = -(far_plane + near_plane) / (far_plane - near_plane);
    float pos_z       = tanf(fov / 2) * near_plane;

    rr_mat4f_s matrix;
    matrix.values[0][0] = scale_x;
    matrix.values[0][1] = 0.0f;
    matrix.values[0][2] = 0.0f;
    matrix.values[0][3] = 0.0f;

    matrix.values[1][0] = 0.0f;
    matrix.values[1][1] = scale_y;
    matrix.values[1][2] = 0.0f;
    matrix.values[1][3] = 0.0f;

    matrix.values[2][0] = 0.0f;
    matrix.values[2][1] = 0.0f;
    matrix.values[2][2] = scale_z;
    matrix.values[2][3] = -1.0f;

    matrix.values[3][0] = 0.0f;
    matrix.values[3][1] = 0.0f;
    matrix.values[3][2] = pos_z;
    matrix.values[3][3] = 0.0f;

    return matrix;
}