#include <libps.h>
#include "MyPad.h"
#include "Tmd.h"
#include "Object.h"
#include "Arena.h"
#include "Sprite.h"

#define TEXTURE_IMAGE_ADDRESS	0x80090000
#define TEXT_IMAGE_ADDRESS		0x80092020
#define NUMBERS_IMAGE_ADDRESS	0x80092460
#define CRAFT_ADDRESS			0x80092ce0
#define ARENA_DATA_ADDRESS		0x800931b0
#define TEXTURE_DATA_ADDRESS	0x800951b0
#define CRAFT_COL_ADDRESS		0x800955f0
#define TMD_DYNAMIC_START		0x80095784
#define OT_LENGTH				12
#define PACKETMAX				8192
#define PACKETMAX2				(PACKETMAX * 24) 
#define SCREEN_WIDTH			320
#define SCREEN_HEIGHT			256

#define Greater(a, b) ( (a) = ((a) < (b)) ? (b) : (a) )
#define Less(a, b) ( (a) = ((a) > (b)) ? (b) : (a) )
#define Abs(a) ( ((a) > 0) ? (a) : (-(a)) )
#define LimitRange(a, b, c) ( ((a)<(b)) ? (b) : ((a)>(c) ? (c) : (a)) )
#define CalculateLight(_or, _og, _ob) \
	Greater(dis, 1), \
	tr = (_or) + ((r<<10)*rad/dis), \
	tg = (_og) + ((g<<10)*rad/dis), \
	tb = (_ob) + ((b<<10)*rad/dis), \
	Greater(tr, 0), \
	Less(tr, 255), \
	Greater(tg, 0), \
	Less(tg, 255), \
	Greater(tb, 0), \
	Less(tb, 255)

typedef struct {
	u_char r0, g0, b0;
	u_char r1, g1, b1;
	u_char r2, g2, b2;
	u_char r3, g3, b3;
} Colour;

Colour arenaCol[MAX_X][MAX_Z];
ArenaType arena;
BlockType arenaData[MAX_X][MAX_Z];
GsOT		WorldOT[2];	
GsOT_TAG	OTTags[2][1<<OT_LENGTH];
static PACKET packetArea[2][PACKETMAX2];
GsF_LIGHT lights[3];
GsRVIEW2 view;
ObjectHandler craft;
volatile u_char *bb0, *bb1;
long GAMEOVER = 0;
char textureNum[MAX_X][MAX_Z];
char checkPoint[MAX_X][MAX_Z];
long minHeight[MAX_X][MAX_Z];
int checkX[9] = {  0,  10,  15,  10,   0, -10, -15, -10,   0};
int checkZ[9] = { 15,  10,   0, -10, -15, -10,   0,  10,   0};
long lapTimes[5] = {0, 0, 0, 0, 0};
GsSPRITE lapTime, bigNum, smallNum;
u_char BLUR = 1;

// table for inverse tangent (of form 512 = 90 degrees)
short Tinvtan[] =
{
   0,   3,   5,   8,  10,  13,  15,  18,  20,  23,  25,  28,  31,  33,  36,  38,  
  41,  43,  46,  48,  51,  53,  56,  58,  61,  63,  66,  69,  71,  74,  76,  79,  
  81,  84,  86,  89,  91,  94,  96,  99, 101, 104, 106, 108, 111, 113, 116, 118, 
 121, 123, 126, 128, 131, 133, 136, 138, 140, 143, 145, 148, 150, 152, 155, 157, 
 160, 162, 164, 167, 169, 172, 174, 176, 179, 181, 183, 186, 188, 190, 193, 195, 
 197, 200, 202, 204, 207, 209, 211, 214, 216, 218, 220, 223, 225, 227, 229, 232, 
 234, 236, 238, 241, 243, 245, 247, 249, 252, 254, 256, 258, 260, 262, 265, 267,
 269, 271, 273, 275, 277, 279, 282, 284, 286, 288, 290, 292, 294, 296, 298, 300, 
 302, 304, 306, 308, 310, 312, 314, 316, 318, 320, 322, 324, 326, 328, 330, 332, 
 334, 336, 338, 340, 342, 344, 346, 347, 349, 351, 353, 355, 357, 359, 360, 362, 
 364, 366, 368, 370, 371, 373, 375, 377, 379, 380, 382, 384, 386, 387, 389, 391, 
 393, 394, 396, 398, 399, 401, 403, 405, 406, 408, 410, 411, 413, 415, 416, 418, 
 419, 421, 423, 424, 426, 428, 429, 431, 432, 434, 435, 437, 439, 440, 442, 443, 
 445, 446, 448, 449, 451, 452, 454, 455, 457, 458, 460, 461, 463, 464, 466, 467, 
 469, 470, 471, 473, 474, 476, 477, 479, 480, 481, 483, 484, 486, 487, 488, 490, 
 491, 492, 494, 495, 496, 498, 499, 500, 502, 503, 504, 506, 507, 508, 509, 511
};
long rinvtan(long x, long y);
long FindRotation(long height1, long height2, long length);

