/*********************************************************
 *	FILE NAME: kmtrfrm.c
 *
 *
 *
 *
 *
 *********************************************************/

/* $Revision:$ */

#define _SH4_

#define _STRICT_UNION_
#define _KM_USE_VERTEX_MACRO_L3_
/* #define _KM_USE_VERTEX_MACRO_ */
#include <math.h>
#include <private.h>
#include <Machine.h>

#include "kmgeo.h"
#include "kamuix.h"

/* Global Vars */
static	float fScale;			/* Scaling Coeff */
static	float fInvScale;		/* Inverse Scale factor */
static	float OrgX, OrgY;		/* スクリーン座標の零点 */
static	float SxDash, SyDash;	/* ズーム係数 */

unsigned long	BiBuffer[70];
#define	CurrentMatrix	((KMGMATRIX *)(((unsigned long)BiBuffer+7)&0xFFFFFFF8))

unsigned long	BiBuffer2[10];
#define	TempVector1		((float *)(((unsigned long)BiBuffer2+7)&0xFFFFFFF8))

unsigned long	BiBuffer3[10];
#define	TempVector2		((float *)(((unsigned long)BiBuffer3+7)&0xFFFFFFF8))

unsigned long	BiBuffer4[10];
#define	LightCoeff		(*((kmgVector *)(((unsigned long)BiBuffer4+7)&0xFFFFFFF8)))

#if	0
#define	FIPR(A,B)	((A)[0]*(B)[0]+(A)[1]*(B)[1]+(A)[2]*(B)[2]+(A)[3]*(B)[3])
#define	FIPR3(A,B)	((A)[0]*(B)[0]+(A)[1]*(B)[1]+(A)[2]*(B)[2])
#else
float	FIPR(float *A,float *B)
{
	return	((A)[0]*(B)[0]+(A)[1]*(B)[1]+(A)[2]*(B)[2]+(A)[3]*(B)[3]);
}

#pragma	inline(FIPR3)
float	FIPR3(float *A,float *B)
{
	return	((A)[0]*(B)[0]+(A)[1]*(B)[1]+(A)[2]*(B)[2]);
}

#endif

#if !defined(_SH4_)

/*
 *	SH4 Build-in Functions for X86
 *
 */
float	FIPR(float	*fVector, float *fVector)
{
	float	fRetValue = 0;

	fRetValue = fVector[0]*fVector[0]+
				fVector[1]*fVector[1]+
				fVector[2]*fVector[2]+
				fVector[3]*fVector[3];

	return	fRetValue;
}

float	fsrra(float	fValue)
{
	float	fRetValue = 1.0f/sqrt(fValue);

	return	fRetValue;
}
#endif

/* Identity Set */
void kmgSetIdentity()
{
	CurrentMatrix->_11 = 1.0f;CurrentMatrix->_12 = 0.0f;CurrentMatrix->_13 = 0.0f;CurrentMatrix->_14 = 0.0f;
	CurrentMatrix->_21 = 0.0f;CurrentMatrix->_22 = 1.0f;CurrentMatrix->_23 = 0.0f;CurrentMatrix->_24 = 0.0f;
	CurrentMatrix->_31 = 0.0f;CurrentMatrix->_32 = 0.0f;CurrentMatrix->_33 = 1.0f;CurrentMatrix->_34 = 0.0f;
	CurrentMatrix->_41 = 0.0f;CurrentMatrix->_42 = 0.0f;CurrentMatrix->_43 = 0.0f;CurrentMatrix->_44 = 1.0f;

	CurrentMatrix->L11 = 1.0f;CurrentMatrix->L12 = 0.0f;CurrentMatrix->L13 = 0.0f;CurrentMatrix->L14 = 0.0f;
	CurrentMatrix->L21 = 0.0f;CurrentMatrix->L22 = 1.0f;CurrentMatrix->L23 = 0.0f;CurrentMatrix->L24 = 0.0f;
	CurrentMatrix->L31 = 0.0f;CurrentMatrix->L32 = 0.0f;CurrentMatrix->L33 = 1.0f;CurrentMatrix->L34 = 0.0f;
	CurrentMatrix->L41 = 0.0f;CurrentMatrix->L42 = 0.0f;CurrentMatrix->L43 = 0.0f;CurrentMatrix->L44 = 0.0f;
}

