/*
 * LINKER_OPTIONS -lm
 */
#include <math.h>

#define Vlibmath_IMPORT
#include "Vlibmath.h"


void VSetPoint(VPoint *p, double x, double y, double z)
{
	p->x = x;
	p->y = y;
	p->z = z;
}


void
VAdd(VPoint *a, VPoint *b, VPoint *sum)
{
	sum->x = a->x + b->x;
	sum->y = a->y + b->y;
	sum->z = a->z + b->z;
}


double VMagnitude(VPoint *p)
{
	return sqrt( p->x*p->x + p->y*p->y + p->z*p->z );
}


double VMagnitude2(VPoint *p)
{
	return p->x*p->x + p->y*p->y + p->z*p->z;
}


void
VSub(VPoint *a, VPoint *b, VPoint *dif)
{
	dif->x = a->x - b->x;
	dif->y = a->y - b->y;
	dif->z = a->z - b->z;
}


double
VDotProd(a, b)
VPoint   *a, *b;
{

	return a->x * b->x + a->y * b->y + a->z * b->z;
}


void
VCrossProd(a, b, r)
VPoint   *a, *b, *r;
{
	int overlap;
	VPoint q, *p;

	if( a == r || b == r ){
		overlap = 1;
		p = &q;
	} else {
		overlap = 0;
		p = r;
	}

	p->x = a->y * b->z - a->z * b->y;
	p->y = a->z * b->x - a->x * b->z;
	p->z = a->x * b->y - a->y * b->x;

	if( overlap )
		*r = *p;
}



static VMatrix ident = { {
	{1.0, 0.0, 0.0, 0.0},
	{0.0, 1.0, 0.0, 0.0},
	{0.0, 0.0, 1.0, 0.0},
	{0.0, 0.0, 0.0, 1.0}
} };


void
VIdentMatrix(VMatrix * Mtx)
{
	*Mtx = ident;
}


void VMatrixTranspose(VMatrix *ori, VMatrix *trans)
{
	int i, j;
	VMatrix *r, m;

	if( ori == trans )
		r = &m;
	else
		r = trans;

	for (i = 0; i < 4; ++i)
		for (j = 0; j < 4; ++j)
			r->m[i][j] = ori->m[j][i];
	
	if( ori == trans )
		*trans = m;
}


void
VMatrixMult(VMatrix *Mt1, VMatrix *Mt2, VMatrix *R)
{
	register int I, J, K;
	register double x;
	int overlap;
	VMatrix *r, M;

	if( Mt1 == R || Mt2 == R ){
		overlap = 1;
		r = &M;
	} else {
		overlap = 0;
		r = R;
	}

	for (I = 0; I < 4; ++I)
		for (J = 0; J < 4; ++J) {
			x = 0.0;
			for (K = 0; K < 4; ++K)
				x += Mt1->m[K][J] * Mt2->m[I][K];
			r->m[I][J] = x;
		}
	
	if( overlap )
		*R = M;
}


#define ZEROFOURTH

void
VMatrixMultByRank(VMatrix * Mt1, VMatrix * Mt2, VMatrix * R, int rank)
{
	int I, J, K;
	double x;
	int overlap;
	VMatrix *r, M;

	if( Mt1 == R || Mt2 == R ){
		overlap = 1;
		r = &M;
	} else {
		overlap = 0;
		r = R;
	}


	for (I = 0; I < rank; ++I)
		for (J = 0; J < rank; ++J) {
			x = 0.0;
			for (K = 0; K < rank; ++K)
				x += Mt1->m[K][J] * Mt2->m[I][K];
			r->m[I][J] = x;
		}

#ifdef ZEROFOURTH
	r->m[0][3] = r->m[1][3] = r->m[2][3] = 0.0;
	r->m[3][0] = r->m[3][1] = r->m[3][2] = 0.0;
	r->m[3][3] = 1.0;
#endif

	if( overlap )
		*R = M;
}


