#ifndef _TPFVEC_C
#define _TPFVEC_C

#include <stdlib.h>
#include "vec.h"
#include "f1616.h"
#include "ang.h"

tpfVec *TpfVec_New()
{
	return (tpfVec *) malloc(sizeof(tpfVec));
}

void TpfVec_Delete(tpfVec *v)
{
	free(v);
}

void TpfVec_Add(tpfVec *a,tpfVec *b)
{
	a->x += b->x;
	a->y += b->y;
	a->z += b->z;
}

void TpfVec_Sub(tpfVec *a,tpfVec *b)
{
	a->x -= b->x;
	a->y -= b->y;
	a->z -= b->z;
}

void TpfVec_Mul(tpfVec *a,tpfVec *b)
{
	a->x = f1616_mul(a->x,b->x);
	a->y = f1616_mul(a->y,b->y);
	a->z = f1616_mul(a->z,b->z);
}

void TpfVec_Div(tpfVec *a,tpfVec *b)
{
	a->x = f1616_div(a->x,b->x);
	a->y = f1616_div(a->y,b->y);
	a->z = f1616_div(a->z,b->z);
}

void TpfVec_Mul2(tpfVec *a,f1616 b)
{
	a->x = f1616_mul(a->x,b);
	a->y = f1616_mul(a->y,b);
	a->z = f1616_mul(a->z,b);
}

void TpfVec_Div2(tpfVec *a,f1616 b)
{
	f1616_tridiv(&a->x,&a->y,&a->z,b);
}

void TpfVec_Div3(f1616 a,tpfVec *b)
{
	b->x = f1616_div(a,b->x);
	b->y = f1616_div(a,b->y);
	b->z = f1616_div(a,b->z);
}

f1616 TpfVec_Len(tpfVec *v)
{
	return f1616_sqrt(f1616_sqr(v->x)+f1616_sqr(v->y)+f1616_sqr(v->z));
}

void TpfVec_Normalize(tpfVec *v)
{
	f1616_tridiv(&v->x,&v->y,&v->z,TpfVec_Len(v));
}

f1616 TpfVec_DotProd(tpfVec *a,tpfVec *b)
{
	return f1616_mul(a->x,b->x) + f1616_mul(a->y,b->y) + f1616_mul(a->z,b->z);
}

void TpfVec_CrossProd(tpfVec *a,tpfVec *b,tpfVec *c)
{
	c->x = f1616_mul(a->y,b->z) - f1616_mul(a->z,b->y);
	c->y = f1616_mul(a->z,b->x) - f1616_mul(a->x,b->z);
	c->z = f1616_mul(a->x,b->y) - f1616_mul(a->y,b->x);
}

f1616 TpfVec_MoveToX(tpfVec *p,tpfVec *d,f1616 tx,f1616 recdx)
{
	f1616 n;
	n = f1616_mul(tx - p->x,recdx);
	p->x = tx;
	p->y += f1616_mul(n,d->y);
	p->z += f1616_mul(n,d->z);
	return n;
}

f1616 TpfVec_MoveToY(tpfVec *p,tpfVec *d,f1616 ty,f1616 recdy)
{
	f1616 n;
	n = f1616_mul(ty - p->y,recdy);
	p->y = ty;
	p->x += f1616_mul(n,d->x);
	p->z += f1616_mul(n,d->z);
	return n;
}

f1616 TpfVec_MoveToZ(tpfVec *p,tpfVec *d,f1616 tz,f1616 recdz)
{
	f1616 n;
	n = f1616_mul(tz - p->z,recdz);
	p->z = tz;
	p->y += f1616_mul(n,d->y);
	p->x += f1616_mul(n,d->x);
	return n;
}

void TpfVec_Rotate(tpfVec *v,tpfAng a)
{
	/* Simple rotation around z axis */
	f1616 nx,ca,sa;
	ca = TpfAng_cos(a);
	sa = TpfAng_sin(a);
	/* nx = cos*x-sin*y
	   ny = sin*x+cos*y */
	nx = f1616_mul(ca,v->x)-f1616_mul(sa,v->y);
	v->y = f1616_mul(sa,v->x)+f1616_mul(ca,v->y);
	v->x = nx;
}

#endif
