#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include "vector3_struct.h"

// operator overload
INLINE_MODE vector3 operator+(const vector3 &v1, const vector3 &v2)
{
        vector3 t;
        t.c[0] = v1.c[0] + v2.c[0];
        t.c[1] = v1.c[1] + v2.c[1];
        t.c[2] = v1.c[2] + v2.c[2];
        return t;
}

INLINE_MODE vector3 operator-(const vector3 &v1, const vector3 &v2)
{
        vector3 t;
        t.c[0] = v1.c[0] - v2.c[0];
        t.c[1] = v1.c[1] - v2.c[1];
        t.c[2] = v1.c[2] - v2.c[2];
        return t;
}

INLINE_MODE vector3 operator+(const vector3 &v1, const v3float f)
{
        vector3 t;
        t.c[0] = v1.c[0] + f;
        t.c[1] = v1.c[1] + f;
        t.c[2] = v1.c[2] + f;
        return t;
}

INLINE_MODE vector3 operator-(const vector3 &v1, const v3float f)
{
        vector3 t;
        t.c[0] = v1.c[0] - f;
        t.c[1] = v1.c[1] - f;
        t.c[2] = v1.c[2] - f;
        return t;
}

INLINE_MODE vector3 operator*(const v3float f, const vector3 &v1)
{
        vector3 t;
        t.c[0] = v1.c[0] * f;
        t.c[1] = v1.c[1] * f;
        t.c[2] = v1.c[2] * f;
        return t;
}

INLINE_MODE vector3 operator*(const vector3 &v1, const v3float f)
{
        vector3 t;
        t.c[0] = v1.c[0] * f;
        t.c[1] = v1.c[1] * f;
        t.c[2] = v1.c[2] * f;
        return t;
}

INLINE_MODE vector3 operator/(const vector3 &v1, const v3float f)
{
        vector3 t;
        t.c[0] = v1.c[0] / f;
        t.c[1] = v1.c[1] / f;
        t.c[2] = v1.c[2] / f;
        return t;
}

INLINE_MODE vector3 operator+=(vector3 &v1, const vector3 &v2)
{
        v1.c[0] += v2.c[0];
        v1.c[1] += v2.c[1];
        v1.c[2] += v2.c[2];
        return v1;
}

INLINE_MODE vector3 operator-=(vector3 &v1, const vector3 &v2)
{
        v1.c[0] -= v2.c[0];
        v1.c[1] -= v2.c[1];
        v1.c[2] -= v2.c[2];
        return v1;
}

INLINE_MODE vector3 operator+=(vector3 &v1, const v3float f)
{
        v1.c[0] += f;
        v1.c[1] += f;
        v1.c[2] += f;
        return v1;
}

INLINE_MODE vector3 operator-=(vector3 &v1, const v3float f)
{
        v1.c[0] -= f;
        v1.c[1] -= f;
        v1.c[2] -= f;
        return v1;
}

INLINE_MODE vector3 operator*=(vector3 &v1, const v3float f)
{
        v1.c[0] *= f;
        v1.c[1] *= f;
        v1.c[2] *= f;
        return v1;
}

INLINE_MODE vector3 operator/=(vector3 &v1, const v3float f)
{
        v1.c[0] /= f;
        v1.c[1] /= f;
        v1.c[2] /= f;
        return v1;
}

INLINE_MODE vector3 operator-(const vector3 &v1)
{
        vector3 t;
        t.c[0] = -v1.c[0];
        t.c[1] = -v1.c[1];
        t.c[2] = -v1.c[2];
        return t;
}

// arithmetic operations
INLINE_MODE v3float vector3_dot(const vector3 &a, const vector3 &b)
{
        return a.c[0]*b.c[0] + a.c[1]*b.c[1] + a.c[2]*b.c[2];
}

INLINE_MODE vector3 vector3_cross(const vector3 &a, const vector3 &b)
{
        vector3 dest;
        dest.c[0] = a.c[1]*b.c[2] - a.c[2]*b.c[1];
        dest.c[1] = a.c[2]*b.c[0] - a.c[0]*b.c[2];
        dest.c[2] = a.c[0]*b.c[1] - a.c[1]*b.c[0];
        return dest;
}

// vector3 unit operations
INLINE_MODE v3float vector3_length(const vector3 &a)
{
        return sqrt(a.c[0]*a.c[0] + a.c[1]*a.c[1] + a.c[2]*a.c[2]);
}

