/*******************************************************************************/
/* COS2000 - Compatible Operating System - LGPL v3 - Hordé Nicolas             */
/*                                                                             */
#include "matrix.h"
#include "types.h"
#include "math.h"

void vector4_show(vector4 src)
{
    printf("vecteur: X=%f Y=%f Z=%f W=%f \r\n", src.x, src.y, src.z, src.w);
}

void vector4_create(float x, float y, float z, float w, vector4 *dst)
{
    dst->x = x;
    dst->y = y;
    dst->z = z;
    dst->w = w;
}

void vector4_copy(vector4 src, vector4 *dst)
{
    vector4_create(src.x, src.y, src.z, src.w, dst);
}

void vector4_add(vector4 v1, vector4 v2, vector4 *dst)
{
    dst->x = v1.x + v2.x;
    dst->y = v1.y + v2.y;
    dst->z = v1.z + v2.z;
}

void vector4_sub(vector4 v1, vector4 v2, vector4 *dst)
{
    dst->x = v1.x - v2.x;
    dst->y = v1.y - v2.y;
    dst->z = v1.z - v2.z;
}

void vector4_scale(vector4 *dst, float factor)
{
    dst->x *= factor;
    dst->y *= factor;
    dst->z *= factor;
    dst->w *= factor;
}

void vector4_crossproduct(vector4 v1, vector4 v2, vector4 *dst)
{
    dst->x = v1.y * v2.z - v1.z * v2.y;
    dst->y = v1.z * v2.x - v1.x * v2.z;
    dst->z = v1.x * v2.y - v1.y * v2.x;
}

void vector4_normalize(vector4 *dst)
{
    float len;
    float norm;
    norm = vector4_norm(*dst);
    if (norm != 0)
    {
        len = 1 / norm;
        dst->x = dst->x * len;
        dst->y = dst->y * len;
        dst->z = dst->z * len;
        dst->w = 0;
    }
}

void vector4_divide(vector4 *v1, vector4 v2, vector4 *dst)
{
    dst->x = v1->x / v2.x;
    dst->y = v1->y / v2.y;
    dst->z = v1->z / v2.z;
}

void vector4_perpendicular(vector4 v1, vector4 v2, vector4 *dst)
{
    float dot = vector4_dotproduct(v1, v2);
    dst->x = v1.x - dot * v2.x;
    dst->y = v1.y - dot * v2.y;
    dst->z = v1.z - dot * v2.z;
}

void vector4_rotate_x(vector4 *dst, float angle)
{
    vector4 origin;
    float sinus, cosinus;
    sinus = sin(angle);
    cosinus = cos(angle);
    origin.x = dst->x;
    origin.y = dst->y;
    origin.z = dst->z;
    dst->y = cosinus * origin.y + sinus * origin.z;
    dst->z = cosinus * origin.z - sinus * origin.y;
}

void vector4_rotate_y(vector4 *dst, float angle)
{
    vector4 origin;
    float sinus, cosinus;
    sinus = sin(angle);
    cosinus = cos(angle);
    origin.x = dst->x;
    origin.y = dst->y;
    origin.z = dst->z;
    dst->x = cosinus * origin.x + sinus * origin.z;
    dst->z = cosinus * origin.z - sinus * origin.x;
}

void vector4_rotate_z(vector4 *dst, float angle)
{
    vector4 origin;
    float sinus, cosinus;
    sinus = sin(angle);
    cosinus = cos(angle);
    origin.x = dst->x;
    origin.y = dst->y;
    origin.z = dst->z;
    dst->x = cosinus * origin.x + sinus * origin.y;
    dst->y = cosinus * origin.y - sinus * origin.x;
}

float vector4_len(vector4 src)
{
    return sqrtf((src.x * src.x) +
                (src.y * src.y) +
                (src.z * src.z));
}

float vector4_dotproduct(vector4 v1, vector4 v2)
{
    return (v1.x * v2.x) + (v1.y * v2.y) + (v1.z * v2.z);
}

float vector4_norm(vector4 src)
{
    return sqrtf((src.x * src.x) + 
                 (src.y * src.y) +
                 (src.z * src.z));
}

float vector4_distance(vector4 v1, vector4 v2)
{
    return sqrt(pow(v2.x - v1.x, 2) +
                pow(v2.y - v1.y, 2) +
                pow(v2.z - v1.z, 2));
}

