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

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

#define setRGB(_s,_r,_g,_b) (_s)->r = (_r), (_s)->g = (_g), (_s)->b = (_b)

#define SetMatrix(_m, _a, _b, _c) \
	(_m)->m[0][0] = (_a)->vx, \
	(_m)->m[1][0] = (_a)->vy, \
	(_m)->m[2][0] = (_a)->vz, \
	(_m)->m[0][1] = (_b)->vx, \
	(_m)->m[1][1] = (_b)->vy, \
	(_m)->m[2][1] = (_b)->vz, \
	(_m)->m[0][2] = (_c)->vx, \
	(_m)->m[1][2] = (_c)->vy, \
	(_m)->m[2][2] = (_c)->vz


typedef struct {			
	GsDOBJ2	handler;
	SVECTOR pos;			// position
	SVECTOR rot;			// angle
	GsCOORDINATE2 coord;	// coordinate system	
} ObjectHandler;


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;
ObjectHandler craft;
TX_NS_GP_TRI_FACE *polyList[16*16*2];
MATRIX vertexList[16*16*2];		// Hold 3 corners of poly in 1 matrix
int polyCount = 0;


void main(void);
void InitGame(void);
void MoveObject(SVECTOR *pos, SVECTOR *rot, GsCOORDINATE2 *coordSystem);
static u_long ReadPad(void);
void MakeTmd(ObjectHandler *craft);
void ResetMatrix(short m[3][3]);

