AStar in Java