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