void main(void);
void InitGame(void);
void AddBlockToArena(ArenaType *arena, int x, int y, int z, u_long *address);
void BuildArena(void);
void BuildBlockData(void);
void DrawArena(int activeBuff, int x, int z);
void InitLights(void);
void AddLights(void);
void AddLight(int x, int z, int rad, int r, int g, int b);
void DealWithPad(void);
void MoveCraft(void);
static u_long ReadPad(void);

void main(void) {
	int activeBuff, vSync, maxVSync=0;
	int viewx, viewz, i, j;
	GsIMAGE imageInfo;
	GsSPRITE leftSprite, rightSprite;
	u_long row[160];
	RECT rect;
	MATRIX ls, lw;

	InitGame();

	FntLoad(960, 256);	
	FntOpen(-140, 96, 280, 64, 0, 512);
	
	// Set up sprites for motion blurring
	rightSprite.attribute	= leftSprite.attribute	= (1<<25)+(1<<30);
	rightSprite.x			= leftSprite.x			= 0;
	rightSprite.y			= leftSprite.y			= 0;
	rightSprite.h			= leftSprite.h			= 256;
	rightSprite.u			= leftSprite.u			= 0;
	rightSprite.v			= leftSprite.v			= 0;
	rightSprite.my			= leftSprite.my			= 128;
	rightSprite.scalex		= leftSprite.scalex		= ONE;
	rightSprite.scaley		= leftSprite.scaley		= ONE;
	rightSprite.rotate		= leftSprite.rotate		= 0;

	leftSprite.w	= 256;
	rightSprite.w	= 64;
	rightSprite.mx	= -96;
	leftSprite.mx	= 160;
	
	leftSprite.r = rightSprite.r = 127;
	leftSprite.g = rightSprite.g = 127;
	leftSprite.b = rightSprite.b = 127;
	
	// Stick textures on frame buffer
	ProperInitialiseTexture(TEXTURE_IMAGE_ADDRESS, &imageInfo);
	ProperInitialiseTexture(CRAFT_COL_ADDRESS, &imageInfo);
	
	// Initialise sprites
	ProperInitialiseTexture(TEXT_IMAGE_ADDRESS, &imageInfo);
	InitSprite(&lapTime, &imageInfo, 0, 16, 58, 16);
	ProperInitialiseTexture(NUMBERS_IMAGE_ADDRESS, &imageInfo);
	InitSprite(&bigNum, &imageInfo, 0, 0, 12, 16);
	InitSprite(&smallNum, &imageInfo, 0, 16, 9, 16);
	InitObject(&craft, (u_long *)CRAFT_ADDRESS);
	craft.rot.vy = 1024;
//	RotateModel(&craft.coord, &craft.rot, 0, 1024, 0);

	// Make the arena
	AddLights();
	BuildArena();

	// Make the screen semi-transparent (just in case)
	setRECT(&rect, 0, 0, 320, 1);
	for (j=0; j<512; j++) {
		rect.y = j;
		StoreImage(&rect, row);
		DrawSync(0);
		for (i=0; i<160; i++)
			row[i] |= (1<<15) + (1<<31);
		LoadImage(&rect, row);
		DrawSync(0);
	}

	VSync(0);			// For maxVSync

	do {
		activeBuff = GsGetActiveBuff();
		GsSetWorkBase((PACKET *)packetArea[activeBuff]);
		GsClearOt(0, 0, &WorldOT[activeBuff]);
		
		DealWithPad();
		MoveCraft();
		
		// Work out camera co-ordinates
		viewx = (craft.pos.vx + (craft.vel.vx<<4)) >> 8;
		viewz = (craft.pos.vz + (craft.vel.vz<<4)) >> 8;
		view.vpx += (viewx - view.vpx)>>3;
		view.vpz += (viewz - view.vpz)>>3;
		view.vrx = view.vpx;
		view.vrz = view.vpz;
		
		// Draw the arena
		GsSetRefView2(&view);
		DrawArena(activeBuff, (view.vrx+32)>>6, (view.vrz+16)>>6);

		// Draw the ship
		GsGetLws(craft.handler.coord2, &lw, &ls);
		GsSetLightMatrix(&lw);
		GsSetLsMatrix(&ls);
		GsSortObject4(&craft.handler, &WorldOT[activeBuff], 3, getScratchAddr(0));
		
		// Display the lap times
		DrawTimes(&lapTime, &bigNum, &smallNum, &WorldOT[activeBuff], craft.bestTime, craft.time, lapTimes);
		
		// Do we want motion blurring or not?
		if (BLUR) {
			leftSprite.tpage = GetTPage(2,0,0,activeBuff<<8);
			rightSprite.tpage = GetTPage(2,0,256,activeBuff<<8);
			GsSortSprite(&leftSprite, &WorldOT[activeBuff], 0);
			GsSortSprite(&rightSprite, &WorldOT[activeBuff], 0);
		}
		
		DrawSync(0);
		vSync = VSync(0);
		GsSwapDispBuff();
		
//		if (!BLUR) {
			GsSortClear(0, 0, 0, &WorldOT[activeBuff]);
//		}
		
		GsDrawOt(&WorldOT[activeBuff]);
		
		if (vSync> maxVSync)
			maxVSync = vSync;
		
		FntPrint(0,"%d\n%d\n",vSync,maxVSync);
		FntFlush(-1);
	} while (!GAMEOVER);
}


