
#include "global.h"
#include "overlays.h"
#include "bff_load.h"
#include "map_draw.h"
#include "maths.h"
#include "debris.h"
#include "timer.h"

#include "route.h"
#include "enemies.h"


extern TextureType *enemy_markerspr;	// used for showing the path in debug mode

ROUTEFIND_STR *route_routes = NULL;

void routefindPreBFF()
{
	route_routes = NULL;
}
void routefindLinkBFF(ROUTEFIND_STR *routes)
{
	int i;

	route_routes = routes;


// Let's convert some integers to pointers...

	routes->boxes = ADD2POINTER(routes, (int)routes->boxes);
	utilPrintf("Router: n_boxes total = %d\n",routes->n_boxes);

	for(i = 0; i < routes->n_boxes; i++)
	{
//		utilPrintf("Original Vector offset = %d. n_vecs = %d\n",(int)routes->boxes[i].vecs,routes->boxes[i].n_vecs);
		routes->boxes[i].vecs = ADD2POINTER(routes, (int)routes->boxes[i].vecs);


	}
}

// This is only relevant as a debug tool
void routeDraw()
{
	int i;
	int p;
	ROUTEBOX *box;
	VECTOR v[4];
	unsigned int col;

	if(!route_routes)
		return;

	if(overlay_show_collision != DEBUG_OVERLAY_ENEMY_INFO)
		return;

	box = route_routes->boxes;
	for(i = 0; i < route_routes->n_boxes; i++,box++)
	{
		for(p = 0; p < box->n_vecs; p++)
		{

			v[0].vx = box->vecs[p].vx;
			v[0].vy = -100;
			v[0].vz = box->vecs[p].vz;

			if(p != box->n_vecs-1)
			{
				col = 0xffff8080;
				v[1].vx = box->vecs[p+1].vx;
				v[1].vy = -100;
				v[1].vz = box->vecs[p+1].vz;
			}
			else
			{
				col = 0xffff8010;
				v[1].vx = box->vecs[0].vx;
				v[1].vy = -100;
				v[1].vz = box->vecs[0].vz;
			}

			v[2].vx = v[1].vz - v[0].vz;
			v[2].vy = 0;
			v[2].vz = v[0].vx - v[1].vx;

			MakeUnit(&v[2]);

			v[2].vx = v[2].vx >> 3;
			v[2].vz = v[2].vz >> 3;


			v[3].vx = v[0].vx + v[2].vx;
			v[3].vy = -100;
			v[3].vz = v[0].vz + v[2].vz;

			v[2].vx = v[1].vx + v[2].vx;
			v[2].vy = -100;
			v[2].vz = v[1].vz + v[2].vz;




			overlayShowDebugLine(
				&v[0],
				&v[1],
				col,
				DEBUGPOLY_SUBDIV);	//+DEBUGPOLY_USEDEPTH);

			overlayShowDebugPolygon(&v[0], 4, 0x000080, DEBUGPOLY_SUBDIV | DEBUGPOLY_NOBORDER | DEBUGPOLY_DUBSIDED | DEBUGPOLY_USEDEPTH);

		}
	}
}

// Okeydokey, this is the fun part...
// What we do is...
// Draw the line from src to dest
// Note if it hits anything.

// If it doesn't, return the straight line.

// If it does, find the shortest circumnavigation of the object to the line's exit point.

// (Always assuming, of course, that the line HAS an exit point. It won't have if the chicken goes somewhere that
// the dog can't go.)

// At this stage, we have a minimum of...
// Vector from src to box intersect
// Vector from intersect to a corner
// Vector from the corner to either another corner, or to the exit point.

// We could return at this stage.
// However, what we should do is try for a shortcuts across the first two & second two
// (corner-exit point can't be clipped, but we could try to shortcut over a few points...
// (corner-next corner can be clipped if it's a concave box)
// The way to DO this is actually by routefinding between 'em (!)
// ...Alternatively, scan for *totally unblocked* shortcuts as we follow the long path. (a tad non-optimal, but prolly less maths)

// Note. Maths errors require our route to be kept outside the box by an error margin
// in any case. Tsk.

void routeDebugMarker(ROUTEBOX_VECTOR *src, int r, unsigned int c)
{
	overlayShowSphere(src->vx, -500, src->vz, r, c);
}


