Astar search method: improved