void InitGame(void) {
	int i, tx, ty;
	
	// General setting up
	SetVideoMode(MODE_PAL);
	GsInitGraph(SCREEN_WIDTH, SCREEN_HEIGHT, GsINTER|GsOFSGPU, 1, 0);
	
	GsDefDispBuff(0, 0, 0, SCREEN_HEIGHT);
	
	GsInit3D();
	// Center the screen for my TV (sorry)
	GsDISPENV.screen.x = 7;
	GsDISPENV.screen.y = 16;
	GsDISPENV.screen.h = 255;
	GsSetOrign(160,128);
	GetPadBuf(&bb0, &bb1);

	for (i = 0; i < 2; i++) {
		WorldOT[i].length = OT_LENGTH;
		WorldOT[i].org = OTTags[i];
		GsClearOt(0,0,&WorldOT[i]);
	}
	
	BuildBlockData();
	InitLights();
	
	// Setup craft
	craft.pos.vx = 21*64<<8;
	craft.pos.vz = 2*64<<8;
	
	tx = craft.pos.vx >> 14;
	ty = craft.pos.vz >> 14;
	craft.pos.vy = Blend(tx, ty, arenaData[tx][ty].y0, arenaData[tx][ty].y1, arenaData[tx][ty].y3, arenaData[tx][ty].y2)<<8;
	craft.vel.vx = 0;
	craft.vel.vy = 0;
	craft.vel.vz = 0;
	craft.flag = 0;
	
	craft.coord.coord.t[0] = craft.pos.vx >> 8;
	craft.coord.coord.t[1] = craft.pos.vy >> 8;
	craft.coord.coord.t[2] = craft.pos.vz >> 8;	
	craft.check = 0;
	craft.time = 0;
	craft.bestTime = 0;

	// Setup camera
	GsSetProjection(256);
	view.vpy = 360;
	view.vry = 0;
	view.vpx = view.vrx = craft.pos.vx>>8;
	view.vpz = view.vrz = craft.pos.vz>>8;
	view.rz = 0;
	view.super = WORLD;
	GsSetRefView2(&view);

	// Setup light
	lights[0].vx = 0; lights[0].vy = -ONE; lights[0].vz = 0;
	GsSetAmbient(32, 32, 32);
}


void AddBlockToArena(ArenaType *arena, int x, int y, int z, u_long *address) {
	int index = arena->totalBlocks;
	
	arena->pointer[index] = address;
	// Skip model id
	arena->pointer[index]++;
	GsMapModelingData(arena->pointer[index]);
	GsInitCoordinate2(WORLD, &arena->coord[index]);
	// Top of model data
	arena->pointer[index] += 2;
	GsLinkObject4((u_long )arena->pointer[index], &arena->handler[index], 0);
	arena->handler[index].attribute = 0;//1<<9;
	arena->handler[index].coord2 = &arena->coord[index];
	arena->coord[index].coord.t[0] = x;
	arena->coord[index].coord.t[1] = y;
	arena->coord[index].coord.t[2] = z;
	arena->coord[index].flg = 0;
	arena->totalBlocks++;
}


