#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();
}