/************************************************************
*
* Bézier example 2
* by Lars Barstad
*
* This little program demonstrates the following:
*	- How to calculate & draw Bézier curves
*	- How to move a given distance along a curve
*	- How to use de Castelja algorithm to move
*	  a given percentage along the curve (thanks to
*	  Nick Porcino for the algorithm tutorial)
*
* Movement is done with the help of a precalculated table
* created by the DrawBezierCurve() function. The precalculation
* can easily be done step by step to avoid long calculation times.
* In this example I have choosen not to do so in order to keep
* things simple, and to present a stand-alone bézier routine.
*
* Even though the visuals on this example is 2D, the calculations
* are 3D.
*
* More curve based examples coming your way soon. A great
* rollercoaster-ride is just days away.. :)
*
* Comments are welcome at: lars.barstad@reptile.no
*
*
* Quick instructions:
*	Select controlpoints with L1/L2
*	Move controlpoints with directionl buttons
*
***********************************************************/

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

#define PAD_STEP			100	// increment for pad
#define	PAD_DELAY			10	// delay for controlpoints selection
#define KNOT_RADIUS			3	// radius of controlknot

#define CURVE_STEPS			50	// number of steps for the Bézier curve

#define OT_LENGTH			9
#define PACKETMAX2			(8000*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	GsRVIEW2 view;

typedef struct {
	SVECTOR		point;				// position of point
	SVECTOR		next;				// vector to next point
	long		nextlen;			// length of vector
	double		next_normx, next_normy, next_normz;
} PathPoint;

PathPoint	path[CURVE_STEPS+1];

int		index,padwait;
int		ProjectionDistance;
int		screenwidth,screenheight;
SVECTOR	points[4];
int		update = 1;

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

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

void main (void);
void InitialiseAll(void);

void CalculatePointOnLine(SVECTOR p1, SVECTOR p2, SVECTOR * dest, int point);
void FindBCPoint(int point, SVECTOR * temp,GsOT * otp);

void MovePoint(SVECTOR * point, short dist, double x, double y, double z);
void MoveAlongCurve(SVECTOR * dest,short dist, short * preDist, int * index);
void DrawBezierCurve(SVECTOR P1, SVECTOR P2, SVECTOR P3, SVECTOR P4,GsOT * otp,int save);

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

/****************************************************************************
functions
****************************************************************************/

void main (void)
{
	int		state,step;
	int		i,page,pathIndex;
	GsBOXF	knot;
	GsLINE  kline;
	short	dist,preDist;
	SVECTOR pos, temp;

	InitialiseAll();

	kline.attribute = 0;
	kline.r = kline.g = kline.b = 64;

	dist = 200;
	preDist = 0;
	index = 0;
	pathIndex = 0;
	step = 0;
	page = GsGetActiveBuff();

	for(;;) {
		
		state = update;

		if (DealWithControllerPad() == 0) break;	   // quitting
		
		GsSetRefView2(&view);

		page = GsGetActiveBuff();
		GsClearOt(0,0,&Wot[page]);
		GsSetWorkBase((PACKET*)packetArea[page]);
		if ((state != update) && (update == 0)) {
			DrawBezierCurve(points[0],points[1],points[2],points[3],&Wot[page],1);
			preDist = 0;
			pathIndex = 0;
		} else DrawBezierCurve(points[0],points[1],points[2],points[3],&Wot[page],0);
		
		// only move & draw the knot if the user isn't modifying it
		if (state == 0) {
			MoveAlongCurve(&temp,dist,&preDist,&pathIndex);
			//if (pathIndex > 47) printf("PathIndex : %d\n",pathIndex);
			
			if (pathIndex == CURVE_STEPS) {
				pos.vx = points[0].vx;
				pos.vy = points[0].vy;
				pos.vz = points[0].vz; 
				pathIndex = 0;
				preDist = 0;
			} else {

				pos.vx = path[pathIndex].point.vx;
				pos.vy = path[pathIndex].point.vy;
				pos.vz = path[pathIndex].point.vz;

				pos.vx += temp.vx;
				pos.vy += temp.vy;
				pos.vz += temp.vz;
			}

			knot.x = (pos.vx/100) - KNOT_RADIUS;
			knot.y = (pos.vy/100) - KNOT_RADIUS;

			knot.w = KNOT_RADIUS*2;
			knot.h = KNOT_RADIUS*2;

			knot.r = knot.g = knot.b = 128;
			GsSortBoxFill(&knot,&Wot[page],1);
		} // if state

		step += 5;
		FindBCPoint(step,&temp,&Wot[page]);
			knot.x = (temp.vx/100) - KNOT_RADIUS;
			knot.y = (temp.vy/100) - KNOT_RADIUS;

			knot.w = KNOT_RADIUS*2;
			knot.h = KNOT_RADIUS*2;

			knot.r = knot.g = knot.b = 0;
			knot.g = 128;
			GsSortBoxFill(&knot,&Wot[page],1);
		if (step == 1000) step = 0;

		// draw control-knots
		for (i=0; i<4; i++) {
			knot.attribute = 0;
			knot.x = (points[i].vx / 100) - KNOT_RADIUS;
			knot.y = (points[i].vy / 100) - KNOT_RADIUS;

			knot.w = KNOT_RADIUS*2;
			knot.h = KNOT_RADIUS*2;

			if (i == index) {
				knot.r = knot.g = 0;
				knot.b = 128;
			} else {
				knot.b = knot.g = 0;
				knot.r = 128;
			}

			GsSortBoxFill(&knot,&Wot[page],2);
		}

		// draw lines between controlpoints on the curve
		// and controlpoints specifying tangent vectors
		kline.x0 = points[0].vx / 100;
		kline.y0 = points[0].vy / 100;
		kline.x1 = points[1].vx / 100;
		kline.y1 = points[1].vy / 100;
		GsSortLine(&kline,&Wot[page],3);
		kline.x0 = points[2].vx / 100;
		kline.y0 = points[2].vy / 100;
		kline.x1 = points[3].vx / 100;
		kline.y1 = points[3].vy / 100;
		GsSortLine(&kline,&Wot[page],3);

		// wait for drawing to complete 
		DrawSync(0);
		VSync(0);
		GsSwapDispBuff();
		
		GsSortClear(0,0,60,&Wot[page]);
		GsDrawOt(&Wot[page]);
					
	} //main render loop

	ResetGraph(1);

} // main