/* Scaling */
void kmgScale(kmgVector sca_vec)
{
	CurrentMatrix->_11 *= sca_vec[0];
	CurrentMatrix->_21 *= sca_vec[0];
	CurrentMatrix->_31 *= sca_vec[0];
	CurrentMatrix->_41 *= sca_vec[0];
	CurrentMatrix->_12 *= sca_vec[1];
	CurrentMatrix->_22 *= sca_vec[1];
	CurrentMatrix->_32 *= sca_vec[1];
	CurrentMatrix->_42 *= sca_vec[1];
	CurrentMatrix->_13 *= sca_vec[2];
	CurrentMatrix->_23 *= sca_vec[2];
	CurrentMatrix->_33 *= sca_vec[2];
	CurrentMatrix->_43 *= sca_vec[2];
}

/* Translate */
void kmgTranslate(kmgVector tran_vec)
{
	CurrentMatrix->_11 += (tran_vec[0] * CurrentMatrix->_14);
	CurrentMatrix->_21 += (tran_vec[0] * CurrentMatrix->_24);
	CurrentMatrix->_31 += (tran_vec[0] * CurrentMatrix->_34);
	CurrentMatrix->_41 += (tran_vec[0] * CurrentMatrix->_44);
	CurrentMatrix->_12 += (tran_vec[1] * CurrentMatrix->_14);
	CurrentMatrix->_22 += (tran_vec[1] * CurrentMatrix->_24);
	CurrentMatrix->_32 += (tran_vec[1] * CurrentMatrix->_34);
	CurrentMatrix->_42 += (tran_vec[1] * CurrentMatrix->_44);
	CurrentMatrix->_13 += (tran_vec[2] * CurrentMatrix->_14);
	CurrentMatrix->_23 += (tran_vec[2] * CurrentMatrix->_24);
	CurrentMatrix->_33 += (tran_vec[2] * CurrentMatrix->_34);
	CurrentMatrix->_43 += (tran_vec[2] * CurrentMatrix->_44);
}

