/************************************************************
*
* Net Yaroze RollerCoaster
*
***********************************************************/

#include <libps.h>
#include <memory.h>
#include "pad.h"
#include "tmdtype.h"

#define	Y_ADJUST		-400

#define CONEMAN_ADDRESS 		0x80090000
#define TEXTURE1_ADDRESS		0x800f0000
#define CAM_ADDRESS				0x800f9000
#define VERTEX_ADDRESS			0x800e0000 + 144

#define	MAX_FIXUP_POLYS			500

#define NUM_STEPS			219 
#define FACES_PER_STEP		3
#define	VERTICES_PER_STEP	4

#define OT_LENGTH			9

#define PACKETMAX2 (4000*24)				/* Max GPU packets */
static PACKET packetArea[2][PACKETMAX2]; 	/* GPU PACKETS AREA */

static GsOT Wot[2];							/* Handler of OT */
static GsOT_TAG wtags[2][1<<OT_LENGTH];
static GsOT groundot[2];					/* Handler of OT */
static GsOT_TAG groundtags[2][1<<OT_LENGTH];
static GsOT cot[2];							/* Handler of OT */
static GsOT_TAG ctags[2][1<<OT_LENGTH];

#define	FLOOR_VERTICES	21
#define	FLOOR_SIZE		800

typedef struct {
	TMDHEADER header;
	TMDOBJTABLE objtable;
	TMDPRIM_FLT4 packets[(FLOOR_VERTICES-1)*(FLOOR_VERTICES-1)];
	SVECTOR vertex[FLOOR_VERTICES*FLOOR_VERTICES];
	SVECTOR normal[1];
} TMDSTRUCT_FLOOR;

typedef struct {
	TMDHEADER header;
	TMDOBJTABLE objtable;
	TMDPRIM_FLT4 packets[(NUM_STEPS)];
	TMDPRIM_FL4 packetsw[(NUM_STEPS*2)];
	TMDPRIM_FL4 packets2[MAX_FIXUP_POLYS];
	SVECTOR vertex[NUM_STEPS*VERTICES_PER_STEP];
	SVECTOR normal[NUM_STEPS*FACES_PER_STEP];
} TMDSTRUCT;

typedef struct {
	GsDOBJ2		handler;
	VECTOR		position;	 // position
	SVECTOR		rotate;		 // angle
	GsCOORDINATE2 coord; // coordinate system
	MATRIX		matrix;			// relation to SUPER
} ObjectHandler;

static GsRVIEW2 view;
GsF_LIGHT		TheLights[3];
GsIMAGE			tex1,camimg;
GsSPRITE		cam;
GsFOGPARAM		fog;

int				ProjectionDistance;
ObjectHandler	levelwalls,levelfloor, coneman;
TMDSTRUCT		mapobject;
TMDSTRUCT_FLOOR	mapfloor;
VECTOR			conemanpos;
SVECTOR			points[24];
int				screenwidth,screenheight;

volatile u_char *bb0, *bb1;	   // low-level pad buffers: never need to touch

/****************************************************************************
 prototypes
****************************************************************************/

void main (void);
void InitialiseAll(void);
void InitGsSprite (GsSPRITE* sprite);

void DrawObject(ObjectHandler * object, short tx, short ty, short tz, int side);
void LoadTexture(u_long * addr, GsIMAGE * image);
void SetUpObject (ObjectHandler * object, u_long * obj_addr, short link);
void UpdateObjectCoordinates (SVECTOR* rotationVector, GsCOORDINATE2* coordSystem);

void CreatePrimitive(TMDPRIM_FLT4 * prim, short type, short normal, short v0, short v1, short v2, short v3);
void CreateQuadPrim(TMDPRIM_FL4 * prim,short normal, short v0, short v1, short v2, short v3, char r, char g, char b);
void CreateRoller();
void CreateFloor();

int DealWithControllerPad(void);
void PadInit (void);
u_long PadRead(void);


void FindPosition(SVECTOR * temp, SVECTOR * tangent, int * dist, int * index);
void CalculatePointOnLine(SVECTOR p1, SVECTOR p2, SVECTOR * dest, int point);
void FindBCPoint(int point, SVECTOR * tangent, SVECTOR * temp,
	SVECTOR p1, SVECTOR p2, SVECTOR p3, SVECTOR p4);


