#include <libps.h>
#include "MyPad.h"
#include "tmd.h"
#include "fractal.h"

#define TMD_DYNAMIC_START (0x80090000)
#define OT_LENGTH		12
#define PACKETMAX		8192
#define PACKETMAX2		(PACKETMAX*24) 
#define SCREEN_WIDTH	320
#define SCREEN_HEIGHT	256

// A nice, simple macro ;-)
// I know I've seen this somewhere before but I can't remember where
#define LimitRange(a, b, c) ( ((a)<(b)) ? (b) : ((a)>(c) ? (c) : (a)) )
#define DotProduct(a, b) ( ((a).vx*(b).vx) + ((a).vy*(b).vy) + ((a).vz*(b).vz) )


typedef struct {
	int loCount, hiCount;
	GsDOBJ2 hiHandler[(MAP_SIZE*MAP_SIZE)>>4];
	GsDOBJ2 loHandler[(MAP_SIZE*MAP_SIZE)>>4];
	GsCOORDINATE2 coord;
} WorldType;


GsOT		WorldOT[2];	
GsOT_TAG	OTTags[2][1<<OT_LENGTH];
static PACKET packetArea[2][PACKETMAX2];
volatile u_char *bb0, *bb1;
static GsRVIEW2 view;
int projection;
GsF_LIGHT lights[3];
u_char map[MAP_SIZE][MAP_SIZE];
WorldType world;
u_char hiRes[32][32];


void main(void);
void InitGame(void);
static u_long ReadPad(void);
void BuildWorld(void);
void AddPatchToWorld(WorldType *world, int x, int y, int z, u_long *address, GsDOBJ2 *handler, int *count);
void DrawWorld(int activeBuff, int x0, int z0, int x1, int z1, int x2, int z2, int xOffset, int zOffset);
void ResetMatrix(short m[3][3]);
void swap(int *a, int *b);
int Blend(int x, int y, int a, int b, int c, int d);
u_char GetNormal(int y0, int y1, int y2, int y3);
void MakePatch(COLOUR *rgb, VERTEX *v, u_char r[MAP_SIZE][MAP_SIZE], u_char g[MAP_SIZE][MAP_SIZE], u_char b[MAP_SIZE][MAP_SIZE], int i, int j);
int IntSqrt(register int n);


void main(void) {
	int activeBuff;
	int hsync, maxHsync=0;
	u_long pad;
	long rot = 0;
	MATRIX temp;
	SVECTOR camera = {0, 0, -256, 0};
	SVECTOR move, strafe;
	
	InitGame();
	GenerateMap(map, rand());
	
	projection = 250;
	GsSetProjection(projection);  
	view.vpx = MAP_SIZE<<6;
	view.vpz = MAP_SIZE<<6;
	view.super = WORLD;
	view.rz = 180*4096;
	GsSetRefView2(&view);
	
	BuildWorld();
	
	FntLoad(960, 256);	
	FntOpen(-140, -108, 280, 216, 0, 512);

	VSync(0);
	
	do {
		pad = ReadPad();
		activeBuff = GsGetActiveBuff();
		GsSetWorkBase((PACKET *)packetArea[activeBuff]);
		GsClearOt(0, 0, &WorldOT[activeBuff]);
		
		if (pad & PADLleft)
			rot += 24;
		if (pad & PADLright)
			rot -= 24;
		
		ResetMatrix(temp.m);
		RotMatrixY(rot, &temp);
		camera.vx = 0;
		camera.vz = 256;
		ApplyMatrixSV(&temp, &camera, &camera);
		
		ResetMatrix(temp.m);
		move.vx = 0;
		move.vz = ONE;
		RotMatrixY(rot, &temp);
		ApplyMatrixSV(&temp, &move, &move);

		ResetMatrix(temp.m);
		strafe.vx = ONE;
		strafe.vz = 0;
		RotMatrixY(rot, &temp);
		ApplyMatrixSV(&temp, &strafe, &strafe);
		
		if (pad & PADLup) {
			view.vpx += move.vx>>8;
			view.vpz += move.vz>>8;
		}
		if (pad & PADLdown) {
			view.vpx -= move.vx>>8;
			view.vpz -= move.vz>>8;
		}
		if (pad & PADLR1) {
			view.vpx -= strafe.vx>>8;
			view.vpz -= strafe.vz>>8;
		}
		if (pad & PADLL1) {
			view.vpx += strafe.vx>>8;
			view.vpz += strafe.vz>>8;
		}
		
		view.vrx = view.vpx + camera.vx;
		view.vrz = view.vpz + camera.vz;
		
		view.vpy = Blend(view.vpx, view.vpz, map[(view.vpx>>7)][(view.vpz>>7)], map[(view.vpx>>7)+1][(view.vpz>>7)], map[(view.vpx>>7)][(view.vpz>>7)+1], map[(view.vpx>>7)+1][(view.vpz>>7)+1])*2+128;
		view.vry = Blend(view.vrx, view.vrz, map[(view.vrx>>7)][(view.vrz>>7)], map[(view.vrx>>7)+1][(view.vrz>>7)], map[(view.vrx>>7)][(view.vrz>>7)+1], map[(view.vrx>>7)+1][(view.vrz>>7)+1])*2+96;

		// It was pointed out to me by my rather clever flatmate
		// that a 3x3 matrix could be used to represent the corners
		// of the view conde and using RotMatrix() the view cone can
		// easily be rotated. Cunning eh?
		temp.m[0][0] = 0;
		temp.m[2][0] = -3;
		temp.m[0][1] = -6;
		temp.m[2][1] = 9;
		temp.m[0][2] = 6;
		temp.m[2][2] = 9;
		RotMatrixY(rot, &temp);

		GsSetRefView2(&view);
		DrawWorld(activeBuff, temp.m[0][0], temp.m[2][0], temp.m[0][1], temp.m[2][1], temp.m[0][2], temp.m[2][2], view.vpx, view.vpz);
		
		DrawSync(0);
		hsync = VSync(0);
		GsSwapDispBuff();
		
		GsSortClear(0, 0, 0, &WorldOT[activeBuff]);
		GsDrawOt(&WorldOT[activeBuff]);
		if (hsync > maxHsync)
			maxHsync = hsync;
		FntPrint(0,"%d, %d\n",view.vpx>>7, view.vpz>>7);
		FntPrint(0,"%d, %d\n",hsync, maxHsync);
		FntFlush(-1);

	} while ((pad != PADLstart+PADLselect) && (pad != PADRselect+PADRstart));
}




