astar_pv finds the shortests path