// Search.c
// 
// Version	Date	 Author	 Description
//	--------------------------------------------------------------------------
//  0.01	24/06/01 BWW	 Initialise, generate and select route from a search tree

#include <c:\psx\include\libps.h>
#include "planner.h"
#include "funcprototypes.h"

extern 	char 						Environment[ENV_HEIGHT][ENV_WIDTH];
extern	Door_Struct					Door[];
extern	Bridge_Struct				Bridge[];

extern	SearchTree_Struct			SearchTree;
extern	SearchGoalNodes_Struct		SearchGoalNodes;

extern	Route_Struct				Route;
extern	Metrics_Struct				Metrics;

extern	int startx, starty, destinationx, destinationy;

extern	int	BRIDGENUM;
extern	int	DOORNUM;


//	--------------------------------------------------------------------------
// 	Generate a Search Tree
//	--------------------------------------------------------------------------
//	Description:	Generate a search tree of routes between 2 physical  
//					locations.
//
//	Parameters:		int		Starting X coordinate. *** Passed as a global ****
//					int		Starting Y coordinate. *** Passed as a global **** 
//					int		Destination X coordinate. *** Passed as a global ****
//					int		Destination Y coordinate. *** Passed as a global ****
//	Returns:		None

void GenerateSearchTree(int includedynobjects)
	{
	
	int	currentnode = 0;
	
	Metrics.e_numsearches++;
	
//	int index;
	
	// Clear down the old search tree
	InitSearchTree();
	
	// Add a node for the starting position
	currentnode = AddNodeToSearchTree(0,startx,starty,0);
	
	// Recursively expand the search tree
	ExpandSearchTree(currentnode, includedynobjects);


//	printf("NumGoalNodes: %d\n", SearchGoalNodes.numgoalnodes);
//	printf("LowestCostNode: %d  Value: %d \n", SearchGoalNodes.lowestcostnode, SearchGoalNodes.lowestcostvalue);
	
//	for (index=0;index<SearchGoalNodes.numgoalnodes;index++)
//		{
//		printf("GoalNode[%d]: %d Costtonode: %d\n", index, SearchGoalNodes.goalnode[index], SearchGoalNodes.costtonode[index]);
//		}


	
	
//	printf("sx: %d  sy: %d  dx: %d dy: %d \n",startx,starty,destinationx, destinationy);

	}

//	--------------------------------------------------------------------------
// 	Select route from the search tree
//	--------------------------------------------------------------------------
//	Description:	Select the cheapest route from the search tree and copy it to
//					the Route structure.
//
//	Parameters:		None
//	Returns:		None

void SelectRoute()
	{
	
	int node, routeindex; 
	
	Metrics.e_numsearchnodes += SearchTree.numnodes;
	Metrics.e_numgoalnodes += SearchGoalNodes.numgoalnodes;

	// Check for the situation where there are no goal nodes
	if (SearchGoalNodes.numgoalnodes == 0)
		{
		Route.numsteps = 0;
		}
	else
		{
		node = SearchGoalNodes.lowestcostnode;

//		printf("Select Node: %d\n",node);

		routeindex = (SearchTree.nodedepth[node]-1);
		Route.numsteps = (routeindex+1);

//		printf("Select Routeindex: %d\n",routeindex);

		
		// Backward chain through the search tree using the 'parent' attribute
		do
			{
			Route.x[routeindex]		= SearchTree.xpos[node];
			Route.y[routeindex]		= SearchTree.ypos[node];
			Route.cost[routeindex] 	= SearchTree.cost[node];
			node = SearchTree.parentnode[node];

//			printf("Select Node: %d\n",node);

			routeindex--;
			} while (node != 0);
		}
	
	
	
//	printf("Number steps in Route: %d\n", Route.numsteps);
//	for (routeindex=0;routeindex<Route.numsteps;routeindex++)
//		{
//		printf("Step[%d] X: %d  Y:%d\n", routeindex, Route.x[routeindex], Route.y[routeindex]);
//		}
	
	}
	
//	--------------------------------------------------------------------------
// 	Expand the Search Tree
//	--------------------------------------------------------------------------
//	Description:	Expand all of the nodes on the search tree.
//
//	Parameters:		int		Starting X coordinate. *** Passed as a global ****
//					int		Starting Y coordinate. *** Passed as a global ****
//					int		Destination X coordinate. *** Passed as a global ****
//					int		Destination Y coordinate. *** Passed as a global ****
//     				int 	Current Node
//	Returns:		None