/****************************************************************************
functions
****************************************************************************/
#define BLINK_VISIBLE	10
#define BLINK_HIDDEN	4
//#define V_SPEED		800; //1400 //1200
void main (void)
{
	long	blinkcount;
	RECT	Bg,area;
	int		i,side;
	double	x,y,z;
	SVECTOR	tangent;
	double	drad,len;
	SVECTOR temp;
	int	dist,dist2;
	int bindex,b2index;

	MATRIX  tmpls, tmplw;

	InitialiseAll();

	blinkcount = 0;
	bindex = b2index = 0;
	
	dist = 0; 
	dist2 = 250; 

	for(;;) {
		
		FindPosition(&temp,&tangent,&dist2,&b2index);

		/* update camera reference point */
		view.vrx = temp.vx / 5;
		view.vry = (temp.vy / 5) + (Y_ADJUST);
		view.vrz = temp.vz/ 5;

		/* reference point is also used for coneman position */
		conemanpos.vx = temp.vx / 5;
		conemanpos.vy = (temp.vy / 5);
		conemanpos.vz = temp.vz/ 5;

		/* calculate the y-rotation for coneman */
		x = tangent.vx / 5;
		y = tangent.vy / 5;
		z = tangent.vz / 5;

		drad = atan(z/x);
		drad = (drad*180)/3.14;
		if (x < 0) drad += 180;
		coneman.rotate.vy = -(drad*4096)/360;
		coneman.rotate.vy += 2048; //3072-(2048/2);
		
		/* just approximate the z-rotation */
		drad = (y/30);
		coneman.rotate.vz = -(drad*4096)/360;

		FindPosition(&temp,&tangent,&dist,&bindex);

		/* update camera point */
		view.vpx = temp.vx / 5;
		view.vpy = (temp.vy / 5) + (Y_ADJUST*2) ;
		view.vpz = temp.vz/ 5;

		GsSetRefView2(&view);

		side = GsGetActiveBuff();
		GsClearOt(0,0,&Wot[side]);
		GsClearOt(0,0,&groundot[side]);						
		GsClearOt(0,0,&cot[side]);						
		GsSetWorkBase((PACKET*)packetArea[side]);

		blinkcount++;
		if (blinkcount > BLINK_HIDDEN+BLINK_VISIBLE) blinkcount = 0;
		if (blinkcount < BLINK_VISIBLE) GsSortSprite(&cam, &Wot[side], 1);

		/* draw map walls  */

		UpdateObjectCoordinates(&levelwalls.rotate, &levelwalls.coord);		
		GsGetLs(&(levelwalls.coord), &tmpls);
		GsSetLightMatrix(&tmpls);
		GsSetLsMatrix(&tmpls);
		GsSortObject4( &(levelwalls.handler), &Wot[side], 3, getScratchAddr(0));


		UpdateObjectCoordinates(&levelfloor.rotate, &levelfloor.coord);		
		GsGetLs(&(levelfloor.coord), &tmpls);
		GsSetLightMatrix(&tmpls);
		GsSetLsMatrix(&tmpls);
		GsSortObject4( &(levelfloor.handler), &groundot[side], 3, getScratchAddr(0));
					
		/* update player position, set rotation & draw */
		DrawObject(&coneman,conemanpos.vx, conemanpos.vy, conemanpos.vz, side);

		/* wait for drawing to complete */
		//DrawSync(0);
		VSync(2);
		//DrawSync(0);
		GsSwapDispBuff();
		
		// draw background gradient
	 	Bg.y = (1-side)*256;
		area.x = 960;
		area.w = 64;
		area.h = 256; 
		area.y = 256;
		for (i=0; i<10; i++) MoveImage(&area,i*64,Bg.y);

		GsDrawOt(&groundot[side]);
		GsDrawOt(&Wot[side]);
		GsDrawOt(&cot[side]);

		//DrawSync(0);

		if (DealWithControllerPad() == 0) {
			break;	   // quitting
		}
					
	} //main render loop
	area.x = 0;
	area.y = 256;
	area.w = 640;
	area.h = (side)*256; 
	StoreImage(&area,(u_long *) 0x80090000);
	ResetGraph(1);		// cleanup

} // main