int vector4_isequals(vector4 v1, vector4 v2)
{
    float x, y, z;
    x = fabsf(v1.x - v2.x);
    y = fabsf(v1.y - v2.y);
    z = fabsf(v1.z - v2.z);
    return (x < 0.000001 && y < 0.000001 && z < 0.000001);
}

void vector4_planenormal(vector4 v1, vector4 v2, vector4 v3, vector4 *dst)
{
    vector4 temp1, temp2;
    vector4_sub(v2, v1, &temp1);
    vector4_sub(v3, v1, &temp2);
    vector4_crossproduct(temp1, temp2, dst);
    vector4_normalize(dst);
}

void matrix44_homogen(matrix44 *matrix)
{
    vector4_create(1, 0, 0, 0, &matrix->V[0]);
    vector4_create(0, 1, 0, 0, &matrix->V[1]);
    vector4_create(0, 0, 1, 0, &matrix->V[2]);
    vector4_create(0, 0, 0, 1, &matrix->V[3]);
}

void matrix44_empty(matrix44 *matrix)
{
    vector4_create(0, 0, 0, 0, &matrix->V[0]);
    vector4_create(0, 0, 0, 0, &matrix->V[1]);
    vector4_create(0, 0, 0, 0, &matrix->V[2]);
    vector4_create(0, 0, 0, 0, &matrix->V[3]);
}

void matrix44_scaling(vector4 v, matrix44 *dst)
{
    matrix44_homogen(dst);
    dst->V[0].x = v.x;
    dst->V[1].y = v.y;
    dst->V[2].z = v.z;
}

void matrix44_translation(vector4 v, matrix44 *dst)
{
    matrix44_homogen(dst);
    dst->V[0].x = v.x;
    dst->V[1].y = v.y;
    dst->V[2].z = v.z;
}

void matrix44_scale(matrix44 *dst, float factor)
{
    vector4_scale(&dst->V[0], factor);
    vector4_scale(&dst->V[1], factor);
    vector4_scale(&dst->V[2], factor);
    vector4_scale(&dst->V[3], factor);
}

void matrix44_scale_translation(vector4 scale, vector4 translation, matrix44 *dst)
{
    matrix44_homogen(dst);
    dst->V[0].x = scale.x;
    dst->V[1].y = scale.y;
    dst->V[2].z = scale.z;
    dst->V[3].x = translation.x;
    dst->V[3].y = translation.y;
    dst->V[3].z = translation.z;
}

void matrix44_rotation_x(float angle, matrix44 *dst)
{
    float sinus,cosinus;
    cosinus = cos(angle);
    sinus = sin(angle);
    matrix44_empty(dst);
    dst->V[0].x = 1;
    dst->V[1].y = cosinus;
    dst->V[1].z = sinus;
    dst->V[2].y = -1 * sinus;
    dst->V[2].z = cosinus;
    dst->V[3].w = 1;
}

void matrix44_rotation_y(float angle, matrix44 *dst)
{
    float sinus,cosinus;
    cosinus = cos(angle);
    sinus = sin(angle);
    matrix44_empty(dst);
    dst->V[0].x = cosinus;
    dst->V[0].z = -1 * sinus;
    dst->V[1].y = 1;
    dst->V[2].x = sinus;
    dst->V[2].z = cosinus;
    dst->V[3].w = 1;
}

void matrix44_rotation_z(float angle, matrix44 *dst)
{
    float sinus,cosinus;
    cosinus = cos(angle);
    sinus = sin(angle);
    matrix44_empty(dst);
    dst->V[0].x = cosinus;
    dst->V[0].y = sinus;
    dst->V[1].x = -1 * sinus;
    dst->V[1].y = cosinus;
    dst->V[2].z = 1;
    dst->V[3].w = 1;
}

void matrix44_rotation(vector4 axis, float angle, matrix44 *dst)
{
    float cosinus, sinus, minuscos;
    sinus = sinf(angle);
    cosinus = cosf(angle);
    vector4_normalize(&axis);
    minuscos = 1 - cosinus;

    dst->V[0].x = (minuscos * axis.x * axis.x) + cosinus;
    dst->V[0].y = (minuscos * axis.x * axis.y) - (axis.z * sinus);
    dst->V[0].z = (minuscos * axis.z * axis.x) + (axis.y * sinus);
    dst->V[0].w = 0;

    dst->V[1].x = (minuscos * axis.x * axis.y) + (axis.z * sinus);
    dst->V[1].y = (minuscos * axis.y * axis.y) + cosinus;
    dst->V[1].z = (minuscos * axis.y * axis.z) - (axis.x * sinus);
    dst->V[1].w = 0;

    dst->V[2].x = (minuscos * axis.z * axis.x) - (axis.y * sinus);
    dst->V[2].y = (minuscos * axis.y * axis.z) + (axis.x * sinus);
    dst->V[2].z = (minuscos * axis.z * axis.z) + cosinus;
    dst->V[2].w = 0;

    dst->V[3].x = 0;
    dst->V[3].y = 0;
    dst->V[3].z = 0;
    dst->V[3].w = 1;
}

