Fixed the A* algorithm