// ----- Init routine -------------------------
void InitialiseAll (void) {
	int		i,df;
	RECT	Bg;
	long	videomode;
	short	* pt;

	/* copy the Bézier control points into an SVECTOR arrray */
	pt = (short *)(VERTEX_ADDRESS-144);
	for (i=0; i<24; i++) {
		points[i].vx = ((short) *pt)*5;
		pt++;
		points[i].vy = ((short) *pt)*5;
		pt++;
		points[i].vz = ((short) *pt)*5;
		pt++;
	}

	/* load textures */
	LoadTexture((u_long *)TEXTURE1_ADDRESS, &tex1);
	LoadTexture((u_long *)CAM_ADDRESS, &camimg);

	/* ghost cam sprite */
	InitGsSprite(&cam);
	cam.u = cam.v = 0; 
	cam.tpage = 11; 
	cam.attribute |= 2 << 24;
	cam.x = (200);
	cam.y = -120;
	cam.w = camimg.pw; 
	cam.h = camimg.ph;

	screenwidth = 640;
	
	//SetVideoMode(MODE_PAL);
	videomode = GetVideoMode();	
	if (videomode == MODE_NTSC) screenheight = 240;
	else screenheight = 256;
	
	GetPadBuf(&bb0, &bb1);		// Get controller reception buffer
	PadInit();					// init controller pad

	// create background gradient
	Bg.x = 960;
	Bg.w = 64;
	Bg.h = 1;
	for (i=0; i<255; i++) {
		Bg.y = i+256;
		ClearImage(&Bg,0,0,i);
	}
	FntLoad(640,256);


	/* initialise all models */
	CreateRoller();
	SetUpObject(&levelwalls,(u_long *) &mapobject,0);
	
	CreateFloor();
	SetUpObject(&levelfloor,(u_long *) &mapfloor,0);

	SetUpObject(&coneman,(u_long *) CONEMAN_ADDRESS,1);

	/* Initialise graphics & all those OT's */
	GsInitGraph(screenwidth,screenheight,GsNONINTER|GsOFSGPU,1,0);
	GsDefDispBuff(0,0,0,screenheight);
	GsInit3D();
	
	Wot[0].length=OT_LENGTH;
	Wot[0].org=wtags[0];
	Wot[1].length=OT_LENGTH;
	Wot[1].org=wtags[1];
	
	GsClearOt(0,0,&Wot[0]);
	GsClearOt(0,0,&Wot[1]);
	
	groundot[0].length=OT_LENGTH;
	groundot[0].org=groundtags[0];
	groundot[1].length=OT_LENGTH;
	groundot[1].org=groundtags[1];
	
	GsClearOt(0,0,&cot[0]);
	GsClearOt(0,0,&cot[1]);
		
	cot[0].length=OT_LENGTH;
	cot[0].org=ctags[0];
	cot[1].length=OT_LENGTH;
	cot[1].org=ctags[1];
	
	GsClearOt(0,0,&cot[0]);
	GsClearOt(0,0,&cot[1]);

	ProjectionDistance = 350;			// screen to viewpoint distance
	GsSetProjection(ProjectionDistance);

	view.rz = 0;
	view.super = WORLD;
	GsSetRefView2(&view);
	
	GsSetAmbient(ONE,ONE,ONE);

	TheLights[0].vx = 0;
	TheLights[0].vy = -ONE;
	TheLights[0].vz = 0;
	TheLights[0].r = 0; 
	TheLights[0].g = 128; 
	TheLights[0].b = 0; 
	GsSetFlatLight(0, &TheLights[0]);
/*	
	GsSetLightMode(1);
	fog.dqa = -12000 *4096/64/ProjectionDistance;
	fog.dqb = 1.25 * 4096 * 4096;
	fog.rfc = 0;
	fog.gfc = 0;
	fog.bfc = 0;
	GsSetFogParam(&fog);
*/	
}

void FindPosition(SVECTOR * temp, SVECTOR * tangent, int * dist, int * index) {
	SVECTOR	p4;
	int		pi1,pi2,pi3;

	pi1 = *index;
	pi2 = (*index)+1;
	pi3 = (*index)+2;
	if (pi3 == 24) pi3 = 0;
	
	// The track is created with automatic mirroring of tangent-points
	// around the control points. This ensures that there are no sudden
	// changes in direction & velocity (equal magnitude & direction of tangent-vectors)
	p4.vx = points[pi3].vx - points[pi3+1].vx + points[pi3].vx;
	p4.vy = points[pi3].vy - points[pi3+1].vy + points[pi3].vy;
	p4.vz = points[pi3].vz - points[pi3+1].vz + points[pi3].vz;
	FindBCPoint(*dist, tangent,temp,points[pi1],points[pi2],p4,points[pi3]);

	*dist += 25;
	if (*dist >= 1000) {
		*dist -= 1000;
		(*index)+=2;
		if (*index == 24) *index = 0;
	}
}

