/*
 *   Sweet is a small library for basic math and small matrix operations.
 *   Copyright 2014 Luc Girod.
 *
 * This library is free software: you can redistribute it and/or
 * modify it under the terms of the GNU Lesser General Public
 * License as published by the Free Software Foundation, either
 * version 3 of the License, or (at your option) any later version.
 *
 * This library is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 * Lesser General Public License for more details.
 *
 * You should have received a copy of the GNU Lesser General Public
 * License along with this library.  If not, see <http://www.gnu.org/licenses/>.
 */



#include <string.h>
#include <math.h>
#include "sweet_matrix.h"
#include "sweet_math.h"
#include "sweet_types.h"

mat2
sweet_matrix_null2 (void)
{
    mat2 m;

    m.v[0] = 0.0;
    m.v[1] = 0.0;

    m.v[2] = 0.0;
    m.v[3] = 0.0;

    return m;
}

mat3
sweet_matrix_null3 (void)
{
    mat3 m;

    m.v[0] = 0.0;
    m.v[1] = 0.0;
    m.v[2] = 0.0;

    m.v[3] = 0.0;
    m.v[4] = 0.0;
    m.v[5] = 0.0;

    m.v[6] = 0.0;
    m.v[7] = 0.0;
    m.v[8] = 0.0;

    return m;
}

mat4
sweet_matrix_null4 (void)
{
    mat4 m;

    m.v[0]  = 0.0;
    m.v[1]  = 0.0;
    m.v[2]  = 0.0;
    m.v[3]  = 0.0;

    m.v[4]  = 0.0;
    m.v[5]  = 0.0;
    m.v[6]  = 0.0;
    m.v[7]  = 0.0;

    m.v[8]  = 0.0;
    m.v[9]  = 0.0;
    m.v[10] = 0.0;
    m.v[11] = 0.0;

    m.v[12] = 0.0;
    m.v[13] = 0.0;
    m.v[14] = 0.0;
    m.v[15] = 0.0;

    return m;
}


mat2
sweet_matrix_identity2 (void)
{
    mat2 m;

    m.v[0] = 1.0;
    m.v[1] = 0.0;

    m.v[2] = 0.0;
    m.v[3] = 1.0;

    return m;
}

mat3
sweet_matrix_identity3 (void)
{
    mat3 m;

    m.v[0] = 1.0;
    m.v[1] = 0.0;
    m.v[2] = 0.0;

    m.v[3] = 0.0;
    m.v[4] = 1.0;
    m.v[5] = 0.0;

    m.v[6] = 0.0;
    m.v[7] = 0.0;
    m.v[8] = 1.0;

    return m;
}

mat4
sweet_matrix_identity4 (void)
{
    mat4 m;

    m.v[0]  = 1.0;
    m.v[1]  = 0.0;
    m.v[2]  = 0.0;
    m.v[3]  = 0.0;

    m.v[4]  = 0.0;
    m.v[5]  = 1.0;
    m.v[6]  = 0.0;
    m.v[7]  = 0.0;

    m.v[8]  = 0.0;
    m.v[9]  = 0.0;
    m.v[10] = 1.0;
    m.v[11] = 0.0;

    m.v[12] = 0.0;
    m.v[13] = 0.0;
    m.v[14] = 0.0;
    m.v[15] = 1.0;

    return m;
}


mat2 
sweet_matrix_ortho_basis2 (vec2 u, vec2 v)
{
    mat2 m;
    m.v[0] = u.x;
    m.v[3] = v.x;

    m.v[2] = u.y;
    m.v[3] = v.y;

    return m;
}

mat3 
sweet_matrix_ortho_basis3 (vec3 u, vec3 v, vec3 w)
{
    mat3 m;
    m.v[0] = u.x;
    m.v[1] = v.x;
    m.v[2] = w.x;

    m.v[3] = u.y;
    m.v[4] = v.y;
    m.v[5] = w.y;

    m.v[6] = u.z;
    m.v[7] = v.z;
    m.v[8] = w.z;

    return m;
}

mat4 
sweet_matrix_ortho_basis4 (vec4 u, vec4 v, vec4 w, vec4 t)
{
    mat4 m;
    m.v[0] = u.x;
    m.v[1] = v.y;
    m.v[2] = w.z;
    m.v[3] = t.w;

    m.v[4] = u.x;
    m.v[5] = v.y;
    m.v[6] = w.z;
    m.v[7] = t.w;

    m.v[8]  = u.x;
    m.v[9]  = v.y;
    m.v[10] = w.z;
    m.v[11] = t.w;

    m.v[12] = u.x;
    m.v[13] = v.y;
    m.v[14] = w.z;
    m.v[15] = t.w;

    return m;
}

mat4
sweet_matrix_perspective (double fovy, double aspect, double z_near, double z_far)
{
    mat4 m;
    float f;

    fovy = sweet_math_degree_2_radian (fovy);
    f = 1.0 / tan (fovy / 2.0);

    m.v[0] = f / aspect;
    m.v[1] = 0.0;
    m.v[2] = 0.0;
    m.v[3] = 0.0;

    m.v[4] = 0.0;
    m.v[5] = f;
    m.v[6] = 0.0;
    m.v[7] = 0.0;

    m.v[8] = 0.0;
    m.v[9] = 0.0;
    m.v[10] = (z_far + z_near) / (z_near - z_far);
    m.v[11] = -1.0;

    m.v[12] = 0.0;
    m.v[13] = 0.0;
    m.v[14] = (2.0 * z_far * z_near) / (z_near - z_far);
    m.v[15] = 0.0;

    return m;
}

