/*---------------------------------------------------------------------------
 *	Import Headers
\*--------------------------------------------------------------------------*/

#ifdef PSX_VERSION
#include <gtemac.h>
#include <inline_c.h>
#include "../spec_psx/typedefs.h"
#include "../spec_psx/typedefs.h"
#include "../spec_psx/memory.h"
#else
#include "../specific/stypes.h"
#endif

#include "anim.h"
#include "draw.h"
#include "sphere.h"

extern sint16 null_rotations[];
extern sint32	*IMptr;
extern sint32	IMstack[12*64];
extern sint32	IM_frac;
extern sint32	IM_rate;

/******************************************************************************
 *		Do Hierarchy Collision Tests against lara
 *			returns true if Collision else false
 *****************************************************************************/
int		TestCollision( ITEM_INFO *item, ITEM_INFO *laraitem )
{
/* NOTES: This creates the slist_lara[] every time this function is called. Might be better to store the result and flag it
	so that if called again this frame, won't recalc the spheres again */
	int		num1;
	int		num2;
	int		x1,y1,z1,r1;
	int		x,y,z,r;
	int		i,j;
	uint32	flags;
	SPHERE	*ptr1;
	SPHERE	*ptr2;
	SPHERE	slist_baddie[34];          	// Baddie Spheres
	SPHERE	slist_lara[34];				// Lara Spheres


	flags = 0;
	num1 = GetSpheres( item,&slist_baddie[0],1 );
	num2 = GetSpheres( laraitem,&slist_lara[0],1 );
	ptr1 = &slist_baddie[0];
	for ( i=0;i<num1;i++,ptr1++ )
	{
		x1 = ptr1->x;								// Get Baddie Sphere...
		y1 = ptr1->y;
		z1 = ptr1->z;
		r1 = ptr1->r;
		if ( r1>0 )
		{
			ptr2 = &slist_lara[0];
			for ( j=0;j<num2;j++,ptr2++ )           // Compare it to all Lara spheres!!!
			{
				if ( ptr2->r>0 )
				{
					x = ptr2->x - x1;
					y = ptr2->y - y1;
					z = ptr2->z - z1;
					r = ptr2->r + r1;
					if ( (x*x + y*y + z*z) < (r*r) )
					{
						flags |= (1<<i);
						break;
					}
				}
			}
		}
	}
	item->touch_bits = flags;
	return( flags );
}

/*****************************************************************************
 *			Get Spheres( 3D Coords, Radius ) for colliding with..
 *				If WorldSpace is Set Coordinates are in Absolute World Space
 *              Else  Coordinates are in Relative View Space
 ****************************************************************************/