void InitialiseAll (void) {
	long videomode;
	int index;
	
	GetPadBuf(&bb0, &bb1);		// Get controller reception buffer
	PadInit();					// init controller pad
	padwait = 0;

	screenwidth = 320;
	videomode = GetVideoMode();
	if (videomode == MODE_NTSC) screenheight = 240;
	else screenheight = 256;

	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]);

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

	view.vrx = view.vry = view.vrz = 0; 
	view.vpx = view.vpy = view.vpz = 0; 
	view.super = WORLD;
	GsSetRefView2(&view);
	
	GsSetAmbient(ONE,ONE,ONE);

	// initialise controlpoints to default values
	for (index = 0; index<4; index++) {
		points[index].vx = points[index].vy = points[index].vz = 0;
	}

	points[0].vx = -12000;
	points[1].vy = -5000;
	points[2].vx = 12000;
	points[3].vy = 10000;

}

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 * temp,GsOT * otp) {
	SVECTOR npoints[6];

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

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

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

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

}

void MovePoint(SVECTOR * point, short dist, double x, double y, double z) {
	point->vx = x * dist;
	point->vy = y * dist;
	point->vz = z * dist;
}

/*

	This routine handles the calculation of our position on the spline

	This is how it works:
		We want to move a distance of [dist] along the curve.
		(1) Check the remaining length of the current line-segment
		(2) If we want to travel further than the length of this line, 
			repeat steps 1 & 2 until dist < length
		(3) Calculate our final position by dividing dist by line-lenght
		    and multiplying with the normalised direction vector (precalculated)

*/
void MoveAlongCurve(SVECTOR * dest,short dist, short * preDist, int * index) {
	short	toTravel;
	int		len;

	toTravel = dist;
	
	while ((toTravel > 0) && (*index < CURVE_STEPS)) {
		
		if (*preDist > 0) {
			/* our current position is between two points */

			/* the length is used 2 places, calculate it once here */
			len = path[*index].nextlen-(*preDist);	
			
			if (toTravel > len) {
				/* our destination is not on this line segment */

				/* update the distance left to travel */
				toTravel -= len;

				/* point to next line-segment and reset distance traveled along line */
				(*index)++;
				*preDist = 0;
			} else {
				/* our destination is on this line segment */
				
				/* update the distance traveled along this linesegment */
				*preDist += toTravel;

				/* find the actual point (normalised vector * distance) */
				MovePoint(dest,*preDist,path[*index].next_normx,path[*index].next_normy,path[*index].next_normz);

				/* we have reached our destination, set toTravel to zero */
				toTravel = 0;
			}
			
		} else {
			/* our current position is at one of the curve-points */
			if (path[*index].nextlen < toTravel) {
				/* our destination is not on this line segment */

				/* update the distance left to travel */
				toTravel -= path[*index].nextlen;		

				/* point to next line-segment and reset distance traveled along line */
				(*index)++;			
				*preDist = 0;
			} else {
				/* our destination is on this line segment */

				/* update the distance traveled along this linesegment */
				*preDist += toTravel; 

				/* find the actual point (normalised vector * distance) */
				MovePoint(dest,*preDist,path[*index].next_normx,path[*index].next_normy,path[*index].next_normz);

				/* we have reached our destination, set toTravel to zero */
				toTravel = 0;
			}
		}
	} // while
}

