/*
 * $Id$
 *
 * $Date$
 * $Revision$
 *
 * (C) 1999 by Hyperion
 * All rights reserved
 *
 * This file is part of the MiniGL library project
 * See the file Licence.txt for more details
 *
 */

#include "sysinc.h"
#include <math.h>

#ifndef PI
	#ifdef M_PI
	#define PI M_PI
	#else
	#define PI 3.1415927
	#endif
#endif
static char rcsid[] UNUSED = "$Id$";

INLINE void m_MatCopy(Matrix *pA, Matrix *pB);
INLINE void m_MatMultGeneral(Matrix *pA, float *pB, Matrix *pC);
INLINE void m_MatMultB0001(Matrix *pA, float *pB, Matrix *pC);
INLINE void m_MatMultA0001(Matrix *pA, float *pB, Matrix *pC);
INLINE void m_MatMultATrans(Matrix *pA, float *pB, Matrix *pC);
INLINE void m_MatMultBTrans(Matrix *pA, float *pB, Matrix *pC);
INLINE void m_LoadMatrixf(Matrix *pA, const float *v);
INLINE void m_LoadMatrixd(Matrix *pA, const double *v);
INLINE void m_LoadIdentity(Matrix *pA);
INLINE void m_Mult(Matrix *pA, float *v, int vtype, Matrix *pC);
void m_CombineMatrices(GLcontext context);
void GLMatrixInit(GLcontext context);
void m_PrintMatrix(Matrix *pA);

/* I am too lazy to convert our old code */
#define CM_0 OF_11
#define CM_1 OF_12
#define CM_2 OF_13
#define CM_3 OF_21
#define CM_4 OF_22
#define CM_5 OF_23
#define CM_6 OF_31
#define CM_7 OF_32
#define CM_8 OF_33

/*
** Computes the determinant of the upper 3x3 submatrix block
** of the supplied matrix
*/
INLINE GLfloat m_Determinant(Matrix *pA)
{
	#define a(x) (pA->v[CM_##x])
	float a0 = a(0);
	float a1 = a(1);
	float a2 = a(2);
	float a3 = a(3);
	float a4 = a(4);
	float a5 = a(5);
	float a6 = a(6);
	float a7 = a(7);
	float a8 = a(8);

	 return ( a0*a4*a8  +  a1*a5*a6  + a2*a3*a7
	   -  a0*a5*a7  -  a1*a3*a8  - a2*a4*a6);

	#undef a
}


/*
 * Compute determinant of 3x3 matrix resulting from pA dropping
 * column i and line j
 * (Adapted from the Matrix and Quaternion FAQ, at
 * http://skal.planet-d.net/demo/matrixfaq.htm)
 */
INLINE GLfloat m_Determinant3(Matrix *pA, int i, int j)
{
	float a[9];
	int di, dj, si, sj;
	
	for (di = 0; di < 3; di++)
	{
		for (dj = 0; dj < 3; dj++)
		{
			si = di + ((di >= i) ? 1 : 0);
			sj = dj + ((dj >= j) ? 1 : 0);
			a[di * 3 + dj] = pA->v[sj * 4 + si]; // really ?
		}
	}

	return a[0] * ( a[4] * a[8] - a[7] * a[5])
		 - a[1] * ( a[3] * a[8] - a[6] * a[5])
		 + a[2] * ( a[3] * a[7] - a[6] * a[4]);
}

INLINE GLfloat m_Determinant4(Matrix *pA)
{
	GLfloat res = 0.0, i = 1;
	int n;
	
	for (n = 0; n < 4; n++)
	{
		res += m_Determinant3(pA, 0, n) * i;
		i *= -1.0;
	}
	
	return res;
}
	
/*
** Builds the inverse of the upper 3x3 submatrix block of the
** supplied matrix for the backtransformation of the normals
*/
void m_DoInvert(Matrix *pA, GLfloat *out)
{
	GLfloat det = 1.f / m_Determinant(pA);

	#define a(x) (pA->v[CM_##x])
	float a0 = a(0);
	float a1 = a(1);
	float a2 = a(2);
	float a3 = a(3);
	float a4 = a(4);
	float a5 = a(5);
	float a6 = a(6);
	float a7 = a(7);
	float a8 = a(8);


	*out++ = (GLfloat)(a4*a8 - a5*a7)*det;
	*out++ = (GLfloat)(a2*a7 - a1*a8)*det;
	*out++ = (GLfloat)(a1*a5 - a2*a4)*det;

	*out++ = (GLfloat)(a5*a6 - a3*a8)*det;
	*out++ = (GLfloat)(a0*a8 - a2*a6)*det;
	*out++ = (GLfloat)(a2*a3 - a0*a5)*det;
	
	*out++ = (GLfloat)(a3*a7 - a4*a6)*det;
	*out++ = (GLfloat)(a1*a6 - a0*a7)*det;
	*out++ = (GLfloat)(a0*a4 - a1*a3)*det;

	#undef a
}

