#include "global.h"
#include "main.h"
//#include <memory.h>
#include "bff_load.h"
#include "map_draw.h"
#include "timer.h"
#include "stilts.h"

// Stilts... Specialist control method research

// Note - ingame, this kind of thing really should be done with overlays...

#define STILT_ZERO_CAMERA
#define MAP_SCALE_ADJUST 8


#define STILT_MOVING_NEITHER	0
#define STILT_MOVING_LEFT_FORE	1
#define STILT_MOVING_LEFT_BACK	2
#define STILT_MOVING_RIGHT_FORE	3
#define STILT_MOVING_RIGHT_BACK	4

#define STILT_MAX_STRETCH 50

#define MAX_OFFSET_ANGLE 400

#define START_DANGLE 30
#define D_DANGLE 5

#define MAX_DANGLE 70

typedef struct stiltsStr
{
	int digital_now;
	int digital_changed;
	int digital_old;

	int LegMoving;


	int LeftLift;
	int RightLift;

//	int LeftPos;	// baseline positions of feet once on floor
//	int RightPos;

	int forward_ya;

	int ang_accel;



	VECTOR lbase;
	VECTOR rbase;
	VECTOR cog;

// (the body always faces "forwards" - this stuff is for the legs)

	int za;		// the beginning & end of the move involve a z-tilt
	int ya;		// most of the move is a rotate

}stiltsStr;



stiltsStr *stiltsCtrl = NULL;

FMA_WORLD *stilts_fma_world = NULL;
TextureBankType	*stilts_textures = 0;
PSIMODEL	*stilts_model = 0;
ACTOR		*stilts_actor = 0;
BFF_Header *stilts_level_bff = NULL;


#define STILT_FOOT_BASE 1000
#define STILT_COG_HEIGHT 2000