void ExpandSearchTree(int currentnode, int includedynobjectss)
	{
	
	int goalnodeindex, directionindex, newnode, index;
	
//	printf("Node: %d  Depth: %d  X: %d  Y: %d  Parent: %d  Route: %d %d %d %d %d  Cost: %d \n",
//			currentnode,
//			SearchTree.nodedepth[currentnode],
//			SearchTree.xpos[currentnode],
//			SearchTree.ypos[currentnode],
//			SearchTree.parentnode[currentnode],
//			SearchTree.nodesinroute[currentnode][0],
//			SearchTree.nodesinroute[currentnode][1],
//			SearchTree.nodesinroute[currentnode][2],
//			SearchTree.nodesinroute[currentnode][3],
//			SearchTree.nodesinroute[currentnode][4],
//			SearchTree.costtonode[currentnode]);

	// Check for goal attaintment
	if ((SearchTree.xpos[currentnode] == destinationx)&&(SearchTree.ypos[currentnode] == destinationy))
		{
		// Goal attained.  Mark it .....
		SearchTree.goalnode[currentnode] = true;
		
		// ..... log it in the goal node array ....
		goalnodeindex = SearchGoalNodes.numgoalnodes++;
		
//		printf("GoalNodeIndex: %d  Node: %d  Depth: %d\n",goalnodeindex, currentnode,SearchTree.nodedepth[currentnode]);
		
		SearchGoalNodes.goalnode[goalnodeindex] = currentnode;
		SearchGoalNodes.costtonode[goalnodeindex] = SearchTree.costtonode[currentnode];
		
		// .... check whether we have found the most efficient route so far .....
		if (SearchTree.costtonode[currentnode] <= SearchGoalNodes.lowestcostvalue)
			{
			SearchGoalNodes.lowestcostvalue = SearchTree.costtonode[currentnode];
			SearchGoalNodes.lowestcostnode = currentnode;
	
	
//			printf("\nLowestCostNode: %d Value: %d\n",SearchGoalNodes.lowestcostnode, SearchGoalNodes.lowestcostvalue);
	
	
			}
		// ..... and return
		return;
		}
	else
		{
		// Goal not attained.  Continue searching
		SearchTree.goalnode[currentnode] = false;

		// *** Heuristic ***.  Limit the depth to which we search to 25 nodes as space for the search tree
		// is constrained
		if (SearchTree.nodedepth[currentnode] == SIZESEARCHDEPTH)
			{
			return;
			}
			
		// *** Heuristic ***.  Also only continue searching if the partial solution we currently have is less costly than the 
		// best solution currently found. 
		if (SearchTree.costtonode[currentnode] >= SearchGoalNodes.lowestcostvalue)
			{
			return;
			}



		// Check which of the directions - E, S, N, W - are accessible.  Heuristically order them to 
		// move towards the Exit first.  Also make sure they haven't already been added to the search tree
		// for this route.
		if ((CheckAlreadyProcessed(currentnode, (SearchTree.xpos[currentnode]+1),(SearchTree.ypos[currentnode]+0)))==false)
			{
			SearchTree.accessibledirections[currentnode][0] = 
					CheckLocationForAccess((SearchTree.xpos[currentnode]+1),(SearchTree.ypos[currentnode]+0),includedynobjectss);
			}
		else
			{
			SearchTree.accessibledirections[currentnode][0] = 0;
			}
		if ((CheckAlreadyProcessed(currentnode, (SearchTree.xpos[currentnode]+0),(SearchTree.ypos[currentnode]+1)))==false)
			{
			SearchTree.accessibledirections[currentnode][1] = 
					CheckLocationForAccess((SearchTree.xpos[currentnode]+0),(SearchTree.ypos[currentnode]+1),includedynobjectss);
			}
		else
			{
			SearchTree.accessibledirections[currentnode][1] = 0;
			}
		if ((CheckAlreadyProcessed(currentnode, (SearchTree.xpos[currentnode]+0),(SearchTree.ypos[currentnode]-1)))==false)
			{
			SearchTree.accessibledirections[currentnode][2] = 
					CheckLocationForAccess((SearchTree.xpos[currentnode]+0),(SearchTree.ypos[currentnode]-1),includedynobjectss);
			}
		else
			{
			SearchTree.accessibledirections[currentnode][2] = 0;
			}
		if ((CheckAlreadyProcessed(currentnode, (SearchTree.xpos[currentnode]-1),(SearchTree.ypos[currentnode]+0)))==false)
			{
			SearchTree.accessibledirections[currentnode][3] = 
					CheckLocationForAccess((SearchTree.xpos[currentnode]-1),(SearchTree.ypos[currentnode]+0),includedynobjectss);
			}
		else
			{
			SearchTree.accessibledirections[currentnode][3] = 0;
			}

		// For each accessible location, add a new node and recursively call this routine to expand it		
//		for (directionindex=0;directionindex<4;directionindex++)
		for (index=0;index<4;index++)
			{
			
			// *** Heuristically *** set the order in which to add the new nodes depending on the agent's current position and
			// the destination location
			if ((SearchTree.xpos[currentnode]<=destinationx)&&(SearchTree.ypos[currentnode]>destinationy))
				{
				switch(index)
					{
					case 0:
						directionindex=0;
						break;
					case 1:
						directionindex=2;
						break;
					case 2:
						directionindex=1;
						break;
					case 3:
						directionindex=3;
						break;
					}
				}
			if ((SearchTree.xpos[currentnode]<=destinationx)&&(SearchTree.ypos[currentnode]<=destinationy))
				{
				switch(index)
					{
					case 0:
						directionindex=0;
						break;
					case 1:
						directionindex=1;
						break;
					case 2:
						directionindex=2;
						break;
					case 3:
						directionindex=3;
						break;
					}
				}
			
			if ((SearchTree.xpos[currentnode]>destinationx)&&(SearchTree.ypos[currentnode]<=destinationy))
				{
				switch(index)
					{
					case 0:
						directionindex=3;
						break;
					case 1:
						directionindex=1;
						break;
					case 2:
						directionindex=2;
						break;
					case 3:
						directionindex=0;
						break;
					}
				}
			
			if ((SearchTree.xpos[currentnode]>destinationx)&&(SearchTree.ypos[currentnode]>destinationy))
				{
				switch(index)
					{
					case 0:
						directionindex=3;
						break;
					case 1:
						directionindex=2;
						break;
					case 2:
						directionindex=1;
						break;
					case 3:
						directionindex=0;
						break;
					}
				}
			
			
			
			if (SearchTree.accessibledirections[currentnode][directionindex]>0)
				{
				if (directionindex == 0)
					{
					newnode = AddNodeToSearchTree(currentnode,(SearchTree.xpos[currentnode]+1),(SearchTree.ypos[currentnode]+0),
													SearchTree.accessibledirections[currentnode][0]);
					}
				if (directionindex == 1)
					{
					newnode = AddNodeToSearchTree(currentnode,(SearchTree.xpos[currentnode]+0),(SearchTree.ypos[currentnode]+1),
													SearchTree.accessibledirections[currentnode][1]);
					}
				if (directionindex == 2)
					{
					newnode = AddNodeToSearchTree(currentnode,(SearchTree.xpos[currentnode]+0),(SearchTree.ypos[currentnode]-1),
													SearchTree.accessibledirections[currentnode][2]);
					}
				if (directionindex == 3)
					{
					newnode = AddNodeToSearchTree(currentnode,(SearchTree.xpos[currentnode]-1),(SearchTree.ypos[currentnode]+0),
													SearchTree.accessibledirections[currentnode][3]);
					}
				
				// Check whether the max size of the search array is being reached.  Expand further if it hasn't.
				if (currentnode > (SIZESEARCHARRAY - 100))
					{
					return;
					}
				else
					{
					ExpandSearchTree(newnode,includedynobjectss);
					}
				}
			}
		}
	}

