AStarSearch implementation