/*
 *   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 <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <stdarg.h>
#include "sweet_math.h"
#include "sweet_types.h"


void
random_float_numbers(float * array, int n)
{
    int i;
    for (i = 0; i < n; i++)
    {
        array[i] = (float)rand() / (float)RAND_MAX;
    }
}

vec2
vec2_zero ()
{
    vec2 v;
    v.x = 0;
    v.y = 0;
    return v;
}

vec3
vec3_zero ()
{
    vec3 v;
    v.x = 0;
    v.y = 0;
    return v;
}

vec4
vec4_zero ()
{
    vec4 v;
    v.x = 0;
    v.y = 0;
    return v;
}

vec2
vec2_new_2f (float a, float b)
{
    vec2 v;
    v.x = a;
    v.y = b;
    return v;
}

vec3
vec3_new_3f (float a, float b, float c)
{
    vec3 v;
    v.x = a;
    v.y = b;
    v.z = c;
    return v;
}

vec4
vec4_new_4f (float a, float b, float c, float d)
{
    vec4 v;
    v.x = a;
    v.y = b;
    v.z = c;
    v.w = d;

    return v;
}

vec2
vec2_new_v3(vec3 w)
{
    vec2 v;
    v.x = w.x;
    v.y = w.y;
    return v;
}

vec3
vec3_new_v2_1f(vec2 w, float z)
{
    vec3 v;
    v.x = w.x;
    v.y = w.y;
    v.z = z;
    return v;
}

vec3
vec3_new_v4(vec4 w)
{
    vec3 v;
    v.x = w.x;
    v.y = w.y;
    v.z = w.z;
    return v;
}

vec4
vec4_new_v2_2f(vec2 u, float z, float w)
{
    vec4 v;
    v.x = u.x;
    v.y = u.y;
    v.z = z;
    v.w = w;
    return v;
}

vec4
vec4_new_v3_1f(vec3 u, float w)
{
    vec4 v;
    v.x = u.x;
    v.y = u.y;
    v.z = u.z;
    v.w = w;
    return v;
}

vec4
vec4_new_v4 (vec4 v)
{
    return v;
}


float
vec2_norm (vec2 v)
{
    return sqrt (v.x * v.x + v.y * v.y);
}

float
vec3_norm (vec3 v)
{
    return sqrt (v.x * v.x + v.y * v.y + v.z * v.z);
}

float
vec4_norm (vec4 v)
{
    return sqrt (v.x * v.x + v.y * v.y + v.z * v.z + v.w * v.w);
}

float
vec2_square_norm (vec2 v)
{
    return (v.x * v.x + v.y * v.y);
}

float
vec3_square_norm (vec3 v)
{
    return (v.x * v.x + v.y * v.y + v.z * v.z);
}

float
vec4_square_norm (vec4 v)
{
    return (v.x * v.x + v.y * v.y + v.z * v.z + v.w * v.w);
}

float
vec2_dist (vec2 a, vec2 b)
{
    float dx = a.x - b.x;
    float dy = a.y - b.y;
    return sqrt (dx * dx + dy * dy);
}

float
vec3_dist (vec3 a, vec3 b)
{
    float dx = a.x - b.x;
    float dy = a.y - b.y;
    float dz = a.z - b.z;
    return sqrt (dx * dx + dy * dy + dz * dz);
}

float
vec4_dist (vec4 a, vec4 b)
{
    float dx = a.x - b.x;
    float dy = a.y - b.y;
    float dz = a.z - b.z;
    return sqrt (dx * dx + dy * dy + dz * dz);
}

float
vec2_square_dist (vec2 a, vec2 b)
{
    float dx = a.x - b.x;
    float dy = a.y - b.y;
    return (dx * dx + dy * dy);
}

float
vec3_square_dist (vec3 a, vec3 b)
{
    float dx = a.x - b.x;
    float dy = a.y - b.y;
    float dz = a.z - b.z;
    return (dx * dx + dy * dy + dz * dz);
}

float
vec4_square_dist (vec4 a, vec4 b)
{
    float dx = a.x - b.x;
    float dy = a.y - b.y;
    float dz = a.z - b.z;
    return (dx * dx + dy * dy + dz * dz);
}

vec2 vec2_interpolate (vec2 u, vec2 v, float t)
{
    float _t = 1.0 - t;
    u.x = u.x * _t + v.x * t;
    u.y = u.y * _t + v.y * t;
    return u;
}

vec3 vec3_interpolate (vec3 u, vec3 v, float t)
{
    float _t = 1.0 - t;
    u.x = u.x * _t + v.x * t;
    u.y = u.y * _t + v.y * t;
    u.z = u.z * _t + v.z * t;
    return u;
}

vec4 vec4_interpolate (vec4 u, vec4 v, float t)
{
    float _t = 1.0 - t;
    u.x = u.x * _t + v.x * t;
    u.y = u.y * _t + v.y * t;
    u.z = u.z * _t + v.z * t;
    u.w = u.w * _t + v.w * t;
    return u;
}

float
vec2_dot (vec2 a, vec2 b)
{
    return (a.x * b.x) + (a.y * b.y);
}

float
vec3_dot (vec3 a, vec3 b)
{
    return (a.x * b.x) + (a.y * b.y) + (a.z * b.z);
}

float
vec4_dot (vec4 a, vec4 b)
{
    return (a.x * b.x) + (a.y * b.y) + (a.z * b.z) + (a.w * b.w);
}

vec3
cross (vec3 v1, vec3 v2)
{
    vec3 n;
    n.x = (v1.y * v2.z) - (v1.z * v2.y);
    n.y = -(v1.x * v2.z) + (v1.z * v2.x);
    n.z = (v1.x * v2.y) - (v1.y * v2.x);

    return n;
}
    
float
triple_product (vec3 v1, vec3 v2, vec3 v3)
{
    v2 = cross (v2, v3);

    return vec3_dot (v1, v2);
}

vec2
vec2_normalize (vec2 v)
{
    vec2 nv;

    float inv_size = 1.0 / sqrt ((v.x * v.x) + (v.y * v.y));

    nv.x = v.x * inv_size;
    nv.y = v.y * inv_size;

    return nv;
}

vec3
vec3_normalize (vec3 v)
{
    vec3 nv;

    float inv_size = 1.0 / sqrt ((v.x * v.x) + (v.y * v.y) + (v.z * v.z));

    nv.x = v.x * inv_size;
    nv.y = v.y * inv_size;
    nv.z = v.z * inv_size;

    return nv;
}

vec4
vec4_normalize (vec4 v)
{
    vec4 nv;

    float inv_size = 1.0 / sqrt ((v.x * v.x) + (v.y * v.y) + (v.z * v.z) + (v.w * v.w));

    nv.x = v.x * inv_size;
    nv.y = v.y * inv_size;
    nv.z = v.z * inv_size;
    nv.w = v.w * inv_size;

    return nv;
}

vec2i
vec2_int (vec2 v)
{
    vec2i u;
    u.x = (int)(v.x > 0.0 ? v.x : v.x - 1);
    u.y = (int)(v.y > 0.0 ? v.y : v.y - 1);
    return u;
}

vec3i
vec3_int (vec3 v)
{
    vec3i u;
    u.x = (int)(v.x > 0.0 ? v.x : v.x - 1);
    u.y = (int)(v.y > 0.0 ? v.y : v.y - 1);
    u.z = (int)(v.z > 0.0 ? v.z : v.z - 1);
    return u;
}

vec4i
vec4_int (vec4 v)
{ 
    vec4i u;
    u.x = (int)(v.x > 0.0 ? v.x : v.x - 1);
    u.y = (int)(v.y > 0.0 ? v.y : v.y - 1);
    u.z = (int)(v.z > 0.0 ? v.z : v.z - 1);
    u.w = (int)(v.w > 0.0 ? v.w : v.w - 1);
    return u;
}

vec2i
vec2i_new (int x, int y)
{
    vec2i v;
    v.x = x;
    v.y = y;
    return v;
}

vec3i
vec3i_new (int x, int y, int z)
{
    vec3i v;
    v.x = x;
    v.y = y;
    v.z = z;
    return v;
}

vec4i
vec4i_new (int x, int y, int z, int w)
{
    vec4i v;
    v.x = x;
    v.y = y;
    v.z = z;
    v.w = w;
    return v;
}

vec2i
vec2i_add (vec2i v1, vec2i v2)
{
    v1.x += v2.x;
    v1.y += v2.y;

    return v1;
}

vec3i
vec3i_add (vec3i v1, vec3i v2)
{
    v1.x += v2.x;
    v1.y += v2.y;
    v1.z += v2.z;

    return v1;
}

vec4i
vec4i_add (vec4i v1, vec4i v2)
{
    v1.x += v2.x;
    v1.y += v2.y;
    v1.z += v2.z;
    v1.w += v2.w;

    return v1;
}

vec2
vec2_scale (vec2 v, float s)
{
    v.x *= s;
    v.y *= s;

    return v;
}

vec3
vec3_scale (vec3 v, float s)
{
    v.x *= s;
    v.y *= s;
    v.z *= s;

    return v;
}

vec4
vec4_scale (vec4 v, float s)
{
    v.x *= s;
    v.y *= s;
    v.z *= s;
    v.w *= s;

    return v;
}

vec2
vec2_rescale (vec2 v, float size)
{
    v = vec2_normalize (v);
    v = vec2_scale (v, size);

    return v;
}

vec3
vec3_rescale (vec3 v, float size)
{
    v = vec3_normalize (v);
    v = vec3_scale (v, size);

    return v;
}

vec4
vec4_rescale (vec4 v, float size)
{
    v = vec4_normalize (v);
    v = vec4_scale (v, size);

    return v;
}

vec2
vec2_inverse (vec2 v)
{
    v.x = - v.x;
    v.y = - v.y;
    return v;
}

vec3
vec3_inverse (vec3 v)
{
    v.x = - v.x;
    v.y = - v.y;
    v.z = - v.z;
    return v;
}

vec4
vec4_inverse (vec4 v)
{
    v.x = - v.x;
    v.y = - v.y;
    v.z = - v.z;
    v.w = - v.w;
    return v;
}

vec2
vec2_add (vec2 v1, vec2 v2)
{
    v1.x += v2.x;
    v1.y += v2.y;

    return v1;
}

vec3
vec3_add (vec3 v1, vec3 v2)
{
    v1.x += v2.x;
    v1.y += v2.y;
    v1.z += v2.z;

    return v1;
}

vec4
vec4_add (vec4 v1, vec4 v2)
{
    v1.x += v2.x;
    v1.y += v2.y;
    v1.z += v2.z;
    v1.w += v2.w;

    return v1;
}

vec2
vec2_middle (vec2 vector1, vec2 vector2)
{
    vec2 v;
    v.x = (vector1.x + vector2.x) * 0.5;
    v.y = (vector1.y + vector2.y) * 0.5;
    return v;
}
    
vec3
vec3_middle (vec3 vector1, vec3 vector2)
{
    vec3 v;
    v.x = (vector1.x + vector2.x) * 0.5;
    v.y = (vector1.y + vector2.y) * 0.5;
    v.z = (vector1.z + vector2.z) * 0.5;
    return v;
}
    
vec4
vec4_middle (vec4 vector1, vec4 vector2)
{
    vec4 v;
    v.x = (vector1.x + vector2.x) * 0.5;
    v.y = (vector1.y + vector2.y) * 0.5;
    v.z = (vector1.z + vector2.z) * 0.5;
    v.w = (vector1.w + vector2.w) * 0.5;
    return v;
}

vec2
vec2_sub (vec2 v1, vec2 v2)
{
    v1.x -= v2.x;
    v1.y -= v2.y;

    return v1;
}

vec3
vec3_sub (vec3 v1, vec3 v2)
{
    v1.x -= v2.x;
    v1.y -= v2.y;
    v1.z -= v2.z;

    return v1;
}

vec4
vec4_sub (vec4 v1, vec4 v2)
{
    v1.x -= v2.x;
    v1.y -= v2.y;
    v1.z -= v2.z;
    v1.w -= v2.w;

    return v1;
}

float
vec2_angle (vec2 a, vec2 b)
{
    vec2 na = vec2_normalize (a);
    vec2 nb = vec2_normalize (b);

    float dot = vec2_dot (na, nb);

    return acos (dot);
}

float
vec3_angle (vec3 a, vec3 b)
{
    vec3 na = vec3_normalize (a);
    vec3 nb = vec3_normalize (b);

    float dot = vec3_dot (na, nb);

    return acos (dot);
}

vec2 
vec2_positive_random ()
{
    vec2 v;
    v.x = (float)rand() / (float)RAND_MAX;
    v.y = (float)rand() / (float)RAND_MAX;
    return vec2_normalize (v);
}

vec3 
vec3_positive_random ()
{
    vec3 v;
    v.x = (float)rand() / (float)RAND_MAX;
    v.y = (float)rand() / (float)RAND_MAX;
    v.z = (float)rand() / (float)RAND_MAX;

    return vec3_normalize (v);
}

vec4 
vec4_positive_random ()
{
    vec4 v;
    v.x = (float)rand() / (float)RAND_MAX;
    v.y = (float)rand() / (float)RAND_MAX;
    v.z = (float)rand() / (float)RAND_MAX;
    v.w = (float)rand() / (float)RAND_MAX;

    return vec4_normalize (v);
}

vec2 
vec2_random ()
{
    vec2 v;
    v.x = (float)rand() / (float)RAND_MAX;
    v.y = (float)rand() / (float)RAND_MAX;
    v.x = 2.0 * (v.x - 0.5);
    v.y = 2.0 * (v.y - 0.5);
    return vec2_normalize (v);
}

vec3 
vec3_random ()
{
    vec3 v;
    v.x = (float)rand() / (float)RAND_MAX;
    v.y = (float)rand() / (float)RAND_MAX;
    v.z = (float)rand() / (float)RAND_MAX;

    v.x = 2.0 * (v.x - 0.5);
    v.y = 2.0 * (v.y - 0.5);
    v.z = 2.0 * (v.z - 0.5);
    return vec3_normalize (v);
}

vec4 
vec4_random ()
{
    vec4 v;
    v.x = (float)rand() / (float)RAND_MAX;
    v.y = (float)rand() / (float)RAND_MAX;
    v.z = (float)rand() / (float)RAND_MAX;
    v.w = (float)rand() / (float)RAND_MAX;

    v.x = 2.0 * (v.x - 0.5);
    v.y = 2.0 * (v.y - 0.5);
    v.z = 2.0 * (v.z - 0.5);
    v.w = 2.0 * (v.w - 0.5);
    return vec4_normalize (v);
}

vec3
vec3_random_ortho(vec3 v)
{
    vec3 u = vec3_random ();
    v = cross(u, v);
    return vec3_normalize(v);
}

vec2
vec2_product (vec2 v1, vec2 v2)
{
    v1.x *= v2.x;
    v1.y *= v2.y;

    return v1;
}

vec3
vec3_product (vec3 v1, vec3 v2)
{
    v1.x *= v2.x;
    v1.y *= v2.y;
    v1.z *= v2.z;

    return v1;
}

vec4
vec4_product (vec4 v1, vec4 v2)
{
    v1.x *= v2.x;
    v1.y *= v2.y;
    v1.z *= v2.z;
    v1.w *= v2.w;

    return v1;
}

quat
quat_new (float w, float x, float y, float z)
{
    quat q;
    q.w = w;
    q.x = x;
    q.y = y;
    q.z = z;

    return q;
}

quat
quat_rotation (float angle, float x, float y, float z)
{
    quat q;
    float s;

    s = sin (0.5 * angle);
    q.w = cos (0.5 * angle);
    q.x = x * s;
    q.y = y * s;
    q.z = z * s;

    return q;
}

quat
quat_conjugate (quat q)
{
    quat conjugate;

    conjugate.x = -q.x;
    conjugate.y = -q.y;
    conjugate.z = -q.z;
    conjugate.w = q.w;

    return conjugate;
}

quat
quat_add (quat q1, quat q2)
{
    quat sum;

    sum.w = q1.w + q2.w;
    sum.x = q1.x + q2.x;
    sum.y = q1.y + q2.y;
    sum.z = q1.z + q2.z;

    return sum;
}

quat
quat_product (quat q1, quat q2)
{
    quat prod;

    prod.w = (q1.w * q2.w) - (q1.x * q2.x) - (q1.y * q2.y) - (q1.z * q2.z);
    prod.x = (q1.w * q2.x) + (q1.x * q2.w) + (q1.y * q2.z) - (q1.z * q2.y);
    prod.y = (q1.w * q2.y) - (q1.x * q2.z) + (q1.y * q2.w) + (q1.z * q2.x);
    prod.z = (q1.w * q2.z) + (q1.x * q2.y) - (q1.y * q2.x) + (q1.z * q2.w);

    return prod;
}

float
quat_norm (quat q)
{
    return sqrt ((q.w * q.w) + (q.x * q.x) + (q.y * q.y) + (q.z * q.z));
}

quat
quat_inverse (quat q)
{
    quat q_;
    float m = 1.0 / vec4_dot(q, q);

    q_.w =  q.w * m;
    q_.x = -q.x * m;
    q_.y = -q.y * m;
    q_.z = -q.z * m;

    return q_;
}

quat
quat_normalize (quat q)
{
    return vec4_normalize (q);
}


quat
quat_scale (quat q, float s)
{
    return vec4_scale (q, s);
}

quat
quat_interpolate (quat q1, quat q2, float t)
{
    return quat_add (quat_scale (q1, 1.0 - t), quat_scale (q2, t));
}

int
nearest_integer (int number, int size, ...)
{
    int i;
    int nearest;
    va_list ap;

    va_start (ap, size);

    /* nearest = va_arg (ap, int); */

    for (i = 0; i < size; i++)
    {
        int arg;
        arg = va_arg (ap, int);

        if (abs (number - arg) < abs (number - nearest))
        {
            nearest = arg;
        }
    }

    va_end (ap);

    return nearest;
}