// return t = 0...4096 along src...dest, or -1 for no isect.
// Be wary of huge numbers incoming. Note that the lines CAN be big.
int routeLineIsectFind(VECTOR *src, VECTOR *dest, ROUTEBOX_VECTOR *v1, ROUTEBOX_VECTOR *v2)
{
//	int x1,z1,
	signed int x2,z2;
	signed int dx1,dz1;
	signed int dx2,dz2;
	signed int num;
	signed int s;
	signed int top,bot;
	signed int r;

// Let's do a quick ballpark boundscheck to start with, eh?
// Moved to caller
/*
	if(dest->vx < src->vx)
	{
		if(v1->vx > src->vx && v2->vx > src->vx) return -1;
	}
	else
	{
		if(v1->vx < src->vx && v2->vx < src->vx) return -1;
	}
	if(dest->vz < src->vz)
	{
		if(v1->vz > src->vz && v2->vz > src->vz) return -1;
	}
	else
	{
		if(v1->vz < src->vz && v2->vz < src->vz) return -1;
	}
*/

// line1:	x = t(destx-srcx)
//			z = t(destz-srcz)
// line2:	x = (v1x-srcx) + t2(v2x-v1x)
//			z = (v1z-srcz) + t2(v2z-v1z)

//		t = ((v1x-srcx)(v2z-v1z) - (v1z-srcz)(v2x-v1x)) / ((destx-srcx)(v2z-v1z) - (destz-srcz)(v2x-v1x))

//		t2 = (v1z-srcz)(destx-srcx) - (v1x-srcx)(destz-srcz) / ((v2x-v1x)(destz-srcz) - (v2z-v1z)(destx-srcx))



// Relocate to src-relative...
//	x1 = srcx;
//	z1 = srcz;
	x2 = v1->vx - src->vx;
	z2 = v1->vz - src->vz;
	dx1 = dest->vx - src->vx;
	dz1 = dest->vz - src->vz;
	dx2 = v2->vx - v1->vx;
	dz2 = v2->vz - v1->vz;

// We want ((number * number * 2) *4096) to not overflow (after which, we divide it by (number * number * 2) again)
// If we leave the 4096-scaling for a mo, then there's a 32768 limit, rather than a 512 one.
	num = ABS(x2) | ABS(z2) | ABS(dx1) | ABS(dz1) | ABS(dx2) | ABS(dz2);
	s = 0;
	while(num > 32768)
	{
		num = num>>2;
		s+=2;
	}

	x2 = x2>>s;
	z2 = z2>>s;
	dx1 = dx1>>s;
	dz1 = dz1>>s;
	dx2 = dx2>>s;
	dz2 = dz2>>s;


// Note. The isect's only valid if both t and t2 are in the range 0....1

// Calculate "t2"
	top = z2 * dx1 - x2 * dz1;
	bot = dx2 * dz1 - dz2 * dx1;

	if(ABS(top) > ABS(bot))
		return -1;
	if(!bot)
		return -1;
	if((top>0 && bot < 0) || (bot>0 && top < 0))
		return -1;






// Calculate "t"
	top = x2 * dz2 - z2 * dx2;
	bot = dx1 * dz2 - dz1 * dx2 ;

// We're not interest in the result if it's outside 0...1
	if(ABS(top) > ABS(bot))
		return -1;
// Zero-length test vector, or parallel.
	if(!bot)
		return -1;

	if((top>0 && bot < 0) || (bot>0 && top < 0))
		return -1;





// return the fraction. Phew.
	while( ABS(top) > 0x7ffff)
	{
		top = top>>1;
		bot = bot>>1;
	}

	r = (top * 4096) / bot;
	return r;

}

