Heapq based implementation passes tests. Still slow though as we search through enumerate(dist_so_far)...