// findpath.c
// GPSNav
//
// Algorithm for finding the best path via portals.
//
// Copyright (C) 2004 Joshua Wise. All rights reserved.

#include <unistd.h>

#include "gpsnav.h"

struct path*	bestpath;
int		bestcost;

inline struct path* path_dup(struct path* p)
{
	struct path* d, *dorig;
	
	if (p == NULL)
		return NULL;
	
	d = dorig = (struct path*) malloc(sizeof(struct path));
	d->portal = p->portal;
	
	while (p)
	{
		d->next = (struct path*) malloc(sizeof(struct path)); d = d->next;
		d->portal = p->portal;
		p = p->next;
	}
	d->next = NULL;
	
	return dorig;
}

inline void path_free(struct path* p)
{
	struct path* pnext;
	
	while (p)
	{
		pnext = p->next;
		free(p);
		p = pnext;
	}
}

void find_path_recurse (struct wall* here, struct wall* there, int cost, struct path* curpath)
{
	struct wall* walls;
	
	if ((here == there) && (cost < bestcost))
	{
		bestcost = cost;
		bestpath = path_dup(curpath);
		return;
	}
	
	if (cost > bestcost)
		return;
		
	if (!here->portal)
	{
		DEBUG("WTF? !here->portal?");
		return;
	}
	
	walls = here->portal->walls;
	cost += here->portal->cost;
	while (walls)
	{
		struct path* newpath, *newpathorig;	// yes I know my asterisk spacing is funky
		
		if (!walls->portal)
		{
			walls = walls->nextwall;
			continue;
		}
		
		newpath = newpathorig = path_dup(curpath);
		if (!curpath)
		{
			newpath = newpathorig = (struct path*) malloc (sizeof(struct path));
			newpath->next = NULL;
		}
		
		while (newpath->next)
			newpath = newpath->next;
		newpath->portal = walls;
		
		find_path_recurse(walls, there, cost + walls->cost, newpathorig);
		
		path_free(newpathorig);		// let's not leak memory. this is OK because we will path_dup if this is the best.
		
		walls = walls->nextwall;
	}
}

struct path* findpath (struct wall* start, struct wall* finish)
{
	bestpath = NULL;
	bestcost = 0x7FFFFFFF;	// largst postiive integer
	
	if (!start->portal)
	{
		DEBUG("start->portal == NULL");
		return NULL;
	}
	
	find_path_recurse(start, finish, 0, NULL);
	
	return bestpath;
}