int		GetSpheres( ITEM_INFO *item, SPHERE *ptr, int WorldSpace )
{
	int 		i,poppush;
	OBJECT_INFO *object;
	sint32 		*bone;
	sint16		*rotation;
	sint16 		*frame;
	sint32		x,y,z;
	sint16		*objptr,**meshpp;
	sint16		*extra_rotation;

	if ( item==NULL )
		return(0);

	if ( WorldSpace )
	{
		x = item->pos.x_pos;
		y = item->pos.y_pos;
		z = item->pos.z_pos;
		phd_PushUnitMatrix();
#ifdef PSX_VERSION
		mSetTrans(0, 0, 0);
#else
		*(phd_mxptr+M03) = 0;
		*(phd_mxptr+M13) = 0;
		*(phd_mxptr+M23) = 0;
#endif
	}
	else
	{
		x = y = z = 0;
		phd_PushMatrix();
		phd_TranslateAbs( item->pos.x_pos,item->pos.y_pos,item->pos.z_pos );
	}
	phd_RotYXZ(item->pos.y_rot, item->pos.x_rot, item->pos.z_rot);
	frame = GetBestFrame( item );
	phd_TranslateRel((sint32)*(frame+6), (sint32)*(frame+7), (sint32)*(frame+8));
	rotation = frame+9;
	gar_RotYXZsuperpack(&rotation, 0);

	object = &objects[item->object_number];
	meshpp = &meshes[object->mesh_index];
	bone = bones + object->bone_index;

	objptr = *(meshpp++);							// Get Sphere stuff Now...
	phd_PushMatrix();
	phd_TranslateRel( (sint32)*(objptr),
					  (sint32)*(objptr+1),
					  (sint32)*(objptr+2) );
#ifdef PSX_VERSION
	gte_sttr(ptr);
	ptr->x += x;
	ptr->y += y;
	ptr->z += z;
#else
	ptr->x = x + (*(phd_mxptr+M03)>>W2V_SHIFT);
	ptr->y = y + (*(phd_mxptr+M13)>>W2V_SHIFT);
	ptr->z = z + (*(phd_mxptr+M23)>>W2V_SHIFT);
#endif
	ptr->r = (sint32)*(objptr+3);
	ptr++;
	phd_PopMatrix();


	extra_rotation = (sint16 *)item->data;
	for ( i=object->nmeshes-1; i>0; i--,bone+=3 )
	{
		poppush = *(bone++);			// Do Automated Hierarchy
		if (poppush & 1)                // Push Pop
			phd_PopMatrix();
		if (poppush & 2)
			phd_PushMatrix();

		phd_TranslateRel(*(bone), *(bone+1), *(bone+2));
		gar_RotYXZsuperpack(&rotation, 0);

		// WIN95: Shouldn't have to do second check, but seem to need it
		//        to avoid access violation (must be NULL sometimes; why?)
		if ((poppush & (ROT_X|ROT_Y|ROT_Z)) && extra_rotation)
		{
			if (poppush & ROT_Y)
				phd_RotY(*(extra_rotation++));
			if (poppush & ROT_X)
				phd_RotX(*(extra_rotation++));
			if (poppush & ROT_Z)
				phd_RotZ(*(extra_rotation++));
		}

		objptr = *(meshpp++);					// Get Sphere stuff now...
		phd_PushMatrix();
		phd_TranslateRel( (sint32)*(objptr),
					  	  (sint32)*(objptr+1),
					  	  (sint32)*(objptr+2) );
#ifdef PSX_VERSION
		gte_sttr(ptr);
		ptr->x += x;
		ptr->y += y;
		ptr->z += z;
#else
		ptr->x = x + (*(phd_mxptr+M03)>>W2V_SHIFT);
		ptr->y = y + (*(phd_mxptr+M13)>>W2V_SHIFT);
		ptr->z = z + (*(phd_mxptr+M23)>>W2V_SHIFT);
#endif
		ptr->r = (sint32)*(objptr+3);
		ptr++;
		phd_PopMatrix();

	}
	phd_PopMatrix();
	return( object->nmeshes );
}

#ifdef PC_VERSION
static void InitInterpolate2( int frac, int rate )
{
	IM_frac = frac;
	IM_rate = rate;

	IMptr = &IMstack[12*32];
	S_LongMemCpy( IMptr,phd_mxptr,12 );
}
#endif

/****************************************************************************************
 *				Get Absolute World pos of certain joint on Baddie
 ***************************************************************************************/