/* Rotation */
void	kmgRotate(float angle, int axis)
{
	float fCos, fSin;
	float temp1, temp2;
	unsigned long ulSave;

	fCos = (float)cos(PI * angle / 180.0f);
	fSin = (float)sin(PI * angle / 180.0f);

	switch (axis){
		case X_AXIS:
#if defined(_SH4_)
            ld_ext((void *) CurrentMatrix);

			TempVector1[0] = 0.0;
			TempVector1[1] = fCos;
			TempVector1[2] = -fSin;
			TempVector1[3] = 0.0;
			ftrv(TempVector1, TempVector1);

			TempVector2[0] = 0.0;
			TempVector2[1] = fSin;
			TempVector2[2] = fCos;
			TempVector2[3] = 0.0;
			ftrv(TempVector2, TempVector2);

			CurrentMatrix->_12 = TempVector1[0];
			CurrentMatrix->_13 = TempVector2[0];
			CurrentMatrix->_22 = TempVector1[1];
			CurrentMatrix->_23 = TempVector2[1];
			CurrentMatrix->_32 = TempVector1[2];
			CurrentMatrix->_33 = TempVector2[2];
			CurrentMatrix->_42 = TempVector1[3];
			CurrentMatrix->_43 = TempVector2[3];

            ld_ext((void *) &CurrentMatrix->L11);
			TempVector1[0] = 0.0;
			TempVector1[1] = fCos;
			TempVector1[2] = -fSin;
			TempVector1[3] = 0.0;
			ftrv(TempVector1, TempVector1);
			TempVector2[0] = 0.0;
			TempVector2[1] = fSin;
			TempVector2[2] = fCos;
			TempVector2[3] = 0.0;
			ftrv(TempVector2, TempVector2);

			CurrentMatrix->L12 = TempVector1[0];
			CurrentMatrix->L13 = TempVector2[0];
			CurrentMatrix->L22 = TempVector1[1];
			CurrentMatrix->L23 = TempVector2[1];
			CurrentMatrix->L32 = TempVector1[2];
			CurrentMatrix->L33 = TempVector2[2];
#else
			temp1 = CurrentMatrix->_12 * fCos - CurrentMatrix->_13 * fSin;
			temp2 = CurrentMatrix->_12 * fSin + CurrentMatrix->_13 * fCos;
			CurrentMatrix->_12 = temp1;
			CurrentMatrix->_13 = temp2;
			temp1 = CurrentMatrix->_22 * fCos - CurrentMatrix->_23 * fSin;
			temp2 = CurrentMatrix->_22 * fSin + CurrentMatrix->_23 * fCos;
			CurrentMatrix->_22 = temp1;
			CurrentMatrix->_23 = temp2;
			temp1 = CurrentMatrix->_32 * fCos - CurrentMatrix->_33 * fSin;
			temp2 = CurrentMatrix->_32 * fSin + CurrentMatrix->_33 * fCos;
			CurrentMatrix->_32 = temp1;
			CurrentMatrix->_33 = temp2;
			temp1 = CurrentMatrix->_42 * fCos - CurrentMatrix->_43 * fSin;
			temp2 = CurrentMatrix->_42 * fSin + CurrentMatrix->_43 * fCos;
			CurrentMatrix->_42 = temp1;
			CurrentMatrix->_43 = temp2;

			temp1 = CurrentMatrix->L12 * fCos - CurrentMatrix->L13 * fSin;
			temp2 = CurrentMatrix->L12 * fSin + CurrentMatrix->L13 * fCos;
			CurrentMatrix->L12 = temp1;
			CurrentMatrix->L13 = temp2;
			temp1 = CurrentMatrix->L22 * fCos - CurrentMatrix->L23 * fSin;
			temp2 = CurrentMatrix->L22 * fSin + CurrentMatrix->L23 * fCos;
			CurrentMatrix->L22 = temp1;
			CurrentMatrix->L23 = temp2;
			temp1 = CurrentMatrix->L32 * fCos - CurrentMatrix->L33 * fSin;
			temp2 = CurrentMatrix->L32 * fSin + CurrentMatrix->L33 * fCos;
			CurrentMatrix->L32 = temp1;
			CurrentMatrix->L33 = temp2;
#endif

			break;

		case Y_AXIS:
#if defined(_SH4_)
            ld_ext((void *) CurrentMatrix);

			TempVector1[0] = fCos;
			TempVector1[1] = 0.0;
			TempVector1[2] = fSin;
			TempVector1[3] = 0.0;
			ftrv(TempVector1, TempVector1);

			TempVector2[0] = -fSin;
			TempVector2[1] = 0.0;
			TempVector2[2] = fCos;
			TempVector2[3] = 0.0;
			ftrv(TempVector2, TempVector2);

			CurrentMatrix->_11 = TempVector1[0];
			CurrentMatrix->_13 = TempVector2[0];
			CurrentMatrix->_21 = TempVector1[1];
			CurrentMatrix->_23 = TempVector2[1];
			CurrentMatrix->_31 = TempVector1[2];
			CurrentMatrix->_33 = TempVector2[2];
			CurrentMatrix->_41 = TempVector1[3];
			CurrentMatrix->_43 = TempVector2[3];

            ld_ext((void *) &CurrentMatrix->L11);
			TempVector1[0] = fCos;
			TempVector1[1] = 0.0;
			TempVector1[2] = fSin;
			TempVector1[3] = 0.0;
			ftrv(TempVector1, TempVector1);

			TempVector2[0] = -fSin;
			TempVector2[1] = 0.0;
			TempVector2[2] = fCos;
			TempVector2[3] = 0.0;
			ftrv(TempVector2, TempVector2);

			CurrentMatrix->L11 = TempVector1[0];
			CurrentMatrix->L13 = TempVector2[0];
			CurrentMatrix->L21 = TempVector1[1];
			CurrentMatrix->L23 = TempVector2[1];
			CurrentMatrix->L31 = TempVector1[2];
			CurrentMatrix->L33 = TempVector2[2];
#else
			temp1 = CurrentMatrix->_11 * fCos + CurrentMatrix->_13 * fSin;
			temp2 =-CurrentMatrix->_11 * fSin + CurrentMatrix->_13 * fCos;
			CurrentMatrix->_11 = temp1;
			CurrentMatrix->_13 = temp2;
			temp1 = CurrentMatrix->_21 * fCos + CurrentMatrix->_23 * fSin;
			temp2 =-CurrentMatrix->_21 * fSin + CurrentMatrix->_23 * fCos;
			CurrentMatrix->_21 = temp1;
			CurrentMatrix->_23 = temp2;
			temp1 = CurrentMatrix->_31 * fCos + CurrentMatrix->_33 * fSin;
			temp2 =-CurrentMatrix->_31 * fSin + CurrentMatrix->_33 * fCos;
			CurrentMatrix->_31 = temp1;
			CurrentMatrix->_33 = temp2;
			temp1 = CurrentMatrix->_41 * fCos + CurrentMatrix->_43 * fSin;
			temp2 =-CurrentMatrix->_41 * fSin + CurrentMatrix->_43 * fCos;
			CurrentMatrix->_41 = temp1;
			CurrentMatrix->_43 = temp2;

			temp1 = CurrentMatrix->L11 * fCos + CurrentMatrix->L13 * fSin;
			temp2 =-CurrentMatrix->L11 * fSin + CurrentMatrix->L13 * fCos;
			CurrentMatrix->L11 = temp1;
			CurrentMatrix->L13 = temp2;
			temp1 = CurrentMatrix->L21 * fCos + CurrentMatrix->L23 * fSin;
			temp2 =-CurrentMatrix->L21 * fSin + CurrentMatrix->L23 * fCos;
			CurrentMatrix->L21 = temp1;
			CurrentMatrix->L23 = temp2;
			temp1 = CurrentMatrix->L31 * fCos + CurrentMatrix->L33 * fSin;
			temp2 =-CurrentMatrix->L31 * fSin + CurrentMatrix->L33 * fCos;
			CurrentMatrix->L31 = temp1;
			CurrentMatrix->L33 = temp2;
#endif
			break;

		case Z_AXIS:
#if defined(_SH4_)
            ld_ext((void *) CurrentMatrix);

			TempVector1[0] = fCos;
			TempVector1[1] = -fSin;
			TempVector1[2] = 0.0;
			TempVector1[3] = 0.0;
			ftrv(TempVector1, TempVector1);

			TempVector2[0] = fSin;
			TempVector2[1] = fCos;
			TempVector2[2] = 0.0;
			TempVector2[3] = 0.0;
			ftrv(TempVector2, TempVector2);

			CurrentMatrix->_11 = TempVector1[0];
			CurrentMatrix->_12 = TempVector2[0];
			CurrentMatrix->_21 = TempVector1[1];
			CurrentMatrix->_22 = TempVector2[1];
			CurrentMatrix->_31 = TempVector1[2];
			CurrentMatrix->_32 = TempVector2[2];
			CurrentMatrix->_41 = TempVector1[3];
			CurrentMatrix->_42 = TempVector2[3];

            ld_ext((void *) &CurrentMatrix->L11);
			TempVector1[0] = fCos;
			TempVector1[1] = -fSin;
			TempVector1[2] = 0.0;
			TempVector1[3] = 0.0;
			ftrv(TempVector1, TempVector1);

			TempVector2[0] = fSin;
			TempVector2[1] = fCos;
			TempVector2[2] = 0.0;
			TempVector2[3] = 0.0;
			ftrv(TempVector2, TempVector2);

			CurrentMatrix->L11 = TempVector1[0];
			CurrentMatrix->L12 = TempVector2[0];
			CurrentMatrix->L21 = TempVector1[1];
			CurrentMatrix->L22 = TempVector2[1];
			CurrentMatrix->L31 = TempVector1[2];
			CurrentMatrix->L32 = TempVector2[2];
#else
			temp1 = CurrentMatrix->_11 * fCos - CurrentMatrix->_12 * fSin;
			temp2 = CurrentMatrix->_11 * fSin + CurrentMatrix->_12 * fCos;
			CurrentMatrix->_11 = temp1;
			CurrentMatrix->_12 = temp2;
			temp1 = CurrentMatrix->_21 * fCos - CurrentMatrix->_22 * fSin;
			temp2 = CurrentMatrix->_21 * fSin + CurrentMatrix->_22 * fCos;
			CurrentMatrix->_21 = temp1;
			CurrentMatrix->_22 = temp2;
			temp1 = CurrentMatrix->_31 * fCos - CurrentMatrix->_32 * fSin;
			temp2 = CurrentMatrix->_31 * fSin + CurrentMatrix->_32 * fCos;
			CurrentMatrix->_31 = temp1;
			CurrentMatrix->_32 = temp2;
			temp1 = CurrentMatrix->_41 * fCos - CurrentMatrix->_42 * fSin;
			temp2 = CurrentMatrix->_41 * fSin + CurrentMatrix->_42 * fCos;
			CurrentMatrix->_41 = temp1;
			CurrentMatrix->_42 = temp2;

			temp1 = CurrentMatrix->L11 * fCos - CurrentMatrix->L12 * fSin;
			temp2 = CurrentMatrix->L11 * fSin + CurrentMatrix->L12 * fCos;
			CurrentMatrix->L11 = temp1;
			CurrentMatrix->L12 = temp2;
			temp1 = CurrentMatrix->L21 * fCos - CurrentMatrix->L22 * fSin;
			temp2 = CurrentMatrix->L21 * fSin + CurrentMatrix->L22 * fCos;
			CurrentMatrix->L21 = temp1;
			CurrentMatrix->L22 = temp2;
			temp1 = CurrentMatrix->L31 * fCos - CurrentMatrix->L32 * fSin;
			temp2 = CurrentMatrix->L31 * fSin + CurrentMatrix->L32 * fCos;
			CurrentMatrix->L31 = temp1;
			CurrentMatrix->L32 = temp2;
#endif
			break;
	}
}

