Как найти кратчайший путь?

queue<int> q;
q.push (s);
vector<bool> used (n);
vector<int> d (n), p (n);
used[s] = true;
p[s] = -1;
while (!q.empty()) {
    int v = q.front();
    q.pop();
    for (size_t i=0; i<g[v].size(); ++i) {
        int to = g[v][i];
        if (!used[to]) {
            used[to] = true;
            q.push (to);
            d[to] = d[v] + 1;
            p[to] = v;
        }
    }
}

const int INF = 1000000000;
 
int main() {
    int n;
    ... чтение n ...
    vector < vector < pair<int,int> > > g (n);
    ... чтение графа ...
    int s = ...; // стартовая вершина
 
    vector<int> d (n, INF),  p (n);
    d[s] = 0;
    vector<char> u (n);
    for (int i=0; i<n; ++i) {
        int v = -1;
        for (int j=0; j<n; ++j)
            if (!u[j] && (v == -1 || d[j] < d[v]))
                v = j;
        if (d[v] == INF)
            break;
        u[v] = true;
 
        for (size_t j=0; j<g[v].size(); ++j) {
            int to = g[v][j].first,
                len = g[v][j].second;
            if (d[v] + len < d[to]) {
                d[to] = d[v] + len;
                p[to] = v;
            }
        }
    }
}

int A[5][5] = {
{7, 0, 0, 0, 0},
{3, 8, 0, 0, 0},
{8, 1, 0, 0, 0},
{2, 7, 4, 4, 0},
{4, 5, 2, 6, 5}
    };