mat4
sweet_matrix_frustum (float left, float right,
                     float bottom, float top,
                     float z_near, float z_far)
{
    mat4 m;

    m.v[0]  = 2.0 * z_near / (right - left);
    m.v[1]  = 0.0;
    m.v[2]  = 0.0;
    m.v[3]  = 0.0;

    m.v[4]  = 0.0;
    m.v[5]  = 2.0 * z_near / (top - bottom);
    m.v[6]  = 0.0;
    m.v[7]  = 0.0;

    m.v[8]  = (right + left) / (right - left);
    m.v[9]  = (top + bottom) / (top - bottom);
    m.v[10] = (z_far + z_near) / (z_near - z_far);
    m.v[11] = -1.0;

    m.v[12] = 0.0;
    m.v[13] = 0.0;
    m.v[14] = (2 * z_far * z_near) / (z_near - z_far);
    m.v[15] = 0.0;

    return m;
}

mat4
sweet_matrix_ortho (float left, float right,
                   float bottom, float top,
                   float zNear, float zFar)
{
    mat4 m;

    m.v[0] = 2/(right - left);
    m.v[1] = 0;
    m.v[2] = 0;
    m.v[3] = 0;
    m.v[4] = 0;
    m.v[5] = 2/(top - bottom);
    m.v[6] = 0;
    m.v[7] = 0;
    m.v[8] = 0;
    m.v[9] = 0;
    m.v[10] = -2 / (zFar - zNear);
    m.v[11] = 0;
    m.v[12] = - (right + left) / (right - left);
    m.v[13] = - (top + bottom) / (top - bottom);
    m.v[14] = - (zFar + zNear) / (zFar - zNear);
    m.v[15] = 1;

    return m;
}

mat4
sweet_matrix_look_at (vec3 pos, vec3 dir, vec3 up)
{
    mat4 m;
    vec3 side;
    side = sweet_vector_cross (dir, up);
    side = sweet_vector_normalize3 (side);
    up = sweet_vector_cross (side, dir);
    up = sweet_vector_normalize3 (up);

    m.v[0] = side.x;
    m.v[1] = up.x;
    m.v[2] = -dir.x;
    m.v[3] = 0;

    m.v[4] = side.y;
    m.v[5] = up.y;
    m.v[6] = -dir.y;
    m.v[7] = 0;

    m.v[8] = side.z;
    m.v[9] = up.z;
    m.v[10] = -dir.z;
    m.v[11] = 0;

    m.v[12] = -pos.x * side.x - pos.y * side.y - pos.z * side.z;
    m.v[13] = -pos.x * up.x - pos.y * up.y - pos.z * up.z;
    m.v[14] = pos.x * dir.x + pos.y * dir.y + pos.z * dir.z;
    m.v[15] = 1;

    return m;
}

mat3
sweet_matrix_quat_rotation3 (quaternion q)
{
    mat3 m;

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

    m.v[3] =   2 * q.x * q.y - 2 * q.w * q.z;
    m.v[4] = - 2 * q.x * q.x - 2 * q.z * q.z + 1;
    m.v[5] =   2 * q.y * q.z + 2 * q.w * q.x;

    m.v[6] =   2 * q.x * q.z + 2 * q.w * q.y;
    m.v[7] =   2 * q.y * q.z - 2 * q.w * q.x;
    m.v[8] = - 2 * q.x * q.x - 2 * q.y * q.y;

    return m;
}

mat4
sweet_matrix_quat_rotation3h (quaternion q)
{
    mat4 m;

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

    m.v[4]  =   2 * q.x * q.y - 2 * q.w * q.z;
    m.v[5]  = - 2 * q.x * q.x - 2 * q.z * q.z + 1;
    m.v[6]  =   2 * q.y * q.z + 2 * q.w * q.x;
    m.v[7]  =   0;

    m.v[8]  =   2 * q.x * q.z + 2 * q.w * q.y;
    m.v[9]  =   2 * q.y * q.z - 2 * q.w * q.x;
    m.v[10] = - 2 * q.x * q.x - 2 * q.y * q.y;
    m.v[11] =   0;

    m.v[12] =   0;
    m.v[13] =   0;
    m.v[14] =   0;
    m.v[15] =   1;

    return m;
}

mat4
sweet_matrix_rotation3h (float angle, float x, float y, float z)
{
    mat4 m;

    m.v[0] = x * x * (1 - cos (angle)) + cos (angle);
    m.v[1] = x * y * (1 - cos (angle)) + z * sin (angle);
    m.v[2] = x * z * (1 - cos (angle)) - y * sin (angle);
    m.v[3] = 0;

    m.v[4] = x * y * (1 - cos (angle)) - z * sin (angle);
    m.v[5] = y * y * (1 - cos (angle)) + cos (angle);
    m.v[6] = y * z * (1 - cos (angle)) + x * sin (angle);
    m.v[7] = 0;

    m.v[8] = x * z * (1 - cos (angle)) + y * sin (angle);
    m.v[9] = y * z * (1 - cos (angle)) - x * sin (angle);
    m.v[10] = z * z * (1 - cos (angle)) + cos (angle);
    m.v[11] = 0;

    m.v[12] = 0;
    m.v[13] = 0;
    m.v[14] = 0;
    m.v[15] = 1;

    return m;
}

mat3
sweet_matrix_rotation3 (float angle, float x, float y, float z)
{
    mat3 m;

    m.v[0] = x * x * (1 - cos (angle)) + cos (angle);
    m.v[1] = x * y * (1 - cos (angle)) + z * sin (angle);
    m.v[2] = x * z * (1 - cos (angle)) - y * sin (angle);

    m.v[3] = x * y * (1 - cos (angle)) - z * sin (angle);
    m.v[4] = y * y * (1 - cos (angle)) + cos (angle);
    m.v[5] = y * z * (1 - cos (angle)) + x * sin (angle);

    m.v[6] = x * z * (1 - cos (angle)) + y * sin (angle);
    m.v[7] = y * z * (1 - cos (angle)) - x * sin (angle);
    m.v[8] = z * z * (1 - cos (angle)) + cos (angle);

    return m;
}