void matrix44_multiply(matrix44 *m1, matrix44 *m2, matrix44 *dst)
{
    dst->V[0].x = (m1->V[0].x * m2->V[0].x + m1->V[0].y * m2->V[1].x +
                   m1->V[0].z * m2->V[2].x + m1->V[0].w * m2->V[3].x);
    dst->V[0].y = (m1->V[0].x * m2->V[0].y + m1->V[0].y * m2->V[1].y +
                   m1->V[0].z * m2->V[2].y + m1->V[0].w * m2->V[3].y);
    dst->V[0].z = (m1->V[0].x * m2->V[0].z + m1->V[0].y * m2->V[1].z +
                   m1->V[0].z * m2->V[2].z + m1->V[0].w * m2->V[3].z);
    dst->V[0].w = (m1->V[0].x * m2->V[0].w + m1->V[0].y * m2->V[1].w +
                   m1->V[0].z * m2->V[2].w + m1->V[0].w * m2->V[3].w);
    dst->V[1].x = (m1->V[1].x * m2->V[0].x + m1->V[1].y * m2->V[1].x +
                   m1->V[1].z * m2->V[2].x + m1->V[1].w * m2->V[3].x);
    dst->V[1].y = (m1->V[1].x * m2->V[0].y + m1->V[1].y * m2->V[1].y +
                   m1->V[1].z * m2->V[2].y + m1->V[1].w * m2->V[3].y);
    dst->V[1].z = (m1->V[1].x * m2->V[0].z + m1->V[1].y * m2->V[1].z +
                   m1->V[1].z * m2->V[2].z + m1->V[1].w * m2->V[3].z);
    dst->V[1].w = (m1->V[1].x * m2->V[0].w + m1->V[1].y * m2->V[1].w +
                   m1->V[1].z * m2->V[2].w + m1->V[1].w * m2->V[3].w);
    dst->V[2].x = (m1->V[2].x * m2->V[0].x + m1->V[2].y * m2->V[1].x +
                   m1->V[2].z * m2->V[2].x + m1->V[2].w * m2->V[3].x);
    dst->V[2].y = (m1->V[2].x * m2->V[0].y + m1->V[2].y * m2->V[1].y +
                   m1->V[2].z * m2->V[2].y + m1->V[2].w * m2->V[3].y);
    dst->V[2].z = (m1->V[2].x * m2->V[0].z + m1->V[2].y * m2->V[1].z +
                   m1->V[2].z * m2->V[2].z + m1->V[2].w * m2->V[3].z);
    dst->V[2].w = (m1->V[2].x * m2->V[0].w + m1->V[2].y * m2->V[1].w +
                   m1->V[2].z * m2->V[2].w + m1->V[2].w * m2->V[3].w);
    dst->V[3].x = (m1->V[3].x * m2->V[0].x + m1->V[3].y * m2->V[1].x +
                   m1->V[3].z * m2->V[2].x + m1->V[3].w * m2->V[3].x);
    dst->V[3].y = (m1->V[3].x * m2->V[0].y + m1->V[3].y * m2->V[1].y +
                   m1->V[3].z * m2->V[2].y + m1->V[3].w * m2->V[3].y);
    dst->V[3].z = (m1->V[3].x * m2->V[0].z + m1->V[3].y * m2->V[1].z +
                   m1->V[3].z * m2->V[2].z + m1->V[3].w * m2->V[3].z);
    dst->V[3].w = (m1->V[3].x * m2->V[0].w + m1->V[3].y * m2->V[1].w +
                   m1->V[3].z * m2->V[2].w + m1->V[3].w * m2->V[3].w);
}

