#include <libps.h>
#include "header.h"

GsOT		WorldOT[2];	
GsOT_TAG	OTTags[2][1<<OT_LENGTH];
PACKET packetArea[2][PACKETMAX2];
PadInfo pad;
TrackData trackData;
GsRVIEW2 view;
VERTEX vertexLut[(MAX_SEGMENTS+1)*6];
TX_NS_FP_TRI_FACE primitiveLut[MAX_SEGMENTS*12];
SegmentInfo *rawTrack;
RacerHandler racer;
GsF_LIGHT lights[3];
int projection;
GsIMAGE racerTexInfo, reflectTexInfo, trackTexInfo;
SVECTOR camRot;
TrackObjectHandler trackObjects;

void main(void);
void InitGame(void);


void main(void) {
	int activeBuff, vSync, i, bonk, cameraChange, racerAng, camAng;
	VECTOR targetVel, camPos;
	MATRIX tempMatrix;
	GsFOGPARAM fog;

	InitGame();
	
	InitTrack();
	InitRacer();
	InitBackground();
	
	// MUST be done after InitRacer()
//	InitParticle(976, 0);
//	CreatePalette(976, 12, 255, 255, 0, 255, 0, 0);
//	CreatePalette(976, 13, 255, 255, 0, 0, 255, 0);

	view.rz = 180*ONE;
	view.super = WORLD;
	SetVector(&camRot, 0, 0, 0);

	// Init fog
	fog.rfc = 120; fog.gfc = 170; fog.bfc = 225;
	fog.dqb = 1.25*4096*4096;
	do {
		ReadPad(&pad);
		activeBuff = GsGetActiveBuff();
		GsSetWorkBase((PACKET *)packetArea[activeBuff]);
		GsClearOt(0, 0, &WorldOT[activeBuff]);
		
		// Read pad
		racer.rot.vy = (racer.rot.vy-(pad.leftStickH>>1)) & 4095;
		if (pad.button & PADLX)
			racer.speed += (MAX_SPEED - racer.speed)>>6;
		if (pad.button & PADLS) {
			racer.speed -= 3<<8;
			if (racer.speed < 0)
				racer.speed = 0;
		}
		// Gradual slow down (slow quicker when turning)
		racer.speed = ((racer.speed*255)>>8)-((racer.speed*Abs(pad.leftStickH))>>13);
		
		// Get camera rotation
		racerAng = racer.rot.vy;
		camAng = camRot.vy;
		if (racerAng-camAng > 2047)
			camAng += 4096;
		else {
			if (camAng-racerAng > 2047)
				racerAng += 4096;
		}
		camRot.vy = (camRot.vy + ((racerAng-camAng)>>3)) & 4095;
		
		// Get velocity
		ResetMatrix(tempMatrix.m);
		RotMatrixX(racer.rot.vx, &tempMatrix);
		RotMatrixZ(racer.rot.vz, &tempMatrix);
		RotMatrixY(racer.rot.vy, &tempMatrix);
		SetVector(&targetVel, 0, 0, racer.speed);
		ApplyMatrixLV(&tempMatrix, &targetVel, &targetVel);
		racer.vel.vx += ((targetVel.vx-racer.vel.vx)*3)>>6;
		racer.vel.vy += ((targetVel.vy-racer.vel.vy)*3)>>6;
		racer.vel.vz += ((targetVel.vz-racer.vel.vz)*3)>>6;
		// Get collision points
		SetVector(&racer.collisionPoint[0], 0, 0, 64<<8);
		SetVector(&racer.collisionPoint[1], -24<<8, 0, -16<<8);
		SetVector(&racer.collisionPoint[2], 24<<8, 0, -16<<8);
		ApplyMatrixLV(&tempMatrix, &racer.collisionPoint[0], &racer.collisionPoint[0]);
		ApplyMatrixLV(&tempMatrix, &racer.collisionPoint[1], &racer.collisionPoint[1]);
		ApplyMatrixLV(&tempMatrix, &racer.collisionPoint[2], &racer.collisionPoint[2]);

		racer.oldPos = racer.pos;
		bonk = TrackCollide();
		racer.pos.vx += racer.vel.vx;
		racer.pos.vz += racer.vel.vz;

		GetHeight();
		if (racer.heightOffTrack <= 0)
			racer.heightOffTrack = 1;
		for (i=0; i<4; i++)
			racer.height[i] += (racer.vel.vy<<3)/racer.heightOffTrack;
		racer.pos.vy = racer.height[3];
	
		UpdateRacer(&racer.pos, &racer.rot, &racer.coord);

		// Get camera info
		projection = LimitRange(160-(racer.speed>>8), 90, 160);
		cameraChange = 160 - projection;	// 0 at rest, 70 at max projection
		RotMatrix(&camRot, &tempMatrix);
		SetVector(&camPos, 0, 0, (80+(cameraChange>>1))<<8);
		ApplyMatrixLV(&tempMatrix, &camPos, &camPos);
		view.vrx = (racer.pos.vx+camPos.vx)>>8;
		view.vry = (racer.pos.vy+camPos.vy)>>8;
		view.vrz = (racer.pos.vz+camPos.vz)>>8;
		SetVector(&camPos, 0, 40, -(cameraChange>>1)-120);
		ApplyMatrixLV(&tempMatrix, &camPos, &camPos);
		view.vpx = view.vrx + camPos.vx;
		view.vpy = view.vry + 135-(cameraChange>>1);
		view.vpz = view.vrz + camPos.vz;
		
//		AddStreamParticle();
		// Draw frame
		GsSetProjection(projection);
		GsSetRefView2(&view);
		fog.dqa = -70*4096/projection;
		GsSetFogParam(&fog);
		DrawTrack(&WorldOT[activeBuff], 0);
		DrawTrackObjects(&WorldOT[activeBuff], 0);
		DrawRacer(&WorldOT[activeBuff], 2);
		GsSetProjection(160);
		DrawBackground(&WorldOT[activeBuff]);
//		DrawParticles(&WorldOT[activeBuff], 5);
		
		DrawSync(0);
		vSync = VSync(0);
		if (vSync < 295)
			VSync(0);
		GsSwapDispBuff();
		GsSortClear(0, 105, 0, &WorldOT[activeBuff]);
		GsDrawOt(&WorldOT[activeBuff]);
		// Use FntPrint Here
		FntPrint(0, "%d\n%d\n", rawTrack->numSegments, vSync);
		if (bonk)
			FntPrint(0, "BONK\n");
		FntFlush(0);
//		FntPrint(1, "%d %d\n", racer.rot.vy,);
//		FntPrint(1, "%d %d\n%d %d\n%d %d\n%d %d\n\n", racer.v0->vx, racer.v0->vz, racer.v1->vx, racer.v1->vz, racer.v2->vx, racer.v2->vz, racer.v3->vx, racer.v3->vz);
//		FntPrint(1, "%d\n", camHeight);
//		FntFlush(1);
	} while ((pad.button != PADLstart+PADLselect) && (pad.button != PADRselect+PADRstart));
//	StoreScreen(0x80090000, 0, 0, SCREEN_WIDTH, SCREEN_HEIGHT);
}