void	GetJointAbsPosition(ITEM_INFO *item, PHD_VECTOR *vec, int joint)
{
	int i,poppush;
	OBJECT_INFO 	*object;
	sint32 		*bone;
	sint16		*rotation1, *rotation2;
	sint16		*extra_rotation;
	sint16		*frmptr[2];
	int			rate,frac;
#ifdef PSX_VERSION
	MATRIX3D *old = Matrix;
#else
	sint32 *MatrixStash = phd_mxptr;
	sint32 *IMStash = IMptr;
#endif

	object = &objects[item->object_number];
	frac=GetFrames( item,frmptr,&rate );

	phd_PushUnitMatrix();
#ifdef PSX_VERSION
	mSetTrans(0, 0, 0);
#else
	*(phd_mxptr+M03) = 0;
	*(phd_mxptr+M13) = 0;
	*(phd_mxptr+M23) = 0;
#endif
	phd_RotYXZ(item->pos.y_rot, item->pos.x_rot, item->pos.z_rot);

	if ( item->data )
		extra_rotation = (sint16 *)item->data;			// Extra rotations
	else
		extra_rotation = null_rotations;				// Extra rotations

	bone = bones + object->bone_index;              	// Bone pointer

/* -------- non-interpolated frame */

	if (!frac)
	{
		phd_TranslateRel((sint32)*(frmptr[0]+6), (sint32)*(frmptr[0]+7), (sint32)*(frmptr[0]+8));
		rotation1 = frmptr[0] + 9;
		gar_RotYXZsuperpack(&rotation1, 0);

		for ( i=0; i<joint; i++,bone+=4)
		{
			poppush = *bone;

			if (poppush & 1)
				phd_PopMatrix();

			if (poppush & 2)
				phd_PushMatrix();

			phd_TranslateRel( *(bone+1), *(bone+2), *(bone+3) );
			gar_RotYXZsuperpack(&rotation1, 0);

			if (poppush & (ROT_X|ROT_Y|ROT_Z))
			{
				if (poppush & ROT_Y)
					phd_RotY(*(extra_rotation++));

				if (poppush & ROT_X)
					phd_RotX(*(extra_rotation++));

				if (poppush & ROT_Z)
					phd_RotZ(*(extra_rotation++));
			}
		}

		phd_TranslateRel(vec->x, vec->y, vec->z);
#ifdef PSX_VERSION
		gte_sttr(vec);
		vec->x += item->pos.x_pos;
		vec->y += item->pos.y_pos;
		vec->z += item->pos.z_pos;
#else
		vec->x = (*(phd_mxptr+M03)>>W2V_SHIFT) + item->pos.x_pos;
		vec->y = (*(phd_mxptr+M13)>>W2V_SHIFT) + item->pos.y_pos;
		vec->z = (*(phd_mxptr+M23)>>W2V_SHIFT) + item->pos.z_pos;
#endif
	}

/* -------- interpolated frame */

	else
	{
#ifdef PSX_VERSION
		InitInterpolate(frac, rate, &iMatrixStack[16]);
#else
		InitInterpolate2(frac, rate);
#endif
		rotation1 = frmptr[0]+9;
		rotation2 = frmptr[1]+9;
		phd_TranslateRel_ID((sint32)*(frmptr[0]+6), (sint32)*(frmptr[0]+7), (sint32)*(frmptr[0]+8),
						(sint32)*(frmptr[1]+6), (sint32)*(frmptr[1]+7), (sint32)*(frmptr[1]+8));
		gar_RotYXZsuperpack_I(&rotation1, &rotation2, 0);

		for ( i=0; i<joint; i++,bone+=4)
		{
			poppush = *bone;						// Do Automated Hierarchy
			if (poppush & 1)                			// Push Pop
				phd_PopMatrix_I();
			if (poppush & 2)
				phd_PushMatrix_I();

			phd_TranslateRel_I(*(bone+1), *(bone+2), *(bone+3));
			gar_RotYXZsuperpack_I(&rotation1, &rotation2, 0);

			if (poppush & (ROT_X|ROT_Y|ROT_Z))			// If any Extra rotations required...
			{
#ifdef PSX_VERSION
				if (poppush & ROT_Y)
				{
					mRotY(*extra_rotation);
					iRotY(*extra_rotation++);
				}

				if (poppush & ROT_X)
				{
					mRotX(*extra_rotation);
					iRotX(*extra_rotation++);
				}

				if (poppush & ROT_Z)
				{
					mRotZ(*extra_rotation);
					iRotZ(*extra_rotation++);
				}
#else
				if (poppush & ROT_Y)
					phd_RotY_I(*(extra_rotation++));

				if (poppush & ROT_X)
					phd_RotX_I(*(extra_rotation++));

				if (poppush & ROT_Z)
					phd_RotZ_I(*(extra_rotation++));
#endif
			}
		}

		phd_TranslateRel_I( vec->x, vec->y, vec->z );
		InterpolateMatrix();
#ifdef PSX_VERSION
		gte_sttr(vec);
		vec->x += item->pos.x_pos;
		vec->y += item->pos.y_pos;
		vec->z += item->pos.z_pos;
#else
		vec->x = (*(phd_mxptr+M03)>>W2V_SHIFT) + item->pos.x_pos;
		vec->y = (*(phd_mxptr+M13)>>W2V_SHIFT) + item->pos.y_pos;
		vec->z = (*(phd_mxptr+M23)>>W2V_SHIFT) + item->pos.z_pos;
#endif
	}

#ifdef PSX_VERSION
	Matrix = old;
	mCopyMatrix(old);
#else
	phd_mxptr = MatrixStash;	//##
	IMptr = IMStash;	//##
#endif
}