void matrix44_transform(matrix44 *matrix, vector4 *dst)
{
    vector4 origin;
    origin.x = dst->x;
    origin.y = dst->y;
    origin.z = dst->z;
    origin.w = dst->w;
    dst->x = origin.x * matrix->V[0].x + origin.y * matrix->V[1].x + origin.z * matrix->V[2].x + origin.w * matrix->V[3].x;
    dst->y = origin.x * matrix->V[0].y + origin.y * matrix->V[1].y + origin.z * matrix->V[2].y + origin.w * matrix->V[3].y;
    dst->z = origin.x * matrix->V[0].z + origin.y * matrix->V[1].z + origin.z * matrix->V[2].z + origin.w * matrix->V[3].z;
    dst->w = origin.x * matrix->V[0].w + origin.y * matrix->V[1].w + origin.z * matrix->V[2].w + origin.w * matrix->V[3].w;
}

float matrix44_determinant(matrix44 *matrix)
{
    float a, b, c, d;
    a = matrix->V[0].x * todeterminant(matrix->V[1].y, matrix->V[2].y, matrix->V[3].y,
                                        matrix->V[1].z, matrix->V[2].z, matrix->V[3].z,
                                        matrix->V[1].w, matrix->V[2].w, matrix->V[3].w);
    b = matrix->V[0].y * todeterminant(matrix->V[1].x, matrix->V[2].x, matrix->V[3].x,
                                        matrix->V[1].z, matrix->V[2].z, matrix->V[3].z,
                                        matrix->V[1].w, matrix->V[2].w, matrix->V[3].w);
    c = matrix->V[0].z * todeterminant(matrix->V[1].x, matrix->V[2].x, matrix->V[3].x,
                                        matrix->V[1].y, matrix->V[2].y, matrix->V[3].y,
                                        matrix->V[1].w, matrix->V[2].w, matrix->V[3].w);
    d = matrix->V[0].w * todeterminant(matrix->V[1].x, matrix->V[2].x, matrix->V[3].x,
                                        matrix->V[1].y, matrix->V[2].y, matrix->V[3].y,
                                        matrix->V[1].z, matrix->V[2].z, matrix->V[3].z);
    return a - b + c - d;
}

float todeterminant(float a1, float a2, float a3, float b1, float b2, float b3, float c1, float c2, float c3)
{
    return (a1 * ((b2 * c3) - (b3 * c2))) - (b1 * ((a2 * c3) - (a3 * c2))) + (c1 * ((a2 * b3) - (a3 * b2)));
}

void matrix44_adjoint(matrix44 *matrix)
{
    float a1, a2, a3, a4, b1, b2, b3, b4, c1, c2, c3, c4, d1, d2, d3, d4;
    a1 = matrix->V[0].x;
    b1 = matrix->V[0].y;
    c1 = matrix->V[0].z;
    d1 = matrix->V[0].w;

    a2 = matrix->V[1].x;
    b2 = matrix->V[1].y;
    c2 = matrix->V[1].z;
    d2 = matrix->V[1].w;

    a3 = matrix->V[2].x;
    b3 = matrix->V[2].y;
    c3 = matrix->V[2].z;
    d3 = matrix->V[2].w;

    a4 = matrix->V[3].x;
    b4 = matrix->V[3].y;
    c4 = matrix->V[3].z;
    d4 = matrix->V[3].w;

    matrix->V[0].x = todeterminant(b2, b3, b4, c2, c3, c4, d2, d3, d4);
    matrix->V[1].x = -todeterminant(a2, a3, a4, c2, c3, c4, d2, d3, d4);
    matrix->V[2].x = todeterminant(a2, a3, a4, b2, b3, b4, d2, d3, d4);
    matrix->V[3].x = -todeterminant(a2, a3, a4, b2, b3, b4, c2, c3, c4);

    matrix->V[0].y = -todeterminant(b1, b3, b4, c1, c3, c4, d1, d3, d4);
    matrix->V[1].y = todeterminant(a1, a3, a4, c1, c3, c4, d1, d3, d4);
    matrix->V[2].y = -todeterminant(a1, a3, a4, b1, b3, b4, d1, d3, d4);
    matrix->V[3].y = todeterminant(a1, a3, a4, b1, b3, b4, c1, c3, c4);

    matrix->V[0].z = todeterminant(b1, b2, b4, c1, c2, c4, d1, d2, d4);
    matrix->V[1].z = -todeterminant(a1, a2, a4, c1, c2, c4, d1, d2, d4);
    matrix->V[2].z = todeterminant(a1, a2, a4, b1, b2, b4, d1, d2, d4);
    matrix->V[3].z = -todeterminant(a1, a2, a4, b1, b2, b4, c1, c2, c4);

    matrix->V[0].w = -todeterminant(b1, b2, b3, c1, c2, c3, d1, d2, d3);
    matrix->V[1].w = todeterminant(a1, a2, a3, c1, c2, c3, d1, d2, d3);
    matrix->V[2].w = -todeterminant(a1, a2, a3, b1, b2, b3, d1, d2, d3);
    matrix->V[3].w = todeterminant(a1, a2, a3, b1, b2, b3, c1, c2, c3);
}