void InitGame(void) {
	int i;
	
	SetVideoMode(MODE_PAL);
	GsInitGraph(SCREEN_WIDTH, SCREEN_HEIGHT, GsINTER|GsOFSGPU, 1, 0);
	
	GsDefDispBuff(0, 0, 0, SCREEN_HEIGHT);
	GsDISPENV.screen.x = 6;
	GsDISPENV.screen.y = 16;
	GsDISPENV.screen.h = 255;
	
	InitPad();
	GsInit3D();
	
	GsSetLightMode(1);
	
	for (i=0; i<2; i++) {
		WorldOT[i].length = OT_LENGTH;
		WorldOT[i].org = OTTags[i];
		GsClearOt(0,0,&WorldOT[i]);
	}
	// Open 2 font streams
	FntLoad(640, 256);	
	FntOpen(-144, -96, 288, 64, 0, 16);
//	FntOpen(32, -110, 96, 110, 0, 256);


	SetVector(&lights[0], 0, -181, 181);
	SetVector(&lights[1], -157, -181, -90);
	SetVector(&lights[2], 157, -181, -90);
	SetRGB(&lights[0], 160, 96, 96);
	SetRGB(&lights[1], 96, 160, 96);
	SetRGB(&lights[2], 96, 96, 160);
	GsSetFlatLight(0, &lights[0]);
	GsSetFlatLight(1, &lights[1]);
	GsSetFlatLight(2, &lights[2]);
	LoadTexture(RACER_TEX_ADDRESS, &racerTexInfo);
	LoadTexture(REFLECT_TEX_ADDRESS, &reflectTexInfo);
	LoadTexture(TRACK_TEX_ADDRESS, &trackTexInfo);
}