// ----- Misc. helper / object routines ------------------------------

void InitGsSprite (GsSPRITE* sprite) {
	sprite->attribute = 0;
	sprite->x = 0;
	sprite->y = 0;
	sprite->w = 0;
	sprite->h = 0;
	sprite->tpage = 0;
	sprite->u = 0;
	sprite->v = 0;
	sprite->cx = 0;
	sprite->cy = 0;
	sprite->r = 128;
	sprite->g = 128;
	sprite->b = 128;
	sprite->mx = 0;
	sprite->my = 0;
	sprite->scalex = ONE;
	sprite->scaley = ONE;
	sprite->rotate = 0;
}

void DrawObject(ObjectHandler * object, short tx, short ty, short tz, int side) {
	MATRIX tmpls; //,tmplw;
	VECTOR	velocity;
	velocity.vx = velocity.vy = velocity.vz = 0;
	object->coord.coord.t[0] = tx; 
	object->coord.coord.t[1] = ty; 
	object->coord.coord.t[2] = tz; 
	object->coord.flg = 0;

	UpdateObjectCoordinates(&object->rotate, &object->coord);
	GsGetLs(&(object->coord), &tmpls);
	GsSetLightMatrix(&tmpls);
	GsSetLsMatrix(&tmpls);
	GsSortObject4( &(object->handler), &cot[side], 3, getScratchAddr(0));
}

void LoadTexture(u_long * addr, GsIMAGE * image) {
	GsGetTimInfo(addr+1, image);
	LoadImage((RECT *)&image->px, image->pixel);
	if((image->pmode>>3)&0x01) {
		LoadImage((RECT *)&image->cx, image->clut);
	}
	DrawSync(0);
}

void SetUpObject (ObjectHandler * object, u_long * obj_addr, short link)
{
	u_long *pointer;
	
	object->position.vx = 0;
	object->position.vy = 0;
	object->position.vz = 0;
	
	object->rotate.vx = 0;
	object->rotate.vy = 0;
	object->rotate.vz = 0;
	
	GsInitCoordinate2(WORLD, &object->coord);
	
	object->handler.coord2 = &(object->coord);
	
	pointer = obj_addr;
	pointer++;
	if (link == 1)	GsMapModelingData(pointer);
	
	pointer += 2;
	GsLinkObject4( (u_long)pointer, &object->handler, 0);
}

void UpdateObjectCoordinates (SVECTOR* rotationVector,
							  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(rotationVector, &tempMatrix);		// get rotation matrix from rotation vector
	coordSystem->coord = tempMatrix;			// assign new matrix to coordinate system
	coordSystem->flg = 0;						// tell GTE that coordinate system has been updated
}

#define STEP	50

// ----- Pad routines ------------------------------
int DealWithControllerPad (void)
{
	long pad = PadRead();
	
	if (pad & PADselect) return 0;
/*	
	if (pad & PADLdown) view.vrz -= STEP; 
	if (pad & PADLup) view.vrz += STEP;  
	if (pad & PADLleft) view.vry -= STEP; 
	if (pad & PADLright) view.vry += STEP; 
	if (pad & PADL1) view.vrx -= STEP; 
	if (pad & PADL2) view.vrx += STEP; 

	if (pad & PADRdown) view.vpz -= STEP; 
	if (pad & PADRup) view.vpz += STEP;  
	if (pad & PADRleft) view.vpy -= STEP; 
	if (pad & PADRright) view.vpy += STEP; 
	if (pad & PADR1) view.vpx -= STEP; 
	if (pad & PADR2) view.vpx += STEP; 	
*/
	return 1;
}

void PadInit (void) {
	GetPadBuf(&bb0, &bb1);
}

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


