Improved A* algorithm