void stiltsCycleStilts()
{
	stiltsStr * stilts = stiltsCtrl;

	stilts->digital_now = padData.digital[0];
	stilts->digital_changed = stilts->digital_now ^ stilts->digital_old;

	switch(stilts->LegMoving)
	{
		case STILT_MOVING_NEITHER:
			stilts->ang_accel = START_DANGLE;

			if(stilts->LeftLift > 0)
				stilts->LeftLift--;
			if(stilts->RightLift > 0)
				stilts->RightLift--;

			if(stilts->LeftLift || stilts->RightLift)
				break;

			if(stilts->digital_now & PAD_L2 && !(stilts->digital_now & PAD_R2))
			{
// tbd - only back up if you're holding back on the pad
/*
				if(stilts->LeftPos > stilts->RightPos)
				{
					stilts->LegMoving = STILT_MOVING_LEFT_BACK;
				}
				else
				{
					stilts->LegMoving = STILT_MOVING_LEFT_FORE;
				}
*/
				if(stilts->digital_now & PAD_DOWN)
				{
					stilts->LegMoving = STILT_MOVING_LEFT_BACK;
				}
				else
				{
					stilts->LegMoving = STILT_MOVING_LEFT_FORE;
				}

			}
			else if(stilts->digital_now & PAD_R2 && !(stilts->digital_now & PAD_L2))
			{
/*
				if(stilts->RightPos > stilts->LeftPos)
				{
					stilts->LegMoving = STILT_MOVING_RIGHT_BACK;
				}
				else
				{
					stilts->LegMoving = STILT_MOVING_RIGHT_FORE;
				}
*/
				if(stilts->digital_now & PAD_DOWN)
				{
					stilts->LegMoving = STILT_MOVING_RIGHT_BACK;
				}
				else
				{
					stilts->LegMoving = STILT_MOVING_RIGHT_FORE;
				}
			}
			break;

// ============== LEFTLEG ============

		case STILT_MOVING_LEFT_BACK:
			if(stilts->digital_now & PAD_L2)
			{
				if(stilts->digital_now & PAD_R2)	// deal with rollover
				{
					stilts->LegMoving = STILT_MOVING_NEITHER;
				}
				if(stilts->LeftLift < 8)
				{
					stilts->LeftLift++;
				}
				else if(stilts->forward_ya > -MAX_OFFSET_ANGLE)
				{
					stilts->forward_ya -= stilts->ang_accel;
					stilts->ang_accel+= D_DANGLE;
					if(stilts->ang_accel > MAX_DANGLE)
						stilts->ang_accel = MAX_DANGLE;
				}
			}
			else
			{
				stilts->LegMoving = STILT_MOVING_NEITHER;
			}
			break;

		case STILT_MOVING_LEFT_FORE:
			if(stilts->digital_now & PAD_L2)
			{

				if(stilts->digital_now & PAD_R2)	// deal with rollover
				{
					stilts->LegMoving = STILT_MOVING_NEITHER;
				}

				if(stilts->LeftLift < 8)
				{
					stilts->LeftLift++;
				}
				else if(stilts->forward_ya < MAX_OFFSET_ANGLE)
				{
					stilts->forward_ya += stilts->ang_accel;
					stilts->ang_accel+= D_DANGLE;
					if(stilts->ang_accel > MAX_DANGLE)
						stilts->ang_accel = MAX_DANGLE;

				}
			}
			else
			{
				stilts->LegMoving = STILT_MOVING_NEITHER;
			}
			break;

// ============== RIGHTLEG ============


		case STILT_MOVING_RIGHT_BACK:
			if(stilts->digital_now & PAD_R2)
			{
				if(stilts->digital_now & PAD_L2)	// deal with rollover
				{
					stilts->LegMoving = STILT_MOVING_NEITHER;
				}
				if(stilts->RightLift < 8)
				{
					stilts->RightLift++;
				}
				else if(stilts->forward_ya < MAX_OFFSET_ANGLE)
				{
					stilts->forward_ya += stilts->ang_accel;
					stilts->ang_accel+= D_DANGLE;
					if(stilts->ang_accel > MAX_DANGLE)
						stilts->ang_accel = MAX_DANGLE;
				}

			}
			else
			{
				stilts->LegMoving = STILT_MOVING_NEITHER;
			}
			break;

		case STILT_MOVING_RIGHT_FORE:
			if(stilts->digital_now & PAD_R2)
			{
				if(stilts->digital_now & PAD_L2)	// deal with rollover
				{
					stilts->LegMoving = STILT_MOVING_NEITHER;
				}
				if(stilts->RightLift < 8)
				{
					stilts->RightLift++;
				}
				else if(stilts->forward_ya > -MAX_OFFSET_ANGLE)
				{
					stilts->forward_ya -= stilts->ang_accel;
					stilts->ang_accel+= D_DANGLE;
					if(stilts->ang_accel > MAX_DANGLE)
						stilts->ang_accel = MAX_DANGLE;
				}

			}
			else
			{
				stilts->LegMoving = STILT_MOVING_NEITHER;
			}
			break;
	}

	stilts->digital_old = stilts->digital_now;

}


// For now, we'll draw Ginger (or would action man be better?)

// Legs:-
//   Rise & move forward according to the counter
//   Or are rigid & on ground if they're the grounded leg
//   Or flail if wobbling

// Arms:-
//   At an angle normally
//   Wave if you're waving 'em, or if you're wobbling. otherwise slowly fall to normal angle
//   Start to rise to waving position if you're *about* to go into wobbling mode

#define STILTS_PANEL_X 64
#define STILTS_PANEL_Y 60