// ----- TMD creation routines ------------------------------
void CreatePrimitive(TMDPRIM_FLT4 * prim, short type, short normal, short v0, short v1, short v2, short v3) {
	short ofs;
	short yofs;
	short	texsize;

	texsize = 64;

	yofs = type / 4; 
	ofs = (type%4)*64;
	prim->mode = 0x2c00;
	prim->ilen = 0x07;
	prim->olen = 0x09;
	
	prim->cba = ((240+type) << 6) + (640 >> 4);
	prim->tsb = 10;
	
	prim->u0 = ofs+0;
	prim->v0 = yofs+(texsize-1);
	prim->u1 = ofs+0;
	prim->v1 = 0+yofs;
	prim->u2 = ofs+(texsize-1);
	prim->v2 = (texsize-1)+yofs;
	prim->u3 = ofs+(texsize-1);
	prim->v3 = yofs+0;
	
	prim->vertex0 = v0;
	prim->vertex1 = v1;
	prim->vertex2 = v2;
	prim->vertex3 = v3;
	
	prim->normal0 = normal;
}

void CreateQuadPrim(TMDPRIM_FL4 * prim,short normal, short v0, short v1, short v2, short v3, char r, char g, char b) {

	prim->olen = 0x05;
	prim->olen = 0x04;
	prim->mode = 0x2800;
	prim->mode2 = 0x28;
	prim->r = r;
	prim->g = g;
	prim->b = b;
	prim->vertex0 = v0;
	prim->vertex1 = v1;
	prim->vertex2= v2;
	prim->vertex3= v3;
	prim->normal0 = normal;
}

void CreateFloor() {
	int	x,y;

	// set tmd header 
	mapfloor.header.id = 0x00000041;
	mapfloor.header.flag = 1;
	mapfloor.header.nobj = 1;
	
	mapfloor.objtable.vert_top = (u_long*) &mapfloor.vertex; //VERTEX_ADDRESS; 
	mapfloor.objtable.n_vert = FLOOR_VERTICES*FLOOR_VERTICES;
	
	mapfloor.objtable.normal_top = (u_long*) &mapfloor.normal;
	mapfloor.objtable.n_normal = 1;
	
	mapfloor.objtable.primitive_top = (u_long*) &mapfloor.packets;
	mapfloor.objtable.n_primitive = (FLOOR_VERTICES-1)*(FLOOR_VERTICES-1);
	
	mapobject.objtable.scale = 0;

	// create vertexlist
	for (y=0; y<FLOOR_VERTICES; y++)
		for (x=0; x < FLOOR_VERTICES; x++) {
			mapfloor.vertex[(y*FLOOR_VERTICES)+x].vx = (x*FLOOR_SIZE)-((FLOOR_VERTICES*FLOOR_SIZE)/2);
			mapfloor.vertex[(y*FLOOR_VERTICES)+x].vy = 0;
			mapfloor.vertex[(y*FLOOR_VERTICES)+x].vz = (y*FLOOR_SIZE)-((FLOOR_VERTICES*FLOOR_SIZE)/2);
		}
	// create packets
	for (y=0; y<FLOOR_VERTICES-1; y++)
		for (x=0; x < FLOOR_VERTICES-1; x++) {
			CreatePrimitive(&mapfloor.packets[(y*(FLOOR_VERTICES-1))+x],4,0,
				((y*FLOOR_VERTICES)+x+1),
				((y*FLOOR_VERTICES)+x),
				(((y+1)*FLOOR_VERTICES)+x+1),
				(((y+1)*FLOOR_VERTICES)+x));
		}
}
#define FIX_R	33
#define FIX_G	49
#define FIX_B	115