mat2
sweet_matrix_rotation2 (float angle)
{
    mat2 m;

    m.v[0] = cos (angle);
    m.v[1] = sin (angle);
    m.v[2] = -sin (angle);
    m.v[3] = cos (angle);

    return m;
}

mat3
sweet_matrix_rotation2h (float angle)
{
    mat3 m;

    m.v[0] = cos (angle);
    m.v[1] = sin (angle);
    m.v[2] = 0.0;
    m.v[3] = -sin (angle);
    m.v[4] = cos (angle);
    m.v[5] = 0.0;
    m.v[6] = 0.0;
    m.v[7] = 0.0;
    m.v[8] = 1.0;

    return m;
}

mat4
sweet_matrix_translation3h (vec3 v)
{
    mat4 m;

    m.v[0] = 1.0;
    m.v[1] = 0.0;
    m.v[2] = 0.0;
    m.v[3] = 0.0;

    m.v[4] = 0.0;
    m.v[5] = 1.0;
    m.v[6] = 0.0;
    m.v[7] = 0.0;

    m.v[8] = 0.0;
    m.v[9] = 0.0;
    m.v[10] = 1.0;
    m.v[11] = 0.0;

    m.v[12] = v.x;
    m.v[13] = v.y;
    m.v[14] = v.z;
    m.v[15] = 1.0;

    return m;
}

mat3
sweet_matrix_translation2h (vec2 v)
{
    mat3 m;

    m.v[0] = 1.0;
    m.v[1] = 0.0;
    m.v[2] = 0.0;

    m.v[4] = 0.0;
    m.v[5] = 1.0;
    m.v[6] = 0.0;

    m.v[8] = v.x;
    m.v[9] = v.y;
    m.v[10] = 1.0;

    return m;
}

mat4
sweet_matrix_scale4 (float x, float y, float z, float w)
{
    mat4 m;

    m.v[0] = x;
    m.v[1] = 0;
    m.v[2] = 0;
    m.v[3] = 0;

    m.v[4] = 0;
    m.v[5] = y;
    m.v[6] = 0;
    m.v[7] = 0;

    m.v[8] = 0;
    m.v[9] = 0;
    m.v[10] = z;
    m.v[11] = 0;

    m.v[12] = 0;
    m.v[13] = 0;
    m.v[14] = 0;
    m.v[15] = w;

    return m;
}

mat3
sweet_matrix_scale3 (float x, float y, float z)
{
    mat3 m;

    m.v[0] = x;
    m.v[1] = 0;
    m.v[2] = 0;

    m.v[3] = 0;
    m.v[4] = y;
    m.v[5] = 0;

    m.v[6] = 0;
    m.v[7] = 0;
    m.v[8] = z;

    return m;
}

mat2
sweet_matrix_scale2 (float x, float y)
{
    mat2 m;

    m.v[0] = x;
    m.v[1] = 0;

    m.v[2] = 0;
    m.v[3] = y;

    return m;
}

mat4
sweet_matrix_texture_bias (void)
{
    mat4 m;
    m.v[0] = 0.5;
    m.v[1] = 0.0;
    m.v[2] = 0.0;
    m.v[3] = 0.0;

    m.v[4] = 0.0;
    m.v[5] = 0.5;
    m.v[6] = 0.0;
    m.v[7] = 0.0;

    m.v[8] = 0.0;
    m.v[9] = 0.0;
    m.v[10] = 0.5;
    m.v[11] = 0.0;

    m.v[12] = 0.5;
    m.v[13] = 0.5;
    m.v[14] = 0.5;
    m.v[15] = 1.0;

    return m;
}

void
sweet_matrix_transpose2 (mat2 * mt, mat2 * m)
{
    float mv1 = m->v[1];
    mt->v[0] = m->v[0];
    mt->v[1] = m->v[2];
    mt->v[2] = mv1;
    mt->v[3] = m->v[3];
}

void
sweet_matrix_transpose3 (mat3 * mt, mat3 * m)
{
    float mv1 = m->v[1];
    float mv2 = m->v[2];
    float mv5 = m->v[5];
    mt->v[0] = m->v[0];
    mt->v[1] = m->v[3];
    mt->v[2] = m->v[6];
    mt->v[3] = mv1;
    mt->v[4] = m->v[4];
    mt->v[5] = m->v[7];
    mt->v[6] = mv2;
    mt->v[7] = mv5;
    mt->v[8] = m->v[8];
}

void
sweet_matrix_transpose4 (mat4 * mt, mat4 * m)
{
    float mv2 = m->v[2];
    float mv3 = m->v[3];
    float mv6 = m->v[6];
    float mv7 = m->v[7];
    float mv11 = m->v[11];

    mt->v[0] = m->v[0];
    mt->v[1] = m->v[4];
    mt->v[2] = m->v[8];
    mt->v[3] = m->v[12];
    mt->v[4] = mv2;
    mt->v[5] = m->v[5];
    mt->v[6] = m->v[9];
    mt->v[7] = m->v[13];
    mt->v[8] = mv2;
    mt->v[9] = mv6;
    mt->v[10] = m->v[10];
    mt->v[11] = m->v[14];
    mt->v[12] = mv3;
    mt->v[13] = mv7;
    mt->v[14] = mv11;
    mt->v[15] = m->v[15];
}

float
sweet_matrix_det2 (mat2 * m)
{
    return m->v[0] * m->v[3] - m->v[1] * m->v[2];
}

float
sweet_matrix_det3 (mat3 * m)
{
    float det =
        m->v[0] * (m->v[4] * m->v[8] - m->v[5] * m->v[7]) -
        m->v[1] * (m->v[3] * m->v[8] - m->v[5] * m->v[6]) +
        m->v[2] * (m->v[3] * m->v[7] - m->v[4] * m->v[6]);

    return det;
}

