using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading.Tasks;

namespace Dijkstra
{
    class Program
    {
        static void Main(string[] args)
        {
            int INF = Dijkstra.INF;
            int[,] graph = new int[,]{
            {INF,   1, INF, INF,   6},
            {  4, INF,   1, INF,   8},
            {INF, INF, INF,   2, INF},
            {INF, INF, INF, INF,   1},
            {INF,   5, INF, INF, INF}};

            var dijkstra = new Dijkstra(graph);
            int[,] ścieżki = {
                            { 1, 4 },
                            { 3, 0 },
                            { 0, 2 },
                            { 2, 3 }
                            };
            for (int i = 0; i < ścieżki.GetLength(0); i++)
            {
                int src = ścieżki[i, 0],
                    dest = ścieżki[i, 1];
                string path = ŚcieżkaDoStringa(dijkstra.PobierzŚcieżkę(src, dest));
                Console.WriteLine("Ścieżka {0} - {1}: {2}", src, dest, path);
            }
            Console.ReadKey();
        }


        private static string ŚcieżkaDoStringa(int[] route)
        {
            if (route != null)
               return String.Join(" => ", route);
            else
                return "Brak";
        }
    }

    public class Dijkstra
    {
        public const int INF = 1000000;
        public int[,] Graph { get; set; }
        public Dijkstra(int[,] graph)
        {
            Graph = graph;
        }

        public int[] PobierzŚcieżkę(int SRC, int DEST)
        {
            int rozmiarGrafu = Graph.GetLength(0);
            int[] dist = new int[rozmiarGrafu];
            int[] prev = new int[rozmiarGrafu];
            int[] nodes = new int[rozmiarGrafu];

            for (int i = 0; i < dist.Length; i++)
            {
                dist[i] = prev[i] = INF;
                nodes[i] = i;
            }

            dist[SRC] = 0;
            do
            {
                int smallest = nodes[0];
                int smallestIndex = 0;
                for (int i = 1; i < rozmiarGrafu; i++)
                {
                    if (dist[nodes[i]] < dist[smallest])
                    {
                        smallest = nodes[i];
                        smallestIndex = i;
                    }
                }
                rozmiarGrafu--;
                nodes[smallestIndex] = nodes[rozmiarGrafu];

                if (dist[smallest] == INF || smallest == DEST)
                    break;

                for (int i = 0; i < rozmiarGrafu; i++)
                {
                    int v = nodes[i];
                    int newDist = dist[smallest] + Graph[smallest, v];
                    if (newDist < dist[v])
                    {
                        dist[v] = newDist;
                        prev[v] = smallest;
                    }
                }
            } while (rozmiarGrafu > 0);
            return odtwórzŚcieżkę(prev, SRC, DEST);
        }

        public int[] odtwórzŚcieżkę(int[] prev, int SRC, int DEST)
        {
            int[] ret = new int[prev.Length];
            int currentNode = 0;
            ret[currentNode] = DEST;
            while (ret[currentNode] != INF && ret[currentNode] != SRC)
            {
                ret[currentNode + 1] = prev[ret[currentNode]];
                currentNode++;
            }
            if (ret[currentNode] != SRC)
                return null;
            int[] reversed = new int[currentNode + 1];
            for (int i = currentNode; i >= 0; i--)
                reversed[currentNode - i] = ret[i];
            return reversed;
        }
    }
}