#include <../source/TAL/objbase.h>

vector_p	V_GLOBAL
vecAlloc(u32_t size)
{
	vector_t *ptr;
	V_ASSERT(size);
	ptr = (vector_t*)memAlloc(sizeof(vector_t)+size*sizeof(vec_t));
	if(!ptr)
		return NULL;
	ptr->size=size;
	ptr->elements=(vec_t*)(ptr+1);
	return ptr;
}

vector_p	V_GLOBAL
vecDuplicate(vector_t *v)
{
	vector_t *v2;
	V_ASSERT(v);
	v2 = vecAlloc(v->size);
	if(v2)
		vecCopy(v2,v);
	return v2;
}

/*
 * Same as above, but new vector may have different size
 */
vector_p	V_GLOBAL
vecDuplicateEx(vector_t *src,u32_t newsize)
{
	u32_t to_copy;
	vector_t *dst;
	V_ASSERT(src && newsize);
	dst = vecAlloc(newsize);
	if(!dst)
		return NULL;
	to_copy = (newsize < src->size) ? newsize : src->size;
	memCopy32(dst->elements,src->elements,to_copy * sizeof(vec_t) >> 2);
	return dst;
}

V_LOCAL void
vec_copy_elements_pwr2(vec_p dst,vec_p src,u32_t dest_size,
	s32_t dest_idx,u32_t src_left,u32_t src_right)
{
	u32_t sc, dc;
	u32_t dest_mask = dest_size - 1;
	dc = dest_idx;
	for(sc = src_left;sc <= src_right;sc++,dc++)
		dst[dc & dest_mask] = src[sc];
}

V_LOCAL void
vec_copy_elements_generic(vec_p dst,vec_p src,u32_t dest_size,
	s32_t dest_idx,u32_t src_left,u32_t src_right)
{
	u32_t sc, dc;
	if(dest_idx < 0)
		dest_idx += ((-dest_idx % dest_size) + 1) * dest_size;
	dc = dest_idx;
	for(sc = src_left;sc <= src_right;sc++,dc++)
		dst[dc % dest_size] = src[sc];
}

void	V_GLOBAL
vecCopyElements(vector_p dest,vector_p source,s32_t dest_idx,
	u32_t src_left,u32_t src_right)
{
	CALLER_NAME(vecCopyElements)
	if(!dest || !source || !dest->size || !source->size)
	{
		ERROR_GENERIC(error_incorrect_parameters);
		return;
	}
	if(src_right >= source->size)
		src_right = source->size - 1;
	if(src_left > src_right)
		return;
	if(IS_POWER_TWO(dest->size))
		vec_copy_elements_pwr2(dest->elements,source->elements,dest->size,
			dest_idx,src_left,src_right);
	else
		vec_copy_elements_generic(dest->elements,source->elements,dest->size,
			dest_idx,src_left,src_right);
}

void	V_GLOBAL
vecAdd(vector_t *dest,vector_t *v1,vector_t *v2)
{
	u32_t i;
	V_ASSERT(dest && v1 && v2);
	V_ASSERT(dest->size==v1->size && v1->size==v2->size);
	for(i=0;i<v1->size;i++)
		dest->elements[i]=v1->elements[i]+v2->elements[i];
}

void	V_GLOBAL
vecSub(vector_t *dest,vector_t *v1,vector_t *v2)
{
	u32_t i;
	V_ASSERT(dest && v1 && v2);
	V_ASSERT(dest->size==v1->size && v1->size==v2->size);
	for(i=0;i<v1->size;i++)
		dest->elements[i]=v1->elements[i]-v2->elements[i];
}

void	V_GLOBAL
vecScale(vector_t *dest,vector_t *v,vec_t x)
{
	u32_t i;
	V_ASSERT(dest && v);
	V_ASSERT(dest->size==v->size);
	for(i=0;i<v->size;i++)
		dest->elements[i]=v->elements[i]*x;
}

void	V_GLOBAL
vecNegate(vector_t *v)
{
	u32_t i;
	V_ASSERT(v);
	for(i=0;i<v->size;i++)
		v->elements[i]=-v->elements[i];
}

vec_t	V_GLOBAL
vecDot(vector_t *v1,vector_t *v2)
{
	u32_t i;
	vec_t dot;
	V_ASSERT(v1 && v2);
	V_ASSERT(v1->size==v2->size);
	dot=0;
	for(i=0;i<v1->size;i++)
		dot+=v1->elements[i]*v2->elements[i];
	return dot;
}

vec_t	V_GLOBAL
vecAbs(vector_t *v)
{
	u32_t i;
	vec_t dot;
	V_ASSERT(v);
	dot=0;
	for(i=0;i<v->size;i++)
		dot+=v->elements[i]*v->elements[i];
	return (vec_t)sqrt(dot);
}

vboolean	V_GLOBAL
vecCompare(vector_t *v1,vector_t *v2)
{
	u32_t i;
	V_ASSERT(v1 && v2);
	V_ASSERT(v1->size == v2->size);
	for(i=0;i<v1->size;i++)
		if(!IsZero((vec_t)(v1->elements[i]-v2->elements[i])))
			return false;
	return true;
}

u32_t		V_GLOBAL
vecMin(vector_t *v)
{
	u32_t i, min;
	V_ASSERT(v);
	min=0;
	for(i=0;i<v->size;i++)
		if(v->elements[i]<v->elements[min])
			min=i;
	return min;
}

u32_t		V_GLOBAL
vecMax(vector_t *v)
{
	u32_t i, max;
	V_ASSERT(v);
	max=0;
	for(i=0;i<v->size;i++)
		if(v->elements[i]>v->elements[max]) max=i;
	return max;
}

#ifdef	VECTOR_NO_MACRO

u32_t		V_GLOBAL
vecCount(vector_t *v)
{
	V_ASSERT(v);
	return v->size;
}

void	V_GLOBAL
vecCopy(vector_t *dst,vector_t *src)
{
	V_ASSERT(dst && src);
	V_ASSERT(dst->size==src->size);
	memCopy32(dst->elements,src->elements,dst->size*sizeof(vec_t)>>2);
}

void	V_GLOBAL
vecZero(vector_t *v)
{
	V_ASSERT(v);
	memZero32(v->elements,v->size*sizeof(vec_t)>>2);
}

void	V_GLOBAL
vecUnit(vector_t *v)
{
	u32_t i;
	vec_t ra;
	V_ASSERT(v);
	ra = vecAbs(v);
	V_ASSERT(ra > FLOAT_ZERO_EPSILON);
	ra=1/ra;
	for(i=0;i<v->size;i++)
		v->elements[i]*=ra;
}

#endif	/* VECTOR_NO_MACRO */