float
sweet_matrix_det4 (mat4 * m)
{
    float det =
         m->v[0] * (m->v[ 5] * (m->v[10] * m->v[15] - m->v[11] * m->v[14]) -
                    m->v[ 6] * (m->v[ 9] * m->v[15] - m->v[11] * m->v[13]) +
                    m->v[ 7] * (m->v[ 9] * m->v[14] - m->v[10] * m->v[13]))
        -m->v[1] * (m->v[ 4] * (m->v[10] * m->v[15] - m->v[11] * m->v[14]) -
                    m->v[ 6] * (m->v[ 8] * m->v[15] - m->v[11] * m->v[12]) +
                    m->v[ 7] * (m->v[ 8] * m->v[14] - m->v[10] * m->v[12]))
        +m->v[2] * (m->v[ 4] * (m->v[ 9] * m->v[15] - m->v[11] * m->v[13]) -
                    m->v[ 5] * (m->v[ 8] * m->v[15] - m->v[11] * m->v[12]) +
                    m->v[ 7] * (m->v[ 8] * m->v[13] - m->v[ 9] * m->v[12]))
        -m->v[3] * (m->v[ 4] * (m->v[ 9] * m->v[14] - m->v[10] * m->v[13]) -
                    m->v[ 5] * (m->v[ 8] * m->v[14] - m->v[10] * m->v[12]) +
                    m->v[ 6] * (m->v[ 8] * m->v[13] - m->v[ 9] * m->v[12]));

    return det;
}

void
sweet_matrix_inverse2 (mat2 * inv, mat2 * m)
{
    float invdet = 1.0 / sweet_matrix_det2 (m);

    float mv0 = m->v[0];
    inv->v[0] = m->v[3] * invdet;
    inv->v[1] = m->v[1] * (-invdet);
    inv->v[2] = m->v[2] * (-invdet);
    inv->v[3] = mv0 * invdet;
}

void
sweet_matrix_inverse3 (mat3 * inv, mat3 * m)
{
    float mv0 = m->v[0];
    float mv1 = m->v[1];
    float mv2 = m->v[2];
    float mv3 = m->v[3];
    float mv4 = m->v[4];
    float mv6 = m->v[6];
    float invdet;

    invdet = 1.0 / sweet_matrix_det3 (m);

    inv->v[0] = ((m->v[4] * m->v[8]) - (m->v[5] * m->v[7])) * invdet;
    inv->v[1] = (-(m->v[1] * m->v[8]) + (m->v[2] * m->v[7])) * invdet;
    inv->v[2] = ((mv1 * m->v[5]) - (m->v[2] * m->v[4])) * invdet;

    inv->v[3] = (-(m->v[3] * m->v[8]) + (m->v[5] * m->v[6])) * invdet;
    inv->v[4] = ((mv0 * m->v[8]) - (mv2 * m->v[6])) * invdet;
    inv->v[5] = (-(mv0 * m->v[5]) + (mv2 * mv3)) * invdet;

    inv->v[6] = ((mv3 * m->v[7]) - (mv4 * m->v[6])) * invdet;
    inv->v[7] = (-(mv0 * m->v[7]) + (mv1 * mv6)) * invdet;
    inv->v[8] = ((mv0 * mv4) - (mv1 * mv3)) * invdet;
}

static void
matrix_inverse4 (mat4 * inv, mat4 * m)
{
    float mv0 = m->v[0];
    float mv1 = m->v[1];
    float mv2 = m->v[2];
    float mv3 = m->v[3];
    float mv4 = m->v[4];
    float mv5 = m->v[5];
    float mv6 = m->v[6];
    float mv7 = m->v[7];
    float mv8 = m->v[8];
    float mv9 = m->v[9];
    float mv10 = m->v[10];
    float mv12 = m->v[12];
    float mv13 = m->v[13];
    float invdet = 1.0 / sweet_matrix_det4 (m);

    inv->v[0] = m->v[5] * (m->v[10] * m->v[15] - m->v[11] * m->v[14]) -
              m->v[6] * (m->v[9] * m->v[15] - m->v[11] * m->v[13]) +
              m->v[7] * (m->v[9] * m->v[14] - m->v[10] * m->v[13]);

    inv->v[1] = -m->v[1] * (m->v[10] * m->v[15] - m->v[11] * m->v[14]) +
              m->v[2] * (m->v[9] * m->v[15] - m->v[11] * m->v[13]) -
              m->v[3] * (m->v[9] * m->v[14] - m->v[10] * m->v[13]);

    inv->v[2] = mv1 * (m->v[6] * m->v[15] - m->v[7] * m->v[14]) -
              m->v[2] * (m->v[5] * m->v[15] - m->v[7] * m->v[13]) +
              m->v[3] * (m->v[5] * m->v[14] - m->v[6] * m->v[13]);

    inv->v[3] = -mv1 * (m->v[6] * m->v[11] - m->v[7] * m->v[10]) +
              mv2 * (m->v[5] * m->v[11] - m->v[7] * m->v[9]) -
              m->v[3] * (m->v[5] * m->v[10] - m->v[6] * m->v[9]);

    inv->v[4] = -m->v[4] * (m->v[10] * m->v[15] - m->v[11] * m->v[14]) +
              m->v[6] * (m->v[8] * m->v[15] - m->v[11] * m->v[12]) -
              m->v[7] * (m->v[8] * m->v[14] - m->v[10] * m->v[12]);

    inv->v[5] = mv0 * (m->v[10] * m->v[15] - m->v[11] * m->v[14]) -
              mv2 * (m->v[8] * m->v[15] - m->v[11] * m->v[12]) +
              mv3 * (m->v[8] * m->v[14] - m->v[10] * m->v[12]);

    inv->v[6] = -mv0 * (m->v[6] * m->v[15] - m->v[7] * m->v[14]) +
              mv2 * (mv4 * m->v[15] - m->v[7] * m->v[12]) -
              mv3 * (mv4 * m->v[14] - m->v[6] * m->v[12]);

    inv->v[7] = mv0 * (mv6 * m->v[11] - m->v[7] * m->v[10]) -
              mv2 * (mv4 * m->v[11] - m->v[7] * m->v[8]) +
              mv3 * (mv4 * m->v[10] - mv6 * m->v[8]);

    inv->v[8] = mv4 * (m->v[9] * m->v[15] - m->v[11] * m->v[13]) -
              mv5 * (m->v[8] * m->v[15] - m->v[11] * m->v[12]) +
              mv7 * (m->v[8] * m->v[13] - m->v[9] * m->v[12]);

    inv->v[9] = -mv0 * (m->v[9] * m->v[15] - m->v[11] * m->v[13]) +
              mv1 * (m->v[8] * m->v[15] - m->v[11] * m->v[12]) -
              mv3 * (m->v[8] * m->v[13] - m->v[9] * m->v[12]);

    inv->v[10] = mv0 * (mv5 * m->v[15] - mv7 * m->v[13]) -
               mv1 * (mv4 * m->v[15] - mv7 * m->v[12]) +
               mv3 * (mv4 * m->v[13] - mv5 * m->v[12]);

    inv->v[11] = -mv0 * (mv5 * m->v[11] - mv7 * mv9) +
               mv1 * (mv4 * m->v[11] - mv7 * mv8) -
               mv3 * (mv4 * mv9 - mv5 * mv8);

    inv->v[12] = -mv4 * (mv9 * m->v[14] - mv10 * m->v[13]) +
               mv5 * (mv8 * m->v[14] - mv10 * m->v[12]) -
               mv6 * (mv8 * m->v[13] - mv9 * m->v[12]);

    inv->v[13] = mv0 * (mv9 * m->v[14] - mv10 * m->v[13]) -
               mv1 * (mv8 * m->v[14] - mv10 * mv12) +
               mv2 * (mv8 * m->v[13] - mv9 * mv12);

    inv->v[14] = -mv0 * (mv5 * m->v[14] - mv6 * mv13) +
               mv1 * (mv4 * m->v[14] - mv6 * mv12) -
               mv2 * (mv4 * mv13 - mv5 * mv12);

    inv->v[15] = mv0 * (mv5 * mv10 - mv6 * mv9) -
               mv1 * (mv4 * mv10 - mv6 * mv8) +
               mv2 * (mv4 * mv9 - mv5 * mv8);

    inv->v[ 0] *= invdet;
    inv->v[ 1] *= invdet;
    inv->v[ 2] *= invdet;
    inv->v[ 3] *= invdet;
    inv->v[ 4] *= invdet;
    inv->v[ 5] *= invdet;
    inv->v[ 6] *= invdet;
    inv->v[ 7] *= invdet;
    inv->v[ 8] *= invdet;
    inv->v[ 9] *= invdet;
    inv->v[10] *= invdet;
    inv->v[11] *= invdet;
    inv->v[12] *= invdet;
    inv->v[13] *= invdet;
    inv->v[14] *= invdet;
    inv->v[15] *= invdet;
}