void m_BuildInverted(GLcontext context)
{
	context->InvRotValid = GL_TRUE;
	m_DoInvert(CurrentMV, context->InvRot);
}

void m_BuildInvertedFull(GLcontext context)
{
	int i, j, sign;
	
	context->InverseModelViewValid = GL_TRUE;
	
	GLfloat mdet = m_Determinant4(CurrentMV);
	
	if (fabs(mdet) < 0.0005)
	{
		m_LoadIdentity(&context->InverseModelView);
	}
	else
	{
		for (i = 0; i < 4; i++)
		{
			for (j = 0; j < 4; j++)
			{
				sign = 1 - ((i + j) % 2) * 2;
				context->InverseModelView.v[i*4+j]
					= (m_Determinant3(CurrentMV, i, j) * (GLfloat)sign) / mdet;
			}
		}
	}
}

/*
** Copy matrix B to matrix A
** A = B
*/
INLINE void m_MatCopy(Matrix *pA, Matrix *pB)
{
	int i;

	for (i=0; i<16; i++) pA->v[i] = pB->v[i];
	pA->flags = pB->flags;
	pA->Inverse = pB->Inverse;
}

/*
** General case: Matrix multiply
** Multiply matrix A by float field, storing the result in matrix C
*/
INLINE void m_MatMultGeneral(Matrix *pA, float *pB, Matrix *pC)
{
	#define a(x) (pA->v[OF_##x])
	#define b(x) (pB[OF_##x])
	#define c(x) (pC->v[OF_##x])

	float a11 = a(11),  b11 = b(11);
	float a12 = a(12),  b12 = b(12);
	float a13 = a(13),  b13 = b(13);
	float a14 = a(14),  b14 = b(14);
	float a21 = a(21),  b21 = b(21);
	float a22 = a(22),  b22 = b(22);
	float a23 = a(23),  b23 = b(23);
	float a24 = a(24),  b24 = b(24);
	float a31 = a(31),  b31 = b(31);
	float a32 = a(32),  b32 = b(32);
	float a33 = a(33),  b33 = b(33);
	float a34 = a(34),  b34 = b(34);
	float a41 = a(41),  b41 = b(41);
	float a42 = a(42),  b42 = b(42);
	float a43 = a(43),  b43 = b(43);
	float a44 = a(44),  b44 = b(44);


	c(11) = a11*b11 + a12*b21 + a13*b31 + a14*b41;
	c(12) = a11*b12 + a12*b22 + a13*b32 + a14*b42;
	c(13) = a11*b13 + a12*b23 + a13*b33 + a14*b43;
	c(14) = a11*b14 + a12*b24 + a13*b34 + a14*b44;
	c(21) = a21*b11 + a22*b21 + a23*b31 + a24*b41;
	c(22) = a21*b12 + a22*b22 + a23*b32 + a24*b42;
	c(23) = a21*b13 + a22*b23 + a23*b33 + a24*b43;
	c(24) = a21*b14 + a22*b24 + a23*b34 + a24*b44;
	c(31) = a31*b11 + a32*b21 + a33*b31 + a34*b41;
	c(32) = a31*b12 + a32*b22 + a33*b32 + a34*b42;
	c(33) = a31*b13 + a32*b23 + a33*b33 + a34*b43;
	c(34) = a31*b14 + a32*b24 + a33*b34 + a34*b44;
	c(41) = a41*b11 + a42*b21 + a43*b31 + a44*b41;
	c(42) = a41*b12 + a42*b22 + a43*b32 + a44*b42;
	c(43) = a41*b13 + a42*b23 + a43*b33 + a44*b43;
	c(44) = a41*b14 + a42*b24 + a43*b34 + a44*b44;

	pC->flags = 0;
	
	#undef a
	#undef b
	#undef c
}

