////////////////////////////////////////////////////////////////////////////////////////
// RAVEN STANDARD TEMPLATE LIBRARY
//  (c) 2002 Activision
//
//
// Graph
// -----
// A Graph object is one of the most generic data structures.  Theoretically, a graph
// could be used to make a tree, list, or any other standard structure.  A graph has
// two data types: NODE and EDGE, and maintains connection information how edges can
// link two nodes.
//
// One of the most intuitive uses of a graph class is a navigation bouy system.  In such
// a use, the nodes would probably be vector based objects with positional information
// and the edges might contain portal information (door is open / closed / locked).
//
// Another example might be a web page, or decision tree, or any other problem space
// which requires object connection data.
//
//
//
//
// Implimentation
// --------------
// This template allocates a pool for NODES, a pool for EDGES, and a grid2_vs<int> to serve
// as an Adjacency Matrix (called Links).  The Adj. Matrix stores indicies to EDGE objects
// in the EDGE pool.
//
//
//
//
// What If You Do Not Need Any Edge Objects?
// -----------------------------------------
// It's fairly common to have a graph with no connection information other than the
// existance of the link.  For this case, you should be able to create a graph with a
// MAXEDGES of 1.  You will want to call the version of connect_node() which does not
// take an edge object, and uses 1 as the "index" in the Adj. Matrix.
//
// 
//
//
// How Do You Search?
// -------------------
// This graph supports 3 search methods:
//  Breadth First - Exausts as many links close to start as possible
//  Depth First	  - Gets as far from start as quickly as possible
//  A*            - Uses a distance heuristic toward end point
//
// First, create a (graph_vs::search) object with the start and end points that you want
// to search for.  Then, call either bfs(), dfs(), or astar().  When you get the
// object back, it will have a vector of all the nodes that were visited and methods
// for iterating over that vector to get the path.
//
//	for (TestSearch.path_begin(); !TestSearch.path_end(); TestSearch.path_inc())
//	{
//		sprintf(Buf, "(%d)", TestSearch.path_at());
//		OutputDebugString(Buf);
//	}
//
// 
//
//
// Complexity Analisis
// -------------------
// All data operations except remove_node() are O(1) constant time.
// remove_node() can be O(n) where n is the number of NODES in the graph.
// 
// Search routines:
//  BFS - 
//  DFS - 
//  A*  - 
//
//
//
//
////////////////////////////////////////////////////////////////////////////////////////
#if !defined(RAGL_GRAPH_VS_INC)
#define RAGL_GRAPH_VS_INC


////////////////////////////////////////////////////////////////////////////////////////
// Includes
////////////////////////////////////////////////////////////////////////////////////////
#if defined(RA_DEBUG_LINKING)
	#pragma message("...including graph_vs.h")
#endif
#if !defined(RAGL_COMMON_INC)
	#include "ragl_common.h"
#endif
#if !defined(RATL_ARRAY_VS_INC)
	#include "..\Ratl\array_vs.h"
#endif
#if !defined(RATL_VECTOR_VS_INC)
	#include "..\Ratl\vector_vs.h"
#endif
#if !defined(RATL_BITS_INC)
	#include "..\Ratl\bits_vs.h"
#endif
#if !defined(RATL_QUEUE_VS_INC)
	#include "..\Ratl\queue_vs.h"
#endif
#if !defined(RATL_STACK_VS_INC)
	#include "..\Ratl\stack_vs.h"
#endif
#if !defined(RBTREE_MAP_VS_INC)
	#include "..\Ratl\map_vs.h"
#endif
#if !defined(RATL_POOL_VS_INC)
	#include "..\Ratl\pool_vs.h"
#endif
#if !defined(RATL_GRID_VS_INC)
	#include "..\Ratl\grid_vs.h"
#endif
namespace ragl
{









////////////////////////////////////////////////////////////////////////////////////////
// The Graph Class
////////////////////////////////////////////////////////////////////////////////////////
template <class TNODE, int MAXNODES, class TEDGE, int MAXEDGES, int MAXNODENEIGHBORS>
class graph_vs : public ratl::ratl_base
{
public:
    ////////////////////////////////////////////////////////////////////////////////////
	// The Graph User Class
	//
	// When executing a search (in particular an A* search), you may want to derive your
	// own user class so that you can provide specific functionality to the search.  For
	// example, you might want characters on one team to bias the cost of going to nodes
	// which are occupied by the other team.  Or you might want to allow specific
	// characters to access some edges where others cannot.  Perhaps one can fly or has
	// a special key to a door...
	//
    ////////////////////////////////////////////////////////////////////////////////////
	class user
	{
	public:
		////////////////////////////////////////////////////////////////////////////////////
		// can be invalid edge (For Region)
		////////////////////////////////////////////////////////////////////////////////////
		virtual		bool	can_be_invalid(const TEDGE& Edge) const = 0;
		
		////////////////////////////////////////////////////////////////////////////////////
		// valid edge (For A* and Region)
		////////////////////////////////////////////////////////////////////////////////////
		virtual		bool	is_valid(TEDGE& Edge, int EndPoint=0) const = 0;

		////////////////////////////////////////////////////////////////////////////////////
		// cost estimate from A to B (For A*)
		////////////////////////////////////////////////////////////////////////////////////
		virtual		float	cost(const TNODE& A, const TNODE& B) const = 0;

		////////////////////////////////////////////////////////////////////////////////////
		// cost estimate of Edge (For A*)
		////////////////////////////////////////////////////////////////////////////////////
		virtual		float	cost(const TEDGE& Edge, const TNODE& B) const = 0;

		////////////////////////////////////////////////////////////////////////////////////
		// same floor check (For Triangulation)
		////////////////////////////////////////////////////////////////////////////////////
		virtual		bool	on_same_floor(const TNODE& A, const TNODE& B) const = 0;

		////////////////////////////////////////////////////////////////////////////////////
		// setup the edge (For Triangulation)
		////////////////////////////////////////////////////////////////////////////////////
		virtual		void	setup_edge(TEDGE& Edge, int A, int B, bool OnHull, const TNODE& NodeA, const TNODE& NodeB, bool CanBeInvalid=false) = 0;
	};



    ////////////////////////////////////////////////////////////////////////////////////
	// Capacity Enum
    ////////////////////////////////////////////////////////////////////////////////////
 	enum 
	{
		CAPACITY = MAXNODES,
		NULLEDGE = -1,
	};


	typedef typename ratl::pool_vs<TNODE,		MAXNODES>				TNodes;
	typedef typename ratl::pool_vs<TEDGE,		MAXEDGES>				TEdges;
	struct	SNodeNeighbor
	{
		short	mEdge;
		short	mNode;
	};
	typedef typename ratl::vector_vs<SNodeNeighbor,  MAXNODENEIGHBORS>	TNodeNeighbors;
	typedef typename ratl::array_vs< TNodeNeighbors, MAXNODES>			TLinks;