//	--------------------------------------------------------------------------
// 	Check whether a location has already been processed
//	--------------------------------------------------------------------------
//	Description:	Check whether a particular location has been processed by
//					this particular route through the search tree
//					
//	Parameters:		int		The node currently being expanded
//					int		The x location to check
//					int		The y location to check
//	Returns:		int		True or False (1 or 0)

int CheckAlreadyProcessed(int currentnode, int xtocheck, int ytocheck)
	{
	int index, nodetocheck;
	
	for (index=0;index<(SearchTree.nodedepth[currentnode]);index++)
		{
		nodetocheck = SearchTree.nodesinroute[currentnode][index];
		if ((SearchTree.xpos[nodetocheck]==xtocheck)&&(SearchTree.ypos[nodetocheck]==ytocheck))
			{
			return(true);
			}
		}
	
	return(false);
	}

//	--------------------------------------------------------------------------
// 	Add a new node to the Search Tree
//	--------------------------------------------------------------------------
//	Description:	Add a new node to the search tree  
//					
//
//	Parameters:		int		The index to the parent node.
//					int		X coordinate.
//					int		Y coordinate.
//					int		Cost to traverse physical location.
//	Returns:		None

int  AddNodeToSearchTree(int Parent, int Xpos, int Ypos, int CostToNode)
	{
	
	int index;
	
	SearchTree.numnodes++;
	SearchTree.parentnode[SearchTree.numnodes] = Parent;
	SearchTree.xpos[SearchTree.numnodes] = Xpos;
	SearchTree.ypos[SearchTree.numnodes] = Ypos;
	SearchTree.costtonode[SearchTree.numnodes] = SearchTree.costtonode[Parent] + CostToNode; 
	SearchTree.cost[SearchTree.numnodes] = CostToNode;
	SearchTree.nodedepth[SearchTree.numnodes] = SearchTree.nodedepth[Parent] + 1; 
	
	// Copy the list of nodes from the parent node and then add the current node itself (so we can check
	// later to ensure that we don't re-visit a node and create an infinite loop)
	for (index=0;index<(SearchTree.nodedepth[SearchTree.numnodes]);index++)
		{
		SearchTree.nodesinroute[SearchTree.numnodes][index] = SearchTree.nodesinroute[Parent][index];
		}
//	SearchTree.nodesinroute[SearchTree.numnodes][SearchTree.nodedepth[SearchTree.numnodes]] = Parent;
	SearchTree.nodesinroute[SearchTree.numnodes][SearchTree.nodedepth[SearchTree.numnodes]] = SearchTree.numnodes;
		
	return (SearchTree.numnodes);
	}

