Shortest path algorithm implemented in C#