void BuildArena(void) {
	int i, j, k, a, b, r, g;
	u_long *pointer;
	char *tex;
	u_long address = TMD_DYNAMIC_START;
	u_char vCount;
	VERTEX v[8];
	COLOUR rgb[12];
	TEXTURE txt[4];
	
	//Work out textures and check-points
	tex = (char *)TEXTURE_DATA_ADDRESS;
	k=0;
	for (j=0; j<MAX_Z; j++) {
		for (i=0; i<MAX_X; i++) {
			textureNum[i][j] = tex[k++] - 48;
			checkPoint[i][j] = 0;
			if (textureNum[i][j] > 3) {
				checkPoint[i][j] = 1 << (textureNum[i][j]+48-97);
				textureNum[i][j] = 1;
			}
		}
		k+=2;
	}
	
	arena.totalBlocks = 0;
	SetTexture(&txt[0], 320, 0, 351, 0, 320, 31, 351, 31, 2, 0, 0);
	SetTexture(&txt[1], 352, 0, 383, 0, 352, 31, 383, 31, 2, 0, 0);
	SetTexture(&txt[2], 320, 32, 351, 32, 320, 63, 351, 63, 2, 0, 0);
	SetTexture(&txt[3], 352, 32, 383, 32, 352, 63, 383, 63, 2, 0, 0);
	
	// And now the hard part...
	for (i=0; i<MAX_X; i++) {
		for (j=0; j<MAX_Z; j++) {
			// Set poly tint
			SetColour(&rgb[0], arenaCol[i][j].r3, arenaCol[i][j].g3, arenaCol[i][j].b3);
			SetColour(&rgb[1], arenaCol[i][j].r0, arenaCol[i][j].g0, arenaCol[i][j].b0);
			SetColour(&rgb[2], arenaCol[i][j].r1, arenaCol[i][j].g1, arenaCol[i][j].b1);
			SetColour(&rgb[3], arenaCol[i][j].r2, arenaCol[i][j].g2, arenaCol[i][j].b2);
			vCount = 4;
			// Add main quad to tmd
			PrepareTMD(address, &v[0], &rgb[0], &txt[0]);
			SetVertex(&v[0], 0+i*64, arenaData[i][j].y3, 65+j*64);
			SetVertex(&v[1],  0+i*64, arenaData[i][j].y0, 0+j*64);
			SetVertex(&v[2], 65+i*64, arenaData[i][j].y1, 0+j*64);
			SetVertex(&v[3], 65+i*64, arenaData[i][j].y2, 65+j*64);
			SetTX_NS_GP_QUAD(0, 1, 3, 2, 0, 1, 3, 2, textureNum[i][j]);
			// Add side if needed
			if (i>0 && (arenaData[i][j].y0 != arenaData[i-1][j].y1 || arenaData[i][j].y3 != arenaData[i-1][j].y2)) {
				r = arenaCol[i-1][j].r1>>1;
				g = arenaCol[i-1][j].g1>>1;
				b = arenaCol[i-1][j].b1>>1;
				SetColour(&rgb[vCount], r, g, b);
				
				r = arenaCol[i-1][j].r2>>1;
				g = arenaCol[i-1][j].g2>>1;
				b = arenaCol[i-1][j].b2>>1;
				SetColour(&rgb[vCount+1], r, g, b);
				
				r = arenaCol[i][j].r0>>1;
				g = arenaCol[i][j].g0>>1;
				b = arenaCol[i][j].b0>>1;
				SetColour(&rgb[vCount+2], r, g, b);
				
				r = arenaCol[i][j].r3>>1;
				g = arenaCol[i][j].g3>>1;
				b = arenaCol[i][j].b3>>1;
				SetColour(&rgb[vCount+3], r, g, b);

				// Work out which way round the side should face
				SetVertex(&v[vCount], 0+i*64, arenaData[i-1][j].y1, 0+j*64);
				SetVertex(&v[vCount+1], 0+i*64, arenaData[i-1][j].y2, 65+j*64);
				a = arenaData[i][j].y0 - arenaData[i-1][j].y1;
				b = arenaData[i][j].y3 - arenaData[i-1][j].y2;
				if (a<0) a = -a;
				if (b<0) b = -b;
				if (a>b)
					SetTX_NS_GP_QUAD(1, 0, vCount, vCount+1, vCount+2, vCount+3, vCount, vCount+1, 0);
				else
					SetTX_NS_GP_QUAD(vCount+1, vCount, 0, 1, vCount+1, vCount, vCount+3, vCount+2, 0);
				vCount += 2;
			}
			// Add side if needed
			if (j<(MAX_Z-1) && (arenaData[i][j].y3 != arenaData[i][j+1].y0 || arenaData[i][j].y2 != arenaData[i][j+1].y1)) {
				r = arenaCol[i][j+1].r0>>1;
				g = arenaCol[i][j+1].g0>>1;
				b = arenaCol[i][j+1].b0>>1;
				SetColour(&rgb[vCount], r, g, b);
				
				r = arenaCol[i][j+1].r1>>1;
				g = arenaCol[i][j+1].g1>>1;
				b = arenaCol[i][j+1].b1>>1;
				SetColour(&rgb[vCount+1], r, g, b);

				r = arenaCol[i][j].r2>>1;
				g = arenaCol[i][j].g2>>1;
				b = arenaCol[i][j].b2>>1;
				SetColour(&rgb[vCount+2], r, g, b);
				
				r = arenaCol[i][j].r3>>1;
				g = arenaCol[i][j].g3>>1;
				b = arenaCol[i][j].b3>>1;
				SetColour(&rgb[vCount+3], r, g, b);

				// Work out which way round the side should face
				SetVertex(&v[vCount], 0+i*64, arenaData[i][j+1].y0, 65+j*64);
				SetVertex(&v[vCount+1], 65+i*64, arenaData[i][j+1].y1, 65+j*64);
				a = arenaData[i][j].y3 - arenaData[i][j+1].y0;
				b = arenaData[i][j].y2 - arenaData[i][j+1].y1;
				if (a<0) a = -a;
				if (b<0) b = -b;
				if (a>b)
					SetTX_NS_GP_QUAD(0, 3, vCount, vCount+1, vCount+3, vCount+2, vCount, vCount+1, 0);
				else
					SetTX_NS_GP_QUAD(vCount+1, vCount, 3, 0, vCount+1, vCount, vCount+2, vCount+3, 0);
				vCount += 2;
			}
			pointer = (u_long *)FinishTMD();
			AddBlockToArena(&arena, 0, 0, 0, pointer);
			address = 0;
		}
	}
}