void matrix44_show(matrix44 *matrix)
{
    printf("Matrice: X=%f Y=%f Z=%f W=%f \r\n", matrix->V[0].x, matrix->V[1].y, matrix->V[2].z, matrix->V[3].w);
    printf("         X=%f Y=%f Z=%f W=%f \r\n", matrix->V[0].x, matrix->V[1].y, matrix->V[2].z, matrix->V[3].w);
    printf("         X=%f Y=%f Z=%f W=%f \r\n", matrix->V[0].x, matrix->V[1].y, matrix->V[2].z, matrix->V[3].w);
}

void matrix44_invert(matrix44 *matrix)
{
    float det;
    det = matrix44_determinant(matrix);
    if (fabs(det) < EPSILON)
    {
        matrix44_homogen(matrix);
    }
    else
    {
        matrix44_adjoint(matrix);
        matrix44_scale(matrix, 1.0 / det);
    }
}

void matrix44_transpose(matrix44 *matrix)
{
    float f;
    f = matrix->V[0].y;
    matrix->V[0].y = matrix->V[1].x;
    matrix->V[1].x = f;

    f = matrix->V[0].z;
    matrix->V[0].z = matrix->V[2].x;
    matrix->V[2].x = f;

    f = matrix->V[0].w;
    matrix->V[0].w = matrix->V[3].x;
    matrix->V[3].x = f;

    f = matrix->V[1].z;
    matrix->V[1].z = matrix->V[2].y;
    matrix->V[2].y = f;

    f = matrix->V[1].w;
    matrix->V[1].w = matrix->V[3].y;
    matrix->V[3].y = f;

    f = matrix->V[2].w;
    matrix->V[2].w = matrix->V[3].z;
    matrix->V[3].z = f;
}

void matrix44_lookat(vector4 eye, vector4 dst, vector4 up, matrix44 *matrix)
{
    vector4 xaxis, yaxis, zaxis, negeye;
    vector4_sub(dst, eye, &zaxis);
    vector4_normalize(&zaxis);

    vector4_crossproduct(zaxis, up, &xaxis);
    vector4_normalize(&xaxis);

    vector4_crossproduct(xaxis, zaxis, &yaxis);

    vector4_copy(xaxis, &matrix->V[0]);
    vector4_copy(yaxis, &matrix->V[1]);
    vector4_copy(zaxis, &matrix->V[2]);

    matrix->V[2].x = -matrix->V[2].x;
    matrix->V[2].y = -matrix->V[2].y;
    matrix->V[2].z = -matrix->V[2].z;

    vector4_create(0, 0, 0, 1, &matrix->V[3]);

    matrix44_transpose(matrix);
    vector4_create(-eye.x, -eye.y, -eye.z, 1, &negeye);

    matrix44_transform(matrix, &negeye);
    vector4_copy(negeye, &matrix->V[3]);
}

int matrix44_isequals(matrix44 *m1, matrix44 *m2)
{
    return vector4_isequals(m1->V[0], m2->V[0]) && vector4_isequals(m1->V[1], m2->V[1]) &&
           vector4_isequals(m1->V[2], m2->V[2]) && vector4_isequals(m1->V[3], m2->V[3]);
}

void toarray(matrix44 *m, float *array)
{
    array[0] = m->V[0].x;
    array[1] = m->V[0].y;
    array[2] = m->V[0].z;
    array[3] = m->V[0].w;
    array[4] = m->V[1].x;
    array[5] = m->V[1].y;
    array[6] = m->V[1].z;
    array[7] = m->V[1].w;
    array[8] = m->V[2].x;
    array[9] = m->V[2].y;
    array[10] = m->V[2].z;
    array[11] = m->V[2].w;
    array[12] = m->V[3].x;
    array[13] = m->V[3].y;
    array[14] = m->V[3].z;
    array[15] = m->V[3].w;
}