// "t" should be 0...4096
void routeVectorFraction(VECTOR *src, VECTOR *dest, int t, ROUTEBOX_VECTOR *out)
{
	VECTOR d;
	int num;
	int s;
	int bot;
	d.vx = dest->vx - src->vx;
	d.vz = dest->vz - src->vz;

	ASSERT(ABS(d.vx) < 0x7ffff);
	ASSERT(ABS(d.vz) < 0x7ffff);

// Actually, we shouldn't have any vectors going over 0x7ffff, surely?
// Well, on the boss chases, yes possibly. But they don't involve full-map-length routefinding.
// Or, if they do, all we need to do is plug this code back in... Hence the asserts above
/*
	num = ABS(d.vx) | ABS(d.vz);
	s = 0;
	while(num > 0x7ffff)
	{
		num = num>>2;
		s+=2;
	}

	d.vx = d.vx >> s;
	d.vz = d.vz >> s;
	bot = 4096 >> s;

	out->vx = src->vx + (d.vx * t) / bot;
	out->vz = src->vz + (d.vz * t) / bot;
*/
	out->vx = src->vx + (d.vx * t) / 4096;
	out->vz = src->vz + (d.vz * t) / 4096;
}

typedef struct ROUTE_BOXISECT_STR
{
	ROUTEBOX *box;
	int n;
	int plo;
	ROUTEBOX_VECTOR isectlo;
	int tlo;
	int phi;
	ROUTEBOX_VECTOR isecthi;
	int thi;
}ROUTE_BOXISECT_STR;

// The normal should be in the BFF compiler...
void routeCalculateCornerPoint(ROUTEBOX *box, int p, ROUTEBOX_VECTOR *isect, int r)
{
	ROUTEBOX_VECTOR v[2];
	int p1,p2;
	VECTOR temp;
	VECTOR temp2;

	p1 = (p+1) % box->n_vecs;
	p2 = (p + box->n_vecs-1) % box->n_vecs;

	v[0].vx = box->vecs[p1].vx - box->vecs[p].vx;
	v[0].vz = box->vecs[p1].vz - box->vecs[p].vz;
	
//	v[1].vx = box->vecs[p2].vx - box->vecs[p].vx;
//	v[1].vz = box->vecs[p2].vz - box->vecs[p].vz;
	v[1].vx = box->vecs[p].vx - box->vecs[p2].vx;
	v[1].vz = box->vecs[p].vz - box->vecs[p2].vz;


	temp.vx = -v[0].vz;
	temp.vz = v[0].vx;
	temp.vy = 0;
	MakeUnit(&temp);
	temp2.vx = -v[1].vz;
	temp2.vz = v[1].vx;
	temp2.vy = 0;
	MakeUnit(&temp2);

	isect->vx = box->vecs[p].vx + (((temp.vx+temp2.vx) * r) >> 12);
	isect->vz = box->vecs[p].vz + (((temp.vz+temp2.vz) * r) >> 12);

/*	
	temp.vx = v[0].vx + v[1].vx;
	temp.vz = v[0].vz + v[1].vz;
	temp.vy = 0;
	MakeUnit(&temp);
	isect->vx = box->vecs[p].vx - ((temp.vx * r) >> 12);
	isect->vz = box->vecs[p].vz - ((temp.vz * r) >> 12);
*/
}

void routeAdjustIsectPoint(ROUTEBOX *box, int p, ROUTEBOX_VECTOR *isect, int r)
{
	int p1;
	ROUTEBOX_VECTOR v;
	VECTOR temp;

// Actually no, we want to project back along the incoming line, rather than directly out from the routebox

	p1 = (p+1) % box->n_vecs;
	v.vx = box->vecs[p1].vx - box->vecs[p].vx;
	v.vz = box->vecs[p1].vz - box->vecs[p].vz;

	temp.vx = -v.vz;
	temp.vz = v.vx;
	temp.vy = 0;
	MakeUnit(&temp);

	isect->vx += ((temp.vx * r) >> 12);
	isect->vz += ((temp.vz * r) >> 12);

}





