// A C++ program for Dijkstra's single source shortest path algorithm.
// The program is for adjacency matrix representation of the graph

#include <stdio.h>
#include <limits.h>

// Number of vertices in the graph
const int V = 16;
const int IN_MAX = 9999;

// A utility function to find the vertex with minimum distance value, from
// the set of vertices not yet included in shortest path tree
int minDistance(int dist[], bool sptSet[])
{
   // Initialize min value
   int min = INT_MAX, min_index;

   for (int v = 0; v < V; v++)
     if (sptSet[v] == false && dist[v] <= min)
         min = dist[v], min_index = v;

   return min_index;
}

// A utility function to print the constructed distance array
int printSolution(int dist[], int n)
{
   Serial.print("Intersections        /  Distance de la source en milimètres\n");
   for (int i = 0; i < V; i++){
      Serial.print("");
      Serial.print(i);
      Serial.print("           / ");
      Serial.println(dist[i]);
   }
}

// Function that implements Dijkstra's single source shortest path algorithm
// for a graph represented using adjacency matrix representation
void dijkstra(int graph[V][V], int src)
{
     int dist[V];     // The output array.  dist[i] will hold the shortest
                      // distance from src to i

     bool sptSet[V]; // sptSet[i] will be true if vertex i is included in shortest
                     // path tree or shortest distance from src to i is finalized

     // Initialize all distances as INFINITE and stpSet[] as false
     for (int i = 0; i < V; i++)
        dist[i] = INT_MAX, sptSet[i] = false;

     // Distance of source vertex from itself is always 0
     dist[src] = 0;

     // Find shortest path for all vertices
     for (int count = 0; count < V-1; count++)
     {
       // Pick the minimum distance vertex from the set of vertices not
       // yet processed. u is always equal to src in the first iteration.
       int u = minDistance(dist, sptSet);

       // Mark the picked vertex as processed
       sptSet[u] = true;

       // Update dist value of the adjacent vertices of the picked vertex.
       for (int v = 0; v < V; v++)

         // Update dist[v] only if is not in sptSet, there is an edge from
         // u to v, and total weight of path from src to  v through u is
         // smaller than current value of dist[v]
         if (!sptSet[v] && graph[u][v] && dist[u] != INT_MAX
                                       && dist[u]+graph[u][v] < dist[v])
            dist[v] = dist[u] + graph[u][v];
     }

     // print the constructed distance array
     printSolution(dist, V);
}

// driver program to test above function
void setup(){
   Serial.begin(9600);

}
void loop()
{
   /* Let us create the example graph discussed above */
   int graph[V][V] = {  {0 , 1700 , 0  , 0  , 0  , 0  , 0  , 0  , 0  , 0  , 0   , 0  , 0   , 0 , 0  , 2790 },
  {1700, 0  , 1380 , 0  , 0  , 0  , 0  , 0  , 0  , 0  , 11480 , 0  , 0   , 0  , 0  , 0  },
  {0 , 1380 , 0  , 1160 , 0  , 2080 , 0  , 0  , 0  , 0  , 0   , 0  , 0   , 0  , 0 , 0  },
  {0 , 0  , 1160 , 0  , 2080 , 0  , 0  , 0  , 0  , 0  , 0   , 0  , 0   , 0  , 0  , 0  },
  {0 , 0  , 0  , 2080 , 0  , 1160 , 0  , 0  , 6820 , 0  , 0   , 0  , 0  , 0  , 0  , 0  },
  {0 , 0  , 2080 , 0  , 1160 , 0  , 0  , 6820 , 0  , 0  , 0   , 0  , 0   , 0  , 0  , 0  },
  {0 , 0  , 0  , 0  , 0  , 0  , 0  , 0  , 0  , 0  , 0   , 8670 , 0   , 0  , 0  , 920 },
  {0 , 0  , 0  , 0  , 0  , 6820 , 0  , 0  , 1040 , 0  , 0   , 0  , 0   , 0  , 0  , 0  },
  {0 , 0 , 0  , 0  , 6820 , 0  , 0  , 1040 , 0  , 0  , 0   , 0  , 0   , 0  , 0  , 0  },
  {0 , 0  , 0  , 0  , 0  , 0  , 0  , 0  , 2600 , 0  , 2570  , 0  , 0   , 0  , 1300 , 0  },
  {0 , 11480, 0  , 0  , 0 , 0  , 0  , 0  , 0  , 2570 , 0   , 670 , 0   , 1320 , 0  , 0  },
  {0 , 0  , 0  , 0  , 0  , 0  , 8670 , 0  , 0  , 0  , 670  , 0  , 0   , 0  , 0  , 0  },
  {0 , 0  , 0  , 0  , 0  , 0  , 0  , 0  , 0  , 0  , 0   , 0  , 0   , 4160 , 0  , 10010},
  {0, 0  , 0  , 0  , 0  , 0  , 0  , 0  , 0  , 0  , 1320  , 0  , 1580  , 0  , 2580 , 0  },
  {0 , 0  , 0  , 0  , 0  , 0  , 0  , 0  , 0  , 1300  , 0   , 0  , 0   , 2580 , 0  , 0  },
  {2790, 0  , 0  , 0  , 0  , 0  , 920 , 0  , 0  , 0  , 0   , 0  , 10010 , 0  , 0  , 0  }
                     };

    dijkstra(graph, 1);
   delay(50000);
}