void main(void) {
	int activeBuff, i, j;
	u_long pad;
	MATRIX tmpls, rot, temp;
	GsIMAGE imageInfo;
	GsSPRITE leftSprite, rightSprite;
	RECT rect;
	SVECTOR haloSize, posRot = {0, 0, 0};
	u_long row[160];

	InitGame();

	projection = 100;
	GsSetProjection(projection);  
	view.vrx = 0;
	view.vry = 0;
	view.vrz = 0;
	view.vpx = 0;
	view.vpy = 0;
	view.vpz = -180;
	view.super = WORLD;
	view.rz = 0;
	GsSetRefView2(&view);

	craft.pos.vx = 0;
	craft.pos.vy = 0;
	craft.pos.vz = 0;
	
	craft.rot.vx = 0;
	craft.rot.vy = 0;
	craft.rot.vz = 0;
	GsInitCoordinate2(WORLD, &craft.coord);
	craft.handler.coord2 = &(craft.coord);
	craft.handler.attribute = 0;
	
	setRECT(&rect, 320, 0, 128, 256);
	GsGetTimInfo((u_long *)(TEXTURE_ADDRESS+4), &imageInfo);
	LoadImage(&rect, imageInfo.pixel);
	DrawSync(0);
	setRECT(&rect, 320, 480, imageInfo.cw, imageInfo.ch);
	LoadImage(&rect, imageInfo.clut);
	DrawSync(0);
	
	MakeTmd(&craft);

	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;

	setRGB(&leftSprite, 127, 127, 127);
	setRGB(&rightSprite, 127, 127, 127);
	
	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);
	}
	
	activeBuff = GsGetActiveBuff();
	GsSetWorkBase((PACKET *)packetArea[activeBuff]);
	GsClearOt(0, 0, &WorldOT[activeBuff]);
	DrawSync(0);
	GsSwapDispBuff();
	GsSortClear(0, 0, 0, &WorldOT[activeBuff]);
	GsDrawOt(&WorldOT[activeBuff]);

	activeBuff = GsGetActiveBuff();
	GsSetWorkBase((PACKET *)packetArea[activeBuff]);
	GsClearOt(0, 0, &WorldOT[activeBuff]);
	DrawSync(0);
	GsSwapDispBuff();
	GsSortClear(0, 0, 0, &WorldOT[activeBuff]);
	GsDrawOt(&WorldOT[activeBuff]);
	
	j = 2048;
	do {
		pad = ReadPad();
		activeBuff = GsGetActiveBuff();
		GsSetWorkBase((PACKET *)packetArea[activeBuff]);
		GsClearOt(0, 0, &WorldOT[activeBuff]);
		
		craft.rot.vx -= 17;
		craft.rot.vy += 19;
		craft.rot.vz -= 13;

		posRot.vx -= 23;
		posRot.vy += 17;
		posRot.vz -= 11;

		RotMatrix(&posRot, &rot);
		craft.pos.vx = 50;
		craft.pos.vy = 40;
		craft.pos.vz = 30;
		ApplyMatrixSV(&rot, &craft.pos, &craft.pos);
		
		RotMatrix(&craft.rot, &rot);
		for (i=0; i<polyCount; i++) {
			MulMatrix0(&rot, &vertexList[i], &temp);
			polyList[i]->u0 = (temp.m[0][0])+128;
			polyList[i]->v0 = (temp.m[1][0])+128;
			polyList[i]->u1 = (temp.m[0][1])+128;
			polyList[i]->v1 = (temp.m[1][1])+128;
			polyList[i]->u2 = (temp.m[0][2])+128;
			polyList[i]->v2 = (temp.m[1][2])+128;
		}
		
		MoveObject(&craft.pos, &craft.rot, &craft.coord);

		haloSize.vx = 0;
		haloSize.vz = 1024;
		ResetMatrix(rot.m);
		RotMatrixY(j, &rot);
		ApplyMatrixSV(&rot, &haloSize, &haloSize);

		j+=8;
		
		if (haloSize.vx < 0)
			haloSize.vx = 0;
		
		rightSprite.scalex		= leftSprite.scalex		= ONE+haloSize.vx;
		rightSprite.scaley		= leftSprite.scaley		= ONE+haloSize.vx;

		GsSetRefView2(&view);
		GsGetLs(&(craft.coord), &tmpls);
		GsSetLsMatrix(&tmpls);
		GsSortObject4(&(craft.handler), &WorldOT[activeBuff], 3, getScratchAddr(0));
		
		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(0);
		GsSwapDispBuff();
		GsDrawOt(&WorldOT[activeBuff]);

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


void InitGame(void) {
	int i;
	
	SetVideoMode(MODE_PAL);
	GsInitGraph(SCREEN_WIDTH, SCREEN_HEIGHT, GsINTER|GsOFSGPU, 0, 0);
	
	GsDefDispBuff(0, 0, 0, SCREEN_HEIGHT);
	GsDISPENV.screen.h = 255;
	
	GsInit3D();
	
	GetPadBuf(&bb0, &bb1);
	
	for (i = 0; i < 2; i++) {
		WorldOT[i].length = OT_LENGTH;
		WorldOT[i].org = OTTags[i];
		GsClearOt(0,0,&WorldOT[i]);
	}
}


void MakeTmd(ObjectHandler *craft) {
	COLOUR rgb[16*16*2];
	VERTEX v[16*16*2];
	TEXTURE txt[1];
	u_long *pointer;
	u_long address = TMD_DYNAMIC_START;
	int i, j=0;
	int ang, r0, r1, p;
	SVECTOR vec;
	MATRIX rot;
	VERTEX normal[16*16*2];
	
	PrepareTMD(address, &v[0], &rgb[0], &txt[0]);

	for (ang=0; ang<16; ang++) {
		for (i=0; i<16; i++) {
			// Make a vertex
			vec.vx = 0;
			vec.vy = 0;
			vec.vz = 20;
			ResetMatrix(rot.m);
			RotMatrixX(i*-256, &rot);
			ApplyMatrixSV(&rot, &vec, &vec);
			
			vec.vz += 30;
			ResetMatrix(rot.m);
			RotMatrixY(ang*256, &rot);
			ApplyMatrixSV(&rot, &vec, &vec);
			SetVertex(&v[j], vec.vx, vec.vy, vec.vz);
			SetColour(&rgb[j], vec.vx+128, vec.vy+128, vec.vz+128);

			// Make the corresponding normal
			vec.vx = 0;
			vec.vy = 0;
			vec.vz = 20*6;
			ResetMatrix(rot.m);
			RotMatrixX(i*-256, &rot);
			ApplyMatrixSV(&rot, &vec, &vec);
			
			ResetMatrix(rot.m);
			RotMatrixY(ang*256, &rot);
			ApplyMatrixSV(&rot, &vec, &vec);
			SetVertex(&normal[j], vec.vx, vec.vy, vec.vz);
			j++;
		}
	}
		
	SetTexture(&txt[0], 320, 0, 320+255, 0, 320, 0+255, 320+255, 0+255, 1, 320, 480);
	
	j = 0;
	for (ang=0; ang<16; ang++) {
		r0 = ang*16;
		r1 = ((ang+1)%16)*16;
		
		for (i=0; i<16; i++) {
			p = (i+1)%16;
			SetMatrix(&vertexList[j], &normal[r0+i], &normal[r0+p], &normal[r1+i]);
			j++;
			SetMatrix(&vertexList[j], &normal[r1+p], &normal[r1+i], &normal[r0+p]);
			j++;
			polyList[polyCount++] = SetTX_NS_GP_TRI(r0+i, r0+p, r1+i, r0+i, r0+p, r1+i, 0);
			polyList[polyCount++] = SetTX_NS_GP_TRI(r1+p, r1+i, r0+p, r1+p, r1+i, r0+p, 0);
		}
	}

	address = 0;

	// Link model
	pointer = (u_long *)FinishTMD();
	pointer++;
	GsMapModelingData(pointer);
	pointer += 2;
	GsLinkObject4((u_long )pointer, &craft->handler, 0);
}


void MoveObject(SVECTOR *pos, SVECTOR *rot, GsCOORDINATE2 *coordSystem) {
	MATRIX tempMatrix;

	tempMatrix.t[0] = coordSystem->coord.t[0];
	tempMatrix.t[1] = coordSystem->coord.t[1];
	tempMatrix.t[2] = coordSystem->coord.t[2];
	
	RotMatrix(rot, &tempMatrix);		// get rotation matrix from rotation vector
	tempMatrix.t[0] = pos->vx;
	tempMatrix.t[1] = pos->vy;
	tempMatrix.t[2] = pos->vz;
	coordSystem->coord = tempMatrix;	// assign new matrix to coordinate system
	coordSystem->flg = 0;				// tell GTE that coordinate system has been updated
}


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;
}


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