fix bugs in astar algorithm