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

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

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

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

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

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

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

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

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

INLINE_MODE vector3& vector3::operator+=(const v3float f)
{
        c[0] += f;
        c[1] += f;
        c[2] += f;
        return *this;
}

INLINE_MODE vector3& vector3::operator-=(const v3float f)
{
        c[0] -= f;
        c[1] -= f;
        c[2] -= f;
        return *this;
}

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

INLINE_MODE vector3& vector3::operator/=(const v3float f)
{
        c[0] /= f;
        c[1] /= f;
        c[2] /= f;
        return *this;
}

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

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

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

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

// vector3 unit operations
INLINE_MODE v3float vector3::length() const
{
        return sqrt(c[0]*c[0] + c[1]*c[1] + c[2]*c[2]);
}

INLINE_MODE vector3& vector3::normalize()
{
        v3float normalizeLength;
        normalizeLength = this->length();

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

        *this /= normalizeLength;
        return *this;
}

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

// vector3 creation
INLINE_MODE vector3& vector3::randomize()
{
        do {
                c[0] = (v3float)rand()/RAND_MAX*2-1;
                c[1] = (v3float)rand()/RAND_MAX*2-1;
                c[2] = (v3float)rand()/RAND_MAX*2-1;
        } while (c[0]*c[0] + c[1]*c[1] + c[2]*c[2] > 1.0);
        return *this;
}

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

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

INLINE_MODE v3float vector3::angle(const vector3 &a)
{
        return acos(this->dot(a) / this->length() / a.length());
}

INLINE_MODE vector3 vector3::reflect(const vector3 &normal)
{
        v3float dp;
        vector3 dest;

        dp = 2*this->dot(normal);
        dest = *this - (dp*normal);
        return dest;
}

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

        a = -one;
        a = one - a;
        a = a + one;
        a.print();

        a = one * 0.5;
        a = a / 2;
        a.print();

        a = one.reflect(y);
        a.print();

        a = zero - (-0.5);
        a = a + 0.5;
        a.print();

        a = one.cross(y);
        a.print();

        srand(3);
        a.randomize();
        a.print();

        printf("%.2f %.2f\n",
                   half.dot(y), half.angle(y));

        printf("%.2f %.2f\n",
                   one.distance(y), one.distancesq(y));

        a = one;
        printf("%.2f %.2f\n",
                   one.length(), a.normalize().length() );
}

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