void VRotate(VMatrix * m, int operation, double angle)
{

	VMatrix   r;
	double co, si;

	VIdentMatrix(&r);
	co = cos(angle);
	si = sin(angle);

	switch (operation) {
	case XRotation:
		r.m[1][1] = r.m[2][2] = co;
		r.m[2][1] = si;
		r.m[1][2] = -si;
		break;
	case YRotation:
		r.m[0][0] = r.m[2][2] = co;
		r.m[0][2] = si;
		r.m[2][0] = -si;
		break;
	case ZRotation:
		r.m[0][0] = r.m[1][1] = co;
		r.m[1][0] = si;
		r.m[0][1] = -si;
		break;
	}

	VMatrixMult(m, &r, m);
}


void VRotateAroundAxis(VMatrix * m, VPoint * axis, double angle)
{
	VPoint u = *axis;
	double k = 1 / VMagnitude(&u);
	u.x *= k;
	u.y *= k;
	u.z *= k;
	double co = cos(angle);   double co1 = 1-co;
	double si = sin(angle);
	VMatrix r;
	VIdentMatrix(&r);
	// FIXME: several redundant calculations -- compiler smart enough?
	r.m[0][0] = co + u.x*u.x*co1;
	r.m[0][1] = u.x*u.y*co1-u.z*si;
	r.m[0][2] = u.x*u.z*co1+u.y*si;
	r.m[1][0] = u.x*u.y*co1+u.z*si;
	r.m[1][1] = co+u.y*u.y*co1;
	r.m[1][2] = u.y*u.z*co1-u.x*si;
	r.m[2][0] = u.x*u.z*co1-u.y*si;
	r.m[2][1] = u.y*u.z*co1+u.x*si;
	r.m[2][2] = co+u.z*u.z*co1;
	VMatrixMult(m, &r, m);
}


void
VScaleMatrix(VMatrix *m, double xscale, double yscale, double zscale)
{
	VMatrix s;

	VIdentMatrix(&s);
	s.m[0][0] = xscale;
	s.m[1][1] = yscale;
	s.m[2][2] = zscale;
	VMatrixMult(m, &s, m);
}


void
VTranslatePoint(VMatrix * m, VPoint t)
{

	m->m[0][3] += t.x;
	m->m[1][3] += t.y;
	m->m[2][3] += t.z;
}


void
VTranslate(VMatrix * m, double x, double y, double z)
{

	m->m[0][3] += x;
	m->m[1][3] += y;
	m->m[2][3] += z;
}


void VTransform(VPoint *p, VMatrix *m, VPoint *newp)
{
	VPoint q;

	q.x = p->x * m->m[0][0] + p->y * m->m[0][1]
		+ p->z * m->m[0][2] + m->m[0][3];

	q.y = p->x * m->m[1][0] + p->y * m->m[1][1]
		+ p->z * m->m[1][2] + m->m[1][3];

	q.z = p->x * m->m[2][0] + p->y * m->m[2][1]
		+ p->z * m->m[2][2] + m->m[2][3];
	
	*newp = q;
}


void VReverseTransform(VPoint *p, VMatrix *m, VPoint *newp)
{
	VPoint q;

	q.x = p->x - m->m[0][3];
	q.y = p->y - m->m[1][3];
	q.z = p->z - m->m[2][3];

	newp->x = q.x * m->m[0][0] + q.y * m->m[1][0] + q.z * m->m[2][0];

	newp->y = q.x * m->m[0][1] + q.y * m->m[1][1] + q.z * m->m[2][1];

	newp->z = q.x * m->m[0][2] + q.y * m->m[1][2] + q.z * m->m[2][2];
}


void VTransform_(VPoint *p, VMatrix *m, VPoint *newp)
{
	VPoint q;

	q.x = p->x * m->m[0][0] + p->y * m->m[0][1] + p->z * m->m[0][2];

	q.y = p->x * m->m[1][0] + p->y * m->m[1][1] + p->z * m->m[1][2];

	q.z = p->x * m->m[2][0] + p->y * m->m[2][1] + p->z * m->m[2][2];
	
	*newp = q;
}