void CreateRoller() {

	int		stepIndex;
	short	leftw,rightw;
	int		tunnel;
	int		i,fixfaces;
	short	leftr,leftg,leftb,rightr,rightg,rightb;

	tunnel = 0;
	fixfaces = 0;
	memcpy((void*)&mapobject.vertex,(void*) VERTEX_ADDRESS,(NUM_STEPS*VERTICES_PER_STEP)*sizeof(SVECTOR));

	// set tmd header 
	mapobject.header.id = 0x00000041;
	mapobject.header.flag = 1;
	mapobject.header.nobj = 1;
	
	mapobject.objtable.vert_top = (u_long*) &mapobject.vertex; //VERTEX_ADDRESS; 
	mapobject.objtable.n_vert = NUM_STEPS*VERTICES_PER_STEP;
	
	mapobject.objtable.normal_top = (u_long*) &mapobject.normal;
	mapobject.objtable.n_normal = NUM_STEPS*FACES_PER_STEP;
	
	mapobject.objtable.primitive_top = (u_long*) &mapobject.packets;
	mapobject.objtable.n_primitive = (NUM_STEPS)*FACES_PER_STEP;
	
	mapobject.objtable.scale = 0;

	for (stepIndex=0; stepIndex<NUM_STEPS-1; stepIndex++) {

		// alternate color on walls
		if ((stepIndex % 2) == 0) {
			rightr = 80;
			rightg = 112;
			rightb = 240;
			leftr = leftg = leftb = 255;
		} else {
			rightr = rightg = rightb = 255;
			leftr = 80;
			leftg = 112;
			leftb = 240;
		}


/*
		crossProduct(	mapobject.vertex[((stepIndex+1)*VERTICES_PER_STEP)],
						mapobject.vertex[((stepIndex+1)*VERTICES_PER_STEP)+1],
						mapobject.vertex[(stepIndex*VERTICES_PER_STEP)],
						&mapobject.normal[(stepIndex*FACES_PER_STEP)]);
		// left face
		crossProduct(	mapobject.vertex[((stepIndex+1)*VERTICES_PER_STEP)],
						mapobject.vertex[(stepIndex*VERTICES_PER_STEP)],
						mapobject.vertex[(stepIndex*VERTICES_PER_STEP)+2],
						&mapobject.normal[(stepIndex*FACES_PER_STEP)+1]);
		
		// right face
		crossProduct(	mapobject.vertex[(stepIndex*VERTICES_PER_STEP)+1],
						mapobject.vertex[((stepIndex+1)*VERTICES_PER_STEP)+1],
						mapobject.vertex[(stepIndex*VERTICES_PER_STEP)+3],
						&mapobject.normal[(stepIndex*FACES_PER_STEP)+2]);
*/
		// create faces
		// top face
		CreatePrimitive(&mapobject.packets[(stepIndex)],0,(stepIndex*FACES_PER_STEP),
						((stepIndex+1)*VERTICES_PER_STEP)+1,
						((stepIndex+1)*VERTICES_PER_STEP),
						((stepIndex)*VERTICES_PER_STEP)+1,
						((stepIndex)*VERTICES_PER_STEP));
		// left face
		CreateQuadPrim(&mapobject.packetsw[(stepIndex*2)],
						(stepIndex*FACES_PER_STEP)+1,
						((stepIndex)*VERTICES_PER_STEP),
						((stepIndex+1)*VERTICES_PER_STEP),
						((stepIndex)*VERTICES_PER_STEP)+2,
						((stepIndex+1)*VERTICES_PER_STEP)+2,
						leftr,leftg,leftb);
		// right face
		CreateQuadPrim(&mapobject.packetsw[(stepIndex*2)+1],
						(stepIndex*FACES_PER_STEP)+2,
						((stepIndex+1)*VERTICES_PER_STEP)+1,
						((stepIndex)*VERTICES_PER_STEP)+1,
						((stepIndex+1)*VERTICES_PER_STEP)+3,
						((stepIndex)*VERTICES_PER_STEP)+3,
						rightr,rightg,rightb);

		// tunnel fixup code
		if (mapobject.vertex[((stepIndex)*VERTICES_PER_STEP)+2].vy != 0) {
			//fixfaces++;
			if (tunnel == 0) {
				// start of tunnel
				tunnel = 1;
			// create wall
			CreateQuadPrim(&mapobject.packets2[fixfaces],0,
						((stepIndex)*VERTICES_PER_STEP)+2,
						((stepIndex)*VERTICES_PER_STEP)+3,
						((stepIndex-1)*VERTICES_PER_STEP)+2,
						((stepIndex-1)*VERTICES_PER_STEP)+3,
						FIX_R,FIX_G,FIX_B);
			fixfaces++;	

			} 
			// create roof
			CreateQuadPrim(&mapobject.packets2[fixfaces],0,
						((stepIndex+1)*VERTICES_PER_STEP)+2,
						((stepIndex+1)*VERTICES_PER_STEP)+3,
						((stepIndex)*VERTICES_PER_STEP)+2,
						((stepIndex)*VERTICES_PER_STEP)+3,
						FIX_R,FIX_G,FIX_B);
			fixfaces++;
		//	}
		} else if (tunnel == 1) {
			// end of tunnel, we have created one extra roof poly, so get rid of it
			fixfaces--;
			// create wall
			CreateQuadPrim(&mapobject.packets2[fixfaces],0,
						((stepIndex-1)*VERTICES_PER_STEP)+3,
						((stepIndex-1)*VERTICES_PER_STEP)+2,
						((stepIndex)*VERTICES_PER_STEP)+3,
						((stepIndex)*VERTICES_PER_STEP)+2,
						FIX_R,FIX_G,FIX_B);
			fixfaces++;	
			tunnel = 0;
		}
	}

		// connect the first & last segments
		// alternate color on walls
		if ((stepIndex % 2) == 0) {
			rightr = 80;
			rightg = 112;
			rightb = 240;
			leftr = leftg = leftb = 255;
		} else {
			rightr = rightg = rightb = 255;
			leftr = 80;
			leftg = 112;
			leftb = 240;
		}

		// create faces
		// top face
		CreatePrimitive(&mapobject.packets[(stepIndex)],0,(stepIndex*FACES_PER_STEP),
						1,
						0,
						((stepIndex)*VERTICES_PER_STEP)+1,
						((stepIndex)*VERTICES_PER_STEP));
		// left face
		CreateQuadPrim(&mapobject.packetsw[(stepIndex*2)],
						(stepIndex*FACES_PER_STEP)+1,
						((stepIndex)*VERTICES_PER_STEP),
						(0),
						((stepIndex)*VERTICES_PER_STEP)+2,
						2,
						leftr,leftg,leftb);
		// right face
		CreateQuadPrim(&mapobject.packetsw[(stepIndex*2)+1],
						(stepIndex*FACES_PER_STEP)+2,
						1,
						((stepIndex)*VERTICES_PER_STEP)+1,
						3,
						((stepIndex)*VERTICES_PER_STEP)+3,
						rightr,rightg,rightb);

	//printf("Fixfaces : %d",fixfaces);
//	mapobject.objtable.n_primitive = (NUM_STEPS-1)*3; //+= fixfaces; 
	mapobject.objtable.n_primitive += fixfaces; 

}

