/*!***************************************************************************
 @Function			PVRTMatrixQuaternionToAxisAngleX
 @Input				qIn		Quaternion to transform
 @Output			vAxis	Axis of rotation
 @Output			fAngle	Angle of rotation
 @Description		Convert a quaternion to an axis and angle. Expects a unit
					quaternion.
*****************************************************************************/
void PVRTMatrixQuaternionToAxisAngleX(
	const PVRTQUATERNIONx	&qIn,
	PVRTVECTOR3x			&vAxis,
	int						&fAngle)
{
	int		fCosAngle, fSinAngle;
	int		temp;

	/* Compute some values */
	fCosAngle	= qIn.w;
	temp		= PVRTF2X(1.0f) - PVRTXMUL(fCosAngle, fCosAngle);
	fAngle		= PVRTXMUL(PVRTXACOS(fCosAngle), PVRTF2X(2.0f));
	fSinAngle	= PVRTF2X(((float)sqrt(PVRTX2F(temp))));

	/* This is to avoid a division by zero */
	if (PVRTABS(fSinAngle)<PVRTF2X(0.0005f))
	{
		fSinAngle = PVRTF2X(1.0f);
	}

	/* Get axis vector */
	vAxis.x = PVRTXDIV(qIn.x, fSinAngle);
	vAxis.y = PVRTXDIV(qIn.y, fSinAngle);
	vAxis.z = PVRTXDIV(qIn.z, fSinAngle);
}
/*!***************************************************************************
 @Function		PVRTMatrixPerspectiveFovRHX
 @Output		mOut		Perspective matrix
 @Input			fFOVy		Field of view
 @Input			fAspect		Aspect ratio
 @Input			fNear		Near clipping distance
 @Input			fFar		Far clipping distance
 @Input			bRotate		Should we rotate it ? (for upright screens)
 @Description	Create a perspective matrix.
*****************************************************************************/
void PVRTMatrixPerspectiveFovRHX(
	PVRTMATRIXx	&mOut,
	const int	fFOVy,
	const int	fAspect,
	const int	fNear,
	const int	fFar,
	const bool  bRotate)
{
	int		f;

	int fCorrectAspect = fAspect;
	if (bRotate)
	{
		fCorrectAspect = PVRTXDIV(PVRTF2X(1.0f), fAspect);
	}
	f = PVRTXDIV(PVRTF2X(1.0f), PVRTXTAN(PVRTXMUL(fFOVy, PVRTF2X(0.5f))));

	mOut.f[ 0] = PVRTXDIV(f, fCorrectAspect);
	mOut.f[ 1] = PVRTF2X(0.0f);
	mOut.f[ 2] = PVRTF2X(0.0f);
	mOut.f[ 3] = PVRTF2X(0.0f);

	mOut.f[ 4] = PVRTF2X(0.0f);
	mOut.f[ 5] = f;
	mOut.f[ 6] = PVRTF2X(0.0f);
	mOut.f[ 7] = PVRTF2X(0.0f);

	mOut.f[ 8] = PVRTF2X(0.0f);
	mOut.f[ 9] = PVRTF2X(0.0f);
	mOut.f[10] = PVRTXDIV(fFar + fNear, fNear - fFar);
	mOut.f[11] = PVRTF2X(-1.0f);

	mOut.f[12] = PVRTF2X(0.0f);
	mOut.f[13] = PVRTF2X(0.0f);
	mOut.f[14] = PVRTXMUL(PVRTXDIV(fFar, fNear - fFar), fNear) << 1;	// Cheap 2x
	mOut.f[15] = PVRTF2X(0.0f);

	if (bRotate)
	{
		PVRTMATRIXx mRotation, mTemp = mOut;
		PVRTMatrixRotationZX(mRotation, PVRTF2X(-90.0f*PVRT_PIf/180.0f));
		PVRTMatrixMultiplyX(mOut, mTemp, mRotation);
	}
}
/*!***************************************************************************
 @Function		PVRTMatrixPerspectiveFovLHX
 @Output		mOut		Perspective matrix
 @Input			fFOVy		Field of view
 @Input			fAspect		Aspect ratio
 @Input			fNear		Near clipping distance
 @Input			fFar		Far clipping distance
 @Input			bRotate		Should we rotate it ? (for upright screens)
 @Description	Create a perspective matrix.
*****************************************************************************/
void PVRTMatrixPerspectiveFovLHX(
	PVRTMATRIXx	&mOut,
	const int	fFOVy,
	const int	fAspect,
	const int	fNear,
	const int	fFar,
	const bool  bRotate)
{
	int		f, fRealAspect;

	if (bRotate)
		fRealAspect = PVRTXDIV(PVRTF2X(1.0f), fAspect);
	else
		fRealAspect = fAspect;

	f = PVRTXDIV(PVRTF2X(1.0f), PVRTXTAN(PVRTXMUL(fFOVy, PVRTF2X(0.5f))));

	mOut.f[ 0] = PVRTXDIV(f, fRealAspect);
	mOut.f[ 1] = PVRTF2X(0.0f);
	mOut.f[ 2] = PVRTF2X(0.0f);
	mOut.f[ 3] = PVRTF2X(0.0f);

	mOut.f[ 4] = PVRTF2X(0.0f);
	mOut.f[ 5] = f;
	mOut.f[ 6] = PVRTF2X(0.0f);
	mOut.f[ 7] = PVRTF2X(0.0f);

	mOut.f[ 8] = PVRTF2X(0.0f);
	mOut.f[ 9] = PVRTF2X(0.0f);
	mOut.f[10] = PVRTXDIV(fFar, fFar - fNear);
	mOut.f[11] = PVRTF2X(1.0f);

	mOut.f[12] = PVRTF2X(0.0f);
	mOut.f[13] = PVRTF2X(0.0f);
	mOut.f[14] = -PVRTXMUL(PVRTXDIV(fFar, fFar - fNear), fNear);
	mOut.f[15] = PVRTF2X(0.0f);

	if (bRotate)
	{
		PVRTMATRIXx mRotation, mTemp = mOut;
		PVRTMatrixRotationZX(mRotation, PVRTF2X(90.0f*PVRT_PIf/180.0f));
		PVRTMatrixMultiplyX(mOut, mTemp, mRotation);
	}
}
/*!***************************************************************************
 @Function		PVRTMatrixOrthoRHX
 @Output		mOut		Orthographic matrix
 @Input			w			Width of the screen
 @Input			h			Height of the screen
 @Input			zn			Near clipping distance
 @Input			zf			Far clipping distance
 @Input			bRotate		Should we rotate it ? (for upright screens)
 @Description	Create an orthographic matrix.
*****************************************************************************/
void PVRTMatrixOrthoRHX(
	PVRTMATRIXx	&mOut,
	const int	w,
	const int	h,
	const int	zn,
	const int	zf,
	const bool  bRotate)
{
	int fCorrectW = w;
	int fCorrectH = h;
	if (bRotate)
	{
		fCorrectW = h;
		fCorrectH = w;
	}
	mOut.f[ 0] = PVRTXDIV(PVRTF2X(2.0f), fCorrectW);
	mOut.f[ 1] = PVRTF2X(0.0f);
	mOut.f[ 2] = PVRTF2X(0.0f);
	mOut.f[ 3] = PVRTF2X(0.0f);

	mOut.f[ 4] = PVRTF2X(0.0f);
	mOut.f[ 5] = PVRTXDIV(PVRTF2X(2.0f), fCorrectH);
	mOut.f[ 6] = PVRTF2X(0.0f);
	mOut.f[ 7] = PVRTF2X(0.0f);

	mOut.f[ 8] = PVRTF2X(0.0f);
	mOut.f[ 9] = PVRTF2X(0.0f);
	mOut.f[10] = PVRTXDIV(PVRTF2X(1.0f), zn - zf);
	mOut.f[11] = PVRTXDIV(zn, zn - zf);

	mOut.f[12] = PVRTF2X(0.0f);
	mOut.f[13] = PVRTF2X(0.0f);
	mOut.f[14] = PVRTF2X(0.0f);
	mOut.f[15] = PVRTF2X(1.0f);

	if (bRotate)
	{
		PVRTMATRIXx mRotation, mTemp = mOut;
		PVRTMatrixRotationZX(mRotation, PVRTF2X(-90.0f*PVRT_PIf/180.0f));
		PVRTMatrixMultiplyX(mOut, mRotation, mTemp);
	}
}
/*!***************************************************************************
 @Function			PVRTMatrixQuaternionNormalizeX
 @Modified			quat	Vector to normalize
 @Description		Normalize quaternion.
					Original quaternion is scaled down prior to be normalized in
					order to avoid overflow issues.
*****************************************************************************/
void PVRTMatrixQuaternionNormalizeX(PVRTQUATERNIONx &quat)
{
	PVRTQUATERNIONx	qTemp;
	int				f, n;

	/* Scale vector by uniform value */
	n = PVRTABS(quat.w) + PVRTABS(quat.x) + PVRTABS(quat.y) + PVRTABS(quat.z);
	qTemp.w = PVRTXDIV(quat.w, n);
	qTemp.x = PVRTXDIV(quat.x, n);
	qTemp.y = PVRTXDIV(quat.y, n);
	qTemp.z = PVRTXDIV(quat.z, n);

	/* Compute quaternion magnitude */
	f = PVRTXMUL(qTemp.w, qTemp.w) + PVRTXMUL(qTemp.x, qTemp.x) + PVRTXMUL(qTemp.y, qTemp.y) + PVRTXMUL(qTemp.z, qTemp.z);
	f = PVRTXDIV(PVRTF2X(1.0f), PVRTF2X(sqrt(PVRTX2F(f))));

	/* Multiply vector components by f */
	quat.x = PVRTXMUL(qTemp.x, f);
	quat.y = PVRTXMUL(qTemp.y, f);
	quat.z = PVRTXMUL(qTemp.z, f);
	quat.w = PVRTXMUL(qTemp.w, f);
}
/*!***************************************************************************
 @Function			PVRTMatrixVec3NormalizeX
 @Output			vOut	Normalized vector
 @Input				vIn		Vector to normalize
 @Description		Normalizes the supplied vector.
					The square root function is currently still performed
					in floating-point.
					Original vector is scaled down prior to be normalized in
					order to avoid overflow issues.
****************************************************************************/
void PVRTMatrixVec3NormalizeX(
	PVRTVECTOR3x		&vOut,
	const PVRTVECTOR3x	&vIn)
{
	int				f, n;
	PVRTVECTOR3x	vTemp;

	/* Scale vector by uniform value */
	n = PVRTABS(vIn.x) + PVRTABS(vIn.y) + PVRTABS(vIn.z);
	vTemp.x = PVRTXDIV(vIn.x, n);
	vTemp.y = PVRTXDIV(vIn.y, n);
	vTemp.z = PVRTXDIV(vIn.z, n);

	/* Calculate x2+y2+z2/sqrt(x2+y2+z2) */
	f = PVRTMatrixVec3DotProductX(vTemp, vTemp);
	f = PVRTXDIV(PVRTF2X(1.0f), PVRTF2X(sqrt(PVRTX2F(f))));

	/* Multiply vector components by f */
	vOut.x = PVRTXMUL(vTemp.x, f);
	vOut.y = PVRTXMUL(vTemp.y, f);
	vOut.z = PVRTXMUL(vTemp.z, f);
}
/*!***************************************************************************
 @Function			PVRTMatrixQuaternionSlerpX
 @Output			qOut	Result of the interpolation
 @Input				qA		First quaternion to interpolate from
 @Input				qB		Second quaternion to interpolate from
 @Input				t		Coefficient of interpolation
 @Description		Perform a Spherical Linear intERPolation between quaternion A
					and quaternion B at time t. t must be between 0.0f and 1.0f
					Requires input quaternions to be normalized
*****************************************************************************/
void PVRTMatrixQuaternionSlerpX(
	PVRTQUATERNIONx			&qOut,
	const PVRTQUATERNIONx	&qA,
	const PVRTQUATERNIONx	&qB,
	const int				t)
{
	int		fCosine, fAngle, A, B;

	/* Parameter checking */
	if (t<PVRTF2X(0.0f) || t>PVRTF2X(1.0f))
	{
		_RPT0(_CRT_WARN, "PVRTMatrixQuaternionSlerp : Bad parameters\n");
		qOut.x = PVRTF2X(0.0f);
		qOut.y = PVRTF2X(0.0f);
		qOut.z = PVRTF2X(0.0f);
		qOut.w = PVRTF2X(1.0f);
		return;
	}

	/* Find sine of Angle between Quaternion A and B (dot product between quaternion A and B) */
	fCosine = PVRTXMUL(qA.w, qB.w) +
		PVRTXMUL(qA.x, qB.x) + PVRTXMUL(qA.y, qB.y) + PVRTXMUL(qA.z, qB.z);

	if(fCosine < PVRTF2X(0.0f))
	{
		PVRTQUATERNIONx qi;

		/*
			<http://www.magic-software.com/Documentation/Quaternions.pdf>

			"It is important to note that the quaternions q and -q represent
			the same rotation... while either quaternion will do, the
			interpolation methods require choosing one over the other.

			"Although q1 and -q1 represent the same rotation, the values of
			Slerp(t; q0, q1) and Slerp(t; q0,-q1) are not the same. It is
			customary to choose the sign... on q1 so that... the angle
			between q0 and q1 is acute. This choice avoids extra
			spinning caused by the interpolated rotations."
		*/
		qi.x = -qB.x;
		qi.y = -qB.y;
		qi.z = -qB.z;
		qi.w = -qB.w;

		PVRTMatrixQuaternionSlerpX(qOut, qA, qi, t);
		return;
	}

	fCosine = PVRT_MIN(fCosine, PVRTF2X(1.0f));
	fAngle = PVRTXACOS(fCosine);

	/* Avoid a division by zero */
	if (fAngle==PVRTF2X(0.0f))
	{
		qOut = qA;
		return;
	}

	/* Precompute some values */
	A = PVRTXDIV(PVRTXSIN(PVRTXMUL((PVRTF2X(1.0f)-t), fAngle)), PVRTXSIN(fAngle));
	B = PVRTXDIV(PVRTXSIN(PVRTXMUL(t, fAngle)), PVRTXSIN(fAngle));

	/* Compute resulting quaternion */
	qOut.x = PVRTXMUL(A, qA.x) + PVRTXMUL(B, qB.x);
	qOut.y = PVRTXMUL(A, qA.y) + PVRTXMUL(B, qB.y);
	qOut.z = PVRTXMUL(A, qA.z) + PVRTXMUL(B, qB.z);
	qOut.w = PVRTXMUL(A, qA.w) + PVRTXMUL(B, qB.w);

	/* Normalise result */
	PVRTMatrixQuaternionNormalizeX(qOut);
}
/*!***************************************************************************
 @Function			PVRTMatrixLinearEqSolveX
 @Input				pSrc	2D array of floats. 4 Eq linear problem is 5x4
							matrix, constants in first column
 @Input				nCnt	Number of equations to solve
 @Output			pRes	Result
 @Description		Solves 'nCnt' simultaneous equations of 'nCnt' variables.
					pRes should be an array large enough to contain the
					results: the values of the 'nCnt' variables.
					This fn recursively uses Gaussian Elimination.
*****************************************************************************/
void PVRTMatrixLinearEqSolveX(
	int			* const pRes,
	int			** const pSrc,
	const int	nCnt)
{
	int		i, j, k;
	int		f;

	if (nCnt == 1)
	{
		_ASSERT(pSrc[0][1] != 0);
		pRes[0] = PVRTXDIV(pSrc[0][0], pSrc[0][1]);
		return;
	}

	// Loop backwards in an attempt avoid the need to swap rows
	i = nCnt;
	while(i)
	{
		--i;

		if(pSrc[i][nCnt] != PVRTF2X(0.0f))
		{
			// Row i can be used to zero the other rows; let's move it to the bottom
			if(i != (nCnt-1))
			{
				for(j = 0; j <= nCnt; ++j)
				{
					// Swap the two values
					f = pSrc[nCnt-1][j];
					pSrc[nCnt-1][j] = pSrc[i][j];
					pSrc[i][j] = f;
				}
			}

			// Now zero the last columns of the top rows
			for(j = 0; j < (nCnt-1); ++j)
			{
				_ASSERT(pSrc[nCnt-1][nCnt] != PVRTF2X(0.0f));
				f = PVRTXDIV(pSrc[j][nCnt], pSrc[nCnt-1][nCnt]);

				// No need to actually calculate a zero for the final column
				for(k = 0; k < nCnt; ++k)
				{
					pSrc[j][k] -= PVRTXMUL(f, pSrc[nCnt-1][k]);
				}
			}

			break;
		}
	}

	// Solve the top-left sub matrix
	PVRTMatrixLinearEqSolveX(pRes, pSrc, nCnt - 1);

	// Now calc the solution for the bottom row
	f = pSrc[nCnt-1][0];
	for(k = 1; k < nCnt; ++k)
	{
		f -= PVRTXMUL(pSrc[nCnt-1][k], pRes[k-1]);
	}
	_ASSERT(pSrc[nCnt-1][nCnt] != PVRTF2X(0));
	f = PVRTXDIV(f, pSrc[nCnt-1][nCnt]);
	pRes[nCnt-1] = f;
}
/*!***************************************************************************
 @Function			PVRTMatrixInverseX
 @Output			mOut	Inversed matrix
 @Input				mIn		Original matrix
 @Description		Compute the inverse matrix of mIn.
					The matrix must be of the form :
					A 0
					C 1
					Where A is a 3x3 matrix and C is a 1x3 matrix.
*****************************************************************************/
void PVRTMatrixInverseX(
	PVRTMATRIXx			&mOut,
	const PVRTMATRIXx	&mIn)
{
	PVRTMATRIXx	mDummyMatrix;
	int			det_1;
	int			pos, neg, temp;

    /* Calculate the determinant of submatrix A and determine if the
       the matrix is singular as limited by the double precision
       floating-point data representation. */
    pos = neg = 0;
    temp =  PVRTXMUL(PVRTXMUL(mIn.f[ 0], mIn.f[ 5]), mIn.f[10]);
    if (temp >= 0) pos += temp; else neg += temp;
    temp =  PVRTXMUL(PVRTXMUL(mIn.f[ 4], mIn.f[ 9]), mIn.f[ 2]);
    if (temp >= 0) pos += temp; else neg += temp;
    temp =  PVRTXMUL(PVRTXMUL(mIn.f[ 8], mIn.f[ 1]), mIn.f[ 6]);
    if (temp >= 0) pos += temp; else neg += temp;
	temp =  PVRTXMUL(PVRTXMUL(-mIn.f[ 8], mIn.f[ 5]), mIn.f[ 2]);
    if (temp >= 0) pos += temp; else neg += temp;
    temp =  PVRTXMUL(PVRTXMUL(-mIn.f[ 4], mIn.f[ 1]), mIn.f[10]);
    if (temp >= 0) pos += temp; else neg += temp;
    temp =  PVRTXMUL(PVRTXMUL(-mIn.f[ 0], mIn.f[ 9]), mIn.f[ 6]);
    if (temp >= 0) pos += temp; else neg += temp;
    det_1 = pos + neg;

    /* Is the submatrix A singular? */
    if (det_1 == 0)
	{
        /* Matrix M has no inverse */
        _RPT0(_CRT_WARN, "Matrix has no inverse : singular matrix\n");
        return;
    }
    else
	{
        /* Calculate inverse(A) = adj(A) / det(A) */
        //det_1 = 1.0 / det_1;
		det_1 = PVRTXDIV(PVRTF2X(1.0f), det_1);
		mDummyMatrix.f[ 0] =   PVRTXMUL(( PVRTXMUL(mIn.f[ 5], mIn.f[10]) - PVRTXMUL(mIn.f[ 9], mIn.f[ 6]) ), det_1);
		mDummyMatrix.f[ 1] = - PVRTXMUL(( PVRTXMUL(mIn.f[ 1], mIn.f[10]) - PVRTXMUL(mIn.f[ 9], mIn.f[ 2]) ), det_1);
		mDummyMatrix.f[ 2] =   PVRTXMUL(( PVRTXMUL(mIn.f[ 1], mIn.f[ 6]) - PVRTXMUL(mIn.f[ 5], mIn.f[ 2]) ), det_1);
		mDummyMatrix.f[ 4] = - PVRTXMUL(( PVRTXMUL(mIn.f[ 4], mIn.f[10]) - PVRTXMUL(mIn.f[ 8], mIn.f[ 6]) ), det_1);
		mDummyMatrix.f[ 5] =   PVRTXMUL(( PVRTXMUL(mIn.f[ 0], mIn.f[10]) - PVRTXMUL(mIn.f[ 8], mIn.f[ 2]) ), det_1);
		mDummyMatrix.f[ 6] = - PVRTXMUL(( PVRTXMUL(mIn.f[ 0], mIn.f[ 6]) - PVRTXMUL(mIn.f[ 4], mIn.f[ 2]) ), det_1);
		mDummyMatrix.f[ 8] =   PVRTXMUL(( PVRTXMUL(mIn.f[ 4], mIn.f[ 9]) - PVRTXMUL(mIn.f[ 8], mIn.f[ 5]) ), det_1);
		mDummyMatrix.f[ 9] = - PVRTXMUL(( PVRTXMUL(mIn.f[ 0], mIn.f[ 9]) - PVRTXMUL(mIn.f[ 8], mIn.f[ 1]) ), det_1);
		mDummyMatrix.f[10] =   PVRTXMUL(( PVRTXMUL(mIn.f[ 0], mIn.f[ 5]) - PVRTXMUL(mIn.f[ 4], mIn.f[ 1]) ), det_1);

        /* Calculate -C * inverse(A) */
        mDummyMatrix.f[12] = - ( PVRTXMUL(mIn.f[12], mDummyMatrix.f[ 0]) + PVRTXMUL(mIn.f[13], mDummyMatrix.f[ 4]) + PVRTXMUL(mIn.f[14], mDummyMatrix.f[ 8]) );
		mDummyMatrix.f[13] = - ( PVRTXMUL(mIn.f[12], mDummyMatrix.f[ 1]) + PVRTXMUL(mIn.f[13], mDummyMatrix.f[ 5]) + PVRTXMUL(mIn.f[14], mDummyMatrix.f[ 9]) );
		mDummyMatrix.f[14] = - ( PVRTXMUL(mIn.f[12], mDummyMatrix.f[ 2]) + PVRTXMUL(mIn.f[13], mDummyMatrix.f[ 6]) + PVRTXMUL(mIn.f[14], mDummyMatrix.f[10]) );

        /* Fill in last row */
        mDummyMatrix.f[ 3] = PVRTF2X(0.0f);
		mDummyMatrix.f[ 7] = PVRTF2X(0.0f);
		mDummyMatrix.f[11] = PVRTF2X(0.0f);
        mDummyMatrix.f[15] = PVRTF2X(1.0f);
	}

   	/* Copy contents of dummy matrix in pfMatrix */
	mOut = mDummyMatrix;
}