void stilt3dLine(VECTOR *v1,VECTOR *v2, unsigned int colour)
{
	LINE_F2	*line;
	VERT	worldPos, parentPos;
	long	worldXY, parentXY;

#ifdef STILT_ZERO_CAMERA
	long t[3];

	t[0] = GsWSMATRIX.t[0];
	t[1] = GsWSMATRIX.t[1];
	t[2] = GsWSMATRIX.t[2];
	GsWSMATRIX.t[0] = 0;
	GsWSMATRIX.t[1] = 0;
	GsWSMATRIX.t[2] = 0;

#endif
	gte_SetTransMatrix(&GsWSMATRIX);
	gte_SetRotMatrix(&GsWSMATRIX);


	worldPos.vx = v1->vx - mycamera.vpx;
	worldPos.vy = v1->vy - mycamera.vpy;
	worldPos.vz = v1->vz - mycamera.vpz;
	
	parentPos.vx = v2->vx - mycamera.vpx;
	parentPos.vy = v2->vy - mycamera.vpy;
	parentPos.vz = v2->vz - mycamera.vpz;

	gte_ldv0(&worldPos);
	gte_rtps();
	gte_stsxy(&worldXY);

	gte_ldv0(&parentPos);
	gte_rtps();
	gte_stsxy(&parentXY);

	BEGINPRIM(line, LINE_F2);
	*(u_long *)&line->x0 = *(u_long *)&worldXY;
	*(u_long *)&line->x1 = *(u_long *)&parentXY;
	setRGB0(line, (colour >> 16) & 255, (colour >> 8) & 255, (colour) & 255);
	setLineF2(line);
	ENDPRIM(line, 0, LINE_F2);

#ifdef STILT_ZERO_CAMERA
	GsWSMATRIX.t[0] = t[0];
	GsWSMATRIX.t[1] = t[1];
	GsWSMATRIX.t[2] = t[2];
#endif


}

void stiltsSetFeet(stiltsStr * stilts)
{
	VECTOR t1;
	VECTOR *src;
	VECTOR *dest;
	int rod_len = 2000;
	int rod_bang = 250;
	int zang;
	int zang2;
	int yang;


	if(stilts->LeftLift)
	{
		src = &stilts->rbase;
		dest = &stilts->lbase;
		zang = -rod_bang + stilts->LeftLift * 20;

		zang2 = rod_bang + stilts->LeftLift * 30;

//		yang = (stilts->LeftPos-stilts->RightPos) * 20;	// temp
//		yang = 1024*3 + yang;
		yang = (stilts->forward_ya + 1024) & 4095;
	}
	else
	{
		src = &stilts->lbase;
		dest = &stilts->rbase;
		zang = -rod_bang + stilts->RightLift * 20;

		zang2 = rod_bang + stilts->RightLift * 30;
		
//		yang = (stilts->LeftPos-stilts->RightPos) * 20;	// temp
//		yang += 1024;
		yang = (stilts->forward_ya - 1024) & 4095;

	}

	src->vy = 0;	// just match the disp & action up for a mo

	t1.vx = -(rod_len * rsin(zang) )>> 12;
	t1.vy = -(rod_len * rcos(zang) )>> 12;
	t1.vz = 0;

	stilts->cog.vx = src->vx + (t1.vx * rcos(yang)>>12);
	stilts->cog.vy = src->vy + t1.vy;
	stilts->cog.vz = src->vz - (t1.vx * rsin(yang)>>12);

//	zang = rod_bang;
	t1.vx = (rod_len * rsin(zang2) )>> 12;
	t1.vy = (rod_len * rcos(zang2) )>> 12;
	t1.vz = 0;

	dest->vx = stilts->cog.vx + (t1.vx * rcos(yang)>>12);
	dest->vy = stilts->cog.vy + t1.vy;
	dest->vz = stilts->cog.vz - (t1.vx * rsin(yang)>>12);

}


void stiltsDrawStilts()
{
	char str[80];
	stiltsStr * stilts = stiltsCtrl;

	sprintf(str,"%5d\n",stilts->LegMoving);
	fontPrint(font, STILTS_PANEL_X,STILTS_PANEL_Y, str, 128,128,64);


//	sprintf(str,"P: %5d,%5d\n",stilts->LeftPos,stilts->RightPos);
//	fontPrint(font, STILTS_PANEL_X,STILTS_PANEL_Y+10, str, 128,128,64);


	stiltsSetFeet(stilts);


	{
		VECTOR t1,t2;
		t1 = stilts->lbase;
		t2 = stilts->cog;
		stilt3dLine(&stilts->lbase,&stilts->cog,0xffffff);
		stilt3dLine(&stilts->rbase,&stilts->cog,0xffffff);

		t1 = stilts->lbase;
		t2 = stilts->cog;
		t1.vy = 0;
		t2.vy = 0;
		stilt3dLine(&t1,&t2,0x000000);
		t1 = stilts->rbase;
		t1.vy = 0;
		stilt3dLine(&t1,&t2,0x000000);
		t2.vy = 0;
	}
}




