#define COST_INFINITE 999999999999

enumflags {
	WP_JUMP, //also implies that the bot must first go behind the wp...
	WP_CLIMB,
	WP_CROUCH,
	WP_USE
};

typedef struct waypoint_s {
	vector org;
	float flRadius;	//used for picking the closest waypoint. aka proximity weight. also relaxes routes inside the area.
	struct wpneighbour_s
	{
		int node;
		float linkcost;
		int iFlags;
	} *neighbour;
	int iNeighbours;
} waypoint_t;

static waypoint_t *waypoints;
static int iWaypoints;
static int bWaypointsLoaded;

static void() Way_WipeWaypoints=
{
	for (int i = 0; i < iWaypoints; i++)
		memfree(waypoints[i].neighbour);
	memfree(waypoints);
	iWaypoints = 0;
}

void Way_DumpWaypoints( string filename )
{
	float file = fopen(filename, FILE_WRITE);
	if (file < 0)
	{
		print("RT_DumpWaypoints: unable to open ", filename, "\n");
		return;
	}
	fputs(file, sprintf("%i\n", iWaypoints));
	for(int i = 0i; i < iWaypoints; i++)
	{
		fputs(file, sprintf("%v %f %i\n", waypoints[i].org, waypoints[i].flRadius, waypoints[i].iNeighbours));
		for(int j = 0i; j < waypoints[i].iNeighbours; j++)
			fputs(file, sprintf(" %i %f %#x\n", waypoints[i].neighbour[j].node, waypoints[i].neighbour[j].linkcost, waypoints[i].neighbour[j].iFlags));
	}
	fclose(file);
}

void Way_ReadWaypoints( string filename )
{
	float file = fopen(filename, FILE_READ);
	bWaypointsLoaded = TRUE;
	if (file < 0)
	{
		print("Way_DumpWaypoints: unable to open ", filename, "\n");
		return;
	}
	Way_WipeWaypoints();
	
	tokenize(fgets(file));
	iWaypoints = stoi(argv(0));
	
	waypoints = memalloc(sizeof(*waypoints)*iWaypoints);
	for(int i = 0i; i < iWaypoints; i++)
	{
		tokenize(fgets(file));
		waypoints[i].org[0] = stof(argv(0));
		waypoints[i].org[1] = stof(argv(1));
		waypoints[i].org[2] = stof(argv(2));
		waypoints[i].flRadius = stof(argv(3));
		waypoints[i].iNeighbours = stoi(argv(4));
		waypoints[i].neighbour = memalloc(sizeof(*waypoints[i].neighbour)*waypoints[i].iNeighbours);
		for(int j = 0i; j < waypoints[i].iNeighbours; j++)
		{
			tokenize(fgets(file));
			waypoints[i].neighbour[j].node = stoi(argv(0));
			waypoints[i].neighbour[j].linkcost = stof(argv(1));
			waypoints[i].neighbour[j].iFlags = stoh(argv(2));
		}
	}
	fclose(file);
}

static void* memrealloc( __variant *oldptr, int elementsize, int oldelements, int newelements )
{
	void *n = memalloc(elementsize*newelements);
	memcpy(n, oldptr, elementsize*min(oldelements,newelements));
	memfree(oldptr);
	
	return n;
}

static void Way_LinkWaypoints( waypoint_t *wp, waypoint_t *w2 )
{
	int w2n = w2-waypoints;
	for (int i = 0i; i < wp->iNeighbours; i++)
	{
		if (wp->neighbour[i].node == w2n)
			return;
	}
	
	int idx = wp->iNeighbours++;
	wp->neighbour = memrealloc(wp->neighbour, sizeof(*wp->neighbour), idx, wp->iNeighbours);
	local struct wpneighbour_s *n = wp->neighbour+idx;
	n->node = w2n;
	n->linkcost = vlen(w2->org - wp->org);
	n->iFlags = 0;
}

static void Way_AutoLink( waypoint_t *wp )
{
	int wpidx = wp-waypoints;
	for (int i = 0i; i < iWaypoints; i++)
	{
		if (i == wpidx)
			continue;	//don't link to ourself...

		if (vlen(wp->org - waypoints[i].org) > autocvar( nav_linksize, 256, "Cuttoff distance between links" ) )
			continue;	//autolink distance cutoff.

		//not going to use the full player size because that makes steps really messy.
		//however, we do need a little size, for sanity's sake
		tracebox(wp->org, '-16 -16 0', '16 16 32', waypoints[i].org, TRUE, world);
		if (trace_fraction < 1)
			continue;	//light of sight blocked, don't try autolinking.
			
		Way_LinkWaypoints(wp, &waypoints[i]);
		Way_LinkWaypoints(&waypoints[i], wp);
	}
}

void Way_Waypoint_Create( entity pl, float autolink )
{
	vector pos = pl.origin;
	int idx = iWaypoints++;
	waypoints = memrealloc( waypoints, sizeof(waypoint_t), idx, iWaypoints );
	waypoint_t *n = waypoints + idx;
	n->org = pos;
	n->neighbour = __NULL__;
	n->iNeighbours = 0;
	
	if (autolink)
		Way_AutoLink(n);
}