void
sweet_matrix_inverse4 (mat4 * inv, mat4 * m)
{
    float detp;
    float invdet;
    mat2 rp;
    mat2 pq;
    mat2 rpq;
    mat2 s;
    mat2 p;

    detp = m->v[0] * m->v[5] - m->v[1] * m->v[4];

    if (sweet_math_approx_equals (detp, 0, 0.0000001))
    {
        /* In this case the standard algorithm is used */
        matrix_inverse4 (inv, m);
        return;
    }

    /* In this case we can use a better approach */

    invdet = 1.0 / detp;

    p.v[0] = m->v[5] * invdet;
    p.v[1] = -m->v[1] * invdet;
    p.v[2] = -m->v[4] * invdet;
    p.v[3] = m->v[0] * invdet;

    rp.v[0] = (p.v[0] * m->v[2] + p.v[1] * m->v[6]);
    rp.v[1] = (p.v[0] * m->v[3] + p.v[1] * m->v[7]);
    rp.v[2] = (p.v[2] * m->v[2] + p.v[3] * m->v[6]);
    rp.v[3] = (p.v[2] * m->v[3] + p.v[3] * m->v[7]);

    pq.v[0] = (p.v[0] * m->v[8] + p.v[2] * m->v[9]);
    pq.v[1] = (p.v[1] * m->v[8] + p.v[3] * m->v[9]);
    pq.v[2] = (p.v[0] * m->v[12] + p.v[2] * m->v[13]);
    pq.v[3] = (p.v[1] * m->v[12] + p.v[3] * m->v[13]);

    rpq.v[0] = rp.v[0] * m->v[8] + rp.v[2] * m->v[9];
    rpq.v[1] = rp.v[1] * m->v[8] + rp.v[3] * m->v[9];
    rpq.v[2] = rp.v[0] * m->v[12] + rp.v[2] * m->v[13];
    rpq.v[3] = rp.v[1] * m->v[12] + rp.v[3] * m->v[13];

    s.v[0] = m->v[10] - rpq.v[0];
    s.v[1] = m->v[11] - rpq.v[1];
    s.v[2] = m->v[14] - rpq.v[2];
    s.v[3] = m->v[15] - rpq.v[3];

    invdet = 1.0 / (s.v[0] * s.v[3] - s.v[1] * s.v[2]);

    inv->v[10] = s.v[3] * invdet;
    inv->v[11] = - s.v[1] * invdet;
    inv->v[14] = - s.v[2] * invdet;
    inv->v[15] = s.v[0] * invdet;

    inv->v[2] = -inv->v[10] * rp.v[0] - m->v[14] * rp.v[1];
    inv->v[3] = -inv->v[11] * rp.v[0] - m->v[15] * rp.v[1];
    inv->v[6] = -inv->v[10] * rp.v[2] - m->v[14] * rp.v[3];
    inv->v[7] = -inv->v[11] * rp.v[2] - m->v[15] * rp.v[3];

    inv->v[8] = -pq.v[0] * inv->v[10] - pq.v[2] * inv->v[11];
    inv->v[9] = -pq.v[1] * inv->v[10] - pq.v[3] * inv->v[11];
    inv->v[12] = -pq.v[0] * inv->v[14] - pq.v[2] * inv->v[15];
    inv->v[13] = -pq.v[1] * inv->v[14] - pq.v[3] * inv->v[15];

    inv->v[0] = p.v[0] - (pq.v[0] * inv->v[2] + pq.v[2] * inv->v[3]);
    inv->v[1] = p.v[1] - (pq.v[1] * inv->v[2] + pq.v[3] * inv->v[3]);
    inv->v[4] = p.v[2] - (pq.v[0] * inv->v[6] + pq.v[2] * inv->v[7]);
    inv->v[5] = p.v[3] - (pq.v[1] * inv->v[6] + pq.v[3] * inv->v[7]);
}