/*
** Matrix B has three rows
** Multiply matrix A by float field, storing the result in matrix C
*/
INLINE void m_MatMultB0001(Matrix *pA, float *pB, Matrix *pC)
{
	#define a(x) (pA->v[OF_##x])
	#define b(x) (pB[OF_##x])
	#define c(x) (pC->v[OF_##x])

	float a11 = a(11),  b11 = b(11);
	float a12 = a(12),  b12 = b(12);
	float a13 = a(13),  b13 = b(13);
	float a14 = a(14),  b14 = b(14);
	float a21 = a(21),  b21 = b(21);
	float a22 = a(22),  b22 = b(22);
	float a23 = a(23),  b23 = b(23);
	float a24 = a(24),  b24 = b(24);
	float a31 = a(31),  b31 = b(31);
	float a32 = a(32),  b32 = b(32);
	float a33 = a(33),  b33 = b(33);
	float a34 = a(34),  b34 = b(34);
	float a41 = a(41);
	float a42 = a(42);
	float a43 = a(43);
	float a44 = a(44);


	c(11) = a11*b11 + a12*b21 + a13*b31;
	c(12) = a11*b12 + a12*b22 + a13*b32;
	c(13) = a11*b13 + a12*b23 + a13*b33;
	c(14) = a11*b14 + a12*b24 + a13*b34 + a14;
	c(21) = a21*b11 + a22*b21 + a23*b31;
	c(22) = a21*b12 + a22*b22 + a23*b32;
	c(23) = a21*b13 + a22*b23 + a23*b33;
	c(24) = a21*b14 + a22*b24 + a23*b34 + a24;
	c(31) = a31*b11 + a32*b21 + a33*b31;
	c(32) = a31*b12 + a32*b22 + a33*b32;
	c(33) = a31*b13 + a32*b23 + a33*b33;
	c(34) = a31*b14 + a32*b24 + a33*b34 + a34;
	c(41) = a41*b11 + a42*b21 + a43*b31;
	c(42) = a41*b12 + a42*b22 + a43*b32;
	c(43) = a41*b13 + a42*b23 + a43*b33;
	c(44) = a41*b14 + a42*b24 + a43*b34 + a44;

	pC->flags = 0;
	
	#undef a
	#undef b
}


/*
** Matrix A has three rows
** Multiply matrix A by float field, storing the result in matrix C
*/
INLINE void m_MatMultA0001(Matrix *pA, float *pB, Matrix *pC)
{
	#define a(x) (pA->v[OF_##x])
	#define b(x) (pB[OF_##x])
	#define c(x) (pC->v[OF_##x])

	float a11 = a(11),  b11 = b(11);
	float a12 = a(12),  b12 = b(12);
	float a13 = a(13),  b13 = b(13);
	float a14 = a(14),  b14 = b(14);
	float a21 = a(21),  b21 = b(21);
	float a22 = a(22),  b22 = b(22);
	float a23 = a(23),  b23 = b(23);
	float a24 = a(24),  b24 = b(24);
	float a31 = a(31),  b31 = b(31);
	float a32 = a(32),  b32 = b(32);
	float a33 = a(33),  b33 = b(33);
	float a34 = a(34),  b34 = b(34);
	float               b41 = b(41);
	float               b42 = b(42);
	float               b43 = b(43);
	float               b44 = b(44);


	c(11) = a11*b11 + a12*b21 + a13*b31 + a14*b41;
	c(12) = a11*b12 + a12*b22 + a13*b32 + a14*b42;
	c(13) = a11*b13 + a12*b23 + a13*b33 + a14*b43;
	c(14) = a11*b14 + a12*b24 + a13*b34 + a14*b44;
	c(21) = a21*b11 + a22*b21 + a23*b31 + a24*b41;
	c(22) = a21*b12 + a22*b22 + a23*b32 + a24*b42;
	c(23) = a21*b13 + a22*b23 + a23*b33 + a24*b43;
	c(24) = a21*b14 + a22*b24 + a23*b34 + a24*b44;
	c(31) = a31*b11 + a32*b21 + a33*b31 + a34*b41;
	c(32) = a31*b12 + a32*b22 + a33*b32 + a34*b42;
	c(33) = a31*b13 + a32*b23 + a33*b33 + a34*b43;
	c(34) = a31*b14 + a32*b24 + a33*b34 + a34*b44;
	c(41) = b41;
	c(42) = b42;
	c(43) = b43;
	c(44) = b44;

	pC->flags = 0;
	
	#undef a
	#undef b
}

/*
** Matrix A is a translation matrix
** Multiply matrix A by float field, storing the result in matrix C
*/
INLINE void m_MatMultATrans(Matrix *pA, float *pB, Matrix *pC)
{
	#define a(x) (pA->v[OF_##x])
	#define b(x) (pB[OF_##x])
	#define c(x) (pC->v[OF_##x])

	float               b11 = b(11);
	float               b12 = b(12);
	float               b13 = b(13);
	float a14 = a(14),  b14 = b(14);
	float               b21 = b(21);
	float               b22 = b(22);
	float               b23 = b(23);
	float a24 = a(24),  b24 = b(24);
	float               b31 = b(31);
	float               b32 = b(32);
	float               b33 = b(33);
	float a34 = a(34),  b34 = b(34);
	float               b41 = b(41);
	float               b42 = b(42);
	float               b43 = b(43);
	float               b44 = b(44);


	c(11) = b11 + a14*b41;
	c(12) = b12 + a14*b42;
	c(13) = b13 + a14*b43;
	c(14) = b14 + a14*b44;
	c(21) = b21 + a24*b41;
	c(22) = b22 + a24*b42;
	c(23) = b23 + a24*b43;
	c(24) = b24 + a24*b44;
	c(31) = b31 + a34*b41;
	c(32) = b32 + a34*b42;
	c(33) = b33 + a34*b43;
	c(34) = b34 + a34*b44;
	c(41) = b41;
	c(42) = b42;
	c(43) = b43;
	c(44) = b44;

	pC->flags = 0;
	
	#undef a
	#undef b
}

