Astar

2 years ago · 1c1221f89d
parent 182b92fb7d
commit 1c1221f89d
5 changed files with 2546 additions and 0 deletions
--- a/src/Astar/8puzzle.cpp
+++ b/src/Astar/8puzzle.cpp
@ -0,0 +1,803 @@
+////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+// STL A* Search implementation
+// (C)2001 Justin Heyes-Jones
+//
+// This uses my A* code to solve the 8-puzzle
+
+////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+#include <iostream>
+#include <stdio.h>
+#include <assert.h>
+#include <new>
+
+#include <ctype.h>
+
+using namespace std;
+
+// Configuration
+
+#define NUM_TIMES_TO_RUN_SEARCH 1
+#define DISPLAY_SOLUTION_FORWARDS 1
+#define DISPLAY_SOLUTION_BACKWARDS 0
+#define DISPLAY_SOLUTION_INFO 1
+#define DEBUG_LISTS 0
+
+// AStar search class
+#include "stlastar.h" // See header for copyright and usage information
+
+// Global data
+
+#define BOARD_WIDTH   (3)
+#define BOARD_HEIGHT  (3)
+
+#define GM_TILE     (-1)
+#define GM_SPACE	 (0)
+#define GM_OFF_BOARD (1)
+
+// Definitions
+
+// To use the search class you must define the following calls...
+
+// Data
+//		Your own state space information
+// Functions
+//		(Optional) Constructor.
+//		Nodes are created by the user, so whether you use a
+//      constructor with parameters as below, or just set the object up after the 
+//      constructor, is up to you.
+//
+//		(Optional) Destructor. 
+//		The destructor will be called if you create one. You 
+//		can rely on the default constructor unless you dynamically allocate something in
+//		your data
+//
+//		float GoalDistanceEstimate( PuzzleState &nodeGoal );
+//		Return the estimated cost to goal from this node (pass reference to goal node)
+//
+//		bool IsGoal( PuzzleState &nodeGoal );
+//		Return true if this node is the goal.
+//
+//		bool GetSuccessors( AStarSearch<PuzzleState> *astarsearch );
+//		For each successor to this state call the AStarSearch's AddSuccessor call to 
+//		add each one to the current search - return false if you are out of memory and the search
+//		will fail
+//
+//		float GetCost( PuzzleState *successor );
+//		Return the cost moving from this state to the state of successor
+//
+//		bool IsSameState( PuzzleState &rhs );
+//		Return true if the provided state is the same as this state
+
+// Here the example is the 8-puzzle state ...
+class PuzzleState
+{
+
+public:
+
+	// defs
+
+	typedef enum
+	{
+		TL_SPACE,
+		TL_1,
+		TL_2,
+		TL_3,
+		TL_4,
+		TL_5,
+		TL_6,
+		TL_7,
+		TL_8
+
+	} TILE;
+
+	// data
+
+	static TILE g_goal[ BOARD_WIDTH*BOARD_HEIGHT];
+	static TILE g_start[ BOARD_WIDTH*BOARD_HEIGHT];
+
+	// the tile data for the 8-puzzle
+	TILE tiles[ BOARD_WIDTH*BOARD_HEIGHT ];
+
+	// member functions
+
+	PuzzleState() {  
+						memcpy( tiles, g_goal, sizeof( TILE ) * BOARD_WIDTH * BOARD_HEIGHT );			
+					}
+
+	PuzzleState( TILE *param_tiles ) 
+					{
+						memcpy( tiles, param_tiles, sizeof( TILE ) * BOARD_WIDTH * BOARD_HEIGHT );			
+					}
+
+	float GoalDistanceEstimate( PuzzleState &nodeGoal );
+	bool IsGoal( PuzzleState &nodeGoal );
+	bool GetSuccessors( AStarSearch<PuzzleState> *astarsearch, PuzzleState *parent_node );
+	float GetCost( PuzzleState &successor );
+	bool IsSameState( PuzzleState &rhs );
+	
+	void PrintNodeInfo(); 
+
+private:
+	// User stuff - Just add what you need to help you write the above functions...
+
+	void GetSpacePosition( PuzzleState *pn, int *rx, int *ry );
+	bool LegalMove( TILE *StartTiles, TILE *TargetTiles, int spx, int spy, int tx, int ty );
+	int GetMap( int x, int y, TILE *tiles );
+
+
+
+
+};
+
+// Goal state
+PuzzleState::TILE PuzzleState::g_goal[] = 
+{
+	TL_1,
+	TL_2,
+	TL_3,
+	TL_8,
+	TL_SPACE,
+	TL_4,
+	TL_7,
+	TL_6,
+	TL_5,
+};
+
+// Some nice Start states
+PuzzleState::TILE PuzzleState::g_start[] = 
+{
+
+	// Three example start states from Bratko's Prolog Programming for Artificial Intelligence
+
+#if 1
+	// ex a - 4 steps
+	 TL_1 ,
+	 TL_3 ,
+	 TL_4 ,
+	 TL_8 ,
+	 TL_SPACE ,
+	 TL_2 ,
+	 TL_7 ,
+	 TL_6 ,
+	 TL_5 ,
+
+#elif 0
+  
+	// ex b - 5 steps
+	 TL_2 ,
+	 TL_8 ,
+	 TL_3 ,
+	 TL_1 ,
+	 TL_6 ,
+	 TL_4 ,
+	 TL_7 ,
+	 TL_SPACE ,
+	 TL_5 ,
+
+#elif 0
+	
+	// ex c - 18 steps
+	 TL_2 ,
+	 TL_1 ,
+	 TL_6 ,
+	 TL_4 ,
+	 TL_SPACE ,
+	 TL_8 ,
+	 TL_7 ,
+	 TL_5 ,
+	 TL_3 ,
+
+#elif 0
+	
+	// nasty one - doesn't solve
+	 TL_6 ,
+	 TL_3 ,	  
+	 TL_SPACE ,
+	 TL_4 ,
+	 TL_8 ,
+	 TL_5 ,
+	 TL_7 ,
+	 TL_2 ,
+	 TL_1 ,
+
+#elif 0
+
+	// sent by email - does work though
+
+	 TL_1 ,	 TL_2 ,  TL_3 ,
+	 TL_4 ,	 TL_5 ,  TL_6 ,
+	 TL_8 ,	 TL_7 ,  TL_SPACE ,
+
+	
+// from http://www.cs.utexas.edu/users/novak/asg-8p.html
+
+//Goal:        Easy:        Medium:        Hard:        Worst:
+
+//1 2 3        1 3 4        2 8 1          2 8 1        5 6 7
+//8   4        8 6 2          4 3          4 6 3        4   8
+//7 6 5        7   5        7 6 5            7 5        3 2 1
+
+
+#elif 0
+
+	// easy 5 
+	 TL_1 ,
+	 TL_3 ,	  
+	 TL_4 ,
+
+	 TL_8 ,
+	 TL_6 ,
+	 TL_2 ,
+	
+	 TL_7 ,
+	 TL_SPACE ,
+	 TL_5 ,
+
+
+#elif 0
+
+	// medium 9
+	 TL_2 ,
+	 TL_8 ,	  
+	 TL_1 ,
+
+	 TL_SPACE ,
+	 TL_4 ,
+	 TL_3 ,
+	
+	 TL_7 ,
+	 TL_6 ,
+	 TL_5 ,
+
+#elif 0
+
+	// hard 12
+	 TL_2 ,
+	 TL_8 ,	  
+	 TL_1 ,
+
+	 TL_4 ,
+	 TL_6 ,
+	 TL_3 ,
+	
+	 TL_SPACE ,
+	 TL_7 ,
+	 TL_5 ,
+
+#elif 0
+
+	// worst 30
+	 TL_5 ,
+	 TL_6 ,	  
+	 TL_7 ,
+
+	 TL_4 ,
+	 TL_SPACE ,
+	 TL_8 ,
+	
+	 TL_3 ,
+	 TL_2 ,
+	 TL_1 ,
+
+#elif 0
+
+	//	123
+	//	784
+	//   65
+
+	// two move simple board
+	 TL_1 ,
+	 TL_2 ,	  
+	 TL_3 ,
+
+	 TL_7 ,
+	 TL_8 ,
+	 TL_4 ,
+	
+	 TL_SPACE ,
+	 TL_6 ,
+	 TL_5 ,
+
+#elif 0
+	//  a1 b2 c3 d4 e5 f6 g7 h8 
+	//C3,Blank,H8,A1,G8,F6,E5,D4,B2
+
+	 TL_3 ,
+	 TL_SPACE ,	  
+	 TL_8 ,
+
+	 TL_1 ,
+	 TL_8 ,
+	 TL_6 ,
+	
+	 TL_5 ,
+	 TL_4 ,
+	 TL_2 ,
+
+
+#endif  
+
+};
+
+bool PuzzleState::IsSameState( PuzzleState &rhs )
+{
+
+	for( int i=0; i<(BOARD_HEIGHT*BOARD_WIDTH); i++ )
+	{
+		if( tiles[i] != rhs.tiles[i] )
+		{
+			return false;
+		}
+	}
+
+	return true;
+
+}
+
+void PuzzleState::PrintNodeInfo()
+{
+	char str[100];
+	sprintf( str, "%c %c %c\n%c %c %c\n%c %c %c\n", 
+			tiles[0] + '0',
+			tiles[1] + '0',
+			tiles[2] + '0',
+			tiles[3] + '0',
+			tiles[4] + '0',
+			tiles[5] + '0',
+			tiles[6] + '0',
+			tiles[7] + '0',
+			tiles[8] + '0'
+		);
+
+	cout << str;
+}
+
+// Here's the heuristic function that estimates the distance from a PuzzleState
+// to the Goal. 
+
+float PuzzleState::GoalDistanceEstimate( PuzzleState &nodeGoal )
+{
+
+	// Nilsson's sequence score
+
+	int i, cx, cy, ax, ay, h = 0, s, t;
+
+	// given a tile this returns the tile that should be clockwise
+	TILE correct_follower_to[ BOARD_WIDTH * BOARD_HEIGHT ] =
+	{
+		TL_SPACE, // always wrong
+		TL_2,
+		TL_3,
+		TL_4,
+		TL_5,
+		TL_6,
+		TL_7,
+		TL_8,
+		TL_1,
+	};
+
+	// given a table index returns the index of the tile that is clockwise to it 3*3 only
+	int clockwise_tile_of[ BOARD_WIDTH * BOARD_HEIGHT ] =
+	{
+		1, 
+		2,  	  // 012	  
+		5,  	  // 345	  
+		0,  	  // 678	  
+		-1,  // never called with center square
+		8, 
+		3, 
+		6, 
+		7 
+	};
+
+	int tile_x[ BOARD_WIDTH * BOARD_HEIGHT ] =
+	{
+		/* TL_SPACE */ 1,
+		/* TL_1 */ 0,    
+		/* TL_2 */ 1,    
+		/* TL_3 */ 2,    
+		/* TL_4 */ 2,    
+		/* TL_5 */ 2,    
+		/* TL_6 */ 1,    
+		/* TL_7 */ 0,    
+		/* TL_8 */ 0,    
+	};
+
+	int tile_y[ BOARD_WIDTH * BOARD_HEIGHT ] =
+	{
+		/* TL_SPACE */ 1,	
+		/* TL_1 */ 0,
+		/* TL_2 */ 0,
+		/* TL_3 */ 0,
+		/* TL_4 */ 1,
+		/* TL_5 */ 2,
+		/* TL_6 */ 2,
+		/* TL_7 */ 2,
+		/* TL_8 */ 1,
+	};
+
+	s=0;
+	
+	// score 1 point if centre is not correct 
+	if( tiles[(BOARD_HEIGHT*BOARD_WIDTH)/2] != nodeGoal.tiles[(BOARD_HEIGHT*BOARD_WIDTH)/2] )
+	{
+ 		s = 1;
+	}
+
+	for( i=0; i<(BOARD_HEIGHT*BOARD_WIDTH); i++ )
+	{
+		// this loop adds up the totaldist element in h and
+		// the sequence score in s
+
+		// the space does not count
+		if( tiles[i] == TL_SPACE )
+		{
+			continue;
+		}
+
+		// get correct x and y of this tile
+		cx = tile_x[tiles[i]];
+		cy = tile_y[tiles[i]];
+
+		// get actual
+		ax = i % BOARD_WIDTH;
+		ay = i / BOARD_WIDTH;
+
+		// add manhatten distance to h
+		h += abs( cx-ax );
+		h += abs( cy-ay );
+
+		// no s score for center tile
+		if( (ax == (BOARD_WIDTH/2)) && (ay == (BOARD_HEIGHT/2)) )
+		{
+			continue;
+		}
+
+		// score 2 points if not followed by successor
+		if( correct_follower_to[ tiles[i] ] != tiles[ clockwise_tile_of[ i ] ] )
+		{
+			s += 2;
+		}
+
+
+	}
+
+	// mult by 3 and add to h
+	t = h + (3*s);
+
+	return (float) t;
+
+}
+
+bool PuzzleState::IsGoal( PuzzleState &nodeGoal )
+{
+	return IsSameState( nodeGoal );
+}
+
+// Helper
+// Return the x and y position of the space tile
+void PuzzleState::GetSpacePosition( PuzzleState *pn, int *rx, int *ry )
+{
+	int x,y;
+
+	for( y=0; y<BOARD_HEIGHT; y++ )
+	{
+		for( x=0; x<BOARD_WIDTH; x++ )
+		{
+			if( pn->tiles[(y*BOARD_WIDTH)+x] == TL_SPACE )
+			{
+				*rx = x;
+				*ry = y;
+
+				return;
+			}
+		}
+	}
+
+	assert( false && "Something went wrong. There's no space on the board" );
+
+}
+
+int PuzzleState::GetMap( int x, int y, TILE *tiles )
+{
+
+	if( x < 0 ||
+	    x >= BOARD_WIDTH ||
+		 y < 0 ||
+		 y >= BOARD_HEIGHT
+	  )
+		return GM_OFF_BOARD;	 
+
+	if( tiles[(y*BOARD_WIDTH)+x] == TL_SPACE )
+	{
+		return GM_SPACE;
+	}
+
+	return GM_TILE;
+}
+
+// Given a node set of tiles and a set of tiles to move them into, do the move as if it was on a tile board
+// note : returns false if the board wasn't changed, and simply returns the tiles as they were in the target
+// spx and spy is the space position while tx and ty is the target move from position
+
+bool PuzzleState::LegalMove( TILE *StartTiles, TILE *TargetTiles, int spx, int spy, int tx, int ty )
+{
+
+	int t;
+	
+	if( GetMap( spx, spy, StartTiles ) == GM_SPACE )
+	{
+		if( GetMap( tx, ty, StartTiles ) == GM_TILE )
+		{
+
+			// copy tiles
+			for( t=0; t<(BOARD_HEIGHT*BOARD_WIDTH); t++ )
+			{
+				TargetTiles[t] = StartTiles[t];
+			}
+
+
+			TargetTiles[ (ty*BOARD_WIDTH)+tx ] = StartTiles[ (spy*BOARD_WIDTH)+spx ];
+			TargetTiles[ (spy*BOARD_WIDTH)+spx ] = StartTiles[ (ty*BOARD_WIDTH)+tx ];
+
+			return true;
+		}
+	}
+
+
+	return false;
+
+}
+
+// This generates the successors to the given PuzzleState. It uses a helper function called
+// AddSuccessor to give the successors to the AStar class. The A* specific initialisation
+// is done for each node internally, so here you just set the state information that
+// is specific to the application
+bool PuzzleState::GetSuccessors( AStarSearch<PuzzleState> *astarsearch, PuzzleState *parent_node )
+{
+	PuzzleState NewNode;
+
+	int sp_x,sp_y;
+
+	GetSpacePosition( this, &sp_x, &sp_y );
+
+	bool ret;
+
+	if( LegalMove( tiles, NewNode.tiles, sp_x, sp_y, sp_x, sp_y-1 ) == true )
+	{
+		ret = astarsearch->AddSuccessor( NewNode );
+
+		if( !ret ) return false;
+	}
+
+	if( LegalMove( tiles, NewNode.tiles, sp_x, sp_y, sp_x, sp_y+1 ) == true )
+	{
+		ret = astarsearch->AddSuccessor( NewNode );
+		
+		if( !ret ) return false;
+	}
+
+	if( LegalMove( tiles, NewNode.tiles, sp_x, sp_y, sp_x-1, sp_y ) == true )
+	{
+		ret = astarsearch->AddSuccessor( NewNode );
+
+		if( !ret ) return false;
+	}
+
+	if( LegalMove( tiles, NewNode.tiles, sp_x, sp_y, sp_x+1, sp_y ) == true )
+	{
+		ret = astarsearch->AddSuccessor( NewNode );
+	
+		if( !ret ) return false;
+	}
+
+	return true; 
+}
+
+// given this node, what does it cost to move to successor. In the case
+// of our map the answer is the map terrain value at this node since that is 
+// conceptually where we're moving
+
+float PuzzleState::GetCost( PuzzleState &successor )
+{
+	return 1.0f; // I love it when life is simple
+
+}
+
+
+// Main
+
+int puzzle( int argc, char *argv[] )
+{
+
+	cout << "STL A* 8-puzzle solver implementation\n(C)2001 Justin Heyes-Jones\n";
+
+	if( argc > 1 )
+	{
+		int i = 0;
+		int c;
+
+		while( (c = argv[1][i]) )
+		{
+			if( isdigit( c ) )
+			{
+				int num = (c - '0');
+
+				PuzzleState::g_start[i] = static_cast<PuzzleState::TILE>(num);
+				
+			}
+		
+			i++;
+		}
+
+
+	}
+
+	// Create an instance of the search class...
+
+	AStarSearch<PuzzleState> astarsearch;
+
+	int NumTimesToSearch = NUM_TIMES_TO_RUN_SEARCH;
+
+	while( NumTimesToSearch-- )
+	{
+
+		// Create a start state
+		PuzzleState nodeStart( PuzzleState::g_start );
+
+		// Define the goal state
+		PuzzleState nodeEnd( PuzzleState::g_goal );
+
+		// Set Start and goal states
+		astarsearch.SetStartAndGoalStates( nodeStart, nodeEnd );
+
+		unsigned int SearchState;
+
+		unsigned int SearchSteps = 0;
+
+		do
+		{
+			SearchState = astarsearch.SearchStep();
+
+#if DEBUG_LISTS
+
+			float f,g,h;
+
+			cout << "Search step " << SearchSteps << endl;
+
+			cout << "Open:\n";
+			PuzzleState *p = astarsearch.GetOpenListStart( f,g,h );
+			while( p )
+			{
+				((PuzzleState *)p)->PrintNodeInfo();
+				cout << "f: " << f << " g: " << g << " h: " << h << "\n\n";
+				
+				p = astarsearch.GetOpenListNext( f,g,h );
+				
+			}
+
+			cout << "Closed:\n";
+			p = astarsearch.GetClosedListStart( f,g,h );
+			while( p )
+			{
+				p->PrintNodeInfo();
+				cout << "f: " << f << " g: " << g << " h: " << h << "\n\n";
+				
+				p = astarsearch.GetClosedListNext( f,g,h );
+			}
+
+#endif
+
+// Test cancel search
+#if 0
+			int StepCount = astarsearch.GetStepCount();
+			if( StepCount == 10 )
+			{
+				astarsearch.CancelSearch();
+			}
+#endif
+			SearchSteps++;
+		}
+		while( SearchState == AStarSearch<PuzzleState>::SEARCH_STATE_SEARCHING );
+
+		if( SearchState == AStarSearch<PuzzleState>::SEARCH_STATE_SUCCEEDED )
+		{
+#if DISPLAY_SOLUTION_FORWARDS
+			cout << "Search found goal state\n";
+#endif
+			PuzzleState *node = astarsearch.GetSolutionStart();
+
+#if DISPLAY_SOLUTION_FORWARDS
+			cout << "Displaying solution\n";
+#endif
+			int steps = 0;
+
+#if DISPLAY_SOLUTION_FORWARDS
+			node->PrintNodeInfo();
+			cout << endl;
+#endif
+			for( ;; )
+			{
+				node = astarsearch.GetSolutionNext();
+
+				if( !node )
+				{
+					break;
+				}
+
+#if DISPLAY_SOLUTION_FORWARDS
+				node->PrintNodeInfo();
+				cout << endl;
+#endif
+				steps ++;
+			
+			};
+
+#if DISPLAY_SOLUTION_FORWARDS
+			// todo move step count into main algorithm
+			cout << "Solution steps " << steps << endl;
+#endif
+
+////////////
+
+			node = astarsearch.GetSolutionEnd();
+
+#if DISPLAY_SOLUTION_BACKWARDS
+			cout << "Displaying reverse solution\n";
+#endif
+			steps = 0;
+
+			node->PrintNodeInfo();
+			cout << endl;
+			for( ;; )
+			{
+				node = astarsearch.GetSolutionPrev();
+
+				if( !node )
+				{
+					break;
+				}
+#if DISPLAY_SOLUTION_BACKWARDS
+				node->PrintNodeInfo();
+                cout << endl;
+#endif
+				steps ++;
+			
+			};
+
+#if DISPLAY_SOLUTION_BACKWARDS
+			cout << "Solution steps " << steps << endl;
+#endif
+
+//////////////
+
+			// Once you're done with the solution you can free the nodes up
+			astarsearch.FreeSolutionNodes();
+		
+		}
+		else if( SearchState == AStarSearch<PuzzleState>::SEARCH_STATE_FAILED ) 
+		{
+#if DISPLAY_SOLUTION_INFO
+			cout << "Search terminated. Did not find goal state\n";
+#endif		
+		}
+		else if( SearchState == AStarSearch<PuzzleState>::SEARCH_STATE_OUT_OF_MEMORY ) 
+		{
+#if DISPLAY_SOLUTION_INFO
+			cout << "Search terminated. Out of memory\n";
+#endif		
+		}
+
+		
+
+		// Display the number of loops the search went through
+#if DISPLAY_SOLUTION_INFO
+		cout << "SearchSteps : " << astarsearch.GetStepCount() << endl;
+#endif
+	}
+
+	return 0;
+}
+
+
--- a/src/Astar/findpath.cpp
+++ b/src/Astar/findpath.cpp
@ -0,0 +1,343 @@
+////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+// STL A* Search implementation
+// (C)2001 Justin Heyes-Jones
+//
+// Finding a path on a simple grid maze
+// This shows how to do shortest path finding using A*
+
+////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+#include "stlastar.h" // See header for copyright and usage information
+
+#include <iostream>
+#include <stdio.h>
+#include <math.h>
+
+#define DEBUG_LISTS 0
+#define DEBUG_LIST_LENGTHS_ONLY 0
+
+using namespace std;
+
+// Global data
+
+// The world map
+
+const int MAP_WIDTH = 20;
+const int MAP_HEIGHT = 20;
+
+int world_map[ MAP_WIDTH * MAP_HEIGHT ] = 
+{
+
+// 0001020304050607080910111213141516171819
+	1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,   // 00
+	1,9,9,9,9,9,9,9,9,9,9,9,9,9,9,9,9,9,9,1,   // 01
+	1,9,9,1,1,9,9,9,1,9,1,9,1,9,1,9,9,9,1,1,   // 02
+	1,9,9,1,1,9,9,9,1,9,1,9,1,9,1,9,9,9,1,1,   // 03
+	1,9,1,1,1,1,9,9,1,9,1,9,1,1,1,1,9,9,1,1,   // 04
+	1,9,1,1,9,1,1,1,1,9,1,1,1,1,9,1,1,1,1,1,   // 05
+	1,9,9,9,9,1,1,1,1,1,1,9,9,9,9,1,1,1,1,1,   // 06
+	1,9,9,9,9,9,9,9,9,1,1,1,9,9,9,9,9,9,9,1,   // 07
+	1,9,1,1,1,1,1,1,1,1,1,9,1,1,1,1,1,1,1,1,   // 08
+	1,9,1,9,9,9,9,9,9,9,1,1,9,9,9,9,9,9,9,1,   // 09
+	1,9,1,1,1,1,9,1,1,9,1,1,1,1,1,1,1,1,1,1,   // 10
+	1,9,9,9,9,9,1,9,1,9,1,9,9,9,9,9,1,1,1,1,   // 11
+	1,9,1,9,1,9,9,9,1,9,1,9,1,9,1,9,9,9,1,1,   // 12
+	1,9,1,9,1,9,9,9,1,9,1,9,1,9,1,9,9,9,1,1,   // 13
+	1,9,1,1,1,1,9,9,1,9,1,9,1,1,1,1,9,9,1,1,   // 14
+	1,9,1,1,9,1,1,1,1,9,1,1,1,1,9,1,1,1,1,1,   // 15
+	1,9,9,9,9,1,1,1,1,1,1,9,9,9,9,1,1,1,1,1,   // 16
+	1,1,9,9,9,9,9,9,9,1,1,1,9,9,9,1,9,9,9,9,   // 17
+	1,9,1,1,1,1,1,1,1,1,1,9,1,1,1,1,1,1,1,1,   // 18
+	1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,   // 19
+
+};
+
+// map helper functions
+
+int GetMap( int x, int y )
+{
+	if( x < 0 ||
+	    x >= MAP_WIDTH ||
+		 y < 0 ||
+		 y >= MAP_HEIGHT
+	  )
+	{
+		return 9;	 
+	}
+
+	return world_map[(y*MAP_WIDTH)+x];
+}
+
+
+
+// Definitions
+
+class MapSearchNode
+{
+public:
+	int x;	 // the (x,y) positions of the node
+	int y;	
+	
+	MapSearchNode() { x = y = 0; }
+	MapSearchNode( int px, int py ) { x=px; y=py; }
+
+	float GoalDistanceEstimate( MapSearchNode &nodeGoal );
+	bool IsGoal( MapSearchNode &nodeGoal );
+	bool GetSuccessors( AStarSearch<MapSearchNode> *astarsearch, MapSearchNode *parent_node );
+	float GetCost( MapSearchNode &successor );
+	bool IsSameState( MapSearchNode &rhs );
+
+	void PrintNodeInfo(); 
+
+
+};
+
+bool MapSearchNode::IsSameState( MapSearchNode &rhs )
+{
+
+	// same state in a maze search is simply when (x,y) are the same
+	if( (x == rhs.x) &&
+		(y == rhs.y) )
+	{
+		return true;
+	}
+	else
+	{
+		return false;
+	}
+
+}
+
+void MapSearchNode::PrintNodeInfo()
+{
+	char str[100];
+	sprintf( str, "Node position : (%d,%d)\n", x,y );
+
+	cout << str;
+}
+
+// Here's the heuristic function that estimates the distance from a Node
+// to the Goal. 
+
+float MapSearchNode::GoalDistanceEstimate( MapSearchNode &nodeGoal )
+{
+	return abs(x - nodeGoal.x) + abs(y - nodeGoal.y);
+}
+
+bool MapSearchNode::IsGoal( MapSearchNode &nodeGoal )
+{
+
+	if( (x == nodeGoal.x) &&
+		(y == nodeGoal.y) )
+	{
+		return true;
+	}
+
+	return false;
+}
+
+// This generates the successors to the given Node. It uses a helper function called
+// AddSuccessor to give the successors to the AStar class. The A* specific initialisation
+// is done for each node internally, so here you just set the state information that
+// is specific to the application
+bool MapSearchNode::GetSuccessors( AStarSearch<MapSearchNode> *astarsearch, MapSearchNode *parent_node )
+{
+
+	int parent_x = -1; 
+	int parent_y = -1; 
+
+	if( parent_node )
+	{
+		parent_x = parent_node->x;
+		parent_y = parent_node->y;
+	}
+	
+
+	MapSearchNode NewNode;
+
+	// push each possible move except allowing the search to go backwards
+
+	if( (GetMap( x-1, y ) < 9) 
+		&& !((parent_x == x-1) && (parent_y == y))
+	  ) 
+	{
+		NewNode = MapSearchNode( x-1, y );
+		astarsearch->AddSuccessor( NewNode );
+	}	
+
+	if( (GetMap( x, y-1 ) < 9) 
+		&& !((parent_x == x) && (parent_y == y-1))
+	  ) 
+	{
+		NewNode = MapSearchNode( x, y-1 );
+		astarsearch->AddSuccessor( NewNode );
+	}	
+
+	if( (GetMap( x+1, y ) < 9)
+		&& !((parent_x == x+1) && (parent_y == y))
+	  ) 
+	{
+		NewNode = MapSearchNode( x+1, y );
+		astarsearch->AddSuccessor( NewNode );
+	}	
+
+		
+	if( (GetMap( x, y+1 ) < 9) 
+		&& !((parent_x == x) && (parent_y == y+1))
+		)
+	{
+		NewNode = MapSearchNode( x, y+1 );
+		astarsearch->AddSuccessor( NewNode );
+	}	
+
+	return true;
+}
+
+// given this node, what does it cost to move to successor. In the case
+// of our map the answer is the map terrain value at this node since that is 
+// conceptually where we're moving
+
+float MapSearchNode::GetCost( MapSearchNode &successor )
+{
+	return (float) GetMap( x, y );
+}
+
+
+// Main
+
+int findpath( int argc, char *argv[] )
+{
+
+	cout << "STL A* Search implementation\n(C)2001 Justin Heyes-Jones\n";
+
+	// Our sample problem defines the world as a 2d array representing a terrain
+	// Each element contains an integer from 0 to 5 which indicates the cost 
+	// of travel across the terrain. Zero means the least possible difficulty 
+	// in travelling (think ice rink if you can skate) whilst 5 represents the 
+	// most difficult. 9 indicates that we cannot pass.
+
+	// Create an instance of the search class...
+
+	AStarSearch<MapSearchNode> astarsearch;
+
+	unsigned int SearchCount = 0;
+
+	const unsigned int NumSearches = 1;
+
+	while(SearchCount < NumSearches)
+	{
+
+		// Create a start state
+		MapSearchNode nodeStart;
+		nodeStart.x = rand()%MAP_WIDTH;
+		nodeStart.y = rand()%MAP_HEIGHT; 
+
+		// Define the goal state
+		MapSearchNode nodeEnd;
+		nodeEnd.x = rand()%MAP_WIDTH;						
+		nodeEnd.y = rand()%MAP_HEIGHT; 
+		
+		// Set Start and goal states
+		
+		astarsearch.SetStartAndGoalStates( nodeStart, nodeEnd );
+
+		unsigned int SearchState;
+		unsigned int SearchSteps = 0;
+
+		do
+		{
+			SearchState = astarsearch.SearchStep();
+
+			SearchSteps++;
+
+	#if DEBUG_LISTS
+
+			cout << "Steps:" << SearchSteps << "\n";
+
+			int len = 0;
+
+			cout << "Open:\n";
+			MapSearchNode *p = astarsearch.GetOpenListStart();
+			while( p )
+			{
+				len++;
+	#if !DEBUG_LIST_LENGTHS_ONLY			
+				((MapSearchNode *)p)->PrintNodeInfo();
+	#endif
+				p = astarsearch.GetOpenListNext();
+				
+			}
+
+			cout << "Open list has " << len << " nodes\n";
+
+			len = 0;
+
+			cout << "Closed:\n";
+			p = astarsearch.GetClosedListStart();
+			while( p )
+			{
+				len++;
+	#if !DEBUG_LIST_LENGTHS_ONLY			
+				p->PrintNodeInfo();
+	#endif			
+				p = astarsearch.GetClosedListNext();
+			}
+
+			cout << "Closed list has " << len << " nodes\n";
+	#endif
+
+		}
+		while( SearchState == AStarSearch<MapSearchNode>::SEARCH_STATE_SEARCHING );
+
+		if( SearchState == AStarSearch<MapSearchNode>::SEARCH_STATE_SUCCEEDED )
+		{
+			cout << "Search found goal state\n";
+
+				MapSearchNode *node = astarsearch.GetSolutionStart();
+
+	#if DISPLAY_SOLUTION
+				cout << "Displaying solution\n";
+	#endif
+				int steps = 0;
+
+				node->PrintNodeInfo();
+				for( ;; )
+				{
+					node = astarsearch.GetSolutionNext();
+
+					if( !node )
+					{
+						break;
+					}
+
+					node->PrintNodeInfo();
+					steps ++;
+				
+				};
+
+				cout << "Solution steps " << steps << endl;
+
+				// Once you're done with the solution you can free the nodes up
+				astarsearch.FreeSolutionNodes();
+
+	
+		}
+		else if( SearchState == AStarSearch<MapSearchNode>::SEARCH_STATE_FAILED ) 
+		{
+			cout << "Search terminated. Did not find goal state\n";
+		
+		}
+
+		// Display the number of loops the search went through
+		cout << "SearchSteps : " << SearchSteps << "\n";
+
+		SearchCount ++;
+
+		astarsearch.EnsureMemoryFreed();
+	}
+	
+	return 0;
+}
+
+////////////////////////////////////////////////////////////////////////////////////////////////////////////////
--- a/src/Astar/fsa.h
+++ b/src/Astar/fsa.h
@ -0,0 +1,252 @@
+/* 
+
+A* Algorithm Implementation using STL is
+Copyright (C)2001-2005 Justin Heyes-Jones
+
+Permission is given by the author to freely redistribute and 
+include this code in any program as long as this credit is 
+given where due.
+ 
+  COVERED CODE IS PROVIDED UNDER THIS LICENSE ON AN "AS IS" BASIS, 
+  WITHOUT WARRANTY OF ANY KIND, EITHER EXPRESSED OR IMPLIED, 
+  INCLUDING, WITHOUT LIMITATION, WARRANTIES THAT THE COVERED CODE 
+  IS FREE OF DEFECTS, MERCHANTABLE, FIT FOR A PARTICULAR PURPOSE
+  OR NON-INFRINGING. THE ENTIRE RISK AS TO THE QUALITY AND 
+  PERFORMANCE OF THE COVERED CODE IS WITH YOU. SHOULD ANY COVERED 
+  CODE PROVE DEFECTIVE IN ANY RESPECT, YOU (NOT THE INITIAL 
+  DEVELOPER OR ANY OTHER CONTRIBUTOR) ASSUME THE COST OF ANY 
+  NECESSARY SERVICING, REPAIR OR CORRECTION. THIS DISCLAIMER OF 
+  WARRANTY CONSTITUTES AN ESSENTIAL PART OF THIS LICENSE. NO USE 
+  OF ANY COVERED CODE IS AUTHORIZED HEREUNDER EXCEPT UNDER
+  THIS DISCLAIMER.
+ 
+  Use at your own risk!
+
+
+
+  FixedSizeAllocator class
+  Copyright 2001 Justin Heyes-Jones
+
+  This class is a constant time O(1) memory manager for objects of 
+  a specified type. The type is specified using a template class.
+
+  Memory is allocated from a fixed size buffer which you can specify in the
+  class constructor or use the default.
+
+  Using GetFirst and GetNext it is possible to iterate through the elements
+  one by one, and this would be the most common use for the class. 
+
+  I would suggest using this class when you want O(1) add and delete
+  and you don't do much searching, which would be O(n). Structures such as binary 
+  trees can be used instead to get O(logn) access time.
+
+*/
+
+#ifndef FSA_H
+#define FSA_H
+
+#include <string.h>
+#include <stdio.h>
+
+template <class USER_TYPE> class FixedSizeAllocator
+{
+
+public: 
+	// Constants
+	enum
+	{
+		FSA_DEFAULT_SIZE = 100
+	};
+
+	// This class enables us to transparently manage the extra data 
+	// needed to enable the user class to form part of the double-linked
+	// list class
+	struct FSA_ELEMENT
+	{
+		USER_TYPE UserType;
+		
+		FSA_ELEMENT *pPrev;
+		FSA_ELEMENT *pNext;
+	};
+
+public: // methods
+	FixedSizeAllocator( unsigned int MaxElements = FSA_DEFAULT_SIZE ) :
+	m_pFirstUsed( NULL ),
+	m_MaxElements( MaxElements )
+	{
+		// Allocate enough memory for the maximum number of elements
+
+		char *pMem = new char[ m_MaxElements * sizeof(FSA_ELEMENT) ];
+
+		m_pMemory = (FSA_ELEMENT *) pMem; 
+
+		// Set the free list first pointer
+		m_pFirstFree = m_pMemory;
+
+		// Clear the memory
+		memset( m_pMemory, 0, sizeof( FSA_ELEMENT ) * m_MaxElements );
+
+		// Point at first element
+		FSA_ELEMENT *pElement = m_pFirstFree;
+
+		// Set the double linked free list
+		for( unsigned int i=0; i<m_MaxElements; i++ )
+		{
+			pElement->pPrev = pElement-1;
+			pElement->pNext = pElement+1;
+
+			pElement++;
+		}
+
+		// first element should have a null prev
+		m_pFirstFree->pPrev = NULL;
+		// last element should have a null next
+		(pElement-1)->pNext = NULL;
+
+	}
+
+
+	~FixedSizeAllocator()
+	{
+		// Free up the memory
+		delete [] (char *) m_pMemory;
+	}
+
+	// Allocate a new USER_TYPE and return a pointer to it 
+	USER_TYPE *alloc()
+	{
+
+		FSA_ELEMENT *pNewNode = NULL;
+
+		if( !m_pFirstFree )
+		{
+			return NULL;
+		}
+		else
+		{
+			pNewNode = m_pFirstFree;
+			m_pFirstFree = pNewNode->pNext;
+
+			// if the new node points to another free node then
+			// change that nodes prev free pointer...
+			if( pNewNode->pNext )
+			{
+				pNewNode->pNext->pPrev = NULL;
+			}
+
+			// node is now on the used list
+
+			pNewNode->pPrev = NULL; // the allocated node is always first in the list
+
+			if( m_pFirstUsed == NULL )
+			{
+				pNewNode->pNext = NULL; // no other nodes
+			}
+			else
+			{
+				m_pFirstUsed->pPrev = pNewNode; // insert this at the head of the used list
+				pNewNode->pNext = m_pFirstUsed;
+			}
+
+			m_pFirstUsed = pNewNode;
+		}	
+		
+		return reinterpret_cast<USER_TYPE*>(pNewNode);
+	}
+
+	// Free the given user type
+	// For efficiency I don't check whether the user_data is a valid
+	// pointer that was allocated. I may add some debug only checking
+	// (To add the debug check you'd need to make sure the pointer is in 
+	// the m_pMemory area and is pointing at the start of a node)
+	void free( USER_TYPE *user_data )
+	{
+		FSA_ELEMENT *pNode = reinterpret_cast<FSA_ELEMENT*>(user_data);
+
+		// manage used list, remove this node from it
+		if( pNode->pPrev )
+		{
+			pNode->pPrev->pNext = pNode->pNext;
+		}
+		else
+		{
+			// this handles the case that we delete the first node in the used list
+			m_pFirstUsed = pNode->pNext;
+		}
+
+		if( pNode->pNext )
+		{
+			pNode->pNext->pPrev = pNode->pPrev;
+		}
+
+		// add to free list
+		if( m_pFirstFree == NULL ) 
+		{
+			// free list was empty
+			m_pFirstFree = pNode;
+			pNode->pPrev = NULL;
+			pNode->pNext = NULL;
+		}
+		else
+		{
+			// Add this node at the start of the free list
+			m_pFirstFree->pPrev = pNode;
+			pNode->pNext = m_pFirstFree;
+			m_pFirstFree = pNode;
+		}
+
+	}
+
+	// For debugging this displays both lists (using the prev/next list pointers)
+	void Debug()
+	{
+		printf( "free list " );
+
+		FSA_ELEMENT *p = m_pFirstFree;
+		while( p )
+		{
+			printf( "%x!%x ", p->pPrev, p->pNext );
+			p = p->pNext;
+		}
+		printf( "\n" );
+
+		printf( "used list " );
+
+		p = m_pFirstUsed;
+		while( p )
+		{
+			printf( "%x!%x ", p->pPrev, p->pNext );
+			p = p->pNext;
+		}
+		printf( "\n" );
+	}
+
+	// Iterators
+
+	USER_TYPE *GetFirst()
+	{
+		return reinterpret_cast<USER_TYPE *>(m_pFirstUsed);
+	}
+
+	USER_TYPE *GetNext( USER_TYPE *node )
+	{
+		return reinterpret_cast<USER_TYPE *>
+			(
+				(reinterpret_cast<FSA_ELEMENT *>(node))->pNext
+			);
+	}
+
+public: // data
+
+private: // methods
+
+private: // data
+
+	FSA_ELEMENT *m_pFirstFree;
+	FSA_ELEMENT *m_pFirstUsed;
+	unsigned int m_MaxElements;
+	FSA_ELEMENT *m_pMemory;
+
+};
+
+#endif // defined FSA_H
--- a/src/Astar/min_path_to_Bucharest.cpp
+++ b/src/Astar/min_path_to_Bucharest.cpp
@ -0,0 +1,315 @@
+////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+// This example code illustrate how to use STL A* Search implementation to find the minimum path between two
+// cities given a map. The example is taken from the book AI: A Modern Approach, 3rd Ed., by Russel, where a map
+// of Romania is given. The target node is Bucharest, and the user can specify the initial city from which the
+// search algorithm will start looking for the minimum path to Bucharest.
+//
+// Usage: min_path_to_Bucharest <start city name>
+//        Example:
+//                min_path_to_Bucharest Arad
+//
+// Thanks to Rasoul  Mojtahedzadeh for this contribution
+// Mojtahedzadeh _atsign_ gmail com
+// 
+// Please note that this exercise is academic in nature and that the distances between the cities may not be
+// correct compared to the latitude and longnitude differences in real life. Thanks to parthi2929 for noticing 
+// this issue. In a real application you would use some kind of exact x,y position of each city in order to get 
+// accurate heuristics. In fact, that is part of the point of this example, in the book you will see Norvig
+// mention that the algorithm does some backtracking because the heuristic is not accurate (yet still admissable).
+////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+#include "stlastar.h"
+#include <iostream>
+#include <string>
+#include <vector>
+#include <stdio.h>
+
+#define DEBUG_LISTS 0
+#define DEBUG_LIST_LENGTHS_ONLY 0
+
+using namespace std;
+
+const int MAX_CITIES = 20;
+
+enum ENUM_CITIES{Arad=0, Bucharest, Craiova, Drobeta, Eforie, Fagaras, Giurgiu, Hirsova, Iasi, Lugoj, Mehadia, Neamt, Oradea, Pitesti, RimnicuVilcea, Sibiu, Timisoara, Urziceni, Vaslui, Zerind};
+vector<string> CityNames(MAX_CITIES);
+float RomaniaMap[MAX_CITIES][MAX_CITIES];
+
+class PathSearchNode
+{
+public:
+
+  ENUM_CITIES city;
+
+	PathSearchNode() { city = Arad; }
+	PathSearchNode( ENUM_CITIES in ) { city = in; }
+
+	float GoalDistanceEstimate( PathSearchNode &nodeGoal );
+	bool IsGoal( PathSearchNode &nodeGoal );
+	bool GetSuccessors( AStarSearch<PathSearchNode> *astarsearch, PathSearchNode *parent_node );
+	float GetCost( PathSearchNode &successor );
+	bool IsSameState( PathSearchNode &rhs );
+
+	void PrintNodeInfo();
+};
+
+// check if "this" node is the same as "rhs" node
+bool PathSearchNode::IsSameState( PathSearchNode &rhs )
+{
+  if(city == rhs.city) return(true);
+  return(false);
+}
+
+// Euclidean distance between "this" node city and Bucharest
+// Note: Numbers are taken from the book
+float PathSearchNode::GoalDistanceEstimate( PathSearchNode &nodeGoal )
+{
+  // goal is always Bucharest!
+  switch(city)
+  {
+    case Arad: return 366; break;
+    case Bucharest: return 0; break;
+    case Craiova: return 160; break;
+    case Drobeta: return 242; break;
+    case Eforie: return 161; break;
+    case Fagaras: return 176; break;
+    case Giurgiu: return 77; break;
+    case Hirsova: return 151; break;
+    case Iasi: return 226; break;
+    case Lugoj: return 244; break;
+    case Mehadia: return 241; break;
+    case Neamt: return 234; break;
+    case Oradea: return 380; break;
+    case Pitesti: return 100; break;
+    case RimnicuVilcea: return 193; break;
+    case Sibiu: return 253; break;
+    case Timisoara: return 329; break;
+    case Urziceni: return 80; break;
+    case Vaslui: return 199; break;
+    case Zerind: return 374; break;
+  }
+  cerr << "ASSERT: city = " << CityNames[city] << endl;
+	return 0;
+}
+
+// check if "this" node is the goal node
+bool PathSearchNode::IsGoal( PathSearchNode &nodeGoal )
+{
+	if( city == Bucharest ) return(true);
+	return(false);
+}
+
+// generates the successor nodes of "this" node
+bool PathSearchNode::GetSuccessors( AStarSearch<PathSearchNode> *astarsearch, PathSearchNode *parent_node )
+{
+  PathSearchNode NewNode;
+  for(int c=0; c<MAX_CITIES; c++)
+  {
+    if(RomaniaMap[city][c] < 0) continue;
+    NewNode = PathSearchNode((ENUM_CITIES)c);
+    astarsearch->AddSuccessor( NewNode );
+  }
+	return true;
+}
+
+// the cost of going from "this" node to the "successor" node
+float PathSearchNode::GetCost( PathSearchNode &successor )
+{
+	return RomaniaMap[city][successor.city];
+}
+
+// prints out information about the node
+void PathSearchNode::PrintNodeInfo()
+{
+	cout << " " << CityNames[city] << "\n";
+}
+
+// Main
+int min_path_to_Bucharest( int argc, char *argv[] )
+{
+  // creating map of Romania
+  for(int i=0; i<MAX_CITIES; i++)
+    for(int j=0; j<MAX_CITIES; j++)
+      RomaniaMap[i][j]=-1.0;
+
+  RomaniaMap[Arad][Sibiu]=140;
+  RomaniaMap[Arad][Zerind]=75;
+  RomaniaMap[Arad][Timisoara]=118;
+  RomaniaMap[Bucharest][Giurgiu]=90;
+  RomaniaMap[Bucharest][Urziceni]=85;
+  RomaniaMap[Bucharest][Fagaras]=211;
+  RomaniaMap[Bucharest][Pitesti]=101;
+  RomaniaMap[Craiova][Drobeta]=120;
+  RomaniaMap[Craiova][RimnicuVilcea]=146;
+  RomaniaMap[Craiova][Pitesti]=138;
+  RomaniaMap[Drobeta][Craiova]=120;
+  RomaniaMap[Drobeta][Mehadia]=75;
+  RomaniaMap[Eforie][Hirsova]=75;
+  RomaniaMap[Fagaras][Bucharest]=211;
+  RomaniaMap[Fagaras][Sibiu]=99;
+  RomaniaMap[Giurgiu][Bucharest]=90;
+  RomaniaMap[Hirsova][Eforie]=86;
+  RomaniaMap[Hirsova][Urziceni]=98;
+  RomaniaMap[Iasi][Vaslui]=92;
+  RomaniaMap[Iasi][Neamt]=87;
+  RomaniaMap[Lugoj][Timisoara]=111;
+  RomaniaMap[Lugoj][Mehadia]=70;
+  RomaniaMap[Mehadia][Lugoj]=70;
+  RomaniaMap[Mehadia][Drobeta]=75;
+  RomaniaMap[Neamt][Iasi]=87;
+  RomaniaMap[Oradea][Zerind]=71;
+  RomaniaMap[Oradea][Sibiu]=151;
+  RomaniaMap[Pitesti][Bucharest]=101;
+  RomaniaMap[Pitesti][RimnicuVilcea]=97;
+  RomaniaMap[Pitesti][Craiova]=138;
+  RomaniaMap[RimnicuVilcea][Pitesti]=97;
+  RomaniaMap[RimnicuVilcea][Craiova]=146;
+  RomaniaMap[RimnicuVilcea][Sibiu]=80;
+  RomaniaMap[Sibiu][RimnicuVilcea]=80;
+  RomaniaMap[Sibiu][Fagaras]=99;
+  RomaniaMap[Sibiu][Oradea]=151;
+  RomaniaMap[Sibiu][Arad]=140;
+  RomaniaMap[Timisoara][Arad]=118;
+  RomaniaMap[Timisoara][Lugoj]=111;
+  RomaniaMap[Urziceni][Bucharest]=85;
+  RomaniaMap[Urziceni][Hirsova]=98;
+  RomaniaMap[Urziceni][Vaslui]=142;
+  RomaniaMap[Vaslui][Urziceni]=142;
+  RomaniaMap[Vaslui][Iasi]=92;
+  RomaniaMap[Zerind][Arad]=75;
+  RomaniaMap[Zerind][Oradea]=71;
+
+  // City names
+  CityNames[Arad].assign("Arad");
+  CityNames[Bucharest].assign("Bucharest");
+  CityNames[Craiova].assign("Craiova");
+  CityNames[Drobeta].assign("Drobeta");
+  CityNames[Eforie].assign("Eforie");
+  CityNames[Fagaras].assign("Fagaras");
+  CityNames[Giurgiu].assign("Giurgiu");
+  CityNames[Hirsova].assign("Hirsova");
+  CityNames[Iasi].assign("Iasi");
+  CityNames[Lugoj].assign("Lugoj");
+  CityNames[Mehadia].assign("Mehadia");
+  CityNames[Neamt].assign("Neamt");
+  CityNames[Oradea].assign("Oradea");
+  CityNames[Pitesti].assign("Pitesti");
+  CityNames[RimnicuVilcea].assign("RimnicuVilcea");
+  CityNames[Sibiu].assign("Sibiu");
+  CityNames[Timisoara].assign("Timisoara");
+  CityNames[Urziceni].assign("Urziceni");
+  CityNames[Vaslui].assign("Vaslui");
+  CityNames[Zerind].assign("Zerind");
+
+  ENUM_CITIES initCity = Arad;
+  if(argc == 2)
+  {
+    bool found = false;
+    for(size_t i=0; i<CityNames.size(); i++)
+      if(CityNames[i].compare(argv[1])==0)
+      {
+        initCity = (ENUM_CITIES)i;
+        found = true;
+        break;
+      }
+    if(not found)
+    {
+      cout << "There is no city named "<<argv[1]<<" in the map!\n";
+      return(1);
+    }
+  }
+
+  // An instance of A* search class
+	AStarSearch<PathSearchNode> astarsearch;
+
+	unsigned int SearchCount = 0;
+	const unsigned int NumSearches = 1;
+
+	while(SearchCount < NumSearches)
+	{
+		// Create a start state
+		PathSearchNode nodeStart;
+		nodeStart.city = initCity;
+
+		// Define the goal state, always Bucharest!
+		PathSearchNode nodeEnd;
+		nodeEnd.city = Bucharest;
+
+		// Set Start and goal states
+		astarsearch.SetStartAndGoalStates( nodeStart, nodeEnd );
+
+		unsigned int SearchState;
+		unsigned int SearchSteps = 0;
+
+		do
+		{
+			SearchState = astarsearch.SearchStep();
+			SearchSteps++;
+
+	#if DEBUG_LISTS
+
+			cout << "Steps:" << SearchSteps << "\n";
+
+			int len = 0;
+
+			cout << "Open:\n";
+			PathSearchNode *p = astarsearch.GetOpenListStart();
+			while( p )
+			{
+				len++;
+	#if !DEBUG_LIST_LENGTHS_ONLY
+				((PathSearchNode *)p)->PrintNodeInfo();
+	#endif
+				p = astarsearch.GetOpenListNext();
+
+			}
+			cout << "Open list has " << len << " nodes\n";
+
+			len = 0;
+
+			cout << "Closed:\n";
+			p = astarsearch.GetClosedListStart();
+			while( p )
+			{
+				len++;
+	#if !DEBUG_LIST_LENGTHS_ONLY
+				p->PrintNodeInfo();
+	#endif
+				p = astarsearch.GetClosedListNext();
+			}
+
+			cout << "Closed list has " << len << " nodes\n";
+	#endif
+
+		}
+		while( SearchState == AStarSearch<PathSearchNode>::SEARCH_STATE_SEARCHING );
+
+		if( SearchState == AStarSearch<PathSearchNode>::SEARCH_STATE_SUCCEEDED )
+		{
+			cout << "Search found the goal state\n";
+      PathSearchNode *node = astarsearch.GetSolutionStart();
+      cout << "Displaying solution\n";
+      int steps = 0;
+      node->PrintNodeInfo();
+      for( ;; )
+      {
+        node = astarsearch.GetSolutionNext();
+        if( !node ) break;
+        node->PrintNodeInfo();
+        steps ++;
+      };
+      cout << "Solution steps " << steps << endl;
+      // Once you're done with the solution you can free the nodes up
+			astarsearch.FreeSolutionNodes();
+		}
+		else if( SearchState == AStarSearch<PathSearchNode>::SEARCH_STATE_FAILED )
+		{
+			cout << "Search terminated. Did not find goal state\n";
+		}
+		// Display the number of loops the search went through
+		cout << "SearchSteps : " << SearchSteps << "\n";
+		SearchCount ++;
+		astarsearch.EnsureMemoryFreed();
+	}
+
+	return 0;
+}
--- a/src/Astar/stlastar.h
+++ b/src/Astar/stlastar.h
@ -0,0 +1,833 @@
+/*
+A* Algorithm Implementation using STL is
+Copyright (C)2001-2005 Justin Heyes-Jones
+
+Permission is given by the author to freely redistribute and 
+include this code in any program as long as this credit is 
+given where due.
+ 
+  COVERED CODE IS PROVIDED UNDER THIS LICENSE ON AN "AS IS" BASIS, 
+  WITHOUT WARRANTY OF ANY KIND, EITHER EXPRESSED OR IMPLIED, 
+  INCLUDING, WITHOUT LIMITATION, WARRANTIES THAT THE COVERED CODE 
+  IS FREE OF DEFECTS, MERCHANTABLE, FIT FOR A PARTICULAR PURPOSE
+  OR NON-INFRINGING. THE ENTIRE RISK AS TO THE QUALITY AND 
+  PERFORMANCE OF THE COVERED CODE IS WITH YOU. SHOULD ANY COVERED 
+  CODE PROVE DEFECTIVE IN ANY RESPECT, YOU (NOT THE INITIAL 
+  DEVELOPER OR ANY OTHER CONTRIBUTOR) ASSUME THE COST OF ANY 
+  NECESSARY SERVICING, REPAIR OR CORRECTION. THIS DISCLAIMER OF 
+  WARRANTY CONSTITUTES AN ESSENTIAL PART OF THIS LICENSE. NO USE 
+  OF ANY COVERED CODE IS AUTHORIZED HEREUNDER EXCEPT UNDER
+  THIS DISCLAIMER.
+ 
+  Use at your own risk!
+
+*/
+
+#ifndef STLASTAR_H
+#define STLASTAR_H
+// used for text debugging
+#include <iostream>
+#include <stdio.h>
+//#include <conio.h>
+#include <assert.h>
+
+// stl includes
+#include <algorithm>
+#include <set>
+#include <vector>
+#include <cfloat>
+
+using namespace std;
+
+// fast fixed size memory allocator, used for fast node memory management
+#include "fsa.h"
+
+// Fixed size memory allocator can be disabled to compare performance
+// Uses std new and delete instead if you turn it off
+#define USE_FSA_MEMORY 1
+
+// disable warning that debugging information has lines that are truncated
+// occurs in stl headers
+#if defined(WIN32) && defined(_WINDOWS)
+#pragma warning( disable : 4786 )
+#endif
+
+template <class T> class AStarState;
+
+// The AStar search class. UserState is the users state space type
+template <class UserState> class AStarSearch
+{
+
+public: // data
+
+	enum
+	{
+		SEARCH_STATE_NOT_INITIALISED,
+		SEARCH_STATE_SEARCHING,
+		SEARCH_STATE_SUCCEEDED,
+		SEARCH_STATE_FAILED,
+		SEARCH_STATE_OUT_OF_MEMORY,
+		SEARCH_STATE_INVALID
+	};
+
+
+	// A node represents a possible state in the search
+	// The user provided state type is included inside this type
+
+	public:
+
+	class Node
+	{
+		public:
+
+			Node *parent; // used during the search to record the parent of successor nodes
+			Node *child; // used after the search for the application to view the search in reverse
+			
+			float g; // cost of this node + it's predecessors
+			float h; // heuristic estimate of distance to goal
+			float f; // sum of cumulative cost of predecessors and self and heuristic
+
+			Node() :
+				parent( 0 ),
+				child( 0 ),
+				g( 0.0f ),
+				h( 0.0f ),
+				f( 0.0f )
+			{			
+			}
+
+			UserState m_UserState;
+	};
+
+
+	// For sorting the heap the STL needs compare function that lets us compare
+	// the f value of two nodes
+
+	class HeapCompare_f 
+	{
+		public:
+
+			bool operator() ( const Node *x, const Node *y ) const
+			{
+				return x->f > y->f;
+			}
+	};
+
+
+public: // methods
+
+
+	// constructor just initialises private data
+	AStarSearch() :
+		m_State( SEARCH_STATE_NOT_INITIALISED ),
+		m_CurrentSolutionNode( NULL ),
+#if USE_FSA_MEMORY
+		m_FixedSizeAllocator( 1000 ),
+#endif
+		m_AllocateNodeCount(0),
+		m_CancelRequest( false )
+	{
+	}
+
+	AStarSearch( int MaxNodes ) :
+		m_State( SEARCH_STATE_NOT_INITIALISED ),
+		m_CurrentSolutionNode( NULL ),
+#if USE_FSA_MEMORY
+		m_FixedSizeAllocator( MaxNodes ),
+#endif
+		m_AllocateNodeCount(0),
+		m_CancelRequest( false )
+	{
+	}
+
+	// call at any time to cancel the search and free up all the memory
+	void CancelSearch()
+	{
+		m_CancelRequest = true;
+	}
+
+	// Set Start and goal states
+	void SetStartAndGoalStates( UserState &Start, UserState &Goal )
+	{
+		m_CancelRequest = false;
+
+		m_Start = AllocateNode();
+		m_Goal = AllocateNode();
+
+		assert((m_Start != NULL && m_Goal != NULL));
+		
+		m_Start->m_UserState = Start;
+		m_Goal->m_UserState = Goal;
+
+		m_State = SEARCH_STATE_SEARCHING;
+		
+		// Initialise the AStar specific parts of the Start Node
+		// The user only needs fill out the state information
+
+		m_Start->g = 0; 
+		m_Start->h = m_Start->m_UserState.GoalDistanceEstimate( m_Goal->m_UserState );
+		m_Start->f = m_Start->g + m_Start->h;
+		m_Start->parent = 0;
+
+		// Push the start node on the Open list
+
+		m_OpenList.push_back( m_Start ); // heap now unsorted
+
+		// Sort back element into heap
+		push_heap( m_OpenList.begin(), m_OpenList.end(), HeapCompare_f() );
+
+		// Initialise counter for search steps
+		m_Steps = 0;
+	}
+
+	// Advances search one step 
+	unsigned int SearchStep()
+	{
+		// Firstly break if the user has not initialised the search
+		assert( (m_State > SEARCH_STATE_NOT_INITIALISED) &&
+				(m_State < SEARCH_STATE_INVALID) );
+
+		// Next I want it to be safe to do a searchstep once the search has succeeded...
+		if( (m_State == SEARCH_STATE_SUCCEEDED) ||
+			(m_State == SEARCH_STATE_FAILED) 
+		  )
+		{
+			return m_State; 
+		}
+
+		// Failure is defined as emptying the open list as there is nothing left to 
+		// search...
+		// New: Allow user abort
+		if( m_OpenList.empty() || m_CancelRequest )
+		{
+			FreeAllNodes();
+			m_State = SEARCH_STATE_FAILED;
+			return m_State;
+		}
+		
+		// Incremement step count
+		m_Steps ++;
+
+		// Pop the best node (the one with the lowest f) 
+		Node *n = m_OpenList.front(); // get pointer to the node
+		pop_heap( m_OpenList.begin(), m_OpenList.end(), HeapCompare_f() );
+		m_OpenList.pop_back();
+
+		// Check for the goal, once we pop that we're done
+		if( n->m_UserState.IsGoal( m_Goal->m_UserState ) )
+		{
+			// The user is going to use the Goal Node he passed in 
+			// so copy the parent pointer of n 
+			m_Goal->parent = n->parent;
+			m_Goal->g = n->g;
+
+			// A special case is that the goal was passed in as the start state
+			// so handle that here
+			if( false == n->m_UserState.IsSameState( m_Start->m_UserState ) )
+			{
+				FreeNode( n );
+
+				// set the child pointers in each node (except Goal which has no child)
+				Node *nodeChild = m_Goal;
+				Node *nodeParent = m_Goal->parent;
+
+				do 
+				{
+					nodeParent->child = nodeChild;
+
+					nodeChild = nodeParent;
+					nodeParent = nodeParent->parent;
+				
+				} 
+				while( nodeChild != m_Start ); // Start is always the first node by definition
+
+			}
+
+			// delete nodes that aren't needed for the solution
+			FreeUnusedNodes();
+
+			m_State = SEARCH_STATE_SUCCEEDED;
+
+			return m_State;
+		}
+		else // not goal
+		{
+
+			// We now need to generate the successors of this node
+			// The user helps us to do this, and we keep the new nodes in
+			// m_Successors ...
+
+			m_Successors.clear(); // empty vector of successor nodes to n
+
+			// User provides this functions and uses AddSuccessor to add each successor of
+			// node 'n' to m_Successors
+			bool ret = n->m_UserState.GetSuccessors( this, n->parent ? &n->parent->m_UserState : NULL ); 
+
+			if( !ret )
+			{
+
+			    typename vector< Node * >::iterator successor;
+
+				// free the nodes that may previously have been added 
+				for( successor = m_Successors.begin(); successor != m_Successors.end(); successor ++ )
+				{
+					FreeNode( (*successor) );
+				}
+
+				m_Successors.clear(); // empty vector of successor nodes to n
+
+				// free up everything else we allocated
+				FreeNode( (n) );
+				FreeAllNodes();
+
+				m_State = SEARCH_STATE_OUT_OF_MEMORY;
+				return m_State;
+			}
+			
+			// Now handle each successor to the current node ...
+			for( typename vector< Node * >::iterator successor = m_Successors.begin(); successor != m_Successors.end(); successor ++ )
+			{
+
+				// 	The g value for this successor ...
+				float newg = n->g + n->m_UserState.GetCost( (*successor)->m_UserState );
+
+				// Now we need to find whether the node is on the open or closed lists
+				// If it is but the node that is already on them is better (lower g)
+				// then we can forget about this successor
+
+				// First linear search of open list to find node
+
+				typename vector< Node * >::iterator openlist_result;
+
+				for( openlist_result = m_OpenList.begin(); openlist_result != m_OpenList.end(); openlist_result ++ )
+				{
+					if( (*openlist_result)->m_UserState.IsSameState( (*successor)->m_UserState ) )
+					{
+						break;					
+					}
+				}
+
+				if( openlist_result != m_OpenList.end() )
+				{
+
+					// we found this state on open
+
+					if( (*openlist_result)->g <= newg )
+					{
+						FreeNode( (*successor) );
+
+						// the one on Open is cheaper than this one
+						continue;
+					}
+				}
+
+				typename vector< Node * >::iterator closedlist_result;
+
+				for( closedlist_result = m_ClosedList.begin(); closedlist_result != m_ClosedList.end(); closedlist_result ++ )
+				{
+					if( (*closedlist_result)->m_UserState.IsSameState( (*successor)->m_UserState ) )
+					{
+						break;					
+					}
+				}
+
+				if( closedlist_result != m_ClosedList.end() )
+				{
+
+					// we found this state on closed
+
+					if( (*closedlist_result)->g <= newg )
+					{
+						// the one on Closed is cheaper than this one
+						FreeNode( (*successor) );
+
+						continue;
+					}
+				}
+
+				// This node is the best node so far with this particular state
+				// so lets keep it and set up its AStar specific data ...
+
+				(*successor)->parent = n;
+				(*successor)->g = newg;
+				(*successor)->h = (*successor)->m_UserState.GoalDistanceEstimate( m_Goal->m_UserState );
+				(*successor)->f = (*successor)->g + (*successor)->h;
+
+				// Successor in closed list
+				// 1 - Update old version of this node in closed list
+				// 2 - Move it from closed to open list
+				// 3 - Sort heap again in open list
+
+				if( closedlist_result != m_ClosedList.end() )
+				{
+					// Update closed node with successor node AStar data
+					//*(*closedlist_result) = *(*successor);
+					(*closedlist_result)->parent = (*successor)->parent;
+					(*closedlist_result)->g      = (*successor)->g;
+					(*closedlist_result)->h      = (*successor)->h;
+					(*closedlist_result)->f      = (*successor)->f;
+
+					// Free successor node
+					FreeNode( (*successor) );
+
+					// Push closed node into open list 
+					m_OpenList.push_back( (*closedlist_result) );
+
+					// Remove closed node from closed list
+					m_ClosedList.erase( closedlist_result );
+
+					// Sort back element into heap
+					push_heap( m_OpenList.begin(), m_OpenList.end(), HeapCompare_f() );
+
+					// Fix thanks to ...
+					// Greg Douglas <gregdouglasmail@gmail.com>
+					// who noticed that this code path was incorrect
+					// Here we have found a new state which is already CLOSED
+
+				}
+
+				// Successor in open list
+				// 1 - Update old version of this node in open list
+				// 2 - sort heap again in open list
+
+				else if( openlist_result != m_OpenList.end() )
+				{
+					// Update open node with successor node AStar data
+					//*(*openlist_result) = *(*successor);
+					(*openlist_result)->parent = (*successor)->parent;
+					(*openlist_result)->g      = (*successor)->g;
+					(*openlist_result)->h      = (*successor)->h;
+					(*openlist_result)->f      = (*successor)->f;
+
+					// Free successor node
+					FreeNode( (*successor) );
+
+					// re-make the heap 
+					// make_heap rather than sort_heap is an essential bug fix
+					// thanks to Mike Ryynanen for pointing this out and then explaining
+					// it in detail. sort_heap called on an invalid heap does not work
+					make_heap( m_OpenList.begin(), m_OpenList.end(), HeapCompare_f() );
+				}
+
+				// New successor
+				// 1 - Move it from successors to open list
+				// 2 - sort heap again in open list
+
+				else
+				{
+					// Push successor node into open list
+					m_OpenList.push_back( (*successor) );
+
+					// Sort back element into heap
+					push_heap( m_OpenList.begin(), m_OpenList.end(), HeapCompare_f() );
+				}
+
+			}
+
+			// push n onto Closed, as we have expanded it now
+
+			m_ClosedList.push_back( n );
+
+		} // end else (not goal so expand)
+
+ 		return m_State; // Succeeded bool is false at this point. 
+
+	}
+
+	// User calls this to add a successor to a list of successors
+	// when expanding the search frontier
+	bool AddSuccessor( UserState &State )
+	{
+		Node *node = AllocateNode();
+
+		if( node )
+		{
+			node->m_UserState = State;
+
+			m_Successors.push_back( node );
+
+			return true;
+		}
+
+		return false;
+	}
+
+	// Free the solution nodes
+	// This is done to clean up all used Node memory when you are done with the
+	// search
+	void FreeSolutionNodes()
+	{
+		Node *n = m_Start;
+
+		if( m_Start->child )
+		{
+			do
+			{
+				Node *del = n;
+				n = n->child;
+				FreeNode( del );
+
+				del = NULL;
+
+			} while( n != m_Goal );
+
+			FreeNode( n ); // Delete the goal
+
+		}
+		else
+		{
+			// if the start node is the solution we need to just delete the start and goal
+			// nodes
+			FreeNode( m_Start );
+			FreeNode( m_Goal );
+		}
+
+	}
+
+	// Functions for traversing the solution
+
+	// Get start node
+	UserState *GetSolutionStart()
+	{
+		m_CurrentSolutionNode = m_Start;
+		if( m_Start )
+		{
+			return &m_Start->m_UserState;
+		}
+		else
+		{
+			return NULL;
+		}
+	}
+	
+	// Get next node
+	UserState *GetSolutionNext()
+	{
+		if( m_CurrentSolutionNode )
+		{
+			if( m_CurrentSolutionNode->child )
+			{
+
+				Node *child = m_CurrentSolutionNode->child;
+
+				m_CurrentSolutionNode = m_CurrentSolutionNode->child;
+
+				return &child->m_UserState;
+			}
+		}
+
+		return NULL;
+	}
+	
+	// Get end node
+	UserState *GetSolutionEnd()
+	{
+		m_CurrentSolutionNode = m_Goal;
+		if( m_Goal )
+		{
+			return &m_Goal->m_UserState;
+		}
+		else
+		{
+			return NULL;
+		}
+	}
+	
+	// Step solution iterator backwards
+	UserState *GetSolutionPrev()
+	{
+		if( m_CurrentSolutionNode )
+		{
+			if( m_CurrentSolutionNode->parent )
+			{
+
+				Node *parent = m_CurrentSolutionNode->parent;
+
+				m_CurrentSolutionNode = m_CurrentSolutionNode->parent;
+
+				return &parent->m_UserState;
+			}
+		}
+
+		return NULL;
+	}
+
+	// Get final cost of solution
+	// Returns FLT_MAX if goal is not defined or there is no solution
+	float GetSolutionCost()
+	{
+		if( m_Goal && m_State == SEARCH_STATE_SUCCEEDED )
+		{
+			return m_Goal->g;
+		}
+		else
+		{
+			return FLT_MAX;
+		}
+	}
+
+	// For educational use and debugging it is useful to be able to view
+	// the open and closed list at each step, here are two functions to allow that.
+
+	UserState *GetOpenListStart()
+	{
+		float f,g,h;
+		return GetOpenListStart( f,g,h );
+	}
+
+	UserState *GetOpenListStart( float &f, float &g, float &h )
+	{
+		iterDbgOpen = m_OpenList.begin();
+		if( iterDbgOpen != m_OpenList.end() )
+		{
+			f = (*iterDbgOpen)->f;
+			g = (*iterDbgOpen)->g;
+			h = (*iterDbgOpen)->h;
+			return &(*iterDbgOpen)->m_UserState;
+		}
+
+		return NULL;
+	}
+
+	UserState *GetOpenListNext()
+	{
+		float f,g,h;
+		return GetOpenListNext( f,g,h );
+	}
+
+	UserState *GetOpenListNext( float &f, float &g, float &h )
+	{
+		iterDbgOpen++;
+		if( iterDbgOpen != m_OpenList.end() )
+		{
+			f = (*iterDbgOpen)->f;
+			g = (*iterDbgOpen)->g;
+			h = (*iterDbgOpen)->h;
+			return &(*iterDbgOpen)->m_UserState;
+		}
+
+		return NULL;
+	}
+
+	UserState *GetClosedListStart()
+	{
+		float f,g,h;
+		return GetClosedListStart( f,g,h );
+	}
+
+	UserState *GetClosedListStart( float &f, float &g, float &h )
+	{
+		iterDbgClosed = m_ClosedList.begin();
+		if( iterDbgClosed != m_ClosedList.end() )
+		{
+			f = (*iterDbgClosed)->f;
+			g = (*iterDbgClosed)->g;
+			h = (*iterDbgClosed)->h;
+
+			return &(*iterDbgClosed)->m_UserState;
+		}
+
+		return NULL;
+	}
+
+	UserState *GetClosedListNext()
+	{
+		float f,g,h;
+		return GetClosedListNext( f,g,h );
+	}
+
+	UserState *GetClosedListNext( float &f, float &g, float &h )
+	{
+		iterDbgClosed++;
+		if( iterDbgClosed != m_ClosedList.end() )
+		{
+			f = (*iterDbgClosed)->f;
+			g = (*iterDbgClosed)->g;
+			h = (*iterDbgClosed)->h;
+
+			return &(*iterDbgClosed)->m_UserState;
+		}
+
+		return NULL;
+	}
+
+	// Get the number of steps
+
+	int GetStepCount() { return m_Steps; }
+
+	void EnsureMemoryFreed()
+	{
+#if USE_FSA_MEMORY
+		assert(m_AllocateNodeCount == 0);
+#endif
+
+	}
+
+private: // methods
+
+	// This is called when a search fails or is cancelled to free all used
+	// memory 
+	void FreeAllNodes()
+	{
+		// iterate open list and delete all nodes
+		typename vector< Node * >::iterator iterOpen = m_OpenList.begin();
+
+		while( iterOpen != m_OpenList.end() )
+		{
+			Node *n = (*iterOpen);
+			FreeNode( n );
+
+			iterOpen ++;
+		}
+
+		m_OpenList.clear();
+
+		// iterate closed list and delete unused nodes
+		typename vector< Node * >::iterator iterClosed;
+
+		for( iterClosed = m_ClosedList.begin(); iterClosed != m_ClosedList.end(); iterClosed ++ )
+		{
+			Node *n = (*iterClosed);
+			FreeNode( n );
+		}
+
+		m_ClosedList.clear();
+
+		// delete the goal
+
+		FreeNode(m_Goal);
+	}
+
+
+	// This call is made by the search class when the search ends. A lot of nodes may be
+	// created that are still present when the search ends. They will be deleted by this 
+	// routine once the search ends
+	void FreeUnusedNodes()
+	{
+		// iterate open list and delete unused nodes
+		typename vector< Node * >::iterator iterOpen = m_OpenList.begin();
+
+		while( iterOpen != m_OpenList.end() )
+		{
+			Node *n = (*iterOpen);
+
+			if( !n->child )
+			{
+				FreeNode( n );
+
+				n = NULL;
+			}
+
+			iterOpen ++;
+		}
+
+		m_OpenList.clear();
+
+		// iterate closed list and delete unused nodes
+		typename vector< Node * >::iterator iterClosed;
+
+		for( iterClosed = m_ClosedList.begin(); iterClosed != m_ClosedList.end(); iterClosed ++ )
+		{
+			Node *n = (*iterClosed);
+
+			if( !n->child )
+			{
+				FreeNode( n );
+				n = NULL;
+
+			}
+		}
+
+		m_ClosedList.clear();
+
+	}
+
+	// Node memory management
+	Node *AllocateNode()
+	{
+
+#if !USE_FSA_MEMORY
+		m_AllocateNodeCount ++;
+		Node *p = new Node;
+		return p;
+#else
+		Node *address = m_FixedSizeAllocator.alloc();
+
+		if( !address )
+		{
+			return NULL;
+		}
+		m_AllocateNodeCount ++;
+		Node *p = new (address) Node;
+		return p;
+#endif
+	}
+
+	void FreeNode( Node *node )
+	{
+
+		m_AllocateNodeCount --;
+
+#if !USE_FSA_MEMORY
+		delete node;
+#else
+		node->~Node();
+		m_FixedSizeAllocator.free( node );
+#endif
+	}
+
+private: // data
+
+	// Heap (simple vector but used as a heap, cf. Steve Rabin's game gems article)
+	vector< Node *> m_OpenList;
+
+	// Closed list is a vector.
+	vector< Node * > m_ClosedList; 
+
+	// Successors is a vector filled out by the user each type successors to a node
+	// are generated
+	vector< Node * > m_Successors;
+
+	// State
+	unsigned int m_State;
+
+	// Counts steps
+	int m_Steps;
+
+	// Start and goal state pointers
+	Node *m_Start;
+	Node *m_Goal;
+
+	Node *m_CurrentSolutionNode;
+
+#if USE_FSA_MEMORY
+	// Memory
+ 	FixedSizeAllocator<Node> m_FixedSizeAllocator;
+#endif
+	
+	//Debug : need to keep these two iterators around
+	// for the user Dbg functions
+	typename vector< Node * >::iterator iterDbgOpen;
+	typename vector< Node * >::iterator iterDbgClosed;
+
+	// debugging : count memory allocation and free's
+	int m_AllocateNodeCount;
+	
+	bool m_CancelRequest;
+
+};
+
+template <class T> class AStarState
+{
+public:
+	virtual ~AStarState() {}
+	virtual float GoalDistanceEstimate( T &nodeGoal ) = 0; // Heuristic function which computes the estimated cost to the goal node
+	virtual bool IsGoal( T &nodeGoal ) = 0; // Returns true if this node is the goal node
+	virtual bool GetSuccessors( AStarSearch<T> *astarsearch, T *parent_node ) = 0; // Retrieves all successors to this node and adds them via astarsearch.addSuccessor()
+	virtual float GetCost( T &successor ) = 0; // Computes the cost of travelling from this node to the successor node
+	virtual bool IsSameState( T &rhs ) = 0; // Returns true if this node is the same as the rhs node
+};
+
+#endif
+
+