void DrawArena(int activeBuff, int x, int z) {
	MATRIX ls, lw;
	int si, sj, ei, ej, i, j, index;
	
	// Work out how many blocks should be drawn, saves drawing whole arena
	ei = x+4;
	ej = z+3;
	si = x-4;
	sj = z-3;
	Less(ei, (MAX_X-1));
	Less(ej, (MAX_Z-1));
	Greater(si, 0);
	Greater(sj, 0);
	
	// Draw part of arena
	GsGetLws(arena.handler[0].coord2, &lw, &ls);
	GsSetLsMatrix(&ls);
	for (j=sj; j<=ej; j++) {
		for (i=si; i<=ei; i++) {
			index = j+i*MAX_Z;
			if (craft.pos.vy+1024 > minHeight[i][j])
				GsSortObject4(&arena.handler[index], &WorldOT[activeBuff], 2, getScratchAddr(0));
			else
				GsSortObject4(&arena.handler[index], &WorldOT[activeBuff], 4, getScratchAddr(0));
		}
	}
}


void BuildBlockData(void) {
	int i, j, k=0;
	short *pointer = (short *)ARENA_DATA_ADDRESS;
	long min;
	
	// Convert raw data into structure
	for (j=0; j<MAX_Z; j++) {
		for (i=0; i<MAX_X; i++) {
			arenaData[i][j].y0 = pointer[k++];
			min = arenaData[i][j].y0;
			arenaData[i][j].y1 = pointer[k++];
			Less(min, arenaData[i][j].y1);
			arenaData[i][j].y2 = pointer[k++];
			Less(min, arenaData[i][j].y2);
			arenaData[i][j].y3 = pointer[k++];
			Less(min, arenaData[i][j].y3);
			minHeight[i][j] = min<<8;
		}
	}
}


void InitLights(void) {
	int i, j;
	u_char col0, col1, col2, col3;
	
	// Reset all tints
	for (j=0; j<MAX_Z; j++) {
		for (i=0; i<MAX_X; i++) {
			// Do a bit of height shading
			col0 = LimitRange(16+(arenaData[i][j].y0>>1), 32, 255);
			col1 = LimitRange(16+(arenaData[i][j].y1>>1), 32, 255);
			col2 = LimitRange(16+(arenaData[i][j].y2>>1), 32, 255);
			col3 = LimitRange(16+(arenaData[i][j].y3>>1), 32, 255);
			arenaCol[i][j].r0 = col0;
			arenaCol[i][j].g0 = col0;
			arenaCol[i][j].b0 = col0;
			arenaCol[i][j].r1 = col1;
			arenaCol[i][j].g1 = col1;
			arenaCol[i][j].b1 = col1;
			arenaCol[i][j].r2 = col2;
			arenaCol[i][j].g2 = col2;
			arenaCol[i][j].b2 = col2;
			arenaCol[i][j].r3 = col3;
			arenaCol[i][j].g3 = col3;
			arenaCol[i][j].b3 = col3;
		}
	}
}


void AddLights(void) {
	// Add a plethora of coloured lights
	AddLight(12, 2, 6, 255, 255, 255);
	AddLight(2, 2 , 6, 255, 0, 0);
	AddLight(2, 23 , 6, 0, 255, 0);
	AddLight(7, 23 , 6, 0, 0, 255);
	AddLight(7, 16 , 5, 255, 255, 255);
	AddLight(7, 5 , 5, 255, 0, 0);
	AddLight(5, 5 , 5, 0, 255, 0);
	AddLight(5, 14 , 6, 0, 255, 255);
	AddLight(13, 12 , 5, 255, 255, 255);
	AddLight(17, 14 , 6, 255, 0, 255);
	AddLight(17, 8 , 6, 255, 255, 0);
	AddLight(13, 8 , 6, 0, 0, 255);
	AddLight(13, 27 , 6, 255, 0, 0);
	AddLight(2, 27 , 5, 0, 255, 255);
	AddLight(2, 30 , 5, 255, 255, 0);
	AddLight(20, 30 , 5, 255, 0, 255);
	AddLight(20, 28 , 5, 0, 255, 0);
	AddLight(17, 28 , 5, 0, 0, 255);
	AddLight(17, 20 , 6, 0, 255, 0);
	AddLight(24, 20 , 6, 255, 0, 0);
	AddLight(24, 26 , 5, 255, 255, 0);
	AddLight(21, 26 , 5, 0, 255, 255);
	AddLight(21, 20, 5, 255, 255, 255);
	AddLight(21, 12, 5, 0, 0, 255);
	AddLight(24, 12, 5, 0, 255, 0);
	AddLight(24, 17, 5, 255, 0, 255);
	AddLight(27, 17, 5, 255, 255, 0);
	AddLight(27, 30, 6, 0, 255, 255);
	AddLight(30, 30, 6, 255, 0, 0);
	AddLight(30, 24, 6, 0, 255, 255);
	AddLight(30, 16, 6, 255, 0, 255);
	AddLight(30, 9, 6, 255, 255, 0);
	AddLight(30, 2, 6, 0, 255, 0);
	AddLight(21, 2, 6, 0, 0, 255);
}


