#ifndef _VLIBMATH_H
#define _VLIBMATH_H

#include <math.h>

#ifdef Vlibmath_IMPORT
	#define EXTERN
#else
	#define EXTERN extern
#endif

typedef struct {
	double x;
	double y;
	double z;
} VPoint;

typedef struct {
	double    m[4][4];
} VMatrix;


EXTERN VPoint VUnitVectorI 
	#ifdef Vlibmath_IMPORT
	= {1.0, 0.0, 0.0}
	#endif
	;

EXTERN VPoint VUnitVectorJ
	#ifdef Vlibmath_IMPORT
	= {0.0, 1.0, 0.0}
	#endif
	;

EXTERN VPoint VUnitVectorK
	#ifdef Vlibmath_IMPORT
	= {0.0, 0.0, 1.0}
	#endif
	;

#define VPointToClipPlaneDistance(pt, pl) \
	(- VDotProd(pl, pt) / ((pl)->x * (pl)->x + (pl)->y * (pl)->y + \
	(pl)->z * (pl)->z))

EXTERN void VSetPoint(VPoint *p, double x, double y, double z);

#define mag(x,y,z) sqrt ( (x) * (x) + (y) * (y) + (z) * (z) )

/**
 * Returns the magnitude.
 * @param p
 * @return Magnitude sqrt(p->x*p->x + p->y*p->y + p->z*p->z).
 */
EXTERN double VMagnitude(VPoint *p);

/**
 * Returns the square magnitude.
 * @param p
 * @return Squared magnitude p->x*p->x + p->y*p->y + p->z*p->z.
 */
EXTERN double VMagnitude2(VPoint *p);

/**
 * Return sum=(a.x+b.x, a.y+b.y, a.z+b.z).
 */
EXTERN void VAdd(VPoint *a, VPoint *b, VPoint *sum);

/**
 * Return dif=(a.x-b.x, a.y-b.y, a.z-b.z).
 */
EXTERN void VSub(VPoint *a, VPoint *b, VPoint *dif);

/**
 * Return a.x*b.x + a.y*b.y + a.z*b.z.
 */
EXTERN double VDotProd (VPoint *a, VPoint *b);

/**
 * Return
 * 
 * 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
 * 
 * The vectors may overlap.
 */
EXTERN void VCrossProd (VPoint *a, VPoint *b, VPoint *p);

/**
 * Set the matrix with the identity transformation (i.e. no rotation,
 * no translation).
 */
EXTERN void VIdentMatrix(VMatrix * Mtx);

/**
 * Transpose 'ori' into 'trans'. Matrices may overlap.
 */
EXTERN void VMatrixTranspose(VMatrix *ori, VMatrix *trans);

/**
 * Return r=b*a. The matrices may overlap.
 * 
 * NOTE that the b matrix is applied NEXT to the a matrix, so that
 * the algebraic expression r=a*b translates into VMatrixMult(b,a,r).
 * Another way to remember the order of the arguments is thinking that
 * the matrices a,b as appears in the formal arguments are applied in
 * the order to a point.
 */
EXTERN void VMatrixMult (VMatrix *a, VMatrix *b, VMatrix *r);

/**
 * Performs a sub-matrix multiplication by the given rank 1-4.
 * Typically rank=3 to perform pure rotations. Matrices may overlap.
 */
EXTERN void VMatrixMultByRank (VMatrix *a, VMatrix *b, VMatrix *r, int rank);

/**
 * Add active translation to the matrix (increments are added to the point
 * components).
 */
EXTERN void VTranslate(VMatrix * m, double x, double y, double z);

/**
 * Add active translation to the matrix (increments are added to the point
 * components).
 */
EXTERN void VTranslatePoint(VMatrix * m, VPoint t);

/*
 * VRotate options (see VRotate() below)
 */

#define XRotation	1			/** rotate about X axis  */
#define YRotation	2			/** rotate about Y axis  */
#define ZRotation	3			/** rotate about Z axis  */