	typedef	typename ragl::graph_vs<TNODE, MAXNODES, TEDGE, MAXEDGES, MAXNODENEIGHBORS>	TGraph;


    ////////////////////////////////////////////////////////////////////////////////////
	// cells class
    ////////////////////////////////////////////////////////////////////////////////////
	template <int NODESPERCELL, int CELLSX, int CELLSY>
	class cells : public ratl::ratl_base
	{
	public:
	 	enum 
		{
			SIZEX = CELLSX,
			SIZEY = CELLSY,
			SIZENODES = NODESPERCELL
		};


		struct SSortNode
		{
			float	mCost;
			short	mHandle;
			bool			operator<(const SSortNode& t) const	
			{
				return	(mCost<t.mCost);
			}
		};
		typedef	typename ratl::vector_vs<SSortNode, (NODESPERCELL*25)>		TSortNodes;
		typedef	typename ratl::vector_vs<short, NODESPERCELL>				TCellNodes;
		struct SCell
		{
			TCellNodes	mNodes;
			TCellNodes	mEdges;
		};
		typedef	typename ratl::grid2_vs<SCell,	CELLSX, CELLSY>				TCells;


	    ////////////////////////////////////////////////////////////////////////////////
		//
	    ////////////////////////////////////////////////////////////////////////////////
		cells(TGraph& g) : mGraph(g)
		{
		}

		void		clear()
		{
			mCells.clear();
			SCell		EmptyCell;
			mCells.init(EmptyCell);
		}

		void		get_cell_upperleft(int x, int y, float& xReal, float& yReal)
		{
			mCells.get_cell_upperleft(x,y,xReal,yReal);
		}
		void		get_cell_lowerright(int x, int y, float& xReal, float& yReal)
		{
			mCells.get_cell_lowerright(x,y,xReal,yReal);
		}

	    ////////////////////////////////////////////////////////////////////////////////
		//
	    ////////////////////////////////////////////////////////////////////////////////
		void			expand_bounds(int nodeHandle)
		{
			TNODE&	node = mGraph.get_node(nodeHandle);
			mCells.expand_bounds(node[0], node[1]);
		}

	    ////////////////////////////////////////////////////////////////////////////////
		//
	    ////////////////////////////////////////////////////////////////////////////////
		SCell&			get_cell(float x, float y)
		{
			return mCells.get(x,y);
		}
		SCell&			get_cell(int x, int y)
		{
			return mCells.get(x,y);
		}
		void			convert_to_cell_coords(float x, float y, int& xint, int& yint)
		{
			mCells.get_cell_coords(x, y, xint, yint);
		}

	    ////////////////////////////////////////////////////////////////////////////////
		//
	    ////////////////////////////////////////////////////////////////////////////////
		void			fill_cells_nodes(float range)
		{
			// I. Fill All The Cells With The Points Contained By Those Cells
			//----------------------------------------------------------------
			bool full = false;
			for (TNodes::iterator it=mGraph.nodes_begin(); it!=mGraph.nodes_end() && !full; it++)
			{
				TNODE&	node = (*it);
				SCell&	cell = mCells.get(node[0], node[1]);

				cell.mNodes.push_back(it.index());
				full = cell.mNodes.full();
				assert(!full || "Cell Filled On Inital Containment"==0);
			}

			mCells.scale_by_largest_axis(range);


			// II. Go To All Neighboring Cells And Get Them
			//==============================================
			int					iRange = (int)(range) + 1;
			TCells::riterator	rcell;
			TCells::riterator	rcellend;
			CVec3				cellCenter(0,0,0);
			CVec3				nodeCenter(0,0,0);

			TSortNodes			*sortnodes	= new TSortNodes;
			SSortNode			sortnode;

			TCells				*sortcells	= new TCells;
			sortcells->copy_bounds(mCells);



			// For Every Cell
			//----------------
			for (int x=0; x<CELLSX; x++)
			{
				for (int y=0; y<CELLSY; y++)
				{
					// Setup the Sortnodes vector And Range Iterators
					//------------------------------------------------
					sortnodes->clear();

					mCells.get_cell_position(x,y, cellCenter.v[0], cellCenter.v[1]);

					for (rcell = mCells.rangeBegin(iRange,x,y); !rcell.at_end(); rcell++)
					{
						SCell& cell = (*rcell);

						// Add The Nodes To The Sort List
						//--------------------------------
						for (int i=0; i<cell.mNodes.size() && !sortnodes->full(); i++)
						{
							int	nodeHandle = cell.mNodes[i];

							TNODE&	node  = mGraph.get_node(nodeHandle);
							nodeCenter[0] = node[0];
							nodeCenter[1] = node[1];

							sortnode.mHandle	= nodeHandle;
							sortnode.mCost		= cellCenter.Dist2(nodeCenter);
							sortnodes->push_back(sortnode);
						}
					}

					// Sort The Results
					//------------------
					sortnodes->sort();


					// Copy The Sorted Nodes Vector Into The Sorted Cell (Of The Sorted Cell Grid)
					//----------------------------------------------------------------------------
					SCell&	cell = sortcells->get(x,y);
					cell.mNodes.clear();
					for (int i=0; i<sortnodes->size() && i<NODESPERCELL; i++)
					{
						cell.mNodes.push_back((*sortnodes)[i].mHandle);
					}
				}
			}

			// Now Copy The Sorted Results To The Main Cells
			//-----------------------------------------------
			for (int xb=0; xb<CELLSX; xb++)
			{
				for (int yb=0; yb<CELLSY; yb++)
				{
					SCell&	scell = sortcells->get(xb,yb);
					SCell&	mcell = mCells.get(xb,yb);
					mcell.mNodes = scell.mNodes;
				}
			}

			delete sortnodes;
			delete sortcells;
		}


