A* algorithm fix