void SetView()
{
	/* Do nothing yet */
}

/* Set Light */
#pragma	inline(kmgSetLight)
void kmgSetLight(LPKMGLIGHT pkmgLight)
{
	float fInvCoeff;

	if ( pkmgLight->Mode & KMG_LIGHT_PARALLEL ) {

		LightCoeff[KMG_X] = pkmgLight->ParallelLightDir[KMG_X];
		LightCoeff[KMG_Y] = pkmgLight->ParallelLightDir[KMG_Y];
		LightCoeff[KMG_Z] = pkmgLight->ParallelLightDir[KMG_Z];
		LightCoeff[3] = 0.0;

		fInvCoeff = fipr(LightCoeff, LightCoeff);
		fInvCoeff = fsrra(fInvCoeff);

		LightCoeff[KMG_X] *= fInvCoeff;
		LightCoeff[KMG_Y] *= fInvCoeff;
		LightCoeff[KMG_Z] *= fInvCoeff;
	}
	else if ( pkmgLight->Mode & KMG_LIGHT_POINT ) {
		/* Do nothing */
	}

	return;
}

void kmgSetScreen(float orgx, float orgy, float sdx, float sdy, float Scale)
{
	OrgX = orgx;
	OrgY = orgy;
	SxDash = sdx;
	SyDash = sdy;
	fInvScale = 1.0f/Scale;
}