	    ////////////////////////////////////////////////////////////////////////////////
		//
	    ////////////////////////////////////////////////////////////////////////////////
		void			fill_cells_edges(float range)
		{
			// I. Fill All The Cells With The Points Contained By Those Cells
			//----------------------------------------------------------------
			bool full = false;
			for (TEdges::iterator eit=mGraph.edges_begin(); eit!=mGraph.edges_end() && !full; eit++)
			{
				TEDGE&	edge = (*eit);
				SCell&	cell = mCells.get(edge[0], edge[1]);

				cell.mEdges.push_back(eit.index());
				full = cell.mEdges.full();
				assert(!full || "Cell Filled On Inital Containment"==0);
			}


			mCells.scale_by_largest_axis(range);


			// II. Go To All Neighboring Cells And Get Them
			//==============================================
			int					iRange = (int)(range) + 1;
			TCells::riterator	rcell;
			TCells::riterator	rcellend;
			CVec3				cellCenter(0,0,0);
			CVec3				nodeCenter(0,0,0);

			TSortNodes			*sortedges	= new TSortNodes;
			SSortNode			sortnode;

			TCells				*sortcells	= new TCells;
			sortcells->copy_bounds(mCells);


			// For Every Cell
			//----------------
			for (int x=0; x<CELLSX; x++)
			{
				for (int y=0; y<CELLSY; y++)
				{
					// Setup the Sortnodes vector And Range Iterators
					//------------------------------------------------
					sortedges->clear();

					mCells.get_cell_position(x,y, cellCenter.v[0], cellCenter.v[1]);

					for (rcell = mCells.rangeBegin(iRange,x,y); !rcell.at_end(); rcell++)
					{
						SCell& cell = (*rcell);

						// Add The Edges To The Sort List
						//--------------------------------
						for (int e=0; e<cell.mEdges.size() && !sortedges->full(); e++)
						{
							int	edgeHandle = cell.mEdges[e];

							TEDGE&	edge  = mGraph.get_edge(edgeHandle);
							nodeCenter[0] = edge[0];
							nodeCenter[1] = edge[1];

							sortnode.mHandle	= edgeHandle;
							sortnode.mCost		= cellCenter.Dist2(nodeCenter);
							sortedges->push_back(sortnode);
						}
					}

					// Sort The Results
					//------------------
					sortedges->sort();


					// Copy The Sorted Nodes Vector Into The Sorted Cell (Of The Sorted Cell Grid)
					//----------------------------------------------------------------------------
					SCell&	cell = sortcells->get(x,y);
					cell.mEdges.clear();
					for (int j=0; j<sortedges->size() && j<NODESPERCELL; j++)
					{
						cell.mEdges.push_back((*sortedges)[j].mHandle);
					}
				}
			}

			// Now Copy The Sorted Results To The Main Cells
			//-----------------------------------------------
			for (int xb=0; xb<CELLSX; xb++)
			{
				for (int yb=0; yb<CELLSY; yb++)
				{
					SCell&	scell = sortcells->get(xb,yb);
					SCell&	mcell = mCells.get(xb,yb);
					mcell.mEdges = scell.mEdges;
				}
			}

			delete sortedges;
			delete sortcells;
		}


	private:
		TGraph&		mGraph;
		TCells		mCells;
	};






    ////////////////////////////////////////////////////////////////////////////////////
	// Remove All Edges
    ////////////////////////////////////////////////////////////////////////////////////
	void		clear_edges()
	{	
		mEdges.clear();
		mEdges.alloc();		// Alloc a dummy edge at location 0
		for (int i=0; i<MAXNODES; i++)
		{
			mLinks[i].clear();
		}
	}

    ////////////////////////////////////////////////////////////////////////////////////
	// Clear Out All Nodes And Edges
    ////////////////////////////////////////////////////////////////////////////////////
	void	clear()
	{
		mNodes.clear();
		mNodes.alloc();		// Alloc A dummy node at location 0
		clear_edges();

#if !defined(FINAL_BUILD)
		mSearchCount = 0;
		mSearchMemorySize = 0;

		mSearchSuccess = 0;
		mSearchSuccessVisited = 0;
		mSearchSuccessPathLen = 0;

		mSearchFail = 0;
		mSearchFailVisited = 0;
#endif
	}


    ////////////////////////////////////////////////////////////////////////////////////
	// Constructor
    ////////////////////////////////////////////////////////////////////////////////////
	graph_vs()
	{
		clear();
	}

	~graph_vs()
	{
	}

    ////////////////////////////////////////////////////////////////////////////////////
	// Number Of Nodes
    ////////////////////////////////////////////////////////////////////////////////////
	int			size_nodes()
	{
		return (mNodes.size()-1);
	}


    ////////////////////////////////////////////////////////////////////////////////////
	// Access A Node
    ////////////////////////////////////////////////////////////////////////////////////
	TNODE&		get_node(int i)
	{
		return mNodes[i];
	}


    ////////////////////////////////////////////////////////////////////////////////////
	// Access The Beginning Of The Nodes List
    ////////////////////////////////////////////////////////////////////////////////////
	typename TNodes::iterator	nodes_begin()
	{
		typename TNodes::iterator x = mNodes.begin();
		x++;
		return x;
	}

	////////////////////////////////////////////////////////////////////////////////////
	// Access The End Of The Nodes List
    ////////////////////////////////////////////////////////////////////////////////////
	typename TNodes::iterator	nodes_end()
	{
		return mNodes.end();
	}



    ////////////////////////////////////////////////////////////////////////////////////
	// Number Of Edges
    ////////////////////////////////////////////////////////////////////////////////////
	int			size_edges()
	{
		return (mEdges.size()-1);
	}

    ////////////////////////////////////////////////////////////////////////////////////
	// Access An Edge
    ////////////////////////////////////////////////////////////////////////////////////
	TEDGE&		get_edge(int i)
	{
		return mEdges[i];
	}

    ////////////////////////////////////////////////////////////////////////////////////
	// Get The Edge That Connects nodeA to nodeB
	//
	// NOTE: This is now a linear scan witout the adjacency matrix (worst case 8 depth)
    ////////////////////////////////////////////////////////////////////////////////////
	int			get_edge_across(int nodeA, int nodeB)
	{
		int numNeighbors = mLinks[nodeA].size();
		for (int curNeighbor=0; curNeighbor<numNeighbors; curNeighbor++)
		{
			if (mLinks[nodeA][curNeighbor].mNode==nodeB)
			{
				if (mLinks[nodeA][curNeighbor].mEdge)
				{
					return mLinks[nodeA][curNeighbor].mEdge;
				}
				return -1;	// -1 signifies that a link exists with no edge
			}
		}
		return 0;
	}

    ////////////////////////////////////////////////////////////////////////////////////
	// Access The Beginning Of The Nodes List
    ////////////////////////////////////////////////////////////////////////////////////
	typename TEdges::iterator	edges_begin()
	{
		typename TEdges::iterator x = mEdges.begin();
		x++;
		return x;
	}

    ////////////////////////////////////////////////////////////////////////////////////
	// Access The End Of The Nodes List
    ////////////////////////////////////////////////////////////////////////////////////
	typename TEdges::iterator	edges_end()
	{
		return mEdges.end();
	}

	int							edge_index(const TEDGE& edge)
	{
		return mEdges.pointer_to_index(&edge);
	}




    ////////////////////////////////////////////////////////////////////////////////////
	// Get All The Neighbors Of A Given Node
    ////////////////////////////////////////////////////////////////////////////////////
	TNodeNeighbors&		get_node_neighbors(const int nodeA)
	{
		return mLinks[nodeA];
	}
	bool						node_has_neighbors(const int nodeA)
	{
		return !(mLinks[nodeA].empty());
	}