void AddLight(int x, int z, int rad, int r, int g, int b) {
	int dis;
	int dx, dz;
	int tr, tg, tb;
	int i, j;
	
	x = x<<6;
	z = z<<6;
	
	// Work out poly tints for each light
	for (j=0; j<MAX_Z; j++) {
		for (i=0; i<MAX_X; i++) {
			dx = x-(i<<6);
			dz = z-(j<<6);
			dis = (dx*dx+dz*dz);
			CalculateLight(arenaCol[i][j].r0, arenaCol[i][j].g0, arenaCol[i][j].b0);
			arenaCol[i][j].r0 = tr;
			arenaCol[i][j].g0 = tg;
			arenaCol[i][j].b0 = tb;
			
			dx = x-((i+1)<<6);
			dz = z-(j<<6);
			dis = (dx*dx+dz*dz);
			CalculateLight(arenaCol[i][j].r1, arenaCol[i][j].g1, arenaCol[i][j].b1);
			arenaCol[i][j].r1 = tr;
			arenaCol[i][j].g1 = tg;
			arenaCol[i][j].b1 = tb;
	
			dx = x-((i+1)<<6);
			dz = z-((j+1)<<6);
			dis = (dx*dx+dz*dz);
			CalculateLight(arenaCol[i][j].r2, arenaCol[i][j].g2, arenaCol[i][j].b2);
			arenaCol[i][j].r2 = tr;
			arenaCol[i][j].g2 = tg;
			arenaCol[i][j].b2 = tb;
	
			dx = x-(i<<6);
			dz = z-((j+1)<<6);
			dis = (dx*dx+dz*dz);
			CalculateLight(arenaCol[i][j].r3, arenaCol[i][j].g3, arenaCol[i][j].b3);
			arenaCol[i][j].r3 = tr;
			arenaCol[i][j].g3 = tg;
			arenaCol[i][j].b3 = tb;
		}
	}
}


void DealWithPad(void) {
	u_long pad = ReadPad();
	
	if (pad & PADLleft)
		craft.rot.vy = (craft.rot.vy+64) & 4095;
		//RotateModel(&craft.coord, &craft.rot, 0, 64, 0);
	if (pad & PADLright)
		craft.rot.vy = (craft.rot.vy-64) & 4095;
		//RotateModel(&craft.coord, &craft.rot, 0, -64, 0);
		
	if (pad & PADLX) {
		SetAcceleration(&craft.vel, &craft.rot, -96);
	}
	if (pad & PADLS) {
		craft.vel.vx = (craft.vel.vx * 31) >> 5;
		craft.vel.vz = (craft.vel.vz * 31) >> 5;
	}
	if (pad & PADLselect)
		BLUR = 1-BLUR;
	
	// Sort out that dodgy drifting thing
	if (craft.vel.vx <0)
		craft.vel.vx = -(((-craft.vel.vx) * 31) >> 5);
	else
		craft.vel.vx = (craft.vel.vx * 31) >> 5;

	if (craft.vel.vz <0)
		craft.vel.vz = -(((-craft.vel.vz) * 31) >> 5);
	else
		craft.vel.vz = (craft.vel.vz * 31) >> 5;


	if ((pad == PADLstart+PADLselect) || (pad == PADRselect+PADRstart))
		GAMEOVER = 1;
}