/*
** Matrix B is a translation matrix
** Multiply matrix A by float field, storing the result in matrix C
*/
INLINE void m_MatMultBTrans(Matrix *pA, float *pB, Matrix *pC)
{
	#define a(x) (pA->v[OF_##x])
	#define b(x) (pB[OF_##x])
	#define c(x) (pC->v[OF_##x])

	float a11 = a(11);
	float a12 = a(12);
	float a13 = a(13);
	float a14 = a(14),  b14 = b(14);
	float a21 = a(21);
	float a22 = a(22);
	float a23 = a(23);
	float a24 = a(24),  b24 = b(24);
	float a31 = a(31);
	float a32 = a(32);
	float a33 = a(33);
	float a34 = a(34),  b34 = b(34);
	float a41 = a(41);
	float a42 = a(42);
	float a43 = a(43);
	float a44 = a(44);


	c(11) = a11;
	c(12) = a12;
	c(13) = a13;
	c(14) = a14*b14 + a12*b24 + a13*b34 + a14;
	c(21) = a21;
	c(22) = a22;
	c(23) = a22;
	c(24) = a21*b14 + a22*b24 + a23*b34 + a24;
	c(31) = a31;
	c(32) = a32;
	c(33) = a33;
	c(34) = a31*b14 + a32*b24 + a33*b34 + a34;
	c(41) = a41;
	c(42) = a42;
	c(43) = a43;
	c(44) = a41*b14 + a42*b24 + a43*b34 + a44;

	pC->flags = 0;
	
	#undef a
	#undef b
}


INLINE void m_LoadMatrixf(Matrix *pA, const float *v)
{
	#define a(x) pA->v[OF_##x] = *v++

	a(11); a(21); a(31); a(41);
	a(12); a(22); a(32); a(42);
	a(13); a(23); a(33); a(43);
	a(14); a(24); a(34); a(44);

	if (*(v-4) == 0.f && *(v-3) == 0.f && *(v-2) == 0.f && *(v-1) == 1.f)
		pA->flags = MGLMAT_0001;
	else
		pA->flags = 0;

	#undef a
}

INLINE void m_LoadMatrixd(Matrix *pA, const double *v)
{
	#define a(x) pA->v[OF_##x] = (float)*v++

	a(11); a(21); a(31); a(41);
	a(12); a(22); a(32); a(42);
	a(13); a(23); a(33); a(43);
	a(14); a(24); a(34); a(44);

	if (*(v-4) == 0.0 && *(v-3) == 0.0 && *(v-2) == 0.0 && *(v-1) == 1.0)
		pA->flags = MGLMAT_0001;
	else
		pA->flags = 0;
	#undef a
}


INLINE void m_LoadIdentity(Matrix *pA)
{
	#define a(x) pA->v[OF_##x] = 0.f;
	#define b(x) pA->v[OF_##x] = 1.f;

	b(11); a(21); a(31); a(41);
	a(12); b(22); a(32); a(42);
	a(13); a(23); b(33); a(43);
	a(14); a(24); a(34); b(44);

	pA->flags = MGLMAT_IDENTITY;

	#undef a
	#undef b
}

/*
** Multiply matrix A by the matrix passed by v.
** The vtype specifies what kind of matrix v points to, so that
** this routine may select an optimized matrix multiplication routine.
*/
INLINE void m_Mult(Matrix *pA, float *v, int vtype, Matrix *pC)
{
#if 1
	if ((pA->flags & vtype) == 0)
	{
		m_MatMultGeneral(pA, v, pC);
	}
	else if (vtype == MGLMAT_TRANSLATION)
	{
		m_MatMultBTrans(pA, v, pC);
	}
	else if (vtype == MGLMAT_ROTATION || vtype == MGLMAT_0001)
	{
		m_MatMultB0001(pA, v, pC);
	}
	else if (pA->flags == MGLMAT_TRANSLATION)
	{
		m_MatMultATrans(pA, v, pC);
	}
	else if (pA->flags == MGLMAT_0001)
	{
		m_MatMultA0001(pA, v, pC);
	}
	else
	{
		m_MatMultGeneral(pA, v, pC);
	}

	pC->flags = 0;

	if (vtype & MGLMASK_0001)
	{
		if (pA->flags & MGLMASK_0001)
		{
			pC->flags = MGLMAT_0001;
		}
	}
#else
	//FIXME: Put those special case mults back.
	m_MatMultGeneral(pA, v, pC);
#endif
}