// Find the intersections between a vector and a single routebox
int routeIsectFind(VECTOR *src, VECTOR *dest, ROUTEBOX *box, ROUTE_BOXISECT_STR *isect)
{
	int p;
	int t;
	ROUTEBOX_VECTOR *rv;
	ROUTEBOX_VECTOR *rv2;
//	int mint, maxt;

// Ideally, this quick reject needs to come before the call.
	if(src->vx < box->minx && dest->vx < box->minx) return 0;
	if(src->vx > box->maxx && dest->vx > box->maxx) return 0;
	if(src->vz < box->minz && dest->vz < box->minz) return 0;
	if(src->vz > box->maxz && dest->vz > box->maxz) return 0;

//	mint = maxt = -1;
	isect->n = 0;
	isect->tlo = -1;
	isect->thi = -1;
	rv = box->vecs;
	for(p = 0; p < box->n_vecs; p++,rv++)
	{
		if(p == box->n_vecs-1)
		{
			rv2 = &box->vecs[0];
		}
		else
		{
			rv2 = rv + 1;
		}


// Quick boundcheck elimination of the line-line isect
		if(dest->vx < src->vx)
		{
			if(rv->vx > src->vx && rv2->vx > src->vx) continue;
		}
		else
		{
			if(rv->vx < src->vx && rv2->vx < src->vx) continue;
		}
		if(dest->vz < src->vz)
		{
			if(rv->vz > src->vz && rv2->vz > src->vz) continue;
		}
		else
		{
			if(rv->vz < src->vz && rv2->vz < src->vz) continue;
		}


		t = routeLineIsectFind(src,dest,rv,rv2);
		if(t != -1)
		{
			VECTOR temp;
			isect->n++;

			if(isect->tlo == -1 || t < isect->tlo)
			{
				isect->plo = p;
// Delay the vector calculation, so it's only done on the first box to be hit
//				routeVectorFraction(src, dest, t, &isect->isectlo);
//				routeDebugMarker(&isect->isectlo);
				isect->tlo = t;
			}
			if(isect->thi == -1 || t > isect->thi)
			{
				isect->phi = p;
				isect->thi = t;
//				routeVectorFraction(src, dest, t, &isect->isecthi);
//				routeDebugMarker(&isect->isecthi);
			}
		}
	}

// If "n" is an odd number, then either "src" or "dest" is inside a box.

// If "n" is an even number, then we need to add the isects to the path

	return isect->n;
}

// Find the nearest point on a routebox's outline to a target which is inside the routebox
int routeFindNearestRoutePoint(VECTOR *src, ROUTEBOX *box, VECTOR *out)
{
	int i;
	VECTOR p1,p2;
	VECTOR isect;

	int best_dist = 0x7fffffff;
	int d;
	int best_point = 0;

//	utilPrintf("Inside Box Check\n");
	for(i = 0; i < box->n_vecs; i++)
	{
		p1.vx = box->vecs[i].vx;
		p1.vy = 0;
		p1.vz = box->vecs[i].vz;
		if(i == box->n_vecs-1)
		{
			p2.vx = box->vecs[0].vx;
			p2.vy = 0;
			p2.vz = box->vecs[0].vz;
		}
		else
		{
			p2.vx = box->vecs[i+1].vx;
			p2.vy = 0;
			p2.vz = box->vecs[i+1].vz;
		}

		d = mathsDistancePointToLine2D(&p1,&p2,src,&isect);
		if(d < best_dist)
		{
			*out = isect;
			best_dist = d;
			best_point = i;
		}
	}

//	utilPrintf("Nearest point on route = %d %d %d\n",out->vx /8, -out->vy /8, out->vz / 8);
	return best_point;
}