void Way_Waypoint_Delete( int idx )
{
	if (idx < 0i || idx >= iWaypoints)
	{
		print("RT_DeleteWaypoint: invalid waypoint\n");
		return;
	}

	//wipe the waypoint
	memfree(waypoints[idx].neighbour);
	memcpy(waypoints+idx, waypoints+idx+1, (iWaypoints-(idx+1))*sizeof(*waypoints));
	iWaypoints--;

	//clean up any links to it.
	for (int i = 0; i < iWaypoints; i++)
	{
		for (int j = 0; j < waypoints[i].iNeighbours; )
		{
			int l = waypoints[i].neighbour[j].node;
			if (l == idx)
			{
				memcpy(waypoints[i].neighbour+j, waypoints[i].neighbour+j+1, (waypoints[i].iNeighbours-(j+1))*sizeof(*waypoints[i].neighbour));
				waypoints[i].iNeighbours--;
				continue;
			}
			else if (l > idx)
				waypoints[i].neighbour[j].node = l-1;
			j++;
		}
	}
}

void Way_Waypoint_SetRadius( int idx, float radius )
{
	if (idx < 0i || idx >= iWaypoints)
	{
		print("RT_Waypoint_SetRadius: invalid waypoint\n");
		return;
	}
	waypoints[idx].flRadius = radius;
}

void Way_Waypoint_MakeJump(int idx)
{
	if (idx < 0i || idx >= iWaypoints)
	{
		print("RT_Waypoint_SetRadius: invalid waypoint\n");
		return;
	}

	for(int j = 0i; j < waypoints[idx].iNeighbours; j++) {
		int target = waypoints[idx].neighbour[j].node;
		
		for(int b = 0i; b < waypoints[target].iNeighbours; b++) {
			if ( waypoints[target].neighbour[b].node == idx ) {
				waypoints[target].neighbour[b].iFlags = WP_JUMP;
			}
		}
	}
}

//-1 for no nodes anywhere...
int Way_FindClosestWaypoint( vector org )
{
	int r = -1i;
	float bestdist = COST_INFINITE;
	for (int i = 0i; i < iWaypoints; i++)
	{
		float dist = vlen(waypoints[i].org - org) - waypoints[i].flRadius;
		if (dist < bestdist)
		{
			if (dist < 0)
			{	//within the waypoint's radius
				bestdist = dist;
				r = i;
			}
			else
			{	//outside the waypoint, make sure its valid.
				traceline(org, waypoints[i].org, TRUE, world);
				if (trace_fraction == 1)
				{	//FIXME: sort them frst, to avoid traces?
					bestdist = dist;
					r = i;
				}
			}
		}
	}
	return r;
}

//Lame visualisation stuff - this is only visible on listen servers.
void SV_AddDebugPolygons( void ) {
	if ( !autocvar( way_display, 0, "Display current waypoints" ) ) {
		return;
	}

	if ( !iWaypoints ) {
		return;
	}

	int nearest = Way_FindClosestWaypoint( self.origin );
	makevectors(self.v_angle);
	R_BeginPolygon( "waypoint", 0, 0 );
	
	for ( int i = 0i; i < iWaypoints; i++ ) {
		waypoint_t *w = waypoints+i;
		vector org = w->org;
		vector rgb = '1 1 1';
		if (nearest == i)
			rgb = '0 1 0';
		R_PolygonVertex(org + v_right*16 - v_up*16, '1 1', rgb, 1);
		R_PolygonVertex(org - v_right*16 - v_up*16, '0 1', rgb, 1);
		R_PolygonVertex(org - v_right*16 + v_up*16, '0 0', rgb, 1);
		R_PolygonVertex(org + v_right*16 + v_up*16, '1 0', rgb, 1);
		R_EndPolygon();
	}
	
	R_BeginPolygon("", 0, 0);
	for ( int i = 0i; i < iWaypoints; i++ ) {
		waypoint_t *w = waypoints+i;
		vector org = w->org;
		
		for (float j = 0; j < 2*M_PI; j += 2*M_PI/4)
			R_PolygonVertex(org + [sin(j), cos(j)]*w->flRadius, '1 1', '0 0.25 0', 1);
		R_EndPolygon();
	}

	R_BeginPolygon("", 1, 0);
	for ( int i = 0i; i < iWaypoints; i++ ) {
		waypoint_t *w = waypoints+i;
		vector org = w->org;
		vector rgb = '1 1 1';
		for ( int j = 0i; j < w->iNeighbours; j++ ) {
			int k = w->neighbour[j].node;
			if ( k < 0i || k >= iWaypoints ) {
				break;
			}
			waypoint_t *w2 = &waypoints[k];
			R_PolygonVertex(org, '0 1', '1 0 1', 1);
			R_PolygonVertex(w2->org, '1 1', '0 1 0', 1);
			R_EndPolygon();
		}
	}
}