#include <math.h>
#include "sp_matrix.h"
#include "sp_state.h"
#include "sp_viewport.h"

/***************************** local constants *****************************/

#define SP_CURRENT_MATRIX	0x0023

/***************************** local variables *****************************/

static SPfloat ModelViewStack[1][16];

static SPfloat ProjectionStack[1][16];

/*static*/ SPfloat* StackPointer[2] = { ModelViewStack[0], ProjectionStack[0] };

static SPboolean WorldInvalid = SP_TRUE;

static SPfloat WorldMatrix[16];

/***************************** local functions *****************************/

static SPvoid spMatrixMultiply( SPfloat* left, SPfloat* right, SPfloat* output )
{
	SPsize r, c, i;
	static SPfloat result[16];
	for( r = 0; r < 4; r++ )
	{
		for( c = 0; c < 16; c += 4 )
		{
			for( i = 0; i < 4; i++ )
			{
				result[r+c] += ( left[r+(i<<2)] * right[i+c] );
			}
		}
	}
	for( i = 0; i < 16; i++ )
	{
		output = result; 
		result = 0;
	}
	return;
}

static SPvoid spMultMatrixVertex( SPfloat* matrix, SPvertex* in, SPvertex* out )
{
    SPint i;
	SPfloat* result;
    for( i = 0, result = &out->X; i < 4; i++, result++ ) 
	{
		*result = ( ( in->X * matrix ) + ( in->Y * matrix[4+i] ) + ( in->Z * matrix[8+i] ) + ( in->W * matrix[12+i] ) );
    }
	return;
}

/***************************** global functions *****************************/

SPvoid spLoadIdentity( SPvoid )
{
	SPfloat identity[16] = { 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1 };
	spLoadMatrix( identity );
	return;
}

SPvoid spLoadMatrix( SPfloat* matrix )
{
	SPsize i;
	SPfloat* current;
	current = spGetStateV( SP_CURRENT_MATRIX );
	for( i = 0; i < 16; i++ )
	{
		current = matrix;
	}
	WorldInvalid = SP_TRUE;
	return;
}

SPvoid spMatrixMode( SPenum mode )
{
	spSetStateI( SP_MATRIX_MODE, mode );
	spSetStateV( SP_CURRENT_MATRIX, StackPointer[mode&1] );
	return;
}

SPvoid spMultMatrix( SPfloat* right )
{
	SPfloat* left;
	left = spGetStateV( SP_CURRENT_MATRIX );
	spMatrixMultiply( left, right, left );
	WorldInvalid = SP_TRUE;
	return;
}

SPvoid spPerspective( SPfloat fovy, SPfloat aspect, SPfloat znear, SPfloat zfar )
{
	SPfloat cot, radians;
	static SPfloat perspective[16] = { 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1 };
	radians = ( ( fovy * 0.5f ) * ( 3.14159f / 180.0f ) );
	cot = (SPfloat)( cos( radians ) / sin( radians ) );
	perspective[0]  = ( cot / aspect );
	perspective[5]  = cot;
	perspective[10] = ( -( zfar + znear ) / ( zfar - znear ) ); 
	perspective[11] = -1.0f;
	perspective[14] = ( ( -2.0f * znear * zfar ) / ( zfar - znear ) );
	perspective[15] = 0.0f;
	spMultMatrix( perspective );
	Viewport.ZFar = zfar;
	Viewport.ZNear = znear;
	return;
}


SPvoid spProject( SPvertex* input, SPvertex* output, SPsize count )
{
	SPsize i;
	SPvertex temp;
	for( i = 0; i < count; i++, input++, output++ )
	{
		input->W = 1.0f;
		spMultMatrixVertex( StackPointer[0], input, &temp );
		spMultMatrixVertex( StackPointer[1], &temp, output );
		if( output->W == 0.0f )
		{
			continue;
		}
		output->X /= output->W;
		output->X = ( ( output->X * 0.5f ) + 0.5f );
		output->X *= Viewport.Width;
		output->Y /= output->W;
		output->Y = ( ( output->Y * 0.5f ) + 0.5f );
		output->Y *= Viewport.Height;
		output->Z = output->W;
	}
	return;
}

SPvoid spTranslate( SPfloat x, SPfloat y, SPfloat z )
{
	static SPfloat translation[16] = { 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1 };
	translation[12] = x;
	translation[13] = y;
	translation[14] = z;
	spMultMatrix( translation );	
	return;
}