void
sweet_matrix_product2 (mat2 * p, mat2 * a, mat2 * b)
{
    float av0 = a->v[0];
    float av1 = a->v[1];
    float bv0 = b->v[0];
    float bv2 = b->v[2];
    p->v[0] = a->v[0] * b->v[0] + a->v[2] * b->v[1];
    p->v[1] = a->v[1] * bv0 + a->v[3] * b->v[1];
    p->v[2] = av0 * b->v[2] + a->v[2] * b->v[3];
    p->v[3] = av1 * bv2 + a->v[3] * b->v[3];
}

void
sweet_matrix_product3 (mat3 * p, mat3 * a, mat3 * b)
{
    float bv0 = b->v[0];
    float bv1 = b->v[1];
    float bv3 = b->v[3];
    float bv4 = b->v[4];
    float bv6 = b->v[6];
    float bv7 = b->v[7];
    float av0 = a->v[0];
    float av1 = a->v[1];
    float av2 = a->v[2];
    float av3 = a->v[3];
    float av4 = a->v[4];
    float av5 = a->v[5];

    p->v[0] = a->v[0] * b->v[0] + a->v[3] * b->v[1] + a->v[6] * b->v[2];
    p->v[1] = a->v[1] * bv0 + a->v[4] * b->v[1] + a->v[7] * b->v[2];
    p->v[2] = a->v[2] * bv0 + a->v[5] * bv1 + a->v[8] * b->v[2];

    p->v[3] = av0 * b->v[3] + a->v[3] * b->v[4] + a->v[6] * b->v[5];
    p->v[4] = av1 * bv3 + a->v[4] * b->v[4] + a->v[7] * b->v[5];
    p->v[5] = av2 * bv3 + a->v[5] * bv4 + a->v[8] * b->v[5];

    p->v[6] = av0 * b->v[6] + av3 * b->v[7] + a->v[6] * b->v[8];
    p->v[7] = av1 * bv6 + av4 * b->v[7] + a->v[7] * b->v[8];
    p->v[8] = av2 * bv6 + av5 * bv7 + a->v[8] * b->v[8];
}

void
sweet_matrix_product4 (mat4 * p, mat4 * a, mat4 * b)
{
    float bv0 = b->v[0];
    float bv1 = b->v[1];
    float bv2 = b->v[2];
    float bv4 = b->v[4];
    float bv5 = b->v[5];
    float bv6 = b->v[6];
    float bv8 = b->v[8];
    float bv9 = b->v[9];
    float bv10 = b->v[10];
    float bv12 = b->v[12];
    float bv13 = b->v[13];
    float bv14 = b->v[14];
    float av0 = a->v[0];
    float av1 = a->v[1];
    float av2 = a->v[2];
    float av3 = a->v[3];
    float av4 = a->v[4];
    float av5 = a->v[5];
    float av6 = a->v[6];
    float av7 = a->v[7];
    float av8 = a->v[8];
    float av9 = a->v[9];
    float av10 = a->v[10];
    float av11 = a->v[11];

    p->v[0] = a->v[0] * b->v[0] + a->v[4] * b->v[1] + a->v[8] * b->v[2] + a->v[12] * b->v[3];
    p->v[1] = a->v[1] * bv0 + a->v[5] * b->v[1] + a->v[9] * b->v[2] + a->v[13] * b->v[3];
    p->v[2] = a->v[2] * bv0 + a->v[6] * bv1 + a->v[10] * b->v[2] + a->v[14] * b->v[3];
    p->v[3] = a->v[3] * bv0 + a->v[7] * bv1 + a->v[11] * bv2 + a->v[15] * b->v[3];

    p->v[4] = av0 * b->v[4] + a->v[4] * b->v[5] + a->v[8] * b->v[6] + a->v[12] * b->v[7];
    p->v[5] = av1 * b->v[4] + a->v[5] * b->v[5] + a->v[9] * b->v[6] + a->v[13] * b->v[7];
    p->v[6] = av2 * bv4 + a->v[6] * bv5 + a->v[10] * b->v[6] + a->v[14] * b->v[7];
    p->v[7] = av3 * bv4 + a->v[7] * bv5 + a->v[11] * bv6 + a->v[15] * b->v[7];

    p->v[8] = av0 * b->v[8] + av4 * b->v[9] + a->v[8] * b->v[10] + a->v[12] * b->v[11];
    p->v[9] = av1 * bv8 + av5 * b->v[9] + a->v[9] * b->v[10] + a->v[13] * b->v[11];
    p->v[10] = av2 * bv8 + av6 * bv9 + a->v[10] * b->v[10] + a->v[14] * b->v[11];
    p->v[11] = av3 * bv8 + av7 * bv9 + a->v[11] * bv10 + a->v[15] * b->v[11];

    p->v[12] = av0 * b->v[12] + av4 * b->v[13] + av8 * b->v[14] + a->v[12] * b->v[15];
    p->v[13] = av1 * bv12 + av5 * b->v[13] + av9 * b->v[14] + a->v[13] * b->v[15];
    p->v[14] = av2 * bv12 + av6 * bv13 + av10 * b->v[14] + a->v[14] * b->v[15];
    p->v[15] = av3 * bv12 + av7 * bv13 + av11 * bv14 + a->v[15] * b->v[15];
}

