#include "header.h"
#include "Objects.h"

OBJECT *objectLut;

void InitTrack(void) {
	COLOUR rgb[3];
	TEXTURE tex[6];
	int i, px, py, cx, cy;
	u_long *pointer;

	rawTrack = (SegmentInfo *)RAW_TRACK_DATA;

	// All patches share same coord information
	GsInitCoordinate2(WORLD, &trackData.coord);
	trackData.coord.flg = 0;
	PrepareTMD(&vertexLut[0], &rgb[0], &tex[0]);
	SetTmdHeader((HEADER *)TMD_DYNAMIC_START);
	objectLut = (OBJECT *)((HEADER *)TMD_DYNAMIC_START + 1);

	// Init first segment info
	SetVertex(&vertexLut[0], -SEGMENT_WIDTH>>1, 0, 0);
	SetVertex(&vertexLut[1], SEGMENT_WIDTH>>1, 0, 0);
	SetVertex(&vertexLut[2], -SEGMENT_WIDTH>>1, 0, SEGMENT_LENGTH);
	SetVertex(&vertexLut[3], SEGMENT_WIDTH>>1, 0, SEGMENT_LENGTH);
	SetVertex(&vertexLut[0+SIDE_OFFSET], (-SEGMENT_WIDTH>>1)-SIDE_WIDTH, SIDE_HEIGHT, 0);
	SetVertex(&vertexLut[1+SIDE_OFFSET], (SEGMENT_WIDTH>>1)+SIDE_WIDTH, SIDE_HEIGHT, 0);
	SetVertex(&vertexLut[2+SIDE_OFFSET], (-SEGMENT_WIDTH>>1)-SIDE_WIDTH, SIDE_HEIGHT, SEGMENT_LENGTH);
	SetVertex(&vertexLut[3+SIDE_OFFSET], (SEGMENT_WIDTH>>1)+SIDE_WIDTH, SIDE_HEIGHT, SEGMENT_LENGTH);

	// Create all the segments
	px = trackTexInfo.px;
	py = trackTexInfo.py;
	cx = trackTexInfo.cx;
	cy = trackTexInfo.cy;
	SetColour(&rgb[0], 128, 128, 128);
	SetColour(&rgb[1], 160, 160, 160);
	SetColour(&rgb[2], 96, 96, 96);
	// Each texture spans 2 track segments
	SetTexture(&tex[0], px, py, 0, 16, 127, 16, 0, 0, 127, 0, 1, cx, cy);
	SetTexture(&tex[1], px, py, 0, 31, 127, 31, 0, 16, 127, 16, 1, cx, cy);

	SetTexture(&tex[2], px, py, 64, 112, 64, 96, 31+64, 112, 31+64, 96, 1, cx, cy);
	SetTexture(&tex[3], px, py, 64, 127, 64, 112, 31+64, 127, 31+64, 112, 1, cx, cy);
	
	SetTexture(&tex[4], px, py, 64, 112, 31+64, 112, 64, 96, 31+64, 96, 1, cx, cy);
	SetTexture(&tex[5], px, py, 64, 127, 31+64, 127, 64, 112, 31+64, 112, 1, cx, cy);

	// The segment primitives are spaced in intervals of 12
	for (i=0; i<MAX_SEGMENTS; i++) {
		// Each call makes a square out of 2 triangles
		MakeSegment(&primitiveLut[i*12], (i<<1), (i<<1)+1, (i<<1)+2, (i<<1)+3, 0, i&1);
		MakeSegment(&primitiveLut[i*12+2], (i<<1), (i<<1)+2, (i<<1)+SIDE_OFFSET, (i<<1)+2+SIDE_OFFSET, 1, 2+(i&1));
		MakeSegment(&primitiveLut[i*12+4], (i<<1)+1, (i<<1)+1+SIDE_OFFSET, (i<<1)+3, (i<<1)+3+SIDE_OFFSET, 2, 4+(i&1));
	}
	
	// Link objects to primitives
	for (i=0; i<MAX_SEGMENTS; i++) {
		objectLut[i].vert_top = (u_long *)&vertexLut[0];
		objectLut[i].n_vert = (MAX_SEGMENTS<<1)+2;
		objectLut[i].normal_top = (u_long *)NULL;
		objectLut[i].n_normal = 0;
		objectLut[i].primitive_top = (u_long *)&primitiveLut[i*12];
		objectLut[i].n_primitive = 6;
		objectLut[i].scale = 0;
		
		if (i == 0) {
			pointer = (u_long *)TMD_DYNAMIC_START;
			pointer++;
			GsMapModelingData(pointer);
			pointer += 2;
		}
		GsLinkObject4((u_long )pointer, &trackData.handler[i], i);
		trackData.handler[i].attribute = 0;
		trackData.handler[i].coord2 = &trackData.coord;
	}
	MakeTrack();
}