void m_CombineMatrices(GLcontext context)
{
	m_Mult(CurrentP,
	       CurrentMV->v,
	       CurrentMV->flags, &(context->CombinedMatrix));
	context->CombinedValid = GL_TRUE;
}

void m_PrintMatrix(Matrix *pA)
{
	#define a(x) (pA->v[OF_##x])
	kprintf("Matrix at 0x%lX\n", (ULONG)pA);
	kprintf("    | %6.3f %6.3f %6.3f %6.3f |\n", a(11), a(12), a(13), a(14));
	kprintf("    | %6.3f %6.3f %6.3f %6.3f |\n", a(21), a(22), a(23), a(24));
	kprintf("A = | %6.3f %6.3f %6.3f %6.3f |\n", a(31), a(32), a(33), a(34));
	kprintf("    | %6.3f %6.3f %6.3f %6.3f |\n", a(41), a(42), a(43), a(44));

	#undef a
}

// Interface functions
void cgl_GLMatrixMode(struct GLContextIFace *Self, GLenum mode)
{
	GLcontext context = GET_INSTANCE(Self);

	GLASSERT(mode == GL_PROJECTION || mode == GL_MODELVIEW || mode == GL_TEXTURE);
	GLASSERT(context != NULL);

	context->transform.CurrentMatrixMode = mode;
}

void cgl_GLLoadIdentity(struct GLContextIFace *Self)
{
	GLcontext context = GET_INSTANCE(Self);

	GLASSERT(context != NULL);
	GLFlagError(context, context->CurrentPrimitive != GL_BASE, GL_INVALID_OPERATION);
	
	switch (context->transform.CurrentMatrixMode)
	{
		case GL_MODELVIEW:
			m_LoadIdentity(CurrentMV);
			context->CombinedValid = GL_FALSE;
			context->InvRotValid = GL_FALSE;
			context->InverseModelViewValid = GL_FALSE;
			
			context->FrameCode++;
			break;
			
		case GL_PROJECTION:
			m_LoadIdentity(CurrentP);
			context->CombinedValid = GL_FALSE;
			context->InvRotValid = GL_FALSE;
			context->InverseModelViewValid = GL_FALSE;
						
			context->FrameCode++;
			break;
			
		case GL_TEXTURE:
			m_LoadIdentity(CurrentT);
			
			context->TexFrameCode++;
			break;
	}
}

void cgl_GLLoadMatrixf(struct GLContextIFace *Self, const GLfloat *m)
{
	GLcontext context = GET_INSTANCE(Self);

	GLASSERT(context != NULL);
	GLFlagError(context, context->CurrentPrimitive != GL_BASE, GL_INVALID_OPERATION);
	
	if (context->transform.CurrentMatrixMode != GL_TEXTURE)
	{
		context->CombinedValid = GL_FALSE;
		context->InvRotValid = GL_FALSE;
		context->InverseModelViewValid = GL_FALSE;
	}
	
	m_LoadMatrixf(CMATRIX(context), m);
	
	if (context->transform.CurrentMatrixMode == GL_TEXTURE)
		context->TexFrameCode++;
	else
		context->FrameCode++;
}

void cgl_GLLoadMatrixd(struct GLContextIFace *Self, const GLdouble *m)
{
	GLcontext context = GET_INSTANCE(Self);

	GLASSERT(context != NULL);
	GLFlagError(context, context->CurrentPrimitive != GL_BASE, GL_INVALID_OPERATION);
	
	if (context->transform.CurrentMatrixMode != GL_TEXTURE)
	{	
		context->CombinedValid = GL_FALSE;
		context->InvRotValid = GL_FALSE;
		context->InverseModelViewValid = GL_FALSE;
	}
	
	m_LoadMatrixd(CMATRIX(context), m);
	
	if (context->transform.CurrentMatrixMode == GL_TEXTURE)
		context->TexFrameCode++;
	else
		context->FrameCode++;
}

