Updated A* algorithm