/****************************************************************
 *	FUNCTION: kmgDraw
 *	DESCRIPTION: Kamui Geometory Draw Function
 *	ORIGINAL CODEING: by Imanishi(NEC)
 *	LAST UPDATED: by K.Sasaki(NMIT)
 ****************************************************************/
void kmgDraw(
	LPKMGLIGHT pkmgLight,
	int		   iNLight,
	PKMVERTEXBUFFDESC pVertexBufferDesc,
	KMGVERTEX *Vertices,
	int numTriangle)
{
	float fXtemp, fYtemp, fZtemp, f1OverZ;
	int i,j;
	float fInten;
	float red, green, blue;

	kmxxGetCurrentPtr(pVertexBufferDesc);
	kmxxStartVertexStrip(pVertexBufferDesc);

	kmgSetLight(pkmgLight);

	for(i = 0; i < numTriangle ; i++) {
		int	iEndFlag = 0;
		while ( iEndFlag == 0) {

			TempVector1[0] = Vertices->fX;
			TempVector1[1] = Vertices->fY;
			TempVector1[2] = Vertices->fZ;
			TempVector1[3] = 1.0;

			fXtemp = fipr(TempVector1, &CurrentMatrix->_11);
			fYtemp = fipr(TempVector1, &CurrentMatrix->_12);
			fZtemp = fipr(TempVector1, &CurrentMatrix->_13);

			TempVector1[0] = Vertices->NormX;
			TempVector1[1] = Vertices->NormY;
			TempVector1[2] = Vertices->NormZ;
			TempVector1[3] = 0.0;

			TempVector2[0] = fipr(TempVector1, &CurrentMatrix->L11);
			TempVector2[1] = fipr(TempVector1, &CurrentMatrix->L12);
			TempVector2[2] = fipr(TempVector1, &CurrentMatrix->L13);
			TempVector2[3] = 0.0;

			if ( pkmgLight->Mode & KMG_LIGHT_PARALLEL ) {
				fInten = FIPR(TempVector2, LightCoeff);
				fInten *= pkmgLight->ParallelLightIntensity;

				if ( fInten <= 0.0 ) {
					red	  = pkmgLight->AmbientColor[KMG_COLOR_R]
						  * Vertices->MaterialColor[KMG_COLOR_R];
					green = pkmgLight->AmbientColor[KMG_COLOR_G]
						  * Vertices->MaterialColor[KMG_COLOR_G];
					blue  = pkmgLight->AmbientColor[KMG_COLOR_B]
						  * Vertices->MaterialColor[KMG_COLOR_B];
				} else {
					red	  = (pkmgLight->AmbientColor[KMG_COLOR_R] + fInten * pkmgLight->ParallelLightColor[KMG_COLOR_R])
							* Vertices->MaterialColor[KMG_COLOR_R];
					green = (pkmgLight->AmbientColor[KMG_COLOR_G] + fInten * pkmgLight->ParallelLightColor[KMG_COLOR_G])
							* Vertices->MaterialColor[KMG_COLOR_G];
					blue  = (pkmgLight->AmbientColor[KMG_COLOR_B] + fInten * pkmgLight->ParallelLightColor[KMG_COLOR_B])
							* Vertices->MaterialColor[KMG_COLOR_B];
				}
			}
			else if ( pkmgLight->Mode & KMG_LIGHT_POINT ) {
				float	fDist;
				float	fTemp;

				TempVector1[0] = pkmgLight->PointLightPosition[KMG_X] - fXtemp;
				TempVector1[1] = pkmgLight->PointLightPosition[KMG_Y] - fYtemp;
				TempVector1[2] = pkmgLight->PointLightPosition[KMG_Z] - fZtemp;
				fDist = fipr(TempVector1,TempVector1);
				if (fDist < 0.00001) fDist = 0.00001;

				fTemp = fsrra(fDist);
				if ( pkmgLight->Mode & KMG_LIGHT_LINEAR ) {
					fDist = fTemp;
				}
				else {
					fDist = 1.0/fDist;
				}
				fTemp *= FIPR( TempVector1, TempVector2 );

				fInten = fTemp * fDist * pkmgLight->PointLightIntensity;
				if ( fInten < 0.0 ) fInten = 0.0;

				red	  = (pkmgLight->AmbientColor[KMG_COLOR_R] + fInten * pkmgLight->PointLightColor[KMG_COLOR_R])
							* Vertices->MaterialColor[KMG_COLOR_R];
				green = (pkmgLight->AmbientColor[KMG_COLOR_G] + fInten * pkmgLight->PointLightColor[KMG_COLOR_G])
							* Vertices->MaterialColor[KMG_COLOR_G];
				blue  = (pkmgLight->AmbientColor[KMG_COLOR_B] + fInten * pkmgLight->PointLightColor[KMG_COLOR_B])
							* Vertices->MaterialColor[KMG_COLOR_B];
			}

			/* 1/w座標系への変換 */
			fZtemp *= fInvScale;
			f1OverZ = 1.0f / fZtemp;

			kmxxSetVertex_5(Vertices->ParamWord,
							(SxDash * fXtemp * f1OverZ) + OrgX,
							(SyDash * fYtemp * f1OverZ) + OrgY,
							f1OverZ,
							Vertices->fU, Vertices->fV,
							Vertices->MaterialColor[KMG_COLOR_A],
							red, green, blue,
							Vertices->OffsetColor[KMG_COLOR_A],
							Vertices->OffsetColor[KMG_COLOR_R],
							Vertices->OffsetColor[KMG_COLOR_G],
							Vertices->OffsetColor[KMG_COLOR_B]
							);

			if ( Vertices->ParamWord == KM_VERTEXPARAM_ENDOFSTRIP ) {
				iEndFlag = 1;
			}
			Vertices++;
		}
	}

	kmxxReleaseCurrentPtr(pVertexBufferDesc);

	return;
}