// Scan through the routeboxes, finding the first one that a vector intersects with
int routeFindIsects(VECTOR *src, VECTOR *dest, ROUTEBOX_VECTOR *route, int max_len)
{
	int bn;
	ROUTEBOX *box;
	int isects;
	ROUTE_BOXISECT_STR isect[2];
	ROUTE_BOXISECT_STR *close_box;

	int current;
	int closest;

	int distance_back;
	int distance_fore;
	int dx,dz;
	int p0,p1;
	int i;

	if(overlay_show_collision == DEBUG_OVERLAY_ENEMY_INFO)
	{
		overlayShowDebugLine(
			src,
			dest,
			0x00ffff,
			DEBUGPOLY_SUBDIV);	//+DEBUGPOLY_USEDEPTH);
	}

	current = 0;
	closest = 1;
	isect[closest].tlo = -1;

	box = route_routes->boxes;
	for(bn = 0; bn < route_routes->n_boxes; bn++,box++)
	{
		isects = routeIsectFind(src,dest,box, &isect[current]);

// isects = 0,1,2. Possibly more if there're any concave boxes.
// Either way, we need to scan round the box once we've got the entry & exit points

		if(isects)
		{
			if(isect[closest].tlo == -1 || isect[current].tlo < isect[closest].tlo)
			{
				closest = current;
				current = 1-closest;
				isect[closest].box = box;
			}
		}
	}


	if(isect[closest].tlo == -1)
	{
		route[0].vx = dest->vx;
		route[0].vz = dest->vz;
		return 1;
	}



// Now then. We need to figure the shortest direction round the obstacle...

	close_box = &isect[closest];
	box = close_box->box;


// Show us the intersections with the nearest routebox
	routeVectorFraction(src, dest, isect[closest].tlo, &isect[closest].isectlo);
	routeAdjustIsectPoint(box, isect[closest].plo, &isect[closest].isectlo, DOG_ROUTEFIND_MARGIN);

//	routeDebugMarker(&isect[closest].isectlo);


// What to do if we have an odd number of isects...
// (ie, one of the points is inside the box)
// Answer, we want the "exit point" to be the nearest point on the box to the dest point.
// This, of course, assumes that it *is* the dest point that's in the box.

// Oh rats. It doesn't work unless we actually ALTER THE DEST to be this new "how close can we get to the dest?" point
// (for shortcutting purposes)

	if(isect[closest].n & 1)
	{
		VECTOR vnear;
		int near_p;
		near_p = routeFindNearestRoutePoint(dest, box, &vnear);

		if(overlay_show_collision == DEBUG_OVERLAY_ENEMY_INFO)
		{
			overlayShowSphere(vnear.vx,-500, vnear.vz, 40, 0xff00ff);
		}

		close_box->isecthi.vx = vnear.vx;
		close_box->isecthi.vz = vnear.vz;
//		close_box->phi = (near_p + box->n_vecs - 1) % box->n_vecs;
		close_box->phi = near_p;
		routeAdjustIsectPoint(box, close_box->phi, &close_box->isecthi, DOG_ROUTEFIND_MARGIN);

// Adjust the route's target point.
		dest->vx = close_box->isecthi.vx;
		dest->vz = close_box->isecthi.vz;

		if(overlay_show_collision == DEBUG_OVERLAY_ENEMY_INFO)
		{
			overlayShowSphere(close_box->isecthi.vx,-500, close_box->isecthi.vz, 200, 0xff00ff);
		}
// Important point here. Under these circumstances, the low & high isects may well be on the same line

// Let's do this special case, then...
//		if(isect[closest].n == 1)
		if(close_box->phi == close_box->plo)
		{
			isect[closest].n = 2;
			route[0] = close_box->isectlo;
			route[1] = close_box->isecthi;
			return 2;
		}
// Otherwise, we now have an entry point and a (closest point == destination), rather than an entry and an exit
// And so, we drop through to the "work out the way round" code.

	}
	else
	{
// Normal exit-point calculation
		routeVectorFraction(src, dest, close_box->thi, &close_box->isecthi);
		routeAdjustIsectPoint(box, close_box->phi, &close_box->isecthi, DOG_ROUTEFIND_MARGIN);
	//	routeDebugMarker(&isect[closest].isecthi);
	}


	if(close_box->plo == close_box->phi)
	{
// Entry & exit points are on the same line.
// (Which can, but won't always, happen if the target is inside a box)
		route[0] = close_box->isectlo;
		return 1;
	}


// Scan Back...

// Distance from Entry to start of that line segment
	p0 = close_box->plo;
	dx = close_box->isectlo.vx - box->vecs[p0].vx;
	dz = close_box->isectlo.vz - box->vecs[p0].vz;
	distance_back = Magnitude2D(dx,dz);
	for(;;)
	{
		p1 = p0-1;
		if(p1 < 0)
			p1 = box->n_vecs-1;

		if(p1 != close_box->phi)
		{
// Lengths of previous line segments
			dx = box->vecs[p1].vx - box->vecs[p0].vx;
			dz = box->vecs[p1].vz - box->vecs[p0].vz;
			distance_back += Magnitude2D(dx,dz);
			p0 = p1;
		}
		else
		{
// distance_back from start of "next" line segment to the exit point
			dx = close_box->isecthi.vx - box->vecs[p0].vx;
			dz = close_box->isecthi.vz - box->vecs[p0].vz;
			distance_back += Magnitude2D(dx,dz);
			break;
		}
	}

// Special case for identical entry & exit points
// (Chicken is inside this routebox being the most likely cause)
	if(close_box->plo == close_box->phi)
	{
//		dx = close_box->isecthi.vx - close_box->isectlo.vx;
//		dz = close_box->isecthi.vz - close_box->isectlo.vz;

		i = 0;
		route[i] = close_box->isectlo;	// In all probability, this point will be optimised out of the route.
		i++;
		route[i] = close_box->isecthi;
		i++;
		return i;
	}

// Scan Forward...

// Distance from Entry to start of the next line segment
	p0 = close_box->plo + 1;
	if(p0 >= box->n_vecs)
		p0 = 0;
	dx = close_box->isectlo.vx - box->vecs[p0].vx;
	dz = close_box->isectlo.vz - box->vecs[p0].vz;
	distance_fore = Magnitude2D(dx,dz);
	for(;;)
	{

// Lengths of next line segments
		if(p0 != close_box->phi)
		{
			p1 = p0 + 1;
			if(p1 >= box->n_vecs)
				p1 = 0;
			dx = box->vecs[p1].vx - box->vecs[p0].vx;
			dz = box->vecs[p1].vz - box->vecs[p0].vz;
			distance_fore += Magnitude2D(dx,dz);
			p0 = p1;
		}
		else
		{
// Distance from start of this segment to the exit point
			dx = close_box->isecthi.vx - box->vecs[p0].vx;
			dz = close_box->isecthi.vz - box->vecs[p0].vz;
			distance_fore += Magnitude2D(dx,dz);
			break;
		}
	}

	i = 0;
	route[i] = close_box->isectlo;
	i++;

	if(distance_fore <= distance_back)
	{
		p0 = close_box->plo;

		for(;p0 != close_box->phi;)
		{
			p0++;
			if(p0 >= box->n_vecs)
				p0 = 0;
//			route[i] = box->vecs[p0];
			routeCalculateCornerPoint(box, p0, &route[i], DOG_ROUTEFIND_MARGIN);

			i++;
//			routeDebugMarker(&box->vecs[p0]);

			if(i >= max_len)
				return i;
		}
	}
	else
	{
		p0 = close_box->plo;

		for(;p0 != close_box->phi;)
		{
//			routeDebugMarker(&box->vecs[p0]);
//			route[i] = box->vecs[p0];
			routeCalculateCornerPoint(box, p0, &route[i], DOG_ROUTEFIND_MARGIN);

			i++;

			p0--;
			if(p0 < 0 )
				p0 = box->n_vecs-1;

			if(i >= max_len)
				return i;

		}
	}
	route[i] = close_box->isecthi;
	i++;
	return i;
}


