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

#define EPSILON 0.0000000001

int
sweet_math_nearest (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
sweet_math_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 (sweet_math_approx_equals (delta, 0, 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.6666666666666666666666666666666666666666666666666666666666666666666
#define FRAC_1_ON_6   0.1666666666666666666666666666666666666666666666666666666666666666666
#define FRAC_1_ON_9   0.1111111111111111111111111111111111111111111111111111111111111111111
#define FRAC_2_ON_27  0.0740740740740740740740740740740740740740740740740740740740740740740
#define FRAC_1_ON_27  0.0370370370370370370370370370370370370370370370370370370370370370370

int
sweet_math_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;
        return 1;
    }
    else if (sweet_math_approx_equals (D, 0, 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 * SWEET_PI_OVER_3) - a_over_3;
        roots->z = m2 * cos (theta - 2 * SWEET_PI_OVER_3) - a_over_3;
        return 3;
    }

    return 0;
}

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

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

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

    return v;
}

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

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

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

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

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

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

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

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

float
sweet_vector_square_norm2h (vec3 v)
{
    v.x = v.x / v.z;
    v.y = v.y / v.z;
    return (v.x * v.x + v.y * v.y);
}

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

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

float
sweet_vector_dist3 (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
sweet_vector_dist4 (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
sweet_vector_square_dist2 (vec2 a, vec2 b)
{
    float dx = a.x - b.x;
    float dy = a.y - b.y;
    return (dx * dx + dy * dy);
}

float
sweet_vector_square_dist3 (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
sweet_vector_square_dist4 (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);
}

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

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

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

float
sweet_vector_dot2h (vec3 a, vec3 b)
{
    a.x = a.x / a.z;
    a.y = a.y / a.z;

    b.x = b.x / b.z;
    b.y = b.y / b.z;
    return (a.x * b.x) + (a.y * b.y);
}

float
sweet_vector_dot3h (vec4 a, vec4 b)
{
    a.x = a.x / a.w;
    a.y = a.y / a.w;
    a.z = a.z / a.w;

    b.x = b.x / b.w;
    b.y = b.y / b.w;
    b.z = b.z / b.w;
    return (a.x * b.x) + (a.y * b.y) + (a.z * b.z);
}

vec3
sweet_vector_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;
}
    
vec4
sweet_vector_crossh (vec4 v1, vec4 v2)
{
    vec4 n;
    v1.x = v1.x / v1.w;
    v1.y = v1.y / v1.w;
    v1.z = v1.z / v1.w;

    v2.x = v2.x / v2.w;
    v2.y = v2.y / v2.w;
    v2.z = v2.z / v2.w;

    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);
    n.w = 1.0;

    return n;
}

float
sweet_vector_triple_product (vec3 v1, vec3 v2, vec3 v3)
{
    v2 = sweet_vector_cross (v2, v3);

    return sweet_vector_dot3 (v1, v2);
}

float
sweet_vector_triple_producth (vec4 v1, vec4 v2, vec4 v3)
{
    v2 = sweet_vector_crossh (v2, v3);

    return sweet_vector_dot3h (v1, v2);
}

vec2
sweet_vector_normalize2 (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
sweet_vector_normalize3 (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
sweet_vector_normalize4 (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;
}


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

    return v;
}

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

    return v;
}

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

    return v;
}

vec2
sweet_vector_rescale2 (vec2 v, float size)
{
    v = sweet_vector_normalize2 (v);
    v = sweet_vector_scale2 (v, size);

    return v;
}

vec3
sweet_vector_rescale3 (vec3 v, float size)
{
    v = sweet_vector_normalize3 (v);
    v = sweet_vector_scale3 (v, size);

    return v;
}

vec4
sweet_vector_rescale4 (vec4 v, float size)
{
    v = sweet_vector_normalize4 (v);
    v = sweet_vector_scale4 (v, size);

    return v;
}

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

    return v1;
}

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

    return v1;
}

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

    return v1;
}

vec2
sweet_vector_middle2 (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
sweet_vector_middle3 (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
sweet_vector_middle4 (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
sweet_vector_sub2 (vec2 v1, vec2 v2)
{
    v1.x -= v2.x;
    v1.y -= v2.y;

    return v1;
}

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

    return v1;
}

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

    return v1;
}

float
sweet_vector_angle2 (vec2 a, vec2 b)
{
    vec2 na = sweet_vector_normalize2 (a);
    vec2 nb = sweet_vector_normalize2 (b);

    float dot = sweet_vector_dot2 (na, nb);

    return acos (dot);
}

float
sweet_vector_angle3 (vec3 a, vec3 b)
{
    vec3 na = sweet_vector_normalize3 (a);
    vec3 nb = sweet_vector_normalize3 (b);

    float dot = sweet_vector_dot3 (na, nb);

    return acos (dot);
}

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

    return v1;
}

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

    return v1;
}

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

    return v1;
}

quaternion
sweet_quaternion_new (float w, float x, float y, float z)
{
    quaternion q;
    q.w = w;
    q.x = x;
    q.y = y;
    q.z = z;

    return q;
}

quaternion
sweet_quaternion_rotation (float angle, float x, float y, float z)
{
    quaternion 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;
}

quaternion
sweet_quaternion_conjugate (quaternion q)
{
    quaternion conjugate;

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

    return conjugate;
}

quaternion
sweet_quaternion_add (quaternion q1, quaternion q2)
{
    quaternion 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;
}

quaternion
sweet_quaternion_product (quaternion q1, quaternion q2)
{
    quaternion 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
sweet_quaternion_norm (quaternion q)
{
    return sqrt ((q.w * q.w) + (q.x * q.x) + (q.y * q.y) + (q.z * q.z));
}