int stilts_camera_ya = 0xA00;
int stilts_camera_xa = 0x800;
int stilts_camera_range = 8000;


void stiltsCycleCamera()
{
	stiltsStr * stilts = stiltsCtrl;
	mycamera.vrx = stilts->cog.vx;
	mycamera.vry = 0; //stilts->cog.vy;
	mycamera.vrz = stilts->cog.vz;

	mycamera.vpx = mycamera.vrx - ((((stilts_camera_range * rsin(stilts_camera_ya))>>12) * rcos(stilts_camera_xa)) >> 12);
	mycamera.vpz = mycamera.vrz - ((((stilts_camera_range * rcos(stilts_camera_ya))>>12) * rcos(stilts_camera_xa)) >> 12);
	mycamera.vpy = mycamera.vry - ((stilts_camera_range * rsin(stilts_camera_xa))>>12);

	if(padData.digital[0] & PAD_TRIANGLE)
	{
		stilts_camera_range += padData.analogY2S[0] >> (8 - MAP_SCALE_ADJUST * 3/10);
	}
	else
	{
		stilts_camera_ya += padData.analogX2S[0] >> 8;
		stilts_camera_xa += padData.analogY2S[0] >> 8;
	}
	if(stilts_camera_range < 100) stilts_camera_range = 100;
	if(stilts_camera_range > 5000 * MAP_SCALE_ADJUST) stilts_camera_range = 5000 * MAP_SCALE_ADJUST;
	if(stilts_camera_xa > 1000) stilts_camera_xa = 1000;
	if(stilts_camera_xa < -1000) stilts_camera_xa = -1000;


// when setting the camera in the game, you must call this
	mapCamSetRefView2L(&mycamera);
}








void stiltsLoadMap(char *leafname)
{
	char name[40];

	psiInitialise(10);
	actorInitialise();
	puzzleInitLevel();

	sprintf(name,"%s\\%s.SPT",leafname,leafname);
	stilts_textures = textureLoadBank(name);
	textureDownloadBank(stilts_textures);
	textureDestroyBank(stilts_textures);


	DB("Loading BFF\n");
	sprintf(name,"%s\\%s.BFF",leafname,leafname);
	stilts_level_bff = BFF_LoadFile(name);
	ASSERT(stilts_level_bff);

	DB("BFF loaded OK\n");

	stilts_fma_world = (void *)BFF_FindObject(BFF_FMA_WORLD_ID,0);	// Should only be one world in the BFF

	psiAllocWorkspace();
	PSImodelctrl.depthShift = 3;
	PSImodelctrl.farclip = 32768;
}

void stiltsFreeMap()
{
	psiDestroy();
	BFF_ReleaseFile(stilts_level_bff);
	textureUnloadBank(stilts_textures);
	stilts_level_bff = NULL;
}









void stiltsDraw()
{
	stiltsDrawStilts();
	TIMER_START(TIMER_MAPREND);
	MapDraw_DrawFMA_World(stilts_fma_world);
	TIMER_STOP(TIMER_MAPREND);
}
void stiltsCycle()
{
	stiltsCycleStilts();
	stiltsCycleCamera();
	if(padData.debounce[0] & PAD_START)
	{
		stiltsEnd();
	}
}




void stiltsStart()
{
	stiltsStr * stilts = stiltsCtrl;

	stiltsLoadMap("RICHTEST");

	draw_routine = stiltsDraw;
	cycle_routine = stiltsCycle;

	stilts = MALLOC(sizeof(stiltsStr));
	memset(stilts,0,sizeof(stiltsStr));

	stiltsCtrl = stilts;

	stilts->lbase.vx = 0;
	stilts->lbase.vy = 0;
	stilts->lbase.vz = -STILT_FOOT_BASE/2;

	stilts->rbase.vx = 0;
	stilts->rbase.vy = 0;
	stilts->rbase.vz = STILT_FOOT_BASE/2;

	stilts->cog.vx = 0;
	stilts->cog.vy = -STILT_COG_HEIGHT;
	stilts->cog.vz = 0;
}

void stiltsEnd()
{
	stiltsFreeMap();
	FREE(stiltsCtrl);
	stiltsCtrl = NULL;
}