    ////////////////////////////////////////////////////////////////////////////////////
	// Add A Node
    ////////////////////////////////////////////////////////////////////////////////////
	int			insert_node(const TNODE& t)
	{
		int	nNode = mNodes.alloc();
		mNodes[nNode] = t;
		return nNode;
	}


    ////////////////////////////////////////////////////////////////////////////////////
	// Remove Node And Clear Out All Edges Once Connected To This Node
	//   Note: This is a fairly expensive operation, not to be done often
    ////////////////////////////////////////////////////////////////////////////////////
	void		remove_node(int node)
	{
		mNodes.free(node);

		// For Each Link To A Neighboring Node
		//--------------------------------------
		for (int i=0; i<mLinks[node].size(); i++)
		{
			int	curNeighbor = mLinks[node][i].mNode;
			int curEdge		= mLinks[node][i].mEdge;

			// Free The Edge
			//---------------
			if (curEdge)
			{
				mEdges.free(curEdge);
			}


			// Remove The Edge From Any Recorded Neighbors
			//---------------------------------------------
			TNodeNeighbors& neighbors = mLinks[curNeighbor];
			for (int j=0; j<neighbors.size(); j++)
			{
				if (neighbors[j].mNode==node)
				{
					neighbors.erase_swap(j);
					break;
				}
			}
		}
		mLinks[node].clear();
	}

    ////////////////////////////////////////////////////////////////////////////////////
	// Connect Node With An Edge Object  (A->B)  if reflexive, also (B->A)
    ////////////////////////////////////////////////////////////////////////////////////
	int			connect_node(const TEDGE& t, int nodeA, int nodeB, bool reflexive=true)
	{
		if (nodeA==nodeB || !nodeA || !nodeB || !mNodes.is_used(nodeA) || !mNodes.is_used(nodeB))
		{
			assert("ERROR: Cannot Connect A and B!"==0);
			return 0;
		}

		if (mLinks[nodeA].full() || (reflexive && mLinks[nodeB].full()))
		{
			assert("ERROR: Max edges per node exceeded!"==0);
			return 0;
		}

		if (mEdges.full())
		{
			assert("ERROR: Max edges exceeded!"==0);
			return 0;
		}

		SNodeNeighbor	nNbr;

		nNbr.mNode = nodeB;
		nNbr.mEdge = mEdges.alloc();
		mEdges[nNbr.mEdge] = t;


		mLinks[nodeA].push_back(nNbr);
		if (reflexive)
		{
			nNbr.mNode = nodeA;
			mLinks[nodeB].push_back(nNbr);
		}

		return nNbr.mEdge;
	}

    ////////////////////////////////////////////////////////////////////////////////////
	// Connect Node Without Allocating An Edge Object  (A->B)  if reflexive, also (B->A)
    ////////////////////////////////////////////////////////////////////////////////////
	void		connect_node(int nodeA, int nodeB, bool reflexive=true)
	{
		if (nodeA==nodeB || !nodeA || !nodeB || !mNodes.is_used(nodeA) || !mNodes.is_used(nodeB))
		{
			assert("ERROR: Cannot Connect A and B!"==0);
			return 0;
		}

		if (mLinks[nodeA].full() || (reflexive && mLinks[nodeB].full()))
		{
			assert("ERROR: Max edges per node exceeded!"==0);
			return 0;
		}


		SNodeNeighbor	nNbr;

		nNbr.mNode = nodeB;
		nNbr.mEdge = 0;


		mLinks[nodeA].push_back(nNbr);
		if (reflexive)
		{
			nNbr.mNode = nodeA;
			mLinks[nodeB].push_back(nNbr);
		}

		return nNbr.mEdge;
	}

    ////////////////////////////////////////////////////////////////////////////////////
	// Remove Edge
    ////////////////////////////////////////////////////////////////////////////////////
	void		remove_edge(int nodeA, int nodeB, bool reflexive=true)
	{
		if (!mNodes.is_used(nodeA) || !mNodes.is_used(nodeB) && nodeA==nodeB)
		{
			assert("Unable To Remove Edge"==0);
			return;
		}

		int	linkNum=0;

		for (linkNum=0; linkNum<mLinks[nodeA].size(); linkNum++)
		{
			if (mLinks[nodeA][linkNum].mNode==nodeB)
			{
				if (mLinks[nodeA][linkNum].mEdge && mEdges.is_used(mLinks[nodeA][linkNum].mEdge))
				{
					mEdges.free(mLinks[nodeA][linkNum].mEdge);
				}
				mLinks[nodeA].erase_swap(linkNum);
				break;
			}
		}


		for (linkNum=0; linkNum<mLinks[nodeB].size(); linkNum++)
		{
			if (mLinks[nodeB][linkNum].mNode==nodeA)
			{
				if (mLinks[nodeB][linkNum].mEdge && mEdges.is_used(mLinks[nodeB][linkNum].mEdge))
				{
					mEdges.free(mLinks[nodeB][linkNum].mEdge);
				}
				mLinks[nodeB].erase_swap(linkNum);
				break;
			}
		}
	}


    ////////////////////////////////////////////////////////////////////////////////////
	// Data
    ////////////////////////////////////////////////////////////////////////////////////
private:
	TNodes		mNodes;
	TEdges		mEdges;
	TLinks		mLinks;


	////////////////////////////////////////////////////////////////////////////////////////
	// The Handle Heap Class
	//
	// A handle heap is a specially designed heap class which remembers the handle of any
	// value that is inserted.  Should the value of an already inserted object change, it is
	// possible to reheapify around that value, no matter where the value lies inside the heap
	// because of the additional memory to convert handle->index.
	//
	// The handle heap is used by A* to sort the open list by cost.
	//
	////////////////////////////////////////////////////////////////////////////////////////
	template<class T>
	class handle_heap
	{
	public:

		////////////////////////////////////////////////////////////////////////////////////
		// Constructor
		////////////////////////////////////////////////////////////////////////////////////
		handle_heap(TNodes& Nodes) : mNodes(Nodes)
		{
			clear();
		}
    
		////////////////////////////////////////////////////////////////////////////////////
		// Get The Size (The Difference Between The Push And Pop "Pointers")
		////////////////////////////////////////////////////////////////////////////////////
		int				size() const
		{
			return (mPush);
		}
		
		////////////////////////////////////////////////////////////////////////////////////
		// Check To See If The Size Is Zero
		////////////////////////////////////////////////////////////////////////////////////
		bool			empty() const
		{
			return (!size());
		}

		////////////////////////////////////////////////////////////////////////////////////
		// Check To See If The Size Is Full
		////////////////////////////////////////////////////////////////////////////////////
		bool			full() const
		{
			return (size()==MAXNODES);
		}