void cgl_GLPushMatrix(struct GLContextIFace *Self)
{
	GLcontext context = GET_INSTANCE(Self);

	uint32 tmu;
	GLASSERT(context != NULL);
	
	tmu = context->texture.ActiveTexture;
	
	GLASSERT(context->ProjectionStackPointer <= PROJECTION_STACK_SIZE);
	GLASSERT(context->ModelViewStackPointer  <= MODELVIEW_STACK_SIZE);
	GLASSERT(context->TextureStackPointer[tmu]  <= TEXTURE_STACK_SIZE);
	GLFlagError(context, context->CurrentPrimitive != GL_BASE, GL_INVALID_OPERATION);

	switch (context->transform.CurrentMatrixMode)
	{
		case GL_PROJECTION:
			m_MatCopy(&(context->ProjectionStack[context->ProjectionStackPointer]),
				CurrentP);
			context->ProjectionStackPointer ++;
			break;
		case GL_MODELVIEW:
			m_MatCopy(&(context->ModelViewStack[context->ModelViewStackPointer]),
				CurrentMV);
			context->ModelViewStackPointer ++;
			break;
		case GL_TEXTURE:
			m_MatCopy(&(context->TextureStack[context->TextureStackPointer[tmu]][tmu]),
				CurrentT);
			context->TextureStackPointer[tmu] ++;
			break;
	}
}

void cgl_GLPopMatrix(struct GLContextIFace *Self)
{
	GLcontext context = GET_INSTANCE(Self);

	uint32 tmu;
	GLASSERT(context != NULL);
	
	tmu = context->texture.ActiveTexture;
	
	GLFlagError(context, context->CurrentPrimitive != GL_BASE, GL_INVALID_OPERATION);
	
	if (context->transform.CurrentMatrixMode != GL_TEXTURE)
	{
		context->InvRotValid = GL_FALSE;
		context->CombinedValid = GL_FALSE;
		context->InverseModelViewValid = GL_FALSE;
	}

	
	switch (context->transform.CurrentMatrixMode)
	{
		case GL_PROJECTION:
			if (context->ProjectionStackPointer <= 0)
			{
				context->CurrentError = GL_INVALID_OPERATION;
				return;
			}
			context->ProjectionStackPointer --;
			m_MatCopy(CurrentP,
				&(context->ProjectionStack[context->ProjectionStackPointer]));
				
			context->FrameCode++;
			
			break;
		case GL_MODELVIEW:
			if (context->ModelViewStackPointer <= 0)
			{
				context->CurrentError = GL_INVALID_OPERATION;
				return;
			}
			context->ModelViewStackPointer --;
			m_MatCopy(CurrentMV,
				&(context->ModelViewStack[context->ModelViewStackPointer]));
				
			context->FrameCode++;
			break;
		case GL_TEXTURE:
			if (context->TextureStackPointer[tmu] <= 0)
			{
				context->CurrentError = GL_INVALID_OPERATION;
				return;
			}
			context->TextureStackPointer[tmu] --;
			m_MatCopy(CurrentT,
				&(context->TextureStack[context->TextureStackPointer[tmu]][tmu]));
		
			context->TexFrameCode++;
			break;
	}

}

void cgl_GLFrustum(struct GLContextIFace *Self, GLdouble left, GLdouble right, GLdouble bottom, GLdouble top, GLdouble zNear, GLdouble zFar)
{
	GLcontext context = GET_INSTANCE(Self);

	float v[16];
	float n2 = 2.0*zNear;
	float rli = 1.f / (float)(right-left);
	float tbi = 1.f / (float)(top-bottom);
	float fni = 1.f / (float)(zFar-zNear);

	//LOG(3, glFrustum, "%lf &lf %lf %lf %lf %lf", left, right, bottom, top, zNear, zFar);

	GLASSERT(context != NULL);
	GLFlagError(context, zFar<=0.0 || zNear <=0.0, GL_INVALID_VALUE);
	GLFlagError(context, context->CurrentPrimitive != GL_BASE, GL_INVALID_OPERATION);
	
	if (context->transform.CurrentMatrixMode != GL_TEXTURE)
	{
		context->InvRotValid = GL_FALSE;
		context->CombinedValid = GL_FALSE;
		context->InverseModelViewValid = GL_FALSE;
		
		context->FrameCode++;
	}
	else
		context->TexFrameCode++;

	v[OF_11] = n2*rli; v[OF_21] = 0.0; v[OF_31] = 0.0; v[OF_41] = 0.0;

	v[OF_22] = n2*tbi; v[OF_12] = 0.0; v[OF_32] = 0.0; v[OF_42] = 0.0;

	v[OF_13] = (right+left)*rli; v[OF_23] = (top+bottom)*tbi;
	v[OF_33] = -(zFar+zNear)*fni; v[OF_43] = -1.0;

	v[OF_14] = 0.0; v[OF_24] = 0.0;
	v[OF_34] = -(2.0*zFar*zNear)*fni; v[OF_44] = 0.0;
	m_Mult(CMATRIX(context),
		v,
		MGLMAT_PERSPECTIVE,
		OMATRIX(context)
		);
	SMATRIX(context);

}

