#ifndef VECTOR_H
#define VECTOR_H

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


#ifndef FORCE_INLINE
INLINE double dot_product(vector* a, vector* b);
INLINE double distance_between(point* a, point* b);
INLINE double vector_length(vector* a);
INLINE double angle_between(vector* a, vector* b);
INLINE vector* cross_product(vector* a, vector* b, vector* dest);
INLINE vector* reflect_vector(vector* dir, vector* normal, vector* r);
INLINE vector* refract_vector(vector* n, vector* d, double refractIndex, vector* t);
INLINE vector* normalize(vector* a);
INLINE vector* add_vectors(vector* dest, vector* v1, vector *v2);
INLINE vector* make_vector(point* to, point* from, vector* v);
INLINE point* make_point(double x, double y, double z, point* p);
INLINE vector* copy_vector(vector *source, vector *dest);
INLINE vector* multiply_vector(vector *dest, vector *v, double n);
INLINE vector* invert_vector(vector *v);
INLINE vector* sub_vectors(vector *dest, vector *minuend, vector *subtrahend);
INLINE vector* random_vector(vector *v);

#endif

#ifdef FORCE_INLINE

extern int DEBUG_LEVEL;


FORCE_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;
}

FORCE_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;
}

FORCE_INLINE point* make_point(double x, double y, double z, point* p)
{
	p->x = x;
	p->y = y;
	p->z = z;

	return p;
}

FORCE_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;
}

FORCE_INLINE vector* copy_vector(vector *source, vector *dest)
{
	dest->x = source->x;
	dest->y = source->y;
	dest->z = source->z;
	return dest;
}

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

FORCE_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;
}

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


FORCE_INLINE double dot_product(vector* a, vector* b)
{
	return a->x*b->x + a->y*b->y + a->z*b->z;
}


/*
//typedef int v4df __attribute__ (( vector_size(4*sizeof(double)) ));
//typedef int v4sf __attribute__ ((mode(V4SF)));
typedef int v4sf __attribute__ ((vector_size (128)));

typedef union 
{
  v4sf v;
  float d[4];
}  d4vector;

FORCE_INLINE double dot_product(vector* a, vector* b)
{
	//static vector result;
	static d4vector A;
	static d4vector B;
	static d4vector C;
	DEBUG_LEVEL = 11;
	
	A.d[0] = (float)a->x;
	A.d[1] = (float)a->y;
	A.d[2] = (float)a->z;
	A.d[3] = (float)0.0;
	
	B.d[0] = (float)b->x;
	B.d[1] = (float)b->y;
	B.d[2] = (float)b->z;
	B.d[3] = (float)0.0;
	
	C.v = A.v + B.v;

//	printf("a:\t%f\t%f\t%f\n", a->x, a->y, a->z);
//	printf("b:\t%f\t%f\t%f\n", b->x, b->y, b->z);
	printf("A:\t%f\t%f\t%f\n", A.d[0], A.d[1], A.d[2]);
	printf("B:\t%f\t%f\t%f\n", B.d[0], B.d[1], B.d[2]);	
	printf("C:\t%f\t%f\t%f\n", C.d[0], C.d[1], C.d[2]);
	
	printf("\n\n");


	return (double)C.d[0]+C.d[1]+C.d[2];
//	return a->x*b->x + a->y*b->y + a->z*b->z;
}*/


FORCE_INLINE double distance_between(point* a, point* b)
{
	return sqrt( pow(a->x - b->x, 2) + pow(a->y - b->y, 2) + 
						 pow(a->z - b->z, 2));
}

FORCE_INLINE double vector_length(vector* a)
{
	return sqrt(a->x*a->x + a->y*a->y + a->z*a->z);
}

FORCE_INLINE double angle_between(vector* a, vector* b) 
{
	return acos(dot_product(a,b) / vector_length(a) / vector_length(b));
}

FORCE_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;
}


FORCE_INLINE vector* reflect_vector(vector* incoming, vector* normal, vector* reflect)
{
	double dp;

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

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

FORCE_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 ref_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;
	ref_index = index_in/index_out;
	ref_index = index_out/index_in;

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

	if(ref_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 = ref_index * nDotD;
	second_part = sqrt(1 - pow(ref_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 = ref_index*(nDotD) - sqrt(1-indx_sq + indx_sq * (nDotD)*(nDotD));

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

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

	invert_vector(refract);

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

	return refract;
}


FORCE_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;
}

#endif


#endif /* VECTOR_H */