int
quadratic_polynomial (vec2 * r, float a, float b, float c)
{
    float delta;
    float a2;

    a2 = a * 2;
    delta = b * b - 2*a2 * c;

    if (delta > 0)
    {
        float d = sqrt (delta);
        r->x = (d - b)/a2;
        r->y = (-d - b)/a2;
        return 2;
    }
    else if (float_approx_zero (delta, EPSILON))
    {
        r->x = -b/a2;
        r->y = r->x;
        return 1;
    }
    return 0;
}


#define FRAC_1_ON_3   0.3333333333333333333333333333333333333333333333333333333333333333333
#define FRAC_2_ON_3   0.6666666666666666666666666666666666666666666666666666666666666666667
#define FRAC_1_ON_6   0.1666666666666666666666666666666666666666666666666666666666666666667
#define FRAC_1_ON_9   0.1111111111111111111111111111111111111111111111111111111111111111111
#define FRAC_2_ON_27  0.0740740740740740740740740740740740740740740740740740740740740740741
#define FRAC_1_ON_27  0.0370370370370370370370370370370370370370370370370370370370370370370

int
cubic_polynomial (vec3 * roots, float t, float a, float b, float c)
{
    double aa;
    double a_over_3;
    double p;
    double q;
    double ppp;
    double D;
    double r;
    double s;

    a /= t;
    b /= t;
    c /= t;

    aa = a * a;

    p = -FRAC_1_ON_9 * aa + FRAC_1_ON_3 * b;
    q = FRAC_1_ON_27 * aa * a - FRAC_1_ON_6 * a * b + 0.5 * c;

    ppp = p * p * p;
    D = -(ppp + q * q);

    s = sqrt (-D);
    r = cbrt (-q + s);
    s = cbrt (-q - s);

    a_over_3 = a/3;

    if (D < 0)
    {
        roots->x = (r + s) - a_over_3;
        roots->y = -r - a_over_3;
        roots->z = roots->y;
        return 1;
    }
    else if (float_approx_zero (D, EPSILON))
    {
        roots->x = r * 2 - a_over_3;
        roots->y = -r - a_over_3;
        roots->z = roots->y;
        return 2;
    }
    else
    {
        float theta;
        float m2;

        m2 = 2 * sqrt (-p);

        theta = FRAC_1_ON_3 * acos (-q / sqrt(-ppp));

        roots->x = m2 * cos (theta) - a_over_3;
        roots->y = m2 * cos (theta + 2 * PI_OVER_3) - a_over_3;
        roots->z = m2 * cos (theta - 2 * PI_OVER_3) - a_over_3;
        return 3;
    }

    return 0;
}