void cgl_GLOrtho(struct GLContextIFace *Self, GLdouble left, GLdouble right, GLdouble bottom, GLdouble top, GLdouble zNear, GLdouble zFar)
{
	GLcontext context = GET_INSTANCE(Self);

	float rli = 1.0  / (right-left);
	float tbi = 1.0  / (top-bottom);
	float fni = 1.0 / (zFar-zNear);
	float v[16];

	//LOG(3, glOrtho, "%lf %lf %lf %lf %lf %lf", left, right, bottom, top, zNear, zFar);

	GLASSERT(context != NULL);
	GLFlagError(context, context->CurrentPrimitive != GL_BASE, GL_INVALID_OPERATION);
	
	if (context->transform.CurrentMatrixMode != GL_TEXTURE)
	{
		context->InvRotValid = GL_FALSE;
		context->CombinedValid = GL_FALSE;
		context->InverseModelViewValid = GL_FALSE;
		
		context->FrameCode++;
	}
	else
		context->TexFrameCode++;

	v[OF_11] = 2.0*rli; v[OF_12] = 0.0; v[OF_13] = 0.0;
	v[OF_14] = -(right+left)*rli;

	v[OF_21] = 0.0; v[OF_22] = 2.0*tbi;
	v[OF_23] = 0.0; v[OF_24] = -(top+bottom)*tbi;

	v[OF_31] = 0.0; v[OF_32] = 0.0;
	v[OF_33] = -2.0*fni; v[OF_34] = -(zFar+zNear)*fni;

	v[OF_41] = v[OF_42] = v[OF_43] = 0.0;
	v[OF_44] = 1.0;

	m_Mult(CMATRIX(context), v, MGLMAT_ORTHO, OMATRIX(context));
	SMATRIX(context);
}

void cgl_GLMultMatrixf(struct GLContextIFace *Self, const GLfloat *mat)
{
	GLcontext context = GET_INSTANCE(Self);

	GLASSERT(context != NULL);
	GLFlagError(context, context->CurrentPrimitive != GL_BASE, GL_INVALID_OPERATION);
	
	if (context->transform.CurrentMatrixMode != GL_TEXTURE)
	{
		context->InvRotValid = GL_FALSE;
		context->CombinedValid = GL_FALSE;
		context->InverseModelViewValid = GL_FALSE;
		
		context->FrameCode++;
	}
	else
		context->TexFrameCode++;
		
	m_Mult(CMATRIX(context), (float *)mat, 0, OMATRIX(context));
	SMATRIX(context);
}

void cgl_GLMultMatrixd(struct GLContextIFace *Self, const GLdouble *mat)
{
	GLcontext context = GET_INSTANCE(Self);

	GLfloat v[16];
	int i;

	GLASSERT(context != NULL);
	GLFlagError(context, context->CurrentPrimitive != GL_BASE, GL_INVALID_OPERATION);

	if (context->transform.CurrentMatrixMode != GL_TEXTURE)
	{
		context->InvRotValid = GL_FALSE;
		context->CombinedValid = GL_FALSE;
		context->InverseModelViewValid = GL_FALSE;
		
		context->FrameCode++;
	}
	else
		context->TexFrameCode++;

	for (i=0; i<16; i++)
	{
		v[i] = (GLfloat) *mat++;
	}

	m_Mult(CMATRIX(context), v, 0, OMATRIX(context));
	SMATRIX(context);
}

