Pathfinding with A* problem

import java.awt.Point;

public class Main 
{	
	public static void main(String[] args)
	{
		//dummy map file
		/*
		 * a = start location
		 * b = target location
		 * 0 = walkable path
		 * 1 = walls, impassible
		 * -----------
		 * a 0 0
		 * 1 1 0
		 * b 1 0
		 * 0 0 0 
		 * -----------
		 */
		int[][] map = new int[][]{{0,0,0},{1,1,0},{0,1,0},{0,0,0}};

		//new instance of the AStar class, with the test map
		AStar astar = new AStar(map);
		
		//set the start location and the target location
		AStar.AStarNode a = astar.new AStarNode(0,0,null);
		AStar.AStarNode b = astar.new AStarNode(0,2,null);
		
		//find path between the two
		astar.findPath(a,b);
			
		//print the final path if there is one.
		astar.printPath();		
	}
}

import java.awt.Point;
import java.util.ArrayList;
import java.util.Iterator;

public class AStar 
{
	//Necessary lists for the algorithm
	private ArrayList openList, closedList, solution, neighbors;
	
	//starting and target nodes
	private AStarNode startNode, goalNode;
	
	//the map of tiles with information on weather the tile is walkable or not
	private int[][] tileMap;
	
	/*
	 * Constructs a new AStar object
	 * @param m is the tile map representing tile information for each tile such as 1, 0 , etc
	 * Currently only 0 values are considered to be passable terrain
	 */
	public AStar(int[][] m)
	{	
		tileMap = new int[m.length][m[0].length];
		for(int i = 0; i < m.length; i++)
		{
			for(int j = 0; j < m[0].length; j++)
			{
				tileMap[j] = m[j];
			}
		}
	}
	
	/*
	 * Methor for finding a path between two nodes
	 * @param A the starting node
	 * @param B the target node
	 */
	public void findPath(AStarNode a, AStarNode b)
	{
		//initialize necessary lists for the algorithm
		openList = new ArrayList();//holds nodes that are to be considered as a possible path to take.
		closedList = new ArrayList();//holds nodes that are already considered.
		solution = new ArrayList();//holds the final path that lead from the starting node to the target node
		neighbors = new ArrayList();//holds neighbors for the current node that is being evaluated,
									//starts fresh everytime we are considering a new node, as it will have new neighbors.
									//each node will have eight neighbors unless the neighbor is an impassible tile or
									//it is out of the map as it would occur on tiles that are at the edges of the map
		
		//set the start node and the target node;
		startNode = a;
		goalNode = b;
		
		//add the start node to the list of open nodes as we prepare to start our search,
		//this list holds nodes that will be considered for the path we will take.
		openList.add(startNode);
		
		//while the open list is not empty, grab the next node in the list with the lowest cost
		while(!openList.isEmpty())
		{
			//if the we've considered all the nodes on the map, and still havent found the target, give up.
			if(closedList.size() == (tileMap.length*tileMap[0].length)-1)
				break;			
			
			//Get the node with the lowest TotalCost from the open list, this is now our current node.
			AStarNode NodeCurrent = getLowestCostNode();			
			
			// If this current node is the same as the target node, we have reached our goal.
			// they are only the same if their x and y values match.
			// copy the path we took to get this target node to the solution list
			if(NodeCurrent.isGoal()) 
			{
				while(NodeCurrent != null) {
					solution.add(0,NodeCurrent);
					NodeCurrent = NodeCurrent.parent;
				}
				//Done, QUIT!
				break;					
			}			
			/*OTHERWISE*/
			//Get all of the surrounding neighbors of the current node, and add them to the neighbors list.
			//The neighbors list is renewed everytime getneighbors method is called, so there is no accumulation.
			//get neighbors gets all the surrounding nodes unless impassible.
			//The impassible status is determinded according to the tileMap.
			NodeCurrent.getNeighbors();

			//for each node in the neighbors list, do the following:
			Iterator it = neighbors.iterator();
			while(it.hasNext())
			{
				//Set the node as our potential successor (the next node to be added to the open list)
				AStarNode successor = (AStarNode)it.next();
				
				// Test if the currect successor node is already on the open list, if it is and
				// the TotalCost is higher than the one we already have on the open list,
				// we will throw away this current successor node.
				AStarNode nodeOpen = null;
				if(openList.contains(successor))
					nodeOpen = (AStarNode)openList.get(openList.indexOf(successor));
				if((nodeOpen != null) && (successor.getCost() > nodeOpen.getCost())) 
					continue;
				
				//Test if the currect successor node is on the closed list, if it is and
				// the TotalCost is higher, we will throw away this current successor.
				AStarNode nodeClosed = null;
				if(closedList.contains(successor))
					nodeClosed = (AStarNode)closedList.get(closedList.indexOf(successor));
				if((nodeClosed != null) && (successor.getCost() > nodeClosed.getCost())) 
					continue;
				
		
				
				//if the successor was already on the open list
				//but the one in the list (old successor) had a higher cost value
				//remove the old successor from the open list
				//and two lines below: replace it with the current successor.
				openList.remove(nodeOpen);

				//if the successor was already on the closed list
				//but the one in the list (old successor) had a higher cost value
				//remove the old successor from the closed list
				//and a line below: replace it with the current successor.
				closedList.remove(nodeClosed);
				
				/*
				pay attention to the fact that the remove(nodeOpen) only works if the nodeOpen is not null,
				and nodeOpen is not null if and only if it was already on one of the lists before.
				This means they are the same node, but with different paths leading to them so
				they have different cost associated with them.
				This node is checked against the one on the list, comparing their total cost.				 
				the one with the lower cost replaces the other, since it has a better path leading to it.
				sorry about the long explanation, if you read on A* algorithm, it will make more sense.
				*/
				
				// Add the current successor to the open list
				openList.add(successor);				
			}		
			
			//closed list holds the already considered nodes.
			//we are done evaluating this node, so we can safely add it to the closed list.			
			closedList.add(NodeCurrent);			
		}
	}
	
	
	