// Highly dodgy collision routine
void MoveCraft(void) {
	int tx[9], ty[9], tz[9];
	int tx2[9], ty2[9], tz2[9];
	int i, dx, dz;
	u_char crashedX=0, crashedZ=0;

	// Work out craft data before it is moved
	for (i=0; i<9; i++) {
		dx = checkX[i]<<8;
		dz = checkZ[i]<<8;
		tx[i] = (craft.pos.vx+dx) >> 14;
		tz[i] = (craft.pos.vz+dz) >> 14;
		ty[i] = Blend((craft.pos.vx+dx)>>8, (craft.pos.vz+dz)>>8, arenaData[tx[i]][tz[i]].y0, arenaData[tx[i]][tz[i]].y1, arenaData[tx[i]][tz[i]].y3, arenaData[tx[i]][tz[i]].y2)<<8;
	}
	
	// If in air, fall
	if (craft.flag == 1) {
		craft.vel.vy -= 1<<7;
		craft.pos.vy += craft.vel.vy;
	}
	// Otherwise slide according to block gradient
	else {
		craft.vel.vx += (arenaData[tx[8]][tz[8]].y0 - arenaData[tx[8]][tz[8]].y1) >> 2;
		craft.vel.vz += (arenaData[tx[8]][tz[8]].y1 - arenaData[tx[8]][tz[8]].y2) >> 2;
	}
	
	// Move!
	craft.pos.vx += craft.vel.vx;
	craft.pos.vz += craft.vel.vz;
	
	// Boundary check
	if ((craft.pos.vx<0) || (craft.pos.vx>>8)>=(MAX_X*64)) {
		craft.pos.vx -= craft.vel.vx;
		craft.vel.vx = -craft.vel.vx;
	}
	
	if ((craft.pos.vz<0) || (craft.pos.vz>>8)>=(MAX_Z*64)) {
		craft.pos.vz -= craft.vel.vz;
		craft.vel.vz = -craft.vel.vz;
	}
	
	// Work out new craft data
	for (i=0; i<9; i++) {
		dx = checkX[i]<<8;
		dz = checkZ[i]<<8;
		tx2[i] = (craft.pos.vx+dx) >> 14;
		tz2[i] = (craft.pos.vz+dz) >> 14;
		ty2[i] = Blend((craft.pos.vx+dx)>>8, (craft.pos.vz+dz)>>8, arenaData[tx2[i]][tz2[i]].y0, arenaData[tx2[i]][tz2[i]].y1, arenaData[tx2[i]][tz2[i]].y3, arenaData[tx2[i]][tz2[i]].y2)<<8;
	}
	
	// More complicated collision detection
	for (i=0; i<9; i++) {
		if (craft.pos.vy < ty2[i]) {
			if ((crashedX==0) && (tx2[i]<tx[i])) {
				if ((arenaData[tx2[i]][tz[i]].y1 > arenaData[tx[i]][tz[i]].y0) || (arenaData[tx2[i]][tz[i]].y2 > arenaData[tx[i]][tz[i]].y3)) {
					craft.pos.vx -= craft.vel.vx;
					craft.vel.vx = -craft.vel.vx;
					crashedX = 1;
				}
			}
			if ((crashedX==0) && (tx2[i]>tx[i])) {
				if ((arenaData[tx2[i]][tz[i]].y0 > arenaData[tx[i]][tz[i]].y1) || (arenaData[tx2[i]][tz[i]].y3 > arenaData[tx[i]][tz[i]].y2)) {
					craft.pos.vx -= craft.vel.vx;
					craft.vel.vx = -craft.vel.vx;
					crashedX = 1;
				}
			}
			if ((crashedZ==0) && (tz2[i]<tz[i])) {
				if ((arenaData[tx[i]][tz2[i]].y3 > arenaData[tx[i]][tz[i]].y0) || (arenaData[tx[i]][tz2[i]].y2 > arenaData[tx[i]][tz[i]].y1)) {
					craft.pos.vz -= craft.vel.vz;
					craft.vel.vz = -craft.vel.vz;
					crashedZ = 1;
					}
			}
			if ((crashedZ==0) && (tz2[i]>tz[i])) {
				if ((arenaData[tx[i]][tz2[i]].y0 > arenaData[tx[i]][tz[i]].y3) || (arenaData[tx[i]][tz2[i]].y1 > arenaData[tx[i]][tz[i]].y2)) {
					craft.pos.vz -= craft.vel.vz;
					craft.vel.vz = -craft.vel.vz;
					crashedZ = 1;
				}
			}
		}
	}
	for (i=0; i<9; i++) {
		dx = checkX[i]<<8;
		dz = checkZ[i]<<8;
		tx2[i] = (craft.pos.vx+dx) >> 14;
		tz2[i] = (craft.pos.vz+dz) >> 14;
		ty2[i] = Blend((craft.pos.vx+dx)>>8, (craft.pos.vz+dz)>>8, arenaData[tx2[i]][tz2[i]].y0, arenaData[tx2[i]][tz2[i]].y1, arenaData[tx2[i]][tz2[i]].y3, arenaData[tx2[i]][tz2[i]].y2)<<8;
	}
	
	// Don't let craft below ground
	craft.flag = 1;
	for (i=0; i<9; i++) {
		if (craft.pos.vy <= ty2[i]) {
			craft.flag = 0;
			craft.vel.vy = ty2[i]-ty[i];
			craft.pos.vy = ty2[i];
		}
	}
	
	// Complete bodge to fix the weird 'jump in the air' bug
	if (craft.vel.vy > 4000) {
		craft.flag = 0;
	}
	
	// Deal with lap times
	craft.check |= checkPoint[tx2[8]][tz2[8]];
	craft.time += 2;
	if (craft.check == 63 && (textureNum[tx2[8]][tz2[8]] == 2 || textureNum[tx2[8]][tz2[8]] == 3)) {
		for (i=4; i>0; i--)
			lapTimes[i] = lapTimes[i-1];
		lapTimes[0] = craft.time;
		if (craft.bestTime == 0) {
			craft.bestTime = craft.time;
			craft.time = 0;
			craft.check = 0;
		}
		else {
			if (craft.time < craft.bestTime)
				craft.bestTime = craft.time;
			craft.time = 0;
			craft.check = 0;
		}
	}
	
	craft.coord.flg = 0;
	craft.coord.coord.t[0] = craft.pos.vx >> 8;
	craft.coord.coord.t[1] = craft.pos.vy >> 8;
	craft.coord.coord.t[2] = craft.pos.vz >> 8;

	// Find offset heights for the front and back of object
//	HeightToFront = ty2[4];
//	HeightToBack = ty2[0];
	// Find offset heights for the left and right of the object
//	HeightToRight = ty2[2];
//	HeightToLeft = ty2[6];
	
	//Set Objects X Rotation Vector
//	Rotation2.vx = FindRotation(HeightToFront,HeightToBack,32);
//	Rotation2.vy = Rotation2.vz = 0;

	//Set Objects Z Rotation Vector.
//	Rotation3.vz = FindRotation(HeightToLeft,HeightToRight,32);
//	Rotation3.vy = Rotation3.vx = 0;

	// Set Objects Y Rotation Vector.
//	Rotation1.vy = theObject->yAngle;
//	Rotation1.vx = Rotation1.vz = 0;

	ResetMatrix(craft.coord.coord.m);	

	RotMatrixY(craft.rot.vy, &craft.coord.coord);

	Greater(ty2[4], craft.pos.vy-(15<<8));
	Greater(ty2[0], craft.pos.vy-(15<<8));
	Greater(ty2[2], craft.pos.vy-(15<<8));
	Greater(ty2[6], craft.pos.vy-(15<<8));
//	if (!craft.flag) {
		craft.rot.vx = FindRotation(ty2[4], ty2[0], 32<<8);
		craft.rot.vz = FindRotation(ty2[2], ty2[6], 32<<8);
//	}

	RotMatrixX(craft.rot.vx, &craft.coord.coord);
	RotMatrixZ(craft.rot.vz, &craft.coord.coord);

	// Set light according to block tint
	lights[0].r = Blend(craft.pos.vx>>8, craft.pos.vz>>8, arenaCol[tx2[8]][tz2[8]].r0, arenaCol[tx2[8]][tz2[8]].r1, arenaCol[tx2[8]][tz2[8]].r3, arenaCol[tx2[8]][tz2[8]].r2);
	lights[0].g = Blend(craft.pos.vx>>8, craft.pos.vz>>8, arenaCol[tx2[8]][tz2[8]].g0, arenaCol[tx2[8]][tz2[8]].g1, arenaCol[tx2[8]][tz2[8]].g3, arenaCol[tx2[8]][tz2[8]].g2);
	lights[0].b = Blend(craft.pos.vx>>8, craft.pos.vz>>8, arenaCol[tx2[8]][tz2[8]].b0, arenaCol[tx2[8]][tz2[8]].b1, arenaCol[tx2[8]][tz2[8]].b3, arenaCol[tx2[8]][tz2[8]].b2);
	GsSetFlatLight(0, &lights[0]);
}