		////////////////////////////////////////////////////////////////////////////////////
		// Empty Out The Entire Heap
		////////////////////////////////////////////////////////////////////////////////////
		void			clear()
		{
			mPush = 0;
			for (int i=0; i<MAXNODES; i++)
			{
				mHandleToPos[i] = -1;
			}
		}

		////////////////////////////////////////////////////////////////////////////////////
		// Check If The Handle Has Been Added To This Heap
		////////////////////////////////////////////////////////////////////////////////////
		bool			used(int handle)
		{
			return (mHandleToPos[handle]!=-1 && mData[mHandleToPos[handle]].handle()==handle);
		}

		////////////////////////////////////////////////////////////////////////////////////
		// Get The Data Value At The Top Of The Heap
		////////////////////////////////////////////////////////////////////////////////////
		T&				top()
		{
			assert(mPush>0);		// Don't Try To Look At This If There Is Nothing In Here
			return (mData[0]);
		}

		////////////////////////////////////////////////////////////////////////////////////
		// Accessor
		////////////////////////////////////////////////////////////////////////////////////
		T&			operator[](int handle)											
		{
			// If You Hit This Assert, Then You Are Asking For Unallocated Data
			//------------------------------------------------------------------
			assert(used(handle));
			return mData[mHandleToPos[handle]];
		}


		////////////////////////////////////////////////////////////////////////////////////
		// Add A Value To The Queue
		////////////////////////////////////////////////////////////////////////////////////
		void			push(const T& nValue)
		{
			assert(size()<MAXNODES);

			// Get The Handle From The Value And Make Sure We Don't Already Have One Stored There
			//------------------------------------------------------------------------------------
			assert(mHandleToPos[nValue.handle()] == -1);

			// Add It
			//--------
			mData[mPush]										= nValue;
			mHandleToPos[nValue.handle()]	= mPush;

			

			// Fix Possible Heap Inconsistancies
			//-----------------------------------
			reheapify_upward(mPush);
			
			mPush++;
		}

		////////////////////////////////////////////////////////////////////////////////////
		// Remove A Value From The Queue
		////////////////////////////////////////////////////////////////////////////////////
		void			pop()
		{
			assert(size()>0);

			mPush--;

			assert(mHandleToPos[mData[0].handle()]==0);
			

			// Swap The Lowest Element Up To The Spot We Just "Erased"
			//---------------------------------------------------------
			swap(0, mPush);

			mHandleToPos[mData[mPush].handle()] = -1;	// Erase This Handles Marker


			// Fix Possible Heap Inconsistancies
			//-----------------------------------
			reheapify_downward(0);
		}

		////////////////////////////////////////////////////////////////////////////////////
		// Call This Func If The Value At The Given Handle Has Changed & Needs Adjustment
		////////////////////////////////////////////////////////////////////////////////////
		void			reheapify(int handle)
		{
			assert(used(handle));

			int Pos = mHandleToPos[handle];
			reheapify_upward(Pos);
			reheapify_downward(Pos);
		}

	private:
		////////////////////////////////////////////////////////////////////////////////////
		// Returns The Location Of Node (i)'s Parent Node (The Parent Node Of Zero Is Zero)
		////////////////////////////////////////////////////////////////////////////////////
		int			parent(int i)				
		{
			return ((i-1)/2);
		}

		////////////////////////////////////////////////////////////////////////////////////
		// Returns The Location Of Node (i)'s Left Child (The Child Of A Leaf Is The Leaf)
		////////////////////////////////////////////////////////////////////////////////////
		int			left(int i)	
		{
			return (2*i)+1;
		}

		////////////////////////////////////////////////////////////////////////////////////
		// Returns The Location Of Node (i)'s Right Child (The Child Of A Leaf Is The Leaf)
		////////////////////////////////////////////////////////////////////////////////////
		int			right(int i)
		{
			return (2*i)+2;
		}

		////////////////////////////////////////////////////////////////////////////////////
		// Returns The Location Of Largest Child Of Node (i)
		////////////////////////////////////////////////////////////////////////////////////
		int			largest_child(int i)
		{
			if (left(i)<mPush)
			{
				if (right(i)<mPush)
				{
					return ( (mData[right(i)] < mData[left(i)]) ? (left(i)) : (right(i)) );
				}
				return left(i);	// Node i only has a left child, so by default it is the biggest
			}
			return i;		// Node i is a leaf, so just return it
		}


		////////////////////////////////////////////////////////////////////////////////////
		// Swaps Two Element Locations
		////////////////////////////////////////////////////////////////////////////////////
		void		swap(int a, int b)
		{
			if (a==b)
			{
				return;
			}

			assert(a>=0 && b>=0 && a<MAXNODES && b<MAXNODES);
			assert(mHandleToPos[mData[a].handle()]==a);
			assert(mHandleToPos[mData[b].handle()]==b);

			// Swap Handles
			//--------------
			mHandleToPos[mData[a].handle()] = b;
			mHandleToPos[mData[b].handle()] = a;

			// Swap Data
			//-----------
			mData[MAXNODES]	= mData[a];		// a->TEMP
			mData[a]		= mData[b];		// b->a
			mData[b]		= mData[MAXNODES];	// TEMP->B

			assert(mHandleToPos[mData[a].handle()]==a);
			assert(mHandleToPos[mData[b].handle()]==b);
		}

		////////////////////////////////////////////////////////////////////////////////////
		// Swaps The Data Up The Heap Until It Reaches A Valid Location
		////////////////////////////////////////////////////////////////////////////////////
		void		reheapify_upward(int Pos)
		{
			while (Pos && mData[parent(Pos)]<mData[Pos])
			{
				swap(parent(Pos), Pos);
				Pos = parent(Pos);
			}
		}

		////////////////////////////////////////////////////////////////////////////////////
		// Swaps The Data Down The Heap Until It Reaches A Valid Location
		////////////////////////////////////////////////////////////////////////////////////
		void		reheapify_downward(int Pos)
		{
			int largestChild = largest_child(Pos);
			while (largestChild!=Pos && mData[Pos]<mData[largestChild])
			{
				swap(largestChild, Pos);
				Pos = largestChild;
				largestChild = largest_child(Pos);
			}
		}

		////////////////////////////////////////////////////////////////////////////////////
		// Validate Will Run Through The Heap And Make Sure The Top Element Is Smallest
		////////////////////////////////////////////////////////////////////////////////////
	#ifdef _DEBUG
		void		validate()
		{
			for (int i=1; i<mPush; i++)
			{
				assert(mData[i]<mData[0]);
			}
		}
	#endif



		////////////////////////////////////////////////////////////////////////////////////
		// Data
		////////////////////////////////////////////////////////////////////////////////////
	private:
		ratl::array_vs<T, MAXNODES+1>	mData;				// The Memory (Plus One For Temp Sort)
		ratl::array_vs<int, MAXNODES>	mHandleToPos;		//