// Stage 2: See if we can shortcut
// from the start to any of the points on the route.
// from the end to any of the points on the route.

int routeTryShortcuts(VECTOR *src, VECTOR *dest, ROUTEBOX_VECTOR *route, int n_route_points)
{
	int i;
	int bn;
	ROUTEBOX *box;
	VECTOR temp;
	int isects;
	ROUTE_BOXISECT_STR isect;

// Try to snip out the first part of the route
	for(i = n_route_points-1; i >= 1; i--)
	{
		temp.vx = route[i].vx;
		temp.vy = 0;
		temp.vz = route[i].vz;

		box = route_routes->boxes;
		isects = 0;
		for(bn = 0; bn < route_routes->n_boxes; bn++,box++)
		{
			isects = routeIsectFind(src,&temp,box, &isect);
			if(isects)
				break;
		}
		if(!isects)
		{
// We have a direct shortcut!
//			utilPrintf("Snipping Entry points. Shortcut to point %d\n",i);

			FMEMCPY(&route[0],&route[i], (n_route_points - i) * sizeof(ROUTEBOX_VECTOR));
			n_route_points = n_route_points - i;

			bn = 65535;
			i = 0;
		}
	}

// Try to snip out the second part of the route

	for(i = 0; i < n_route_points-1; i++)
	{
		temp.vx = route[i].vx;
		temp.vy = 0;
		temp.vz = route[i].vz;

		box = route_routes->boxes;
		isects = 0;
		for(bn = 0; bn < route_routes->n_boxes; bn++,box++)
		{
			isects = routeIsectFind(&temp,dest, box, &isect);
			if(isects)
				break;
		}
		if(!isects)
		{
//			utilPrintf("Snipping Exit points. Shortcut from point %d\n",i);
			route[i+1].vx = dest->vx;
			route[i+1].vz = dest->vz;
			n_route_points = i+2;

			bn = 65535;
			i = 65535;
		}
	}
	return n_route_points;
}