	/*
	 * iterates through the list of open nodes
	 * returns the one with the lowest cost.
	 */
	public AStarNode getLowestCostNode()
	{
		int f = Integer.MAX_VALUE;
		AStarNode node = null;		
		Iterator it = openList.iterator();		
		while(it.hasNext())
		{
			node = (AStarNode)it.next();
			if(node.getCost() < f)
				f = node.getCost();
		}
		return node;
	}
	
	/*
	 * @return The arraylist holding
	 * the final path leading to the target node if it is not empty.
	 * If it is empty, it means there is no path.
	 */
	public ArrayList getPath()
	{
		if(!solution.isEmpty())
			return solution;
		return null;			
	}
	
	/*
	 * prints the final path leading to the target node if there is a path.
	 */
	public void printPath()
	{		
		if(!solution.isEmpty())
		{
			System.out.println("Printing Path...");
			Iterator it = solution.iterator();
			while(it.hasNext())
			{
				AStarNode n = (AStarNode)it.next();
				System.out.println(n);
			}
		}
		else
		{
			System.out.println("Path is Empty");
		}
	}
	
	/*An inner class representing the node*/
	public class AStarNode
	{
		//row, column
		public int x,y;
		//cost variables
		public int g,h;
		//the parent node to this node
		public AStarNode parent;
		//the unique id of this node for comparing
		public Point ID;
		//the terrain type of this node
		public int type;
		
		/*
		 * Constructs a new node.
		 * @param x the Row location of this node on the tileMap.
		 * @param y the Column location of this node on the tileMap.
		 * @param parent the parent node that is leading to this node,
		 * it is null if we are the starting node.
		 */
		public AStarNode(int x, int y, AStarNode parent)
		{
			//parent node
			this.parent = parent;
			
			//set this nodes row, column info
			this.x = x;
			this.y = y;
			
			//ID, for comparing
			ID = new Point(x,y);

			//movement cost to this node
			g = 0;
			
			//heuristic cost, set using manhattan approach
			h = Math.abs(goalNode.x-x)+Math.abs(goalNode.y - y);

			//this nodes terrain type accordinig to the tileMap, 0,1,etc...
			type = tileMap[x][y];		
		}
		
		/*
		 * compares the ID of the param to this node's ID
		 */
		public boolean equals(Object o)
		{
			return ID.equals(((AStarNode)o).ID);
		}
		
		/*
		 * The string representation of this node
		 */
		public String toString()
		{
			return "X: " + x + " Y: " + y;
		}
		
		/*
		 * return the total cost of this node
		 * f=g+h
		 */		
		public int getCost()
		{
			return g+h;			
		}
		
		/*
		 * checks to see if this node is the same as the target node
		 */
		public boolean isGoal()
		{
			return ID.equals(goalNode.ID);
		}
		
		/*
		 * Gets all the surrounding neighbors of this node, 
		 * ignoring itself, its parent and the tiles that are impassible.
		 */
		public void getNeighbors()
		{			
			neighbors = new ArrayList();
			for(int b = y - 1; b <= y + 1; b++)
			{
				for(int a = x-1; a <= x + 1; a++)
				{
					//if a and b is on the map					
					if(a < tileMap.length && b< tileMap[0].length && a > -1 && b >-1)
					{
						//create a node with its parent being this
						AStarNode n = new AStarNode(a,b,this);
						//set the newly created nodes movement cost value
						n.g = g + 10;						
						//if we are the first node, we don't have a parent
						if(parent == null)
						{
							//so just make sure it isn't us (x,y) we are trying to add, and make sure it is walkable
							if(!ID.equals(n.ID) && tileMap[a] == 0)
							{
								//add to neighbors
								neighbors.add(n);
							}
							
						}
						else //if we are not the first node, we have a parent
						{
							//if the node we are trying to add is not our parent and it is not us, and it is walkable						
							if(!ID.equals(n.ID) && !parent.ID.equals(n.ID) && tileMap[a]==0)
							{
								//add to neighbors
								neighbors.add(n);								
							}							
						}	
					}
					
				}
			}
		}
		
	}

}