void crossProduct(SVECTOR p1, SVECTOR p2, SVECTOR p3, SVECTOR *p4) {
	VECTOR u,v;
	double x,y,z,len;

	/* regn ut u */
	u.vx = p2.vx - p1.vx;
	u.vy = p2.vy - p1.vy;
	u.vz = p2.vz - p1.vz;
	
	/* regn ut v */
	v.vx = p3.vx - p1.vx;
	v.vy = p3.vy - p1.vy;
	v.vz = p3.vz - p1.vz;
	
	/* regn ut normal */
	x = (double) ((u.vy*v.vz) - (u.vz*v.vy) << 12);
	y = (double) ((u.vz*v.vx) - (u.vx*v.vz) << 12);
	z = (double) ((u.vx*v.vy) - (u.vy*v.vx) << 12);

	len = (x * x) + (y * y) + (z * z);
	len = sqrt( len );
	
	/* normaliser n */
	p4->vx = (short) (x / len);
	p4->vy = (short) (y / len);
	p4->vz = (short) (z / len);
}

void CalculatePointOnLine(SVECTOR p1, SVECTOR p2, SVECTOR * dest, int point) {
	dest->vx = (((p2.vx - p1.vx) * point) / 1000) + p1.vx;
	dest->vy = (((p2.vy - p1.vy) * point) / 1000) + p1.vy;
	dest->vz = (((p2.vz - p1.vz) * point) / 1000) + p1.vz;
}

/* point is the position along the curve in 100% = 1000 */ 
void FindBCPoint(int point, SVECTOR * tangent, SVECTOR * temp,
	SVECTOR p1, SVECTOR p2, SVECTOR p3, SVECTOR p4) {
	SVECTOR npoints[6];

	CalculatePointOnLine(p1,p2,&npoints[0],point);
	CalculatePointOnLine(p2,p3,&npoints[1],point);
	CalculatePointOnLine(p3,p4,&npoints[2],point);

	CalculatePointOnLine(npoints[0],npoints[1],&npoints[3],point);
	CalculatePointOnLine(npoints[1],npoints[2],&npoints[4],point);

	tangent->vx = npoints[4].vx - npoints[3].vx;
	tangent->vy = npoints[4].vy - npoints[3].vy;
	tangent->vz = npoints[4].vz - npoints[3].vz;

	CalculatePointOnLine(npoints[3],npoints[4],&npoints[5],point);

	temp->vx = npoints[5].vx;
	temp->vy = npoints[5].vy;
	temp->vz = npoints[5].vz;

}