		int								mPush;				// Address Of Next Add Location

		TNodes&							mNodes;
	};







	////////////////////////////////////////////////////////////////////////////////////
	// A Search Node
    ////////////////////////////////////////////////////////////////////////////////////
	class	search_node
	{
	public:
		////////////////////////////////////////////////////////////////////////////////
		// Constructors
		////////////////////////////////////////////////////////////////////////////////
		search_node(int Node=-1, int Parent=-1) : 
			mNode(Node), 
			mParentVisit(Parent),
			mCostToGoal(-1),
			mCostFromStart(0)
		{}
		search_node(const search_node& t) : 
			mNode(t.mNode), 
			mParentVisit(t.mParentVisit)
			mCostToGoal(t.mCostToGoal),
			mCostFromStart(t.mCostFromStart)
		{}

		////////////////////////////////////////////////////////////////////////////////
		// Assignment Operator
		////////////////////////////////////////////////////////////////////////////////
		void		operator=(const search_node& t)
		{
			mNode			= (t.mNode); 
			mParentVisit	= (t.mParentVisit);
			mCostToGoal		= (t.mCostToGoal);
			mCostFromStart	= (t.mCostFromStart);	
		}

		////////////////////////////////////////////////////////////////////////////////
		// 
		////////////////////////////////////////////////////////////////////////////////
		int			handle() const
		{
			return mNode;
		}

		////////////////////////////////////////////////////////////////////////////////
		// 
		////////////////////////////////////////////////////////////////////////////////
		float		cost_estimate() const
		{
			return mCostFromStart+mCostToGoal;
		}

		////////////////////////////////////////////////////////////////////////////////
		// 
		////////////////////////////////////////////////////////////////////////////////
		bool		operator<  (const search_node& t) const	
		{
			return (cost_estimate() > t.cost_estimate());
		}

		////////////////////////////////////////////////////////////////////////////////
		// Data
		////////////////////////////////////////////////////////////////////////////////
		int		mNode;					// Which Node Is This (Index To Pool mNodes)
		int		mParentVisit;			// Which Search Node (In Visited)

		float	mCostToGoal;			// How Far From The Start Of The Search Are We?
		float	mCostFromStart;			// How Far From The End Of The Search Are We?
	};



public:
 	typedef		ratl::vector_vs<search_node, MAXNODES>	TVisited;
 	typedef		ratl::array_vs<int, MAXNODES>			TVisitedHandles;
	typedef		ratl::bits_vs<MAXNODES>					TNodeState;

	////////////////////////////////////////////////////////////////////////////////////
	// The Search Data Object
    ////////////////////////////////////////////////////////////////////////////////////
	class	search
	{
	public:
		////////////////////////////////////////////////////////////////////////////////
		// Constructor
		////////////////////////////////////////////////////////////////////////////////
		search(int nodeStart=0, int nodeEnd=0) :
			mStart(nodeStart),
			mEnd(nodeEnd)
		{
			clear(true, false);
		}

		enum
		{
			NULL_NODE			= 0,
			NULL_NODE_INDEX		= -1,
			NULL_VISIT_INDEX	= -1,
			NULL_COST			= -1,
		};


		////////////////////////////////////////////////////////////////////////////////
		// Reset All The Search Parameters
		////////////////////////////////////////////////////////////////////////////////
		void			clear(bool clearNodesPtr=true, bool clearStartAndEnd=true)
		{
			// Reset All Data
			//----------------
			mClosed.clear();
			mVisited.clear();
			mNodeIndexToVisited.fill(NULL_VISIT_INDEX);


			mNext.mNode			= NULL_NODE;
			mNext.mParentVisit	= NULL_VISIT_INDEX;
			mNext.mCostToGoal	= NULL_COST;
			mNext.mCostFromStart= NULL_COST;

			mPrevIndex			= NULL_NODE_INDEX;
			mNextIndex			= NULL_NODE_INDEX;

			mPathVisit			= NULL_VISIT_INDEX;

			if (clearNodesPtr)
			{
				mNodesPtr		= 0;
			}

			// Clear Out The Start And End Handles
			//-------------------------------------
			if (clearStartAndEnd)
			{
				mStart			= NULL_NODE;
				mEnd			= NULL_NODE;
			}

			// Otherwise, We Can Start The Next Index
			//----------------------------------------
			else if (mNodesPtr && mStart!=NULL_NODE && mEnd!=NULL_NODE)
			{
				mNextIndex		= mStart;
			}

		}

		////////////////////////////////////////////////////////////////////////////////
		// Call This Function To Clear Out Everything EXCEPT Start And End Points
		////////////////////////////////////////////////////////////////////////////////
		void			reset()
		{
			clear(false, false);
		}

		////////////////////////////////////////////////////////////////////////////////
		// Mark This Node As Closed
		////////////////////////////////////////////////////////////////////////////////
		void			close(int node)
		{
			mClosed.set_bit(node);
		}
		////////////////////////////////////////////////////////////////////////////////
		// Mark This Node As Closed
		////////////////////////////////////////////////////////////////////////////////
		void			close(const TNodeState& AllNodesToClose)
		{
			mClosed |= AllNodesToClose;
		}


		////////////////////////////////////////////////////////////////////////////////
		// Return True If We Have Found The Node We Were Searching For
		////////////////////////////////////////////////////////////////////////////////
		bool			success()
		{
			if (mEnd && mPrevIndex!=NULL_NODE_INDEX)
			{
				return (mPrevIndex==mEnd);
			}
			return false;
		}

		////////////////////////////////////////////////////////////////////////////////
		// Get An Index To The Beginning Of The Path
		////////////////////////////////////////////////////////////////////////////////
		void			path_begin()
		{
			if (success())
			{
				mPathVisit = (mVisited.size()-1);
			}
			else
			{
				mPathVisit = NULL_VISIT_INDEX;
			}
		}

		////////////////////////////////////////////////////////////////////////////////
		// Check ForThe End Of The Path (Use In A for() Loop)
		////////////////////////////////////////////////////////////////////////////////
		bool			path_end()
		{
			return (mPathVisit==NULL_VISIT_INDEX);
		}

		////////////////////////////////////////////////////////////////////////////////
		// Index Path Inc (Get The Old Path Node's Parent)
		////////////////////////////////////////////////////////////////////////////////
		void			path_inc()
		{
			assert(mPathVisit!=NULL_VISIT_INDEX);
			mPathVisit = mVisited[mPathVisit].mParentVisit;
		}

		////////////////////////////////////////////////////////////////////////////////
		// Get the node handle of the current node
		////////////////////////////////////////////////////////////////////////////////
		int				path_at()
		{
			assert(mPathVisit!=NULL_VISIT_INDEX);
			return mVisited[mPathVisit].mNode;
		}