void InitGame(void) {
	int i, j;
	
	SetVideoMode(MODE_PAL);
	GsInitGraph(SCREEN_WIDTH, SCREEN_HEIGHT, GsINTER|GsOFSGPU, 1, 0);
	
	GsDefDispBuff(0, 0, 0, 256);
	
	GsInit3D();
	GsDISPENV.screen.h = 255;
	
	GetPadBuf(&bb0, &bb1);

	lights[0].vx = 2048; lights[0].vy = 1024; lights[0].vz = 2048;
	lights[0].r = 255; lights[0].g = 255; lights[0].b = 255;
	GsSetFlatLight(0, &lights[0]);
	
	for (i = 0; i < 2; i++) {
		WorldOT[i].length = OT_LENGTH;
		WorldOT[i].org = OTTags[i];
		GsClearOt(0,0,&WorldOT[i]);
	}
	
	for (j=-16; j<16; j++) {
		for (i=-16; i<16; i++) {
			if (i*i+j*j < 8)
				hiRes[i+16][j+16] = 1;
			else
				hiRes[i+16][j+16] = 0;
		}
	}			
}


void BuildWorld(void) {
	u_long *pointer;
	int i, j, x, y;
	u_long address = TMD_DYNAMIC_START;
	VERTEX v[25];
	COLOUR rgb[25];
	u_char r[MAP_SIZE][MAP_SIZE], lor[4];
	u_char g[MAP_SIZE][MAP_SIZE], log[4];
	u_char b[MAP_SIZE][MAP_SIZE], lob[4];
	u_char n;
	
	
	// Get Landscape colour
	for (j=0; j<MAP_SIZE; j++) {
		for (i=0; i<MAP_SIZE; i++) {
			r[i][j] = (3*map[i][j]>>2) + 63;
			g[i][j] = map[i][j];
			b[i][j] = rand();
		}
	}
	
	// Work out lighting
	for (j=0; j<MAP_SIZE; j++) {
		for (i=0; i<MAP_SIZE; i++) {
			n = GetNormal(map[i][j], map[(i+1)%MAP_SIZE][j], map[i][(j+1)%MAP_SIZE], map[(i+1)%MAP_SIZE][(j+1)%MAP_SIZE]);
			r[i][j] = LimitRange((r[i][j] * n)>>7, 0, 255);
			g[i][j] = LimitRange((g[i][j] * n)>>7, 0, 255);
			b[i][j] = LimitRange((b[i][j] * n)>>7, 0, 255);
		}
	}
	
	// Build World
	world.loCount = world.hiCount = 0;
	for (j=2; j<MAP_SIZE; j+=4) {
		for (i=2; i<MAP_SIZE; i+=4) {
			PrepareTMD(address, &v[0], &rgb[0], 0);
			
			MakePatch(rgb, v, r, g, b, i, j);
			
			pointer = (u_long *)FinishTMD();
			AddPatchToWorld(&world, 0, 0, 0, pointer, world.hiHandler, &world.hiCount);
			address = 0;
			
			n = 0;
			for (y=j; y<j+5; y+=4) {
				for (x=i; x<i+5; x+=4) {
					lor[n] = r[x][y];
					log[n] = g[x][y];
					lob[n] = b[x][y];
					n++;
				}
			}
			
			// Build Lo-Res Patch
			PrepareTMD(address, &v[0], &rgb[0], 0);
			n = 0;
			for (y=j; y<j+5; y+=4) {
				for (x=i; x<i+5; x+=4) {
					SetColour(&rgb[n], lor[n], log[n], lob[n]);
					SetVertex(&v[n], x*128, map[x][y]*2, y*128);
					n++;
				}
			}
			SetNS_GP_TRI(0, 1, 2, 0, 1, 2);
			SetNS_GP_TRI(2, 1, 3, 2, 1, 3);
			pointer = (u_long *)FinishTMD();
			AddPatchToWorld(&world, 0, 0, 0, pointer, world.loHandler, &world.loCount);
			
		}
	}
	printf("%d\n",((u_long)pointer)-((u_long )TMD_DYNAMIC_START)+sizeof(WorldType));
}