// The longest possible route contains...
// 1) From start to a box...
// 2) N_Boxes from box to box
// 3) Total number of sides of each box, per box.
// Blimey.
// But that IS an absolute max, which we can use as a side
// Hang on, what about shortcuts?
// A shortcut can only add points which were weren't hit in the original path.


// tbd - we actually need to keep track of the max route length
int routeFindRoute(VECTOR *src, VECTOR *dest_in, ROUTEBOX_VECTOR *route, int max_len)
{
//	ROUTEBOX_VECTOR route[20];
	int n_route_points;
	int i;
	VECTOR temp[2];
	int base_point = 0;
	VECTOR localdest;

	localdest = *dest_in;

	if(!route_routes)
		return 0;



	TIMER_START(TIMER_ROUTE);
//	DB("find\n");
/*
	overlayShowDebugLine(
		src,
		dest,
		0x00ffff,
		DEBUGPOLY_SUBDIV);	//+DEBUGPOLY_USEDEPTH);
*/

	n_route_points = routeFindIsects(src,&localdest,&route[0],max_len);


// Okay. We now have a route that navigates us round the first obstacle.

// See if we can take shortcut from the start to any of the points, and from the end to any of the points.

	if(overlay_show_collision == DEBUG_OVERLAY_ENEMY_INFO)
	{
		for(i = 0; i < n_route_points; i++)
		{
			routeDebugMarker(&route[i], 80, 0xffff00);
		}
	}

	n_route_points = routeTryShortcuts(src,&localdest,&route[0],n_route_points);

// We need to know what angle we're going to turn at the first corner.
// But we may only have one point at this point, or two points of which the second isn't in a "real" position.
// So if we ain't got enough for a good angle decision, then routefind  from the first point in our path


	if(
//		n_route_points < 3
//		&& 
		route[n_route_points-1].vx != localdest.vx 
		&& route[n_route_points-1].vz != localdest.vz
		)
	{
		temp[0].vx = route[0].vx;
		temp[0].vy = src->vy;
		temp[0].vz = route[0].vz;

		n_route_points = routeFindIsects(&temp[0],&localdest,&route[1],max_len-1);
		n_route_points = routeTryShortcuts(&temp[0],&localdest,&route[1],n_route_points);

		n_route_points++;	// re-introduce the original
	}

// Make sure all points are clear of the boxes. Necessary near concave corners.
// The dog itself keeps itself (margin-10) away from the lines, The points need to be a tad further out
// (This is now done by the caller)
/*
	for(i = 0; i < n_route_points; i++)
	{
		routeKeepOutOfRouteboxes(&route[i].vx,&route[i].vz,DOG_ROUTEFIND_MARGIN);
	}
*/

	if(overlay_show_collision == DEBUG_OVERLAY_ENEMY_INFO)
	{

		temp[1].vx = src->vx;
		temp[1].vy = -500;
		temp[1].vz = src->vz;


		for(i = 0; i < n_route_points; i++)
		{
			temp[0] = temp[1];
			temp[1].vx = route[i].vx;
			temp[1].vy = -500;
			temp[1].vz = route[i].vz;

			overlayShowDebugLine(
				&temp[0],
				&temp[1],
				0xffffff,
				DEBUGPOLY_SUBDIV+DEBUGPOLY_USEDEPTH);

			routeDebugMarker(&route[i], 100, 0x00ffff);
		}
	}

	TIMER_STOP(TIMER_ROUTE);
	return n_route_points;

}