		////////////////////////////////////////////////////////////////////////////////
		// The Total Cost Of The Path
		////////////////////////////////////////////////////////////////////////////////
		float			path_cost()
		{
			if (success())
			{
				return (mVisited[(mVisited.size()-1)].mCostFromStart);
			}
			return NULL_COST;
		}


		////////////////////////////////////////////////////////////////////////////////
		// How Many Nodes Were Looked At In This Search
		////////////////////////////////////////////////////////////////////////////////
		int				num_visited()
		{
			return mVisited.size();
		}

		////////////////////////////////////////////////////////////////////////////////
		// Get An Iterator To The Beginning Of The Visited Pool
		////////////////////////////////////////////////////////////////////////////////
		typename TVisited::iterator	visited_begin()
		{
			return mVisited.begin();
		}

		////////////////////////////////////////////////////////////////////////////////
		// Get An Iterator To The End Of The Visited Pool
		////////////////////////////////////////////////////////////////////////////////
		typename TVisited::iterator	visited_end()
		{
			return mVisited.end();
		}





	private:
		////////////////////////////////////////////////////////////////////////////////
		// Start At The First NON Closed Node
		////////////////////////////////////////////////////////////////////////////////
		void			setup(TNodes* nodesPtr)
		{
			mNodesPtr			= nodesPtr;
			clear(false, false);
		}


		////////////////////////////////////////////////////////////////////////////////
		// Pretend the next index is open, probably because we found a shorter route 
		// than the first time it was closed, and want it back on the open list
		////////////////////////////////////////////////////////////////////////////////
		void			reopen_next_index()
		{
			assert(mNextIndex!=NULL_NODE_INDEX);

			mNodeIndexToVisited[mNextIndex] = NULL_VISIT_INDEX;
			mClosed.clear_bit(mNextIndex);
		}

		////////////////////////////////////////////////////////////////////////////////
		// The current estimated cost of reaching a give node, if the node was never
		// visited, but IS closed, it returns NULL cost
		////////////////////////////////////////////////////////////////////////////////
		float			visited_cost(int NodeIndex)
		{
			int VisitedIndex = mNodeIndexToVisited[NodeIndex];
			if (VisitedIndex!=NULL_VISIT_INDEX)
			{
				return mVisited[VisitedIndex].cost_estimate();
			}
			return NULL_COST;
		}


		////////////////////////////////////////////////////////////////////////////////
		// Add This Search Node To The Visited List And Keep Track Of It For Later
		////////////////////////////////////////////////////////////////////////////////
		void			visit(search_node& t)
		{
			assert(mNodesPtr!=0);
			mPrevIndex						= t.mNode;

			// Add It To The Visited List, And Mark The Location In The Node Index Array
			//---------------------------------------------------------------------------
			mVisited.push_back(t);
			mNodeIndexToVisited[mPrevIndex] = (mVisited.size()-1);
			mClosed.set_bit(mPrevIndex);

			// Setup Our Next Node To Know It Came From The Last Location In The Visited Vector
			//----------------------------------------------------------------------------------
			mNext.mParentVisit				= (mVisited.size()-1);
		}

		////////////////////////////////////////////////////////////////////////////////
		// Check To See If The Next Index Has Already Been Closed
		////////////////////////////////////////////////////////////////////////////////
		bool			next_index_closed()
		{
			assert(mNextIndex!=NULL_NODE_INDEX);

			return (mClosed.get_bit(mNextIndex));
		}

		////////////////////////////////////////////////////////////////////////////////
		// The Simple "Get Next" Just Converts The Next Index To A Handle In mNext
		////////////////////////////////////////////////////////////////////////////////
		search_node&	get_next()
		{
			assert(mNodesPtr);
			assert(mNextIndex!=NULL_NODE_INDEX);

			mClosed.set_bit(mNextIndex);

			mNext.mNode				= mNextIndex;
			mNext.mCostToGoal		= 0;
			mNext.mCostFromStart	= 0;

			return mNext;
		}
		
		////////////////////////////////////////////////////////////////////////////////
		// This "Get Next" Function Is For A* and Sets Up THe Costs Of The Search Node
		////////////////////////////////////////////////////////////////////////////////
		search_node&	get_next(const user& suser, TEDGE& edge_parent_to_next)
		{
			assert(mNodesPtr);
			assert(mNextIndex!=NULL_NODE_INDEX);

			//NOTE: we do not do a "mClosed.set_bit(mNextIndex);" here because A* only closes nodes that are visited

			mNext.mNode				= mNextIndex;
			mNext.mCostToGoal		= suser.cost((*mNodesPtr)[mNext.mNode], (*mNodesPtr)[mEnd]);
			mNext.mCostFromStart	= suser.cost(edge_parent_to_next, (*mNodesPtr)[mNext.mNode]);
			
			if (mNext.mParentVisit!=NULL_VISIT_INDEX)
			{
				mNext.mCostFromStart += mVisited[mNext.mParentVisit].mCostFromStart;
			}
			return mNext;
		}

		////////////////////////////////////////////////////////////////////////////////
		// Parameters (Start And End Are External Handles)
		////////////////////////////////////////////////////////////////////////////////
	public:
		int							mStart;
		int							mEnd;


		////////////////////////////////////////////////////////////////////////////////
		// Data
		////////////////////////////////////////////////////////////////////////////////
	private:
		TNodes*						mNodesPtr;
		
		int							mPathVisit;
		int							mPrevIndex;
		int							mNextIndex;
		search_node					mNext;

		ratl::bits_vs<MAXNODES>		mClosed;
		TVisited					mVisited;
		TVisitedHandles				mNodeIndexToVisited;

		friend class graph_vs<TNODE, MAXNODES, TEDGE, MAXEDGES, MAXNODENEIGHBORS>;
	};

    ////////////////////////////////////////////////////////////////////////////////////
	// Setup The Search Data
    ////////////////////////////////////////////////////////////////////////////////////
	void		setup_search(search& sdata)
	{
		sdata.setup(&mNodes);
	}