// Link patch model to world data
void AddPatchToWorld(WorldType *world, int x, int y, int z, u_long *address, GsDOBJ2 *handler, int *count) {
	int index = *count;
	u_long *pointer;
	
	// Skip model id
	pointer = address+1;
	GsMapModelingData(pointer);
	GsInitCoordinate2(WORLD, &world->coord);
	// Top of model data
	pointer += 2;
	GsLinkObject4((u_long )pointer, &handler[index], 0);
	handler[index].attribute = (1<<9);
	handler[index].coord2 = &world->coord;
	world->coord.coord.t[0] = 0;
	world->coord.coord.t[1] = 0;
	world->coord.coord.t[2] = 0;
	world->coord.flg = 0;
	(*count)++;
}


// Go on, take a guess...
inline void swap(int *a, int *b) {
	int temp;
	
	temp = *a;
	*a = *b;
	*b = temp;
}


// Draw slice of the world
void DrawWorld(int activeBuff, int x0, int z0, int x1, int z1, int x2, int z2, int xOffset, int zOffset) {
	int x, z, index;			// Loop variables
	int lx0, lx1, dx0, dx1;		// Accurate coords
	int *px0, *px1;				// Pointers to accurate
	int sx, ex;					// Row start & end
	MATRIX ls, lw;				// Local screen & world

	x0 += xOffset>>9;
	x1 += xOffset>>9;
	x2 += xOffset>>9;
	z0 += zOffset>>9;
	z1 += zOffset>>9;
	z2 += zOffset>>9;
	
	xOffset = (xOffset-256)>>9;
	zOffset = (zOffset-256)>>9;
	
	// Sort verticies in z-order
	if (z0>z1) { swap(&x0, &x1); swap(&z0, &z1); }
	if (z1>z2) { swap(&x1, &x2); swap(&z1, &z2); }
	if (z0>z1) { swap(&x0, &x1); swap(&z0, &z1); }
	
	// Initialse accurate variables
	lx0 = lx1 = x0<<10;
	
	// Get gradients for triangle edges
	if (z0 != z1)
		dx0 = ((x1-x0)<<10) / (z1-z0);
	else {
		lx0 = x1<<10;		// A bodge to fix a z0==z1 problem
		dx0 = (x1-x0)<<10;
	}
	
	if (z0 != z2)
		dx1 = ((x2-x0)<<10) / (z2-z0);
	else
		dx1 = (x2-x0)<<10;
	
	if (dx0 < dx1) {
		px0 = &lx0;
		px1 = &lx1;
	}
	else {
		px0 = &lx1;
		px1 = &lx0;
	}
	
	GsGetLws(world.hiHandler[0].coord2, &lw, &ls);	// All the coord2's are the same
	GsSetLsMatrix(&ls);								// Same goes for the ls matrix
	for (z=z0; z<z1; z++) {
		if (z>=0 && z<MAP_SIZE>>2) {
			sx = LimitRange((*px0)>>10, 0, (MAP_SIZE>>2)-1);
			ex = LimitRange(((*px1)>>10)+1, 0, (MAP_SIZE>>2)-1);
			index = sx+z*(MAP_SIZE>>2);
			for (x=sx; x<ex; x++) {
				if (hiRes[x-xOffset+16][z-zOffset+16])
					GsSortObject4(&world.hiHandler[index++], &WorldOT[activeBuff], 4, getScratchAddr(0));
				else
					GsSortObject4(&world.loHandler[index++], &WorldOT[activeBuff], 4, getScratchAddr(0));
			}
		}
		lx0 += dx0;
		lx1 += dx1;
	}
	
	// Get gradient of last triangle edge
	if (z1 != z2)
		dx0 = ((x2-x1)<<10) / (z2-z1);
	else
		dx0 = 0;
	
	for (z=z1; z<z2; z++) {
		if (z>=0 && z<MAP_SIZE>>2) {
			sx = LimitRange((*px0)>>10, 0, (MAP_SIZE>>2)-1);
			ex = LimitRange(((*px1)>>10)+1, 0, (MAP_SIZE>>2)-1);
			index = sx+z*(MAP_SIZE>>2);
			for (x=sx; x<ex; x++) {
				if (hiRes[x-xOffset+16][z-zOffset+16])
					GsSortObject4(&world.hiHandler[index++], &WorldOT[activeBuff], 4, getScratchAddr(0));
				else
					GsSortObject4(&world.loHandler[index++], &WorldOT[activeBuff], 4, getScratchAddr(0));
			}
		}
		lx0 += dx0;
		lx1 += dx1;
	}
}