void VReverseTransform_(VPoint *p, VMatrix *m, VPoint *newp)
{
	VPoint q;

	q.x = p->x * m->m[0][0] + p->y * m->m[1][0] + p->z * m->m[2][0];

	q.y = p->x * m->m[0][1] + p->y * m->m[1][1] + p->z * m->m[2][1];

	q.z = p->x * m->m[0][2] + p->y * m->m[1][2] + p->z * m->m[2][2];
	
	*newp = q;
}


#define EPSILON	1e-6


void VEulerToMatrix(double phi, double theta, double psi, VMatrix *m)
{
	double sinPhi, cosPhi, sinTheta, cosTheta, sinPsi, cosPsi;

	sinPhi = sin(phi);
	cosPhi = cos(phi);
	sinTheta = sin(theta);
	cosTheta = cos(theta);
	sinPsi = sin(psi);
	cosPsi = cos(psi);

	m->m[0][0] = cosTheta * cosPsi;
	m->m[0][1] = sinPhi * sinTheta * cosPsi - cosPhi * sinPsi;
	m->m[0][2] = cosPhi * sinTheta * cosPsi + sinPhi * sinPsi;
	m->m[1][0] = cosTheta * sinPsi;
	m->m[1][1] = sinPhi * sinTheta * sinPsi + cosPhi * cosPsi;
	m->m[1][2] = cosPhi * sinTheta * sinPsi - sinPhi * cosPsi;
	m->m[2][0] = -sinTheta;
	m->m[2][1] = sinPhi * cosTheta;
	m->m[2][2] = cosPhi * cosTheta;
	m->m[0][3] = m->m[1][3] = m->m[2][3] = 0.0;
	m->m[3][0] = m->m[3][1] = m->m[3][2] = 0.0;
	m->m[3][3] = 1.0;
}


void VMatrixToEuler(VMatrix *m, double *phi, double *theta, double *psi)
{
	double    sin_theta;

	sin_theta = -m->m[2][0];

/*
	if (fabs(sin_theta) > 1.0 - EPSILON) {
		/ * we have the nose pointing very close to straight up or straight down,
		   set roll to zero and compute the resulting heading * /

		*psi = atan2(-m->m[0][1], m->m[1][1]);

		if (sin_theta > 0.0)
			*theta = M_PI / 2.0;
		else
			*theta = -M_PI / 2.0;

		*phi = 0.0;
	
	} else {
*/
		*psi = atan2(m->m[1][0], m->m[0][0]);

		*theta = asin(sin_theta);

		*phi = atan2(m->m[2][1], m->m[2][2]);
/*
	}
*/
	
	/* Normalize ranges: */
	if (*psi < 0.0)
		*psi += 2.0 * M_PI;
}


/*
 *  Quaternion code based on code from "Advanced Animation and Rendering
 *  Techniques: Theory and Practice" by Alan Watt and Mark Watt.
 */

void
VQuaternionToMatrix(VQuaternion * q, VMatrix * m)
{
	double    s, xs, ys, zs, wx, wy, wz, xx, xy, xz, yy, yz, zz;

	s = 2.0 / (q->v.x * q->v.x + q->v.y * q->v.y +
			   q->v.z * q->v.z + q->s * q->s);

	xs = q->v.x * s;
	ys = q->v.y * s;
	zs = q->v.z * s;
	wx = q->s * xs;
	wy = q->s * ys;
	wz = q->s * zs;
	xx = q->v.x * xs;
	xy = q->v.x * ys;
	xz = q->v.x * zs;
	yy = q->v.y * ys;
	yz = q->v.y * zs;
	zz = q->v.z * zs;

	VIdentMatrix(m);

	m->m[0][0] = 1.0 - (yy + zz);
	m->m[0][1] = xy + wz;
	m->m[0][2] = xz - wy;

	m->m[1][0] = xy - wz;
	m->m[1][1] = 1.0 - (xx + zz);
	m->m[1][2] = yz + wx;

	m->m[2][0] = xz + wy;
	m->m[2][1] = yz - wx;
	m->m[2][2] = 1.0 - (xx + yy);
}


