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

// arithmetic operations
INLINE_MODE vector3* vector3_add(vector3 *dest, const vector3 *v1, const vector3 *v2)
{
	dest->x = v1->x + v2->x;
	dest->y = v1->y + v2->y;
	dest->z = v1->z + v2->z;
	return dest;
}

INLINE_MODE vector3* vector3_subtract(vector3 *dest, const vector3 *minuend, const vector3 *subtrahend)
{
	dest->x = minuend->x - subtrahend->x;
	dest->y = minuend->y - subtrahend->y;
	dest->z = minuend->z - subtrahend->z;
	return dest;
}

INLINE_MODE vector3* vector3_multiply(vector3 *dest, const vector3 *v, const v3float n)
{
	dest->x = v->x * n;
	dest->y = v->y * n;
	dest->z = v->z * n;
	return dest;
}

INLINE_MODE vector3* vector3_divide(vector3 *dest, const vector3 *v,const  v3float n)
{
	dest->x = v->x / n;
	dest->y = v->y / n;
	dest->z = v->z / n;
	return dest;
}

INLINE_MODE vector3 *vector3_scalar_add(vector3 *dest, const vector3 *v, const v3float s)
{
	dest->x = v->x + s;
	dest->y = v->y + s;
	dest->z = v->z + s;
	return dest;
}

INLINE_MODE vector3 *vector3_scalar_sub(vector3 *dest, const vector3 *v, const v3float s)
{
	dest->x = v->x - s;
	dest->y = v->y - s;
	dest->z = v->z - s;
	return dest;
}

INLINE_MODE v3float vector3_dot(const vector3 *a, const vector3 *b)
{
	return a->x*b->x + a->y*b->y + a->z*b->z;
}

INLINE_MODE vector3* vector3_cross(vector3 *dest, const vector3 *a, const vector3 *b)
{
	dest->x = a->y*b->z - a->z*b->y;
	dest->y = a->z*b->x - a->x*b->z;
	dest->z = a->x*b->y - a->y*b->x;
	
	return dest;
}

// vector3 unit operations
INLINE_MODE v3float vector3_length(const vector3 *a)
{
	return sqrt(a->x*a->x + a->y*a->y + a->z*a->z);
}

INLINE_MODE vector3* vector3_normalize(vector3 *a)
{
	v3float normalizeLength;
	normalizeLength = vector3_length(a);
	
	if(normalizeLength <= EPSILON)
	{
		printf("cannot normalize degenerate vector3\n");
		return a;
	}
	
	vector3_divide(a, a, normalizeLength);
	return a;
}

INLINE_MODE vector3* vector3_invert(vector3 *dest, const vector3 *v)
{
	dest->x = -v->x;
	dest->y = -v->y;
	dest->z = -v->z;
	return dest;
}

INLINE_MODE void vector3_print(const vector3 *v)
{
	printf("%.2f %.2f %.2f\n", v->x, v->y, v->z);
}

// vector3 creation
INLINE_MODE vector3* vector3_copy(vector3 *dest, const vector3 *source)
{
	dest->x = source->x;
	dest->y = source->y;
	dest->z = source->z;
	return dest;
}

INLINE_MODE vector3* vector3_random(vector3 *v)
{
	do {
		v->x = (double)rand()/RAND_MAX*2-1;
		v->y = (double)rand()/RAND_MAX*2-1;
		v->z = (double)rand()/RAND_MAX*2-1;
	} while (v->x*v->x + v->y*v->y + v->z*v->z > 1.0);
	return v;
}

INLINE_MODE vector3* vector3_make3f(vector3 *v, const v3float x, const v3float y, const v3float z)
{
	v->x = x;
	v->y = y;
	v->z = z;
	return v;
}

INLINE_MODE vector3* vector3_make2v(vector3 *v, const vector3 *to, const vector3 *from)
{
	v->x = to->x - from->x;
	v->y = to->y - from->y;
	v->z = to->z - from->z;
	return v;
}

// vector3 combination operations
INLINE_MODE v3float vector3_distance(const vector3 *a, const vector3 *b)
{
	return sqrt( pow(a->x - b->x, 2) + pow(a->y - b->y, 2) + 
				pow(a->z - b->z, 2));
}

INLINE_MODE double vector3_distancesq(vector3 *a, vector3 *b)
{
	return pow(a->x - b->x, 2) + pow(a->y - b->y, 2) + 
				pow(a->z - b->z, 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(vector3 *dest, const vector3 *incoming, const vector3 *normal)
{
	v3float dp;
	dp = 2*vector3_dot(normal, incoming);

	dest->x = incoming->x - dp*normal->x;
	dest->y = incoming->y - dp*normal->y;
	dest->z = incoming->z - dp*normal->z;
	
	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;
	
	vector3_invert(&a, &one);
	vector3_subtract(&a, &one, &a);
	vector3_add(&a, &a, &one);
	vector3_print(&a);
	
	vector3_multiply(&a, &one, 0.5);
	vector3_divide(&a, &a, 2);
	vector3_print(&a);
	
	vector3_reflect(&a, &one, &y);
	vector3_print(&a);
	
	vector3_scalar_sub(&a, &zero, -0.5);
	vector3_scalar_add(&a, &a, 0.5);
	vector3_print(&a);
	
	vector3_cross(&a, &one, &y);
	vector3_print(&a);
	
	srand(3);
	vector3_random(&a);
	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));
	
	vector3_copy(&a, &one);
	printf("%.2f %.2f\n", 
		   vector3_length(&one), vector3_length(vector3_normalize(&a)) );
}

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