long FindRotation(long height1, long height2, long length) {
	register int delta, delta2;  

	// If heights the same exit as there is no slope
	if(height1==height2) return(0);

	// If front higher then the back
	if(height1 > height2) {
		// first delta is the difference between the heights.
		delta= (height1 - height2);
		//uses advanced atan function which is very fast
		delta2=rinvtan(length, delta);		
		return ((long)(delta2 & 4095)); 	
	
	} // If the front is lower then the back
	else {
		delta= (height2 - height1);
		delta2=rinvtan(length, delta);	
		return ((long)4096-(delta2 & 4095));
	}
}


// returns inverse tangent from x and y values
// return value is of form 4096 = 360 degrees
// where x = y = 0 and the result is undefined the function returns 0
long rinvtan(long x, long y) {
	long t;

	if (x == 0 && y == 0)
		return 0;

	if (Abs(x) > Abs(y)) {
		t = (y << 8) / x;

		if (t >= 0)
			return Tinvtan[t & 255] + (x < 0 ? 2048 : 0);

		return (x < 0 ? 2048 : 4096) - Tinvtan[(-t) & 255];
	}

	t = (x << 8) / y;

	if (t >= 0)
		return (y < 0 ? 3072 : 1024) - Tinvtan[t & 255];

	return Tinvtan[(-t) & 255] + (y < 0 ? 3072 : 1024);
} 
	



u_long ReadPad(void) {
	return(~(*(bb0+3) | *(bb0+2) << 8 | *(bb1+3) << 16 | *(bb1+2) << 24));
}