A* algorithm in place