#include <math.h>
#include <stdlib.h>
#include "../common/objects.h"
#include "vector.h"
#include "../common/structs.h"
#include "../common/debug.h"


#ifndef FORCE_INLINE_VECTORS

/* returns the dot product of 2 vectors */
INLINE double dot_product(vector* a, vector* b)
{
	return a->x*b->x + a->y*b->y + a->z*b->z;
}


/*
typedef int v4sf __attribute__ (( vector_size(4*sizeof(float)) ));

INLINE double dot_product(vector* a, vector* b)
{
	v4sf A;
	v4sf B;
	
	A[0] = a->x;
	A[1] = a->y;
	A[2] = a->z;

	B[0] = b->x;
	B[1] = b->y;
	B[2] = b->z;
	
	//double x = _mm_mul_ps( a->x, b->y );
	return a->x*b->x + a->y*b->y + a->z*b->z;
}*/



/* returns the distance between two points */
INLINE double distance_between(point* a, point* b)
{
/*
	static double distanceBetweenDiffX;
	static double distanceBetweenDiffY;
	static double distanceBetweenDiffZ;

	double dist;
	distanceBetweenDiffX = abs(a->x-b->x);
	distanceBetweenDiffY = abs(a->y-b->y);
	distanceBetweenDiffZ = abs(a->z-b->z);
	return distanceBetweenDiffX + distanceBetweenDiffY + distanceBetweenDiffZ;

	return sqrt(diffX*diffX + diffY*diffY + diffZ*diffZ);
*/
	return sqrt( pow(a->x - b->x, 2) + pow(a->y - b->y, 2) + 
						 pow(a->z - b->z, 2));
}

/* gets the length of a vector */
INLINE double vector_length(vector* a)
{
	return sqrt(a->x*a->x + a->y*a->y + a->z*a->z);
}

/* gets the angle between two vectors */
INLINE double angle_between(vector* a, vector* b) 
{
	return acos(dot_product(a,b) / vector_length(a) / vector_length(b));
}

/* calcs the cross product */
INLINE vector* cross_product(vector* a, vector* b, vector* dest)
{
	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;
}

/* gets the reflect_vectorion vector, given a normal and incoming vector */
//input must be normalized!!
INLINE vector* reflect_vector(vector* incoming, vector* normal, vector* reflect)
{
	double dp;

	//projection of n onto l
	dp = 2*dot_product(normal, incoming);

	if(dp == 0)
		dp = 1;

	reflect->x = incoming->x - dp*normal->x;
	reflect->y = incoming->y - dp*normal->y;
	reflect->z = incoming->z - dp*normal->z;
	
	return reflect;
}

/* gets the refraction vector, given the incoming, normal, and index */
INLINE vector* refract_vector(vector* norm, vector* incoming, double index_out, 
		vector* refract)
{
	vector temp;
	double index_in = 1;  // refractive index of a vacuum
	double nDotD;
	double index; 
	double indx_sq;
	double index_calc;

	double first_part = 0.0;
	double second_part = 0.0;
	double scalar_calc = 0.0;

	indx_sq = index_out * index_out;
	index = index_in/index_out;
	index = index_out/index_in;

	temp.x = -incoming->x;
	temp.y = -incoming->y;
	temp.z = -incoming->z;
	nDotD = dot_product(norm, &temp);

	if(index * sin(angle_between(norm, &temp)) > 1)  //total reflect_vectorion
	{
		refract->x = 0;
		refract->y = 0;
		refract->z = 0;
	print_vector(CRAZY, "REFRACT: ", refract);
		return refract;
	}

	first_part = index * nDotD;
	second_part = sqrt(1 - pow(index, 2) * (1 - pow(nDotD, 2)));
	scalar_calc = first_part + second_part;

	if(nDotD > 0)
		scalar_calc = first_part - second_part;

	//printd(100, "f: %f, s:%f, c:%f\n", first_part, second_part, scalar_calc);

	

	index_calc = index*(nDotD) - sqrt(1-indx_sq + indx_sq * (nDotD)*(nDotD));

	refract->x = index_calc * norm->x - index*( -incoming->x );
	refract->y = index_calc * norm->y - index*( -incoming->y );
	refract->z = index_calc * norm->z - index*( -incoming->z );

	refract->x = scalar_calc * norm->x - index * incoming->x;
	refract->y = scalar_calc * norm->y - index * incoming->y;
	refract->z = scalar_calc * norm->z - index * incoming->z;

	invert_vector(refract);

	//print_vector(100, "REFRACT: ", refract);

	return refract;
}


/* normalize a vector */
INLINE vector* normalize(vector* a)
{
	double normalizeLength;
	normalizeLength = vector_length(a);

	if(normalizeLength <= EPSILON)
		return a;

	a->x = a->x/normalizeLength;
	a->y = a->y/normalizeLength;
	a->z = a->z/normalizeLength;

	return a;
}


/* add vectors */
INLINE vector* add_vectors(vector* dest, vector* v1, vector *v2)
{
	dest->x = v1->x + v2->x;
	dest->y = v1->y + v2->y;
	dest->z = v1->z + v2->z;

	return dest;
}

/* create a vector given 2 points */
INLINE vector* make_vector(point* to, point* from, vector* v)
{
	v->x = to->x - from->x;
	v->y = to->y - from->y;
	v->z = to->z - from->z;

	return v;
}

/* return a point given 3 coords */
INLINE point* make_point(double x, double y, double z, point* p)
{
	p->x = x;
	p->y = y;
	p->z = z;

	return p;
}

/* scalar multiply a vector */
INLINE vector* multiply_vector(vector *dest, vector *v, double n)
{
	dest->x = v->x * n;
	dest->y = v->y * n;
	dest->z = v->z * n;

	return v;
}

/* copy a vector */
INLINE vector* copy_vector(vector *source, vector *dest)
{
	dest->x = source->x;
	dest->y = source->y;
	dest->z = source->z;
	return dest;
}

INLINE vector* invert_vector(vector *v)
{
	v->x = -v->x;
	v->y = -v->y;
	v->z = -v->z;
	return v;
}

INLINE vector* sub_vectors(vector *dest, vector *minuend, vector *subtrahend)
{
	dest->x = minuend->x - subtrahend->x;
	dest->y = minuend->y - subtrahend->y;
	dest->z = minuend->z - subtrahend->z;

	return dest;
}

INLINE vector* random_vector(vector *v)
{
	v->x = (double)rand()/RAND_MAX*2-1;
	v->y = (double)rand()/RAND_MAX*2-1;
	v->z = (double)rand()/RAND_MAX*2-1;
	return v;
}

#endif