//	--------------------------------------------------------------------------
// 	Initialise the Search Tree
//	--------------------------------------------------------------------------
//	Description:	Initialise the structures used to search for a physical 
//					route between locations
//
//	Parameters:		None
//	Returns:		None

void InitSearchTree ()
	{

	// Reset the number of nodes in the tree and in the goal array to zero
	SearchTree.numnodes = 			0;
	SearchGoalNodes.numgoalnodes = 	0;
	SearchGoalNodes.lowestcostvalue = 50000;
	
	// Element 0 is a dummy node at the top of the tree, so set some defaults
	SearchTree.costtonode[0] = 0;
	SearchTree.nodedepth[0] = 0;
	SearchTree.nodesinroute[0][0] = 0;
	
	}
	
//	--------------------------------------------------------------------------
// 	Check whether a location is accessibe
//	--------------------------------------------------------------------------
//	Description:	Check whether an agent is able to access a location and  
//					return the cost of doing so if it is
//
//	Parameters:		Int		X coordinate of the location to be checked
//					Int		Y coordinate of the location to be checked
//	Returns:		Int		Cost of accessing location. '0' = inaccessible.

int CheckLocationForAccess(int Xpos, int Ypos, int includedynobjects)
	{
	
	int cost = 0;	
	int	index;
	
	// Check for the edge of the arena
	if ((Xpos>=ENV_WIDTH)||(Xpos<0)||(Ypos>=ENV_HEIGHT)||(Ypos<0))
		{
		return(cost);
		}
	
	// Check for basic accessible areas	
	if (Environment[Ypos][Xpos]==OBJ_FLAT)
		{
		cost = 100;
		}
	if (Environment[Ypos][Xpos]==OBJ_UP)
		{
		cost = 150;
		}
	if (Environment[Ypos][Xpos]==OBJ_DOWN)
		{
		cost = 50;
		}
	

	// Check for bridges at this location
	for (index=0;index<BRIDGENUM;index++)
		{
		if ((Bridge[index].x == Xpos)&&(Bridge[index].y == Ypos))
			{
		
			// Check the bridge status .... must be lowered to be accessible
			if (Bridge[index].status == BRIDGELOWERED)
				{
				cost = 125;
				}
			else
				{
				cost = 0;
				}
			}
		}

	// Check for doors at this location
	if (includedynobjects==true)
		{
	
		for (index=0;index<DOORNUM;index++)
			{
			if ((Door[index].x == Xpos)&&(Door[index].y == Ypos))
				{
			
				// Check the door status .. must be unlocked and open to be accessible
				if ((Door[index].doorstatus == DOOROPEN)&&(Door[index].lockstatus == DOORUNLOCKED))
					{
					cost = 125;
					}
				else
					{
					cost = 0;
					}
				}
			}
		}

	return(cost);
	}