/**
 * Add the active rotation around the specified coordinate axis to the right
 * of the given matrix. For example, a rotation by +90 DEG around the z axis
 * changes the components of the point [1,0,0] into [0,1,0].
 * @param m Subject matrix.
 * @param axis Rotation around: 1=X, 2=Y, 3=Z.
 * @param theta Angle of the rotation (RAD).
 */
EXTERN void VRotate(VMatrix *m, int axis, double theta);

/**
 * Add the active rotational around the specified axis to the right of the
 * given matrix.
 * @param axis Axis of the rotation. Does not need to be normalized to 1, but
 * its length must not be zero.
 * @param angle Angle of rotation.
 * @param m Resulting rotational matrix.
 */
EXTERN void VRotateAroundAxis(VMatrix * m, VPoint * axis, double angle);

/**
 * Add a scale transformation next to the given matrix. Example:
 */
EXTERN void VScaleMatrix(VMatrix *m, double xscale, double yscale, double zscale);

/**
 * Apply the rotation and translation matrix m to the vector p, giving
 * q[i] = m[i,j]*p[j]+m[i,3] with i,j=0..2. p,q may overlap.
 */
EXTERN void VTransform(VPoint *p, VMatrix *m, VPoint *q);

/**
 * Apply the rotational matrix only. p,q may overlap.
 */
EXTERN void VTransform_(VPoint *p, VMatrix *m, VPoint *q);

/**
 * Apply the reverse rotation and translation matrix m to the vector p, giving
 * q[i] = m[j,i]*(p[j]+m[j,3]) with i,j=0..2. p,q may overlap.
 */
EXTERN void VReverseTransform(VPoint *p, VMatrix *m, VPoint *q);

/**
 * Apply the reverse rotational matrix only. p,q may overlap.
 */
EXTERN void VReverseTransform_(VPoint *p, VMatrix *m, VPoint *q);

/**
 * Compute the rotational matrix given the Euler's angles. The resulting matrix
 * rotates from the rotated reference system to the "fixed" reference system.
 * The rotated reference system undergone to there rotations, in the order:
 * 1. Around z of an angle psi.
 * 2. Around y of an angle theta.
 * 3. Around x of an angle phi.
 * @param phi   Rotation around the x axis, or "roll" (RAD).
 * @param theta Rotation around the y axis, or "pitch" (RAD).
 * @param psi   Rotation around the z axis, or "heading" (RAD).
 * @param m     Here returns the computed rotational matrix.
 */
EXTERN void VEulerToMatrix(double phi, double theta, double psi, VMatrix *m);

/**
 * Computes the Euler's angles given the rotational matrix.
 * @param m     Rotational matrix.
 * @param phi   Here returns the angle of rotation around x axis, or "roll",
 *              in the range [-M_PI/2, M_PI/2] (RAD).
 * @param theta Here returns the angle of rotation around y axis, or "pitch",
 *              in the range [-M_PI/2, M_PI/2] (RAD).
 * @param psi   Here returns the angle of rotation around z axis, or "heading",
 *              in the range [0, 2*M_PI] (RAD).
 */
EXTERN void VMatrixToEuler(VMatrix *m, double *phi, double *theta, double *psi);

typedef struct {
	double    s;
	VPoint    v;
} VQuaternion;

EXTERN void VQuaternionToMatrix(VQuaternion * q, VMatrix * m);

EXTERN void VMatrixToQuaternion(VMatrix * m, VQuaternion * q);

EXTERN void VInterpolateQuaternion(VQuaternion * p, VQuaternion * q, double t, VQuaternion * qt);

EXTERN VQuaternion * VQuaternionMult(VQuaternion * a, VQuaternion * b, VQuaternion * r);

EXTERN VQuaternion * VQuaternionComplement(VQuaternion * a, VQuaternion * r);

#undef EXTERN
#endif
