Rewriting for my Dijkstra algorithm - C#