#include "PersonManager.h"
#define BodySize 150

static long SpacePosition[3]; // The position in space
static long ViewingAngle; // Horrizontal
static long DroppingAngle; // Vertival Angle
static GsCOORDINATE2 PositionDescr; // Coordinate system of person without vertical angle
static GsCOORDINATE2 FinalRotation; // Complete coordinate system of person
static GsRVIEW2 Viewer; // The viewing system
static short MovePhase; // Animation phase for head movement

long getRotation()
{
	return ViewingAngle;
}

long getVertical()
{
	return DroppingAngle;
}

static void SetViewing()
{
	GsInitCoordinate2(WORLD,&PositionDescr);
	PositionDescr.coord=GsIDMATRIX;
	RotMatrixY(ViewingAngle,&PositionDescr.coord);
	PositionDescr.coord.t[0]=SpacePosition[0];
	PositionDescr.coord.t[1]=SpacePosition[1];
	PositionDescr.coord.t[2]=SpacePosition[2];
	PositionDescr.flg=0;
	GsInitCoordinate2(&PositionDescr,&FinalRotation);
	FinalRotation.coord=GsIDMATRIX;
	RotMatrixX(-DroppingAngle,&FinalRotation.coord);
	FinalRotation.flg=0;
	Viewer.vpx=0;
	Viewer.vpz=0;
	Viewer.vpy=0;
	Viewer.vrx=Viewer.vry=0;
	Viewer.vrz=1000;
	Viewer.rz=0;
	Viewer.super=&FinalRotation;
	GsSetRefView2(&Viewer);
}

void TurnRight()
{
	ViewingAngle=(ViewingAngle+100)%ONE;
	SetViewing();
}

void TurnLeft()
{
	ViewingAngle=(ViewingAngle+ONE-100)%ONE;
	SetViewing();
}

static int FieldFree(unsigned short xpos,unsigned short ypos)
{ 
	return (NoWall(xpos,ypos)&&noBox(xpos,ypos));
}



// Moves the person. Moving values are given in the relative coordinate 
// system of the person.
static void MovePerson(int zdirection,int xdirection)
{
	MATRIX Test; // Matrix needed to get the transformation from local in global coordinates
	SVECTOR Increment; // Moving increment in local coordinates
	SVECTOR Result;  // Moving incerement in global coordinates
	unsigned short newposition[2]; // Position in geometry space
	unsigned short testposition[2]; // Testpotition for collision detection in map space
	unsigned short olddiscrete[2]; // Actual position in map space
	unsigned short newdiscrete[2];  // New position in map space
	int i,j;
	int righttouch; // Count variables for collision detection
	int lefttouch;
	int fronttouch;
	int backtouch;
	
	
	righttouch=0;
	lefttouch=0;
	backtouch=0;
	fronttouch=0;
	Test=GsIDMATRIX;
	RotMatrixY(ViewingAngle,&Test);
	Increment.vx=xdirection;
	Increment.vy=0;
	Increment.vz=zdirection;
	ApplyMatrixSV(&Test,&Increment,&Result); // Collision detection
	newposition[0]=SpacePosition[0]+Result.vx;
	newposition[1]=SpacePosition[2]+Result.vz;
	testposition[0]=(newposition[0]-BodySize)>>9; 
	testposition[1]=(newposition[1]-BodySize)>>9;	
	lefttouch+=1-FieldFree(testposition[0],testposition[1]);
	backtouch+=1-FieldFree(testposition[0],testposition[1]);
	testposition[0]=(newposition[0]+BodySize)>>9; 
	testposition[1]=(newposition[1]-BodySize)>>9;
	righttouch+=1-FieldFree(testposition[0],testposition[1]);
	backtouch+=1-FieldFree(testposition[0],testposition[1]);
	testposition[0]=(newposition[0]-BodySize)>>9; 
	testposition[1]=(newposition[1]+BodySize)>>9;
	lefttouch+=1-FieldFree(testposition[0],testposition[1]);
	fronttouch+=1-FieldFree(testposition[0],testposition[1]);
	testposition[0]=(newposition[0]+BodySize)>>9; 
	testposition[1]=(newposition[1]+BodySize)>>9;
	righttouch+=1-FieldFree(testposition[0],testposition[1]);
	fronttouch+=1-FieldFree(testposition[0],testposition[1]);
	if (lefttouch+righttouch+fronttouch+backtouch==2) // Collision reaction
	{
		if (fabs(Result.vz)>fabs(Result.vx)) // We have touched a corner and slide sidewards to cirumvent obstacle
		{
			if (righttouch>0)
				newposition[0]=SpacePosition[0]-60;
			if (lefttouch>0)
				newposition[0]=SpacePosition[0]+60;
			if ((fronttouch>0)&&(Result.vz>0))
				newposition[1]=SpacePosition[2];
			if ((backtouch>0)&&(Result.vz<0))
				newposition[1]=SpacePosition[2];
		}
		else
		{
			if ((righttouch>0)&&(Result.vx>0))
				newposition[0]=SpacePosition[0];
			if ((lefttouch>0)&&(Result.vx<0))
				newposition[0]=SpacePosition[0];
			if (fronttouch>0)
				newposition[1]=SpacePosition[2]-60;
			if (backtouch>0)
				newposition[1]=SpacePosition[2]+60;
		}
	}
	if (lefttouch+righttouch+fronttouch+backtouch>2) // We have touched a wall and project out the conflicting movement component
	{
		
		if ((righttouch>1)&&(Result.vx>0)) 
			newposition[0]=SpacePosition[0];
		if ((lefttouch>1)&&(Result.vx<0))
			newposition[0]=SpacePosition[0];
		if ((fronttouch>1)&&(Result.vz>0))
			newposition[1]=SpacePosition[2];
		if ((backtouch>1)&&(Result.vz<0))
			newposition[1]=SpacePosition[2];
		if ((lefttouch==1)&&(righttouch==1)&&(fronttouch==1)&&(backtouch==1))
		{
			newposition[0]=SpacePosition[0];
			newposition[1]=SpacePosition[2];
		}
	}
	olddiscrete[0]=(SpacePosition[0]>>9);
	olddiscrete[1]=(SpacePosition[2]>>9);
	newdiscrete[0]=(newposition[0]>>9);
	newdiscrete[1]=(newposition[1]>>9); // Noe we have got the new position
	if ((olddiscrete[0]!=newdiscrete[0])||
		(olddiscrete[1]!=newdiscrete[1]))
	{
		for(i=-3;i<4;i++) // Undo poly subdivision
			for(j=-3;j<4;j++)
			{
				Unify(olddiscrete[0]+i,olddiscrete[1]+j);
				UnifyBox(olddiscrete[0]+i,olddiscrete[1]+j);
			}
		for(i=-3;i<4;i++) // Do poly subdivision 
			for(j=-3;j<4;j++)
			{		
				if ((i==-3)||(i==3)||(j==-3)||(j==3))
				{
					RoughSubdivide(newdiscrete[0]+i,newdiscrete[1]+j);
					RoughSubdivideBox(newdiscrete[0]+i,newdiscrete[1]+j);
				}
				else
				{
					Subdivide(newdiscrete[0]+i,newdiscrete[1]+j);
					SubdivideBox(newdiscrete[0]+i,newdiscrete[1]+j);
				}
			}
	}
	if ((SpacePosition[0]==newposition[0])&&
		(SpacePosition[2]==newposition[1]))
		PlayNoResult();
	else // Head goes slightly up and down
	{	
		if (MovePhase>1)
			SpacePosition[1]-=3;
		else
			SpacePosition[1]+=3;
		MovePhase=(MovePhase+1)%4;
		PlayStep();
		SpacePosition[0]=newposition[0];
		SpacePosition[2]=newposition[1];
	}
	SetViewing();
}