void MakeTrack(void) {
	VERTEX *v1, *v2, *v3, *v4;
	MATRIX tm;
	VECTOR pos;
	SVECTOR rot;
	int i, j, a=0, y0=0, y1=0;
	char ang, dy0, dy1;
	u_char type, r, g, b;
	u_long info;
	
	trackObjects.numObjects = 0;
	if (track1[0].segment==0 && track1[0].type!=NO_MORE) {
		trackObjects.numObjects++;
		trackObjects.x[0] = -track1[0].dis;
		trackObjects.z[0] = 0;
		trackObjects.rot[0] = track1[0].rot;
		SetVector(&pos, trackObjects.x[0], track1[0].height, trackObjects.z[0]);
		SetVector(&rot, 0, trackObjects.rot[0], 0);
		switch (track1[0].type) {
			case BLOCK:
				LinkObject((u_long *)BLOCK_ADDRESS, &trackObjects.coord[0], &trackObjects.handler[0]);
				break;
			case FACTORY:
				LinkObject((u_long *)FACTORY_ADDRESS, &trackObjects.coord[0], &trackObjects.handler[0]);
				break;
			case HIRISE:
				LinkObject((u_long *)HIRISE_ADDRESS, &trackObjects.coord[0], &trackObjects.handler[0]);
				break;
			case PYRAMID:
				LinkObject((u_long *)PYRAMID_ADDRESS, &trackObjects.coord[0], &trackObjects.handler[0]);
				break;
			case TOWER:
				LinkObject((u_long *)TOWER_ADDRESS, &trackObjects.coord[0], &trackObjects.handler[0]);
				break;
		}
		MoveObject(&pos, &rot, &trackObjects.coord[0]);
	}
	
	for (i=1; i<rawTrack->numSegments; i++) {
		// Extract track information
		info = rawTrack->info[i];
		ang = (info&15)-8;
		dy0 = ((info&(63<<4))>>4)-32;
		dy1 = ((info&(63<<10))>>10)-32;
		type = ((info&(31<<16))>>16);
		r = ((info&(15<<20))>>20);
		g = ((info&(15<<24))>>24);
		b = ((info&(15<<28))>>28);
		
		a -= ang<<5;
		y0 += dy0<<2;
		y1 += dy1<<2;
		v1 = &vertexLut[(i<<1)-2];
		v2 = &vertexLut[(i<<1)-1];
		v3 = &vertexLut[i<<1];
		v4 = &vertexLut[(i<<1)+1];
		if (ang > 0) {
			tm.m[0][0] = 0;
			tm.m[2][0] = SEGMENT_LENGTH;
			tm.m[0][1] = SEGMENT_WIDTH;
			tm.m[2][1] = SEGMENT_LENGTH;
			RotMatrixY(a, &tm);
			SetVertex(&vertexLut[i<<1], v1->vx+tm.m[0][0], y1, v1->vz+tm.m[2][0]);
			SetVertex(&vertexLut[(i<<1)+1], v1->vx+tm.m[0][1], y0, v1->vz+tm.m[2][1]);
			tm.m[0][0] = -SIDE_WIDTH;
			tm.m[2][0] = SEGMENT_LENGTH;
			tm.m[0][1] = SEGMENT_WIDTH+SIDE_WIDTH;
			tm.m[2][1] = SEGMENT_LENGTH;
			RotMatrixY(a, &tm);
			SetVertex(&vertexLut[(i<<1)+SIDE_OFFSET], v1->vx+tm.m[0][0], y1+SIDE_HEIGHT, v1->vz+tm.m[2][0]);
			SetVertex(&vertexLut[(i<<1)+1+SIDE_OFFSET], v1->vx+tm.m[0][1], y0+SIDE_HEIGHT, v1->vz+tm.m[2][1]);
		}
		else {
			tm.m[0][0] = -SEGMENT_WIDTH;
			tm.m[2][0] = SEGMENT_LENGTH;
			tm.m[0][1] = 0;
			tm.m[2][1] = SEGMENT_LENGTH;
			RotMatrixY(a, &tm);
			SetVertex(&vertexLut[i<<1], v2->vx+tm.m[0][0], y1, v2->vz+tm.m[2][0]);
			SetVertex(&vertexLut[(i<<1)+1], v2->vx+tm.m[0][1], y0, v2->vz+tm.m[2][1]);
			tm.m[0][0] = -SEGMENT_WIDTH-SIDE_WIDTH;
			tm.m[2][0] = SEGMENT_LENGTH;
			tm.m[0][1] = SIDE_WIDTH;
			tm.m[2][1] = SEGMENT_LENGTH;
			RotMatrixY(a, &tm);
			SetVertex(&vertexLut[(i<<1)+SIDE_OFFSET], v2->vx+tm.m[0][0], y1+SIDE_HEIGHT, v2->vz+tm.m[2][0]);
			SetVertex(&vertexLut[(i<<1)+1+SIDE_OFFSET], v2->vx+tm.m[0][1], y0+SIDE_HEIGHT, v2->vz+tm.m[2][1]);
		}
		
		// Init track object
		j = trackObjects.numObjects;
		if (track1[j].segment==i && track1[j].type != NO_MORE) {
			trackObjects.numObjects++;
			// Get position relative to track segment
			tm.m[0][0] = -track1[j].dis;
			tm.m[2][0] = 0;
			RotMatrixY(a, &tm);
			trackObjects.x[j] = v3->vx+tm.m[0][0];
			trackObjects.z[j] = v3->vz+tm.m[2][0];
			trackObjects.rot[j] = track1[j].rot+a;
			SetVector(&pos, trackObjects.x[j], v3->vy+track1[j].height, trackObjects.z[j]);
			SetVector(&rot, 0, trackObjects.rot[j], 0);
			switch (track1[j].type) {
				case BLOCK:
					LinkObject((u_long *)BLOCK_ADDRESS, &trackObjects.coord[j], &trackObjects.handler[j]);
					break;
				case FACTORY:
					LinkObject((u_long *)FACTORY_ADDRESS, &trackObjects.coord[j], &trackObjects.handler[j]);
					break;
				case HIRISE:
					LinkObject((u_long *)HIRISE_ADDRESS, &trackObjects.coord[j], &trackObjects.handler[j]);
					break;
				case PYRAMID:
					LinkObject((u_long *)PYRAMID_ADDRESS, &trackObjects.coord[j], &trackObjects.handler[j]);
					break;
				case TOWER:
					LinkObject((u_long *)TOWER_ADDRESS, &trackObjects.coord[j], &trackObjects.handler[j]);
					break;
				default:
					printf("Doh! %d\n", track1[j].type);
					break;
			}
			MoveObject(&pos, &rot, &trackObjects.coord[j]);
		}
	}
	i = rawTrack->numSegments;
	v1 = &vertexLut[0];
	v2 = &vertexLut[1];
	SetVertex(&vertexLut[i<<1], v1->vx, v1->vy, v1->vz);
	SetVertex(&vertexLut[(i<<1)+1], v2->vx, v2->vy, v2->vz);
	v1 = &vertexLut[SIDE_OFFSET];
	v2 = &vertexLut[1+SIDE_OFFSET];
	SetVertex(&vertexLut[(i<<1)+SIDE_OFFSET], v1->vx, v1->vy, v1->vz);
	SetVertex(&vertexLut[(i<<1)+1+SIDE_OFFSET], v2->vx, v2->vy, v2->vz);
}