void ResetMatrix(short m[3][3]) {
	m[0][0] = m[1][1] = m[2][2] = ONE;
	m[0][1] = m[0][2] = m[1][0] = 0;
	m[1][2] = m[2][0] = m[2][1] = 0;
}


int Blend(int x, int y, int a, int b, int c, int d) {
	long t1, t2;
	
	x = x & 127;
	y = y & 127;
	t1 = a*128+x*(b-a);
	t2 = c*128+x*(d-c);
	return ((t1*128+y*(t2-t1))>>14);
}


// Calculate landscape shading
u_char GetNormal(int y0, int y1, int y2, int y3) {
	int nx, ny, nz, div;
	u_char n0, n1;
	long mul;
	
	nx = y0-y1;
	ny = 32;
	nz = y0-y2;
	div = IntSqrt(nx*nx+ny*ny+nz*nz);
	nx = 128*nx/div;
	ny = 128*ny/div;
	nz = 128*nz/div;
	
	mul = ((nx*lights[0].vx) + (ny*lights[0].vy) + (nz*lights[0].vz))>>11;
	n0 = LimitRange(mul, 32, 255);
	
	nx = y2-y3;
	ny = 32;
	nz = y1-y3;
	div = IntSqrt(nx*nx+ny*ny+nz*nz);
	nx = 128*nx/div;
	ny = 128*ny/div;
	nz = 128*nz/div;
	
	mul = ((nx*lights[0].vx) + (ny*lights[0].vy) + (nz*lights[0].vz))>>11;
	n1 = LimitRange(mul, 32, 255);
	
	return ((n0 + n1) >> 1);
}


// Make a hi-res patch
void MakePatch(COLOUR *rgb, VERTEX *v, u_char r[MAP_SIZE][MAP_SIZE], u_char g[MAP_SIZE][MAP_SIZE], u_char b[MAP_SIZE][MAP_SIZE], int i, int j) {
	int x, y, a=0;	
	
	for (y=j; y<j+5; y++) {
		for (x=i; x<i+5; x++) {
			SetColour(&rgb[a], r[x][y], g[x][y], b[x][y]);
			SetVertex(&v[a], x*128, map[x][y]*2, y*128);
			a++;
		}
	}
	
	for (y=0; y<4; y++) {
		for (x=0; x<4; x++) {
			a = x+y*5;
			SetNS_GP_TRI(a, a+1, a+5, a, a+1, a+5);
			SetNS_GP_TRI(a+5, a+1, a+6, a+5, a+1, a+6);
		}
	}
}


int IntSqrt(register int n) {
	register int i;
	register long k0, k1, nn;

	for (nn = i = n, k0 = 2; i > 0; i >>= 2, k0 <<= 1)
		;
	
	nn <<= 2;
	for (;;) {
		k1 = (nn / k0 + k0) >> 1;
		if (((k0 ^ k1) & ~1) == 0)
			break;
		k0 = k1;
	}
	return (int )((k1 + 1) >> 1);
}


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