 <p>There might be other problems in your snippet, so far I've noticed you're using <code>std::map</code> which is unusual data-structure for this purpose as <code>std::map</code> will overwrite previous <code>nodeId</code> with same <code>weight</code>. You can use <code>std::multimap</code> but you will need some more <code>std::iterator</code> and stuffs for looping on key-value pairs. Use <code>std::priority_queue</code> which will allow you to keep the code precise.</p> <p>Here is a simple snippet solution with <code>std::priority_queue</code> which calculate shortest distance from source to destination node. You can modify or write similarly for your problems:</p> <pre><code>#define Max 20010 // MAX is the maximum nodeID possible vector &lt;pair&lt;int, int&gt;&gt; adj[Max]; // adj[u] contains the adjacent nodes of node u int dis[Max]; // dis[i] is the distance from source to node i priority_queue &lt;pair&lt;int, int&gt;, vector&lt;pair&lt;int, int&gt; &gt;, greater&lt;pair&lt;int, int&gt; &gt; &gt; Q; int dijkstra(int src, int des) { memset(dis, MAX, sizeof dis); Q = priority_queue &lt;pair&lt;int, int&gt;, vector&lt;pair&lt;int, int&gt; &gt;, greater&lt;pair&lt;int, int&gt; &gt; &gt; (); dis[src] = 0; Q.push({dis[src] , src}); while(!Q.empty()) { int u = Q.top().second; Q.pop(); if(u == des) { return dis[des]; } for(int i = 0; i &lt; (int)adj[u].size(); ++i) { int v = adj[u][i].first; int cost = adj[u][i].second; if(dis[v] &gt; dis[u] + cost) { dis[v] = dis[u] + cost; Q.push({dis[v], v)}); } } } return -1; // no path found } </code></pre>