void
VMatrixToQuaternion(VMatrix * m, VQuaternion * q)
{
	double    tr, s, *q1 = (double *) &q->v;
	int       i, j, k;

	tr = m->m[0][0] + m->m[1][1] + m->m[2][2];
	if (tr > 0.0) {
		s = sqrt(tr + 1.0);
		q->s = s * 0.5;
		s = 0.5 / s;
		q->v.x = (m->m[1][2] - m->m[2][1]) * s;
		q->v.y = (m->m[2][0] - m->m[0][2]) * s;
		q->v.z = (m->m[0][1] - m->m[1][0]) * s;
	}
	else {
		i = 0;
		if (m->m[1][1] > m->m[0][0]) {
			i = 1;
		}
		if (m->m[2][2] > m->m[i][i]) {
			i = 2;
		}
		j = (i + 1) % 3;
		k = (j + 1) % 3;

		s = sqrt((m->m[i][i] - (m->m[j][j] + m->m[k][k])) + 1.0);

		q1[i] = s * 0.5;
		s = 0.5 / s;
		q->s = (m->m[j][k] + m->m[k][j]) * s;
		q1[j] = (m->m[i][j] + m->m[j][i]) * s;
		q1[k] = (m->m[i][k] + m->m[k][i]) * s;
	}
}


void
VInterpolateQuaternion(VQuaternion * p, VQuaternion * q, double t, VQuaternion * qt)
{
	double    omega, cosom, sinom, sclp, sclq;

	cosom = p->v.x * p->v.x + p->v.y * p->v.y +
		p->v.z * p->v.z + p->s * p->s;

	if ((1.0 + cosom) > EPSILON) {

		if ((1.0 - cosom) > EPSILON) {
			omega = acos(cosom);
			sinom = sin(omega);
			sclp = sin((1.0 - t) * omega) / sinom;
			sclq = sin(t * omega) / sinom;
		}
		else {
			sclp = 1.0 - t;
			sclq = t;
		}

		qt->v.x = p->v.x * sclp + q->v.x * sclq;
		qt->v.y = p->v.y * sclp + q->v.y * sclq;
		qt->v.z = p->v.z * sclp + q->v.z * sclq;
		qt->s = p->s * sclp + q->s * sclq;

	}
	else {

		qt->v.x = -p->v.y;
		qt->v.y = p->v.x;
		qt->v.z = -p->s;
		qt->s = p->v.z;

		sclp = sin((1.0 - t) * M_PI_2);
		sclq = sin(t * M_PI_2);

		qt->v.x = p->v.x * sclp + qt->v.x * sclq;
		qt->v.y = p->v.y * sclp + qt->v.y * sclq;
		qt->v.z = p->v.z * sclp + qt->v.z * sclq;
	}

}


VQuaternion *
VQuaternionMult(VQuaternion * a, VQuaternion * b, VQuaternion * r)
{
	VPoint c;

	r->s = a->s * b->s - VDotProd(&a->v, &b->v);
	VCrossProd(&a->v, &b->v, &c);
	c.x += a->s * b->v.x + b->s * a->v.x;
	c.y += a->s * b->v.y + b->s * a->v.y;
	c.z += a->s * b->v.z + b->s * a->v.z;
	r->v = c;
	return r;
}


VQuaternion *
VQuaternionComplement(VQuaternion * a, VQuaternion * r)
{
	r->s = a->s;
	r->v.x = -a->v.x;
	r->v.y = -a->v.y;
	r->v.z = -a->v.z;
	return r;
}