/*
** The following routine was ripped from the Mesa source code.
** I did use this because it is much better than my original design. The code
** is just modified a bit to adapt to my code layout, but otherwise is unmodified.
** This code is included with the kind permission of Brian Paul, the author of Mesa.
*/
void cgl_GLRotatef(struct GLContextIFace *Self, GLfloat angle, GLfloat x, GLfloat y, GLfloat z)
{
	GLcontext context = GET_INSTANCE(Self);

	float v[16];
	#define v(x) v[OF_##x]
	float mag, s, c;
	float xx,yy,zz,xy,yz,zx,xs,ys,zs,one_c;

	//LOG(3, glRotatef, "%f %f %f %f", angle, x,y,z);

	GLASSERT(context != NULL);
	GLFlagError(context, context->CurrentPrimitive != GL_BASE, GL_INVALID_OPERATION);

	if (context->transform.CurrentMatrixMode != GL_TEXTURE)
	{
		context->InvRotValid = GL_FALSE;
		context->CombinedValid = GL_FALSE;
		context->InverseModelViewValid = GL_FALSE;
		
		context->FrameCode++;
	}
	else
		context->TexFrameCode++;

	s = (float)sin(angle * PI/180.0);
	c = (float)cos(angle * PI/180.0);
	mag = sqrt(x*x+y*y+z*z);

	if (mag == 0.0) return;

	if (mag != 1.0)
	{
		x /= mag;
		y /= mag;
		z /= mag;
	}

	xx = x*x;
	yy = y*y;
	zz = z*z;
	xy = x*y;
	yz = y*z;
	zx = z*x;
	xs = x*s;
	ys = y*s;
	zs = z*s;
	one_c = 1.f - c;

	v(11) = one_c*xx + c;
	v(12) = one_c*xy - zs;
	v(13) = one_c*zx + ys;
	v(14) = 0.f;

	v(21) = one_c*xy + zs;
	v(22) = one_c*yy + c;
	v(23) = one_c*yz - xs;
	v(24) = 0.f;

	v(31) = one_c*zx - ys;
	v(32) = one_c*yz + xs;
	v(33) = one_c*zz + c;
	v(34) = 0.f;

	v(41) = 0.f;
	v(42) = 0.f;
	v(43) = 0.f;
	v(44) = 1.f;

	m_Mult(CMATRIX(context), v, MGLMAT_ROTATION, OMATRIX(context));
	SMATRIX(context);
	#undef v
}

void cgl_GLScalef(struct GLContextIFace *Self, GLfloat x, GLfloat y, GLfloat z)
{
	GLcontext context = GET_INSTANCE(Self);

	float v[16];
	//LOG(3, glScalef, "%f %f %f", x,y,z);
	GLASSERT(context!=NULL);

	if (context->transform.CurrentMatrixMode != GL_TEXTURE)
	{
		context->InvRotValid = GL_FALSE;
		context->CombinedValid = GL_FALSE;
		context->InverseModelViewValid = GL_FALSE;
		
		context->FrameCode++;
	}
	else
		context->TexFrameCode++;
	
	#define v(x) v[OF_##x]
	v(11) = x;   v(12) = 0.0; v(13) = 0.0; v(14) = 0.0;
	v(21) = 0.0; v(22) = y;   v(23) = 0.0; v(24) = 0.0;
	v(31) = 0.0; v(32) = 0.0; v(33) = z;   v(34) = 0.0;
	v(41) = 0.0; v(42) = 0.0; v(43) = 0.0; v(44) = 1.0;
	m_Mult(CMATRIX(context), v, MGLMAT_GENERAL_SCALE, OMATRIX(context));
	SMATRIX(context);
	#undef v
}


void cgl_GLTranslatef(struct GLContextIFace *Self, GLfloat x, GLfloat y, GLfloat z)
{
	float vv[16];
	GLcontext context = GET_INSTANCE(Self);

	GLASSERT(context != NULL);

	if (context->transform.CurrentMatrixMode != GL_TEXTURE)
	{
		context->InvRotValid = GL_FALSE;
		context->CombinedValid = GL_FALSE;
		context->InverseModelViewValid = GL_FALSE;
		
		context->FrameCode++;
	}
	else
		context->TexFrameCode++;

	#define v(x) vv[OF_##x]
	v(11) = v(22) = v(33) = v(44) = 1.f;
	v(21) = v(31) = v(41) = v(12) = v(32) = v(42) = v(13) = v(23) = v(43) = 0.f;
	v(14) = (float)x; v(24) = (float)y; v(34) = (float)z;
	#undef v
	m_Mult(CMATRIX(context), vv, MGLMAT_TRANSLATION, OMATRIX(context));
	SMATRIX(context);
}

void GLMatrixInit(GLcontext context)
{ 
	int i;
	uint32 current = context->texture.ActiveTexture;
	struct GLContextIFace *Self = context->Self;
	
	context->ModelViewStackPointer = 0;
	context->ProjectionStackPointer = 0;
	context->ModelViewNr = 0;
	context->ProjectionNr = 0;

	for (i = 0; i < MAX_TEXTURE_UNITS; i++)
	{
		context->texture.ActiveTexture = i;
		context->TextureStackPointer[i] = 0;
		context->TextureNr[i] = 0;
		Self->GLMatrixMode(GL_TEXTURE);
		Self->GLLoadIdentity();	
	}
	
	context->texture.ActiveTexture = current;
	
	context->CombinedValid = GL_FALSE;
	context->InvRotValid = GL_FALSE;
	context->InverseModelViewValid = GL_FALSE;
	
	Self->GLMatrixMode(GL_PROJECTION);
	Self->GLLoadIdentity();
	Self->GLMatrixMode(GL_MODELVIEW);
	Self->GLLoadIdentity();
}