void
sweet_matrix_sub3 (mat2 * sub, mat3 * m)
{
    sub->v[0] = m->v[0];
    sub->v[1] = m->v[1];
    sub->v[2] = m->v[4];
    sub->v[3] = m->v[5];
}

void
sweet_matrix_sub4 (mat3 * sub, mat4 * m)
{
    sub->v[0] = m->v[0];
    sub->v[1] = m->v[1];
    sub->v[2] = m->v[2];
    sub->v[3] = m->v[4];
    sub->v[4] = m->v[5];
    sub->v[5] = m->v[6];
    sub->v[6] = m->v[8];
    sub->v[7] = m->v[9];
    sub->v[8] = m->v[10];
}

vec2
sweet_matrix_column2 (mat2 * m, int idx)
{
    vec2 v;
    idx = idx * 2;
    v.x = m->v[idx];
    v.y = m->v[idx+1];

    return v;
}

vec3
sweet_matrix_column3 (mat3 * m, int idx)
{
    vec3 v;
    idx = idx * 3;
    v.x = m->v[idx];
    v.y = m->v[idx+1];
    v.z = m->v[idx+2];

    return v;
}

vec4
sweet_matrix_column4 (mat4 * m, int idx)
{
    vec4 v;
    idx = idx * 4;
    v.x = m->v[idx];
    v.y = m->v[idx+1];
    v.z = m->v[idx+2];
    v.w = m->v[idx+3];

    return v;
}

vec2
sweet_matrix_mult2 (mat2 * mat, vec2 vec)
{
    vec2 result;

    result.x = mat->v[0] * vec.x + mat->v[2] * vec.y;
    result.y = mat->v[1] * vec.x + mat->v[3] * vec.y;

    return result;
}

vec3
sweet_matrix_mult3 (mat3 * mat, vec3 vec)
{
    vec3 r;

    r.x = (mat->v[0] * vec.x) + (mat->v[3] * vec.y) + (mat->v[6] * vec.z);
    r.y = (mat->v[1] * vec.x) + (mat->v[4] * vec.y) + (mat->v[7] * vec.z);
    r.z = (mat->v[2] * vec.x) + (mat->v[5] * vec.y) + (mat->v[8] * vec.z);

    return r;
}

vec4
sweet_matrix_mult4 (mat4 * mat, vec4 vec)
{
    vec4 result;

    result.x = (mat->v[0] * vec.x) + (mat->v[4] * vec.y) + (mat->v[8] * vec.z) + (mat->v[12] * vec.w);
    result.y = (mat->v[1] * vec.x) + (mat->v[5] * vec.y) + (mat->v[9] * vec.z) + (mat->v[13] * vec.w);
    result.z = (mat->v[2] * vec.x) + (mat->v[6] * vec.y) + (mat->v[10] * vec.z) + (mat->v[14] * vec.w);
    result.w = (mat->v[3] * vec.x) + (mat->v[7] * vec.y) + (mat->v[11] * vec.z) + (mat->v[15] * vec.w);

    return result;
}

int
sweet_matrix_eigen_value3 (float * h1, float * h2, float * h3, mat3 * m)
{
    float a;
    float b;
    float c;
    int n;
    vec3 v;

    a = m->v[0] + m->v[4] + m->v[8];

    b = m->v[1] * m->v[3] + m->v[2] * m->v[6] + m->v[7] * m->v[5] -
        m->v[0] * m->v[4] - m->v[0] * m->v[8] - m->v[4] * m->v[8];

    c = m->v[0] * m->v[4] * m->v[8] - m->v[0] * m->v[7] * m->v[5] -
        m->v[1] * m->v[3] * m->v[8] + m->v[1] * m->v[6] * m->v[5] +
        m->v[2] * m->v[3] * m->v[7] - m->v[2] * m->v[6] * m->v[4];

    n = sweet_math_cubic_polynomial (&v, -1, a, b, c);
    *h1 = v.x;
    *h2 = v.y;
    *h3 = v.z;
    return n;
}

/**
|a b c x|
|d e f y|
|g h i z|
*/

/**/
static int find_pivot (float * max, mat3 * a, int k)
{
    int i_max;

    if (k == 0)
    {
        *max = fabs (a->v[k]); i_max = 0;
        if (fabs (a->v[k+1]) > *max) { *max = fabs (a->v[k+1]); i_max = 1;}
        if (fabs (a->v[k+2]) > *max) { *max = fabs (a->v[k+2]); i_max = 2;}
    }
    else if (k == 1)
    {
        k = 4;
        *max = fabs (a->v[k]); i_max = 1;
        if (fabs (a->v[k+1]) > *max) { *max = fabs (a->v[k+1]); i_max = 2;}
    }
    else
    {
        k = 8;
        *max = fabs (a->v[k]);
        i_max = 2;
    }
    return i_max;
}

static void swap_rows (mat3 * a, float * s, int l1, int l2)
{
    float tmp;
    if (l1 == l2) { return; }

    /* swap left part */
    tmp = s[l1];
    s[l1] = s[l2];
    s[l2] = tmp;

    /* swap first column */
    tmp = a->v[l1];
    a->v[l1] = a->v[l2];
    a->v[l2] = tmp;

    /* swap second column */
    l1 += 3;
    l2 += 3;
    tmp = a->v[l1];
    a->v[l1] = a->v[l2];
    a->v[l2] = tmp;

    /* swap third column */
    l1 += 3;
    l2 += 3;
    tmp = a->v[l1];
    a->v[l1] = a->v[l2];
    a->v[l2] = tmp;
}

static void normalize_row (mat3 * a, float * s, int l, int i)
{
    float d = a->v[l+i*3];
    a->v[l+i*3] = 1.0;

    s[l] = s[l] / d;
    if (i <= 1) { a->v[l+6] = a->v[l+6] / d; }
    if (i == 0) { a->v[l+3] = a->v[l+3] / d; }
}