void WalkFront()
{
	MovePerson(60,0);
}

void WalkBack()
{
	MovePerson(-60,0);
}
	
void SlideRight()
{
	MovePerson(0,60);
}

void SlideLeft()
{
	MovePerson(0,-60);
}


// Initializes the person at the center of a field
void InitializePerson(unsigned short xcoord,unsigned short ycoord)
{	
	int i,j;
	
	ViewingAngle=1024;
	DroppingAngle=0;
	SpacePosition[0]=xcoord*512+256;
	SpacePosition[1]=-600;
	SpacePosition[2]=ycoord*512+256;
	MovePhase=0;
	for(i=-3;i<4;i++) // Poly subdivision
		for(j=-3;j<4;j++)
		{		
			if ((i==-3)||(i==3)||(j==-3)||(j==3))
			{
				RoughSubdivide(xcoord+i,ycoord+j);
				RoughSubdivideBox(xcoord+i,ycoord+j);
			}
			else
			{
				Subdivide(xcoord+i,ycoord+j);
				SubdivideBox(xcoord+i,ycoord+j);
			}
		}
	SetViewing();
}

// The jump consits of three phases all described a parrable, whic is natural
// for the jump itself. The first and the third step would be better of degree 
// three to allow for C1 continuity. As there are only very few samples 
// this is simplifyed
void Jump()
{
	volatile long step;
	volatile long buffer;
	
	buffer=SpacePosition[1];	
	for(step=0;step<13;step++)
	{
		SpacePosition[1]=(-15726*step*step+196608*step-2457600)/ONE;
		SetViewing();
		RedrawAll();
	}
	PlayBreath();
	for(step=0;step<26;step++)
	{
		SpacePosition[1]=(18717*step*step-467928*step-2457600)/ONE;
		DroppingAngle=(-10049*step*step+251224*step)/ONE;
		SetViewing();
		RedrawAll();
	} 
	PlayLoudStep();
	for(step=0;step<7;step++)
	{
		SpacePosition[1]=(-2621*8*step*step+65536*2*step-2457600)/ONE;
		SetViewing();
		RedrawAll();
	}
	SpacePosition[1]=buffer;
	DroppingAngle=0;
	SetViewing();
	RedrawAll();
}

// Pushes the box
void PushBox()
{
	unsigned short discretePosition[2]; // Players position
	unsigned short pushPosition[2]; // Position od the box
	unsigned short targetPosition[2];  // The target position of the box
	
	
	discretePosition[0]=SpacePosition[0]>>9;
	discretePosition[1]=SpacePosition[2]>>9; // Here we are on the map
	if ((ViewingAngle<=512)||(ViewingAngle>=3584)) // Let us distinguish four cases
	{
		pushPosition[0]=targetPosition[0]=discretePosition[0];
		pushPosition[1]=discretePosition[1]+1;
		targetPosition[1]=discretePosition[1]+2;
	}
	else if (ViewingAngle<=1536)
	{
		pushPosition[1]=targetPosition[1]=discretePosition[1];
		pushPosition[0]=discretePosition[0]+1;
		targetPosition[0]=discretePosition[0]+2;
	}
	else if (ViewingAngle<=2560)
	{
		pushPosition[0]=targetPosition[0]=discretePosition[0];
		pushPosition[1]=discretePosition[1]-1;
		targetPosition[1]=discretePosition[1]-2;
	}
	else 
	{
		pushPosition[1]=targetPosition[1]=discretePosition[1];
		pushPosition[0]=discretePosition[0]-1;
		targetPosition[0]=discretePosition[0]-2;
	}
	if (!noBox(pushPosition[0],pushPosition[1]))
		MoveBox(pushPosition,targetPosition);
}