void DrawBezierCurve(SVECTOR P1, SVECTOR P2, SVECTOR P3, SVECTOR P4,GsOT * otp,int save)
{

	long len;
	double dlen;
	float t,t2,t3;
	float delta;
	int	i;
	SVECTOR	pnew;
	SVECTOR pold;
	GsLINE	line;

	delta =(1.0/CURVE_STEPS);
	
	pnew.vx = pold.vx = P1.vx;
	pnew.vy = pold.vy = P1.vy;
	pnew.vz = pold.vz = P1.vz;
	
	line.attribute = 0;
	line.r = line.g = line.b = 128;


	for (i=0; i<CURVE_STEPS; i++)
	{

		t=(i+1)*delta;
		t2=t*t;
		t3=t2*t;

		pnew.vx= (short)((1-t)*(1-t)*(1-t)*P1.vx
						+3*t *(1-t)*(1-t)*P2.vx
						+3*t2 *(1-t)*P3.vx
						+t3*P4.vx);

		pnew.vy=(short)((1-t)*(1-t)*(1-t)*P1.vy
					+3*t *(1-t)*(1-t)*P2.vy
					+3*t2 *(1-t)*P3.vy
					+t3  *P4.vy);

		pnew.vz=(float)(	(1-t)*(1-t)*(1-t)*P1.vz
					+3*t *(1-t)*(1-t)*P2.vz
					+3*t2 *(1-t)*P3.vz
					+t3 *P4.vz);


		line.x0 = pnew.vx / 100;
		line.y0 = pnew.vy / 100;

		line.x1 = pold.vx / 100;
		line.y1 = pold.vy / 100;

		GsSortLine(&line,otp,1);
		
		if (save == 1) {
			path[i].point = pold;

			// find vector between points
			path[i].next.vx = pnew.vx - pold.vx;
			path[i].next.vy = pnew.vy - pold.vy;
			path[i].next.vz = pnew.vz - pold.vz;
			
			// calculate length of vector
			len = (path[i].next.vx*path[i].next.vx) + (path[i].next.vy*path[i].next.vy) +(path[i].next.vz*path[i].next.vz);
			dlen = sqrt(len);
			path[i].nextlen = (long) dlen;

			// normalise vector
			path[i].next_normx = path[i].next.vx / dlen;
			path[i].next_normy = path[i].next.vy / dlen;
			path[i].next_normz = path[i].next.vz / dlen;
		}

		pold.vx = pnew.vx;
		pold.vy = pnew.vy;
		pold.vz = pnew.vz;
	}

	if (save == 1) {
		path[i].point.vx = pnew.vx;
		path[i].point.vy = pnew.vy;
		path[i].point.vz = pnew.vz;
		path[i].nextlen = 0;
		path[i].next.vx = path[i].next.vy = path[i].next.vy = 0;
		path[i].next_normx = path[i].next_normy = path[i].next_normy = 0;
	}
}

// ----- Pad routines ------------------------------
int DealWithControllerPad (void)
{
	long pad = PadRead();
	
	if (pad & PADselect) return 0;

	// allow the user to select which controlpoint to move
	// have to use a delay, or the index will increment too fast
	if (padwait == 0) {
		if (pad & PADL1) if (index<3) { index++; padwait = PAD_DELAY;}
		if (pad & PADL2) if (index>0) { index--; padwait = PAD_DELAY; }
	} else padwait--;

	// move the selected controlpoint
	if (pad & PADLleft) points[index].vx -= PAD_STEP;
    if (pad & PADLright) points[index].vx += PAD_STEP;
    if (pad & PADLup) points[index].vy -= PAD_STEP;
    if (pad & PADLdown) points[index].vy += PAD_STEP;

	if ((pad & PADLleft) ||
		(pad & PADLright) ||
		(pad & PADLup) ||
		(pad & PADLdown)) {
		update = 1;
	} else update = 0;


	return 1;
}

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

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