static void substract_row (mat3 * a, float * s, int l1, int l2, int i)
{
    float r;
    if (i == 0)
    {
        r = - a->v[l2];
        a->v[l2] = 0;
        a->v[l2+3] = a->v[l2+3] + a->v[l1+3] * r;
        a->v[l2+6] = a->v[l2+6] + a->v[l1+6] * r;
    }
    else if (i == 1)
    {
        r = - a->v[l2+3];
        a->v[l2+3] = 0;
        a->v[l2+6] = a->v[l2+6] + a->v[l1+6] * r;
    }
    else if (i == 2)
    {
        r = - a->v[l2+6];
        a->v[l2+6] = 0;
    }

    s[l2] = s[l2] + s[l1] * r;

}

#define EPSILON 0.00005

int
sweet_matrix_gaussian_elimination3 (mat3 * a, vec3 * v)
{
    float s[3];
    float max;
    int i_max;

    s[0] = v->x;
    s[1] = v->y;
    s[2] = v->z;

    i_max = find_pivot (&max, a, 0);
    /* If matrix is singular return error */
    if (sweet_math_approx_equals (max, 0, EPSILON)) { return -1; }
    swap_rows (a, s, 0, i_max);
    normalize_row (a, s, 0, 0);
    substract_row (a, s, 0, 1, 0);
    substract_row (a, s, 0, 2, 0);

    i_max = find_pivot (&max, a, 1);
    /* If matrix is singular return error */
    if (sweet_math_approx_equals (max, 0, EPSILON)) { return -1; }
    swap_rows (a, s, 1, i_max);
    normalize_row (a, s, 1, 1);
    substract_row (a, s, 1, 2, 1);

    /* If matrix is singular return error */
    if (sweet_math_approx_equals (a->v[8], 0, EPSILON)) { return -1; }
    normalize_row (a, s, 2, 2);

    v->x = s[0];
    v->y = s[1];
    v->z = s[2];
    return 0;
}

int
sweet_matrix_solve3 (mat3 * m, vec3 * v)
{
    mat3 a = *m;
    if (sweet_matrix_gaussian_elimination3 (&a, v) < 0) { return -1; }

    v->y = v->y - v->z * a.v[7];
    v->x = v->x - v->z * a.v[6] - v->y * a.v[3];
    return 0;
}

static int is_diagonal (mat3 * m)
{
    if (!sweet_math_approx_equals (m->v[1], 0.0, EPSILON)) { return 0; }
    if (!sweet_math_approx_equals (m->v[2], 0.0, EPSILON)) { return 0; }
    if (!sweet_math_approx_equals (m->v[3], 0.0, EPSILON)) { return 0; }
    if (!sweet_math_approx_equals (m->v[5], 0.0, EPSILON)) { return 0; }
    if (!sweet_math_approx_equals (m->v[6], 0.0, EPSILON)) { return 0; }
    if (!sweet_math_approx_equals (m->v[7], 0.0, EPSILON)) { return 0; }
    return 1;
}

static int col_n_zero (mat3 * m, int i)
{
   i = i * 3;
   return (sweet_math_approx_equals (m->v[i], 0, EPSILON) &&
           sweet_math_approx_equals (m->v[i+1], 0, EPSILON) &&
           sweet_math_approx_equals (m->v[i+2], 0, EPSILON));
}

static int row_n_zero (mat3 * m, int i)
{
   return (sweet_math_approx_equals (m->v[i], 0, EPSILON) &&
           sweet_math_approx_equals (m->v[i+3], 0, EPSILON) &&
           sweet_math_approx_equals (m->v[i+6], 0, EPSILON));
}

static void get_eigen_vector (vec3 * v, mat3 a, float h)
{
    a.v[0] = a.v[0] - h;
    a.v[4] = a.v[4] - h;
    a.v[8] = a.v[8] - h;

    v->x = 0;
    v->y = 0;
    v->z = 0;

    sweet_matrix_gaussian_elimination3 (&a, v);

    if (col_n_zero (&a, 0))
        v->x = 1;
    else if (col_n_zero(&a, 1))
        v->y = 1; 
    else if (col_n_zero(&a, 2))
        v->z = 1;
    else
    {
        if (sweet_math_approx_equals (a.v[2], 0, EPSILON) &&
            sweet_math_approx_equals (a.v[5], 0, EPSILON))
        {
            if (sweet_math_approx_equals (a.v[8], 0, EPSILON))
            {
                v->z = 1;
                v->y = - a.v[7];
                v->x = (-v->y * a.v[3] - a.v[6]);
            }
            else
            {
                v->z = 0;
                v->y = 1;
                v->x = (-a.v[3]);
            }
        } 
        else if (row_n_zero (&a, 1))
        {
            v->y = 1;
            v->z = 0;
            v->x = (-v->y * a.v[3]);
        }
    }
    *v = sweet_vector_normalize3 (*v);
}

int
sweet_matrix_eigen_vector3 (vec3 * vectors, mat3 * m)
{
    /* (A - hI)V = 0 */
    mat3 a;
    int n;
    float h1, h2, h3;

    if (is_diagonal (m))
    {
        vectors[0] = sweet_vector_new3 (1,0,0);
        vectors[1] = sweet_vector_new3 (0,1,0);
        vectors[2] = sweet_vector_new3 (0,0,1);
        return 3;
    }

    n = sweet_matrix_eigen_value3 (&h1, &h2, &h3, m);

    /* vector associated with h1 */
    get_eigen_vector (&vectors[0], *m, h1);

    /* vector associated with h2 */
    get_eigen_vector (&vectors[1], *m, h2);

    /* vector associated with h3 */
    if (n > 2)
        get_eigen_vector (&vectors[2], *m, h3);
    else
        vectors[2] = sweet_vector_cross (vectors[0], vectors[1]);
    return n;
}