/*
 * 3D Engine 
 * matrice.c: 
 * Based on pikuma.com 3D software renderer in C
 * Copyright (c) 2021 986-Studio. All rights reserved.
 *
 * Created by Manoël Trapier on 06/03/2021.
 */

#include <math.h>

#include <matrix.h>

/* Matrix operations */
vec4_t mat4ProdVec4(matrix4_t mat, vec4_t v)
{
    vec4_t ret =
    {
        .x = FastGet4(mat, 0, 0) * v.x +
             FastGet4(mat, 0, 1) * v.y +
             FastGet4(mat, 0, 2) * v.z +
             FastGet4(mat, 0, 3) * v.w,
        .y = FastGet4(mat, 1, 0) * v.x +
             FastGet4(mat, 1, 1) * v.y +
             FastGet4(mat, 1, 2) * v.z +
             FastGet4(mat, 1, 3) * v.w,
        .z = FastGet4(mat, 2, 0) * v.x +
             FastGet4(mat, 2, 1) * v.y +
             FastGet4(mat, 2, 2) * v.z +
             FastGet4(mat, 2, 3) * v.w,
        .w = FastGet4(mat, 3, 0) * v.x +
             FastGet4(mat, 3, 1) * v.y +
             FastGet4(mat, 3, 2) * v.z +
             FastGet4(mat, 3, 3) * v.w,
    };
    return ret;
}

vec4_t mat4ProdVec4Project(matrix4_t mat, vec4_t v)
{
    vec4_t result = mat4ProdVec4(mat, v);

    /* Normalise the vector if needed */
    if (result.w != 0.0)
    {
        result.x /= result.w;
        result.y /= result.w;
        result.z /= result.w;
    }

    return result;
}

/* Matrix operations */
matrix4_t mat4ProdMat4(matrix4_t a, matrix4_t b)
{
    matrix4_t ret;
    int x, y;
    for (y = 0 ; y < 4 ; y++)
    {
        for (x = 0 ; x < 4 ; x++)
        {
            double v = FastGet4(a, x, 0) * FastGet4(b, 0, y) +
                       FastGet4(a, x, 1) * FastGet4(b, 1, y) +
                       FastGet4(a, x, 2) * FastGet4(b, 2, y) +
                       FastGet4(a, x, 3) * FastGet4(b, 3, y);
            FastSet4(ret, x, y, v);
        }
    }
    return ret;
}

/* Matrix creations */
matrix4_t mat4Scale(double scaleX, double scaleY, double scaleZ)
{
    matrix4_t ret =
    {
        .v =
        {
            scaleX, 0, 0, 0,
            0, scaleY, 0, 0,
            0, 0, scaleZ, 0,
            0, 0, 0, 1
        }
    };
    return ret;
}

matrix4_t mat4Translate(double tX, double tY, double tZ)
{
    matrix4_t ret =
    {
        .v =
        {
            1, 0, 0, tX,
            0, 1, 0, tY,
            0, 0, 1, tZ,
            0, 0, 0, 1
        }
    };
    return ret;
}

matrix4_t mat4RotationX(double angle)
{
    matrix4_t ret =
    {
        .v =
        {
            1, 0, 0, 0,
            0, cos(angle), -sin(angle), 0,
            0, sin(angle), cos(angle), 0,
            0, 0, 0, 1
        }
    };
    return ret;
}

matrix4_t mat4RotationY(double angle)
{
    matrix4_t ret =
    {
        .v =
        {
            cos(angle), 0, sin(angle), 0,
            0, 1, 0, 0,
            -sin(angle), 0, cos(angle), 0,
            0, 0, 0, 1
        }
    };
    return ret;
}

matrix4_t mat4RotationZ(double angle)
{
    matrix4_t ret =
    {
        .v =
        {
            cos(angle), -sin(angle), 0, 0,
            sin(angle), cos(angle), 0, 0,
            0, 0, 1, 0,
            0, 0, 0, 1
        }
    };
    return ret;
}

matrix4_t mat4Perspective(double aspectRatio, double FOV, double zNear, double zFar)
{
    double radFOV = (FOV * M_PI) / 180.;
    double f = 1. / tan(radFOV / 2.);
    matrix4_t ret =
    {
        .v =
            {
                aspectRatio * f, 0, 0, 0,
                0, f, 0, 0,
                0, 0, zFar / (zFar - zNear), -(zFar * zNear) / (zFar - zNear),
                0, 0, 1, 0
            }
    };
    return ret;
}

matrix4_t mat4LookAt(vec3_t eye, vec3_t target, vec3_t up)
{
    vec3_t forward = vec3SubVectors(target, eye);
    vec3Normalize(&forward);
    vec3_t right = vec3Cross(up, forward);
    vec3Normalize(&right);
    vec3_t trueUp = vec3Cross(forward, right);

    matrix4_t ret =
    {
        .v =
        {
            right.x,   right.y,   right.z,   -vec3Dot(right, eye),
            trueUp.x,  trueUp.y,  trueUp.z,  -vec3Dot(trueUp, eye),
            forward.x, forward.y, forward.z, -vec3Dot(forward, eye),
            0,         0,          0,        1
        }
    };

    return ret;
}