void routeShowRoute(VECTOR *src, ROUTEBOX_VECTOR *route, int route_len)
{
	VECTOR temp[2];
	int i;

	temp[1].vx = src->vx;
	temp[1].vy = -500;
	temp[1].vz = src->vz;


	for(i = 0; i < route_len; i++)
	{
		temp[0] = temp[1];
		temp[1].vx = route[i].vx;
		temp[1].vy = -500;
		temp[1].vz = route[i].vz;

		overlayShowDebugLine(
			&temp[0],
			&temp[1],
			0xff00ff,
			DEBUGPOLY_SUBDIV+DEBUGPOLY_USEDEPTH);

//		routeDebugMarker(&route[i], 200, 0x00ffff);
	}
}

// This is for returning to your old path, after giving up.
// Returns point number at start of the nearest line-segment
int routeFindNearestPoint(VECTOR *src, ENEMYPOINTPOINTER path, int n_points, VECTOR *out)
{
	int i;
	VECTOR *p1,*p2;
	VECTOR isect;


	int best_dist = 0x7fffffff;
	int d;
	int best_point = 0;

	if(n_points < 2)	// one point. Go to it. eg - summoned persuit dogs
	{
		*out = path[0].v;
		return 0;
	}

//	utilPrintf("----------\n");
//	utilPrintf("point = %d %d %d\n",src->vx/8,-src->vy/8,src->vz/8);
	for(i = 0; i < n_points; i++)
	{
		p1 = &path[i].v;
		if(i == n_points-1)
			p2 = &path[0].v;
		else
			p2 = &path[i+1].v;

		d = mathsDistancePointToLine2D(p1,p2,src,&isect);
		if(d < best_dist)
		{
			*out = isect;
			best_dist = d;
			best_point = i;
		}
	}

//	utilPrintf("Nearest point on route = %d %d %d\n",out->vx /8, -out->vy /8, out->vz / 8);
	return best_point;
}


// If the enemy's velocity puts it inside a routebox, then it starts misbehaving. So push it out...
int routeKeepOutOfRouteboxes(int *vx, int *vz, int margin)
{
	ROUTEBOX *box;
	int bn;
	int p;
	int d;
	VECTOR out;
	VECTOR dirn;
	int rval;
	VECTOR src;

	VECTOR v1,v2;



	if(!route_routes)
		return 0;


	TIMER_START(TIMER_ROUTE);

	rval = 0;
	box = route_routes->boxes;
	for(bn = 0; bn < route_routes->n_boxes; bn++,box++)
	{
		// If we can do a quick reject, let's do so.

		if(*vx < box->minx -margin) continue;
		if(*vx > box->maxx +margin) continue;
		if(*vz < box->minz -margin) continue;
		if(*vz > box->maxz +margin) continue;

		// okay, do the sums

		v1.vx = box->vecs[box->n_vecs-1].vx;
		v1.vy = 0;
		v1.vz = box->vecs[box->n_vecs-1].vz;
		for(p = 0; p < box->n_vecs; p++)
		{

			v2 = v1;
			v1.vx = box->vecs[p].vx;
			v1.vz = box->vecs[p].vz;	// y's still zero


			// Avoid calculating distance from (for eg) every line in the level border
			if(*vx < v1.vx - margin && *vx < v2.vx-margin) continue;
			if(*vz < v1.vz - margin && *vz < v2.vz-margin) continue;
			if(*vx > v1.vx + margin && *vx > v2.vx+margin) continue;
			if(*vz > v1.vz + margin && *vz > v2.vz+margin) continue;

			src.vx = *vx;
			src.vz = *vz;
			d = mathsDistancePointToLine2D(&v1, &v2, &src, &out);
			if(d < margin)
			{
// "out" is a point on the line. We wanna project out from there by "margin"
// (means we'll get pointed ends. drat)
// Alternatively, src->out will be a vector that's rounded at the ends, but using it will project us INTO
// the box is we manage to get over half-way through.
				dirn.vx = *vx - out.vx;
				dirn.vy = 0;
				dirn.vz = *vz - out.vz;

				MakeUnit(&dirn);

//				*vx += (dirn.vx * margin) >> 12;
//				*vz += (dirn.vz * margin) >> 12;

				*vx = out.vx + ((dirn.vx * margin) >> 12);
				*vz = out.vz + ((dirn.vz * margin) >> 12);

				rval = 1;
			}
		}
	}
	TIMER_STOP(TIMER_ROUTE);
	return rval;
}