void DrawTrack(GsOT *ot, int offset) {
	MATRIX ls;
	register int i, s, e, mod;
	VERTEX *v0, *v1;
	GsDOBJ2 *handler = trackData.handler;
	
	// Init drawing suff
		mod = rawTrack->numSegments;
	GsGetLs(&trackData.coord, &ls);
	GsSetLsMatrix(&ls);
	
	// Determine if the camera is facing up or down the track
	s = racer.heightSegment[3]<<1;
	e = ((racer.heightSegment[3]+1)%mod)<<1;
	v0 = &vertexLut[s];
	v1 = &vertexLut[e];
	s = InvTan(v0->vz-v1->vz, v0->vx-v1->vx);	// Angle of track side
	// Get difference between craft angle and track angle
	e = Abs(s-camRot.vy);
	if (e > 2048)
		e = 4096-e;

	// Craft facing correct direction
	if (e > 1024) {
		s = racer.heightSegment[3]+rawTrack->numSegments-16;
		e = s+12;
		// Draw back most segments with 2x2 subdivision
		for (i=s; i<e; i++) {
			handler[i%mod].attribute = (1<<9);
			GsSortObject4(&handler[i%mod], ot, offset, getScratchAddr(0));
			
		}
		s = e;
		e += 10;
		// Draw most viewable tiles with 4x4 subdivision
		for (i=s; i<e; i++) {
			handler[i%mod].attribute = (2<<9);
			GsSortObject4(&handler[i%mod], ot, offset, getScratchAddr(0));
		}
		s = e-1;
		e += 8;
		// Draw further tiles with 2x2 subdivision
		for (i=s; i<e; i++) {
			handler[i%mod].attribute = (1<<9);
			GsSortObject4(&handler[i%mod], ot, offset, getScratchAddr(0));
		}
		s = e-1;
		e += 66;
		// Draw distant tiles with no subdivision
		for (i=s; i<e; i++) {
			handler[i%mod].attribute = 0;
			GsSortObject4(&handler[i%mod], ot, offset, getScratchAddr(0));
		}
	}
	// Wrong Way!
	else {
		s = racer.heightSegment[3]+rawTrack->numSegments-80;
		e = s+67;
		// Draw distant tiles with no subdivision
		for (i=s; i<e; i++) {
			handler[i%mod].attribute = 0;
			GsSortObject4(&handler[i%mod], ot, offset, getScratchAddr(0));
		}
		s = e-1;
		e += 8;
		// Draw further tiles with 2x2 subdivision
		for (i=s; i<e; i++) {
			handler[i%mod].attribute = (1<<9);
			GsSortObject4(&handler[i%mod], ot, offset, getScratchAddr(0));
		}
		s = e-1;
		e += 9;
		// Draw most viewable tiles with 4x4 subdivision
		for (i=s; i<e; i++) {
			handler[i%mod].attribute = (2<<9);
			GsSortObject4(&handler[i%mod], ot, offset, getScratchAddr(0));
		}
		s = e;
		e += 12;
		// Draw back most segments with 2x2 subdivision
		for (i=s; i<e; i++) {
			handler[i%mod].attribute = (1<<9);
			GsSortObject4(&handler[i%mod], ot, offset, getScratchAddr(0));
		}
	}
}


void DrawTrackObjects(GsOT *ot, int offset) {
	int i, dx, dz, dis;
	MATRIX ls;
	
	for (i=0; i<trackObjects.numObjects; i++) {
		dx = trackObjects.x[i] - (racer.pos.vx>>8);
		dz = trackObjects.z[i] - (racer.pos.vz>>8);
		dis = dx*dx+dz*dz;
		// If object is closer than 100 segment lengths then draw it
		if (dis < 6400*6400) {
			// Subdivide if close
//			if (dis < 1280*1280)
				trackObjects.handler[i].attribute = (2<<9);
//			else
//				trackObjects.handler[i].attribute = 0;
			GsGetLs(&trackObjects.coord[i], &ls);
			GsSetLsMatrix(&ls);
			GsSortObject4(&trackObjects.handler[i], ot, offset, getScratchAddr(0));
		}
	}
}