    ////////////////////////////////////////////////////////////////////////////////////
	// A* Search
    ////////////////////////////////////////////////////////////////////////////////////
	void		astar(search& sdata, const user& suser)
	{
		// Make Sure The Nodes We Are Searching For Exist
		//------------------------------------------------
		assert(MAXEDGES>1);
		sdata.setup(&mNodes);

		// Allocate Our Data Structures
		//------------------------------
		handle_heap<search_node>		open(mNodes);
		int								curNeighbor;
		int								curEdge;
		float							curCost;

		// Run Through The Open List
		//---------------------------
		open.push(sdata.get_next());
		while (!open.empty() && !sdata.success())
		{
			sdata.visit(open.top());
			open.pop();

			// Search Through The Non Closed Nodes Edges
			//-------------------------------------------
			TNodeNeighbors&		curNeighbors = get_node_neighbors(sdata.mPrevIndex);
			for (curNeighbor=0; curNeighbor<curNeighbors.size(); curNeighbor++)
			{
				curEdge = curNeighbors[curNeighbor].mEdge;
				if (curEdge==-1 || suser.is_valid(mEdges[curEdge], sdata.mEnd))
				{
					sdata.mNextIndex			= curNeighbors[curNeighbor].mNode;
					search_node& snode			= sdata.get_next(suser, mEdges[curEdge]);
					curCost						= snode.cost_estimate();

					// Is It Already In The Open List?
					//---------------------------------
					if (open.used(snode.mNode))
					{
						if (curCost<(open[snode.mNode]).cost_estimate())
						{
							open[snode.mNode]		= snode;		// Use This As The Node (With New Parent & Cost)
							open.reheapify(snode.mNode);			// Resort the node in the heap
						}
					}

					// Is It Already In The Closed List?
					//-----------------------------------
					else if (sdata.next_index_closed())
					{
						if (curCost < sdata.visited_cost(snode.mNode))
						{
							sdata.reopen_next_index();				// Pull it off the closed list
							open.push(snode);						// Add it to open
						}
					}

					// It Must Be A Whole New Node
					//------------------------------
					else
					{
						open.push(snode);
					}
				}
			}
		}


#if !defined(FINAL_BUILD)
		mSearchCount++;
		mSearchMemorySize += (sizeof(sdata) + sizeof(suser) + sizeof(open));

		if (sdata.success())
		{
			mSearchSuccess++;
			mSearchSuccessVisited += sdata.num_visited();
			for (sdata.path_begin(); !sdata.path_end(); sdata.path_inc())
			{
				mSearchSuccessPathLen ++;
			}
		}
		else
		{
			mSearchFail++;
			mSearchFailVisited += sdata.num_visited();
		}
#endif
	}








    ////////////////////////////////////////////////////////////////////////////////////
	// Breadth First Search (Use Queue Open List)
    ////////////////////////////////////////////////////////////////////////////////////
	void		bfs(search& sdata)
	{
		sdata.setup(&mNodes);

		// Allocate Our Data Structures
		//------------------------------
		ratl::queue_vs<search_node, MAXNODES>	open;

		// Run Through The Open List
		//---------------------------
		open.push(sdata.get_next());
		while (open.size()>0 && !sdata.success())
		{
			sdata.visit(open.top());
			open.pop();


			// Search Through The Non Closed Nodes Edges
			//-------------------------------------------
			for (sdata.mNextIndex=0; sdata.mNextIndex<MAXNODES; sdata.mNextIndex++)
			{
				if (!sdata.next_index_closed())
				{
					if (get_edge_across(sdata.mPrevIndex, sdata.mNextIndex))
					{
						open.push(sdata.get_next());
					}
				}
			}
		}

#if !defined(FINAL_BUILD)
		mSearchCount++;
		mSearchMemorySize += (sizeof(sdata) + sizeof(open));

		if (sdata.success())
		{
			mSearchSuccess++;
			mSearchSuccessVisited += sdata.num_visited();
		}
		else
		{
			mSearchFail++;
			mSearchFailVisited += sdata.num_visited();
		}
#endif
	}

    ////////////////////////////////////////////////////////////////////////////////////
	// Depth First Search (Use Stack Open List)
    ////////////////////////////////////////////////////////////////////////////////////
	void		dfs(search& sdata)
	{
		sdata.setup(&mNodes);

		// Allocate Our Data Structures
		//------------------------------
		ratl::stack_vs<search_node, MAXNODES>	open;

		// Run Through The Open List
		//---------------------------
		open.push(sdata.get_next());
		while (open.size()>0 && !sdata.success())
		{
			sdata.visit(open.top());
			open.pop();


			// Search Through The Non Closed Nodes Edges
			//-------------------------------------------
			for (sdata.mNextIndex=0; sdata.mNextIndex<MAXNODES; sdata.mNextIndex++)
			{
				if (!sdata.next_index_closed())
				{
					if (get_edge_across(sdata.mPrevIndex, sdata.mNextIndex))
					{
						open.push(sdata.get_next());
					}
				}
			}
		}

#if !defined(FINAL_BUILD)
		mSearchCount++;
		mSearchMemorySize += (sizeof(sdata) + sizeof(open));

		if (sdata.success())
		{
			mSearchSuccess++;
			mSearchSuccessVisited += sdata.num_visited();
		}
		else
		{
			mSearchFail++;
			mSearchFailVisited += sdata.num_visited();
		}
#endif
	}



#if !defined(FINAL_BUILD)
	int		mSearchCount;
	int		mSearchMemorySize;

	int		mSearchSuccess;
	int		mSearchSuccessVisited;
	int		mSearchSuccessPathLen;

	int		mSearchFail;
	int		mSearchFailVisited;

	void	ProfileSpew()
	{
		ProfilePrint("");
		ProfilePrint("");
		ProfilePrint("--------------------------------------------------------");
		ProfilePrint("RAVEN STANDARD LIBRARY  -  COMPUTATIONAL GEOMETRY MODULE");
		ProfilePrint("               Graph Profile Results                    ");
		ProfilePrint("--------------------------------------------------------");
		ProfilePrint("");
		ProfilePrint("GRAPH SIZE (Bytes): (%d)  (KiloBytes): (%5.3f)  MeggaBytes(%3.3f)", 
			(sizeof(*this)), 
			((float)(sizeof(*this))/1024.0f), 
			((float)(sizeof(*this))/1048576.0f)
			);
		ProfilePrint("GRAPH COUNT: (%d) Nodes  (%d) Edges", mNodes.size(), mEdges.size());
		if (mNodes.size())
		{
			ProfilePrint("GRAPH COUNT: (%f) Edges/Node", (float)mEdges.size()/(float)mNodes.size());
		}
		ProfilePrint("");
		if (mSearchCount)
		{
			ProfilePrint("SEARCH: (%d) Searches  (%f) AveSize", mSearchCount,  ((float)mSearchMemorySize/(float)mSearchCount));
			if (mSearchSuccess)
			{
				ProfilePrint("SEARCH: (%d) Successes  (%f) AveVisited  (%f) AvePathLen", 
					mSearchSuccess,  
					((float)mSearchSuccessVisited/(float)mSearchSuccess),
					((float)mSearchSuccessPathLen/(float)mSearchSuccess)					
					);
			}
			if (mSearchFail)
			{
				ProfilePrint("SEARCH: (%d) Failures  (%f) AveVisited", mSearchFail,  ((float)mSearchFailVisited/(float)mSearchFail));
			}
		}
		ProfilePrint("");
	};
#endif
};

}
#endif