INLINE_MODE vector3 vector3_normalize(vector3 &a)
{
        v3float normalizeLength;
        normalizeLength = vector3_length(a);

        if(normalizeLength <= EPSILON)
        {
                printf("cannot normalize degenerate vector3\n");
                return a;
        }

        a = a / normalizeLength;
        return a;
}

INLINE_MODE vector3 vector3_invert(const vector3 &v)
{
        vector3 dest;
        dest.c[0] = -v.c[0];
        dest.c[1] = -v.c[1];
        dest.c[2] = -v.c[2];
        return dest;
}

INLINE_MODE void vector3_print(const vector3 &v)
{
        printf("%.2f %.2f %.2f\n", v.c[0], v.c[1], v.c[2]);
}

// vector3 creation
INLINE_MODE vector3 vector3_copy(const vector3 &source)
{
        vector3 dest;
        dest.c[0] = source.c[0];
        dest.c[1] = source.c[1];
        dest.c[2] = source.c[2];
        return dest;
}

INLINE_MODE vector3 vector3_random()
{
        vector3 v;
        do {
                v.c[0] = (v3float)rand()/RAND_MAX*2-1;
                v.c[1] = (v3float)rand()/RAND_MAX*2-1;
                v.c[2] = (v3float)rand()/RAND_MAX*2-1;
        } while (v.c[0]*v.c[0] + v.c[1]*v.c[1] + v.c[2]*v.c[2] > 1.0);
        return v;
}

INLINE_MODE vector3 vector3_make3f(const v3float x, const v3float y, const v3float z)
{
        vector3 v;
        v.c[0] = x;
        v.c[1] = y;
        v.c[2] = z;
        return v;
}

INLINE_MODE vector3 vector3_make2v(const vector3 &to, const vector3 &from)
{
        vector3 v;
        v.c[0] = to.c[0] - from.c[0];
        v.c[1] = to.c[1] - from.c[1];
        v.c[2] = to.c[2] - from.c[2];
        return v;
}

// vector3 combination operations
INLINE_MODE v3float vector3_distance(const vector3& a, const vector3 &b)
{
        return sqrt( pow(a.c[0] - b.c[0], 2) + pow(a.c[1] - b.c[1], 2) +
                                pow(a.c[2] - b.c[2], 2));
}

INLINE_MODE v3float vector3_distancesq(vector3 &a, vector3 &b)
{
        return pow(a.c[0] - b.c[0], 2) + pow(a.c[1] - b.c[1], 2) +
                                pow(a.c[2] - b.c[2], 2);
}

INLINE_MODE v3float vector3_angle(const vector3 &a, const vector3 &b)
{
        return acos(vector3_dot(a,b) / vector3_length(a) / vector3_length(b));
}

INLINE_MODE vector3 vector3_reflect(const vector3 &incoming, const vector3 &normal)
{
        vector3 dest;
        v3float dp;
        dp = 2*vector3_dot(normal, incoming);

        dest.c[0] = incoming.c[0] - dp*normal.c[0];
        dest.c[1] = incoming.c[1] - dp*normal.c[1];
        dest.c[2] = incoming.c[2] - dp*normal.c[2];

        return dest;
}

void vector3_test()
{
        vector3 zero = {0,0,0};
        vector3 one = {1,1,1};
        vector3 y = {0,1,0};
        vector3 half = {0.5,0.5,0.5};
        vector3 a;

        a = vector3_invert(one);
        a = one - a;
        a = a + one;
        vector3_print(a);

        a = one * 0.5;
        a = a / 2;
        vector3_print(a);

        a = vector3_reflect(one, y);
        vector3_print(a);

        a = zero - (-0.5);
        a = a + 0.5;
        vector3_print(a);

        a = vector3_cross(one, y);
        vector3_print(a);

        srand(3);
        a = vector3_random();
        vector3_print(a);

        printf("%.2f %.2f\n",
                   vector3_dot(half, y), vector3_angle(half, y));

        printf("%.2f %.2f\n",
                   vector3_distance(one, y), vector3_distancesq(one, y));

        a = vector3_copy(one);
        printf("%.2f %.2f\n",
                   vector3_length(one), vector3_length(vector3_normalize(a)) );
}

int main(int argc, char **argv)
{
        vector3_test();
}