Simple Python shortest path first algorithm