Problem p00528 (C++)

Buggy submission (status: Runtime Error):
#include <algorithm>
#include <iostream>
#include <queue>
#include <vector>
#define int long long
#define INF 1e+18
using namespace std;

typedef pair<int, int> P;

signed main() {
  int m, n, k;
  bool start = false;
  int d[200002][2]; // 0:tate 1:yoko
  vector<P> G[200002][2];
  vector<P> tate[100000], yoko[100000];
  cin >> m >> n >> k;
  for (int i = 0; i < k + 2; i++) {
    d[i][0] = INF;
    d[i][1] = INF;
  }
  for (int i = 0; i < k; i++) {
    int a, b;
    cin >> a >> b;
    if (a == 1 && b == 1)
      start = true;
    tate[b - 1].push_back(P(a - 1, i + 1));
    yoko[a - 1].push_back(P(b - 1, i + 1));
  }
  tate[0].push_back(P(0, 0));
  yoko[0].push_back(P(0, 0));
  tate[n - 1].push_back(P(m - 1, k + 1));
  yoko[m - 1].push_back(P(n - 1, k + 1));
  for (int i = 0; i < n; i++) {
    if (tate[i].size() <= 1)
      continue;
    sort(tate[i].begin(), tate[i].end());
    for (int j = 0; j < tate[i].size() - 1; j++) {
      G[tate[i][j + 1].second][1].push_back(
          P(tate[i][j + 1].first - tate[i][j].first, tate[i][j].second));
      G[tate[i][j].second][1].push_back(
          P(tate[i][j + 1].first - tate[i][j].first, tate[i][j + 1].second));
    }
  }
  for (int i = 0; i < m; i++) {
    if (yoko[i].size() <= 1)
      continue;
    sort(yoko[i].begin(), yoko[i].end());
    for (int j = 0; j < yoko[i].size() - 1; j++) {
      G[yoko[i][j + 1].second][0].push_back(
          P(yoko[i][j + 1].first - yoko[i][j].first, yoko[i][j].second));
      G[yoko[i][j].second][0].push_back(
          P(yoko[i][j + 1].first - yoko[i][j].first, yoko[i][j + 1].second));
    }
  }
  priority_queue<P, vector<P>, greater<P>> que;
  que.push(P(0, 0));
  d[0][0] = 0;
  if (start) {
    que.push(P(1, k + 2));
    d[0][1] = 1;
  }
  while (!que.empty()) {
    P p = que.top();
    que.pop();
    int v = p.second % (k + 2), flag = p.second / (k + 2),
        _flag = (flag + 1) % 2;
    if (d[v][flag] < p.first)
      continue;
    for (int i = 0; i < G[v][flag].size(); i++) {
      P u = G[v][flag][i];
      if (d[u.second][flag] > d[v][flag] + u.first) {
        d[u.second][flag] = d[v][flag] + u.first;
        que.push(P(d[u.second][flag], u.second + flag * (k + 2)));
      }
      if (d[u.second][_flag] > d[v][flag] + u.first + 1) {
        d[u.second][_flag] = d[v][flag] + u.first + 1;
        que.push(P(d[u.second][_flag], u.second + _flag * (k + 2)));
      }
    }
    for (int i = 0; i < G[v][_flag].size(); i++) {
      P u = G[v][_flag][i];
      if (d[u.second][_flag] > d[v][_flag] + u.first) {
        d[u.second][_flag] = d[v][_flag] + u.first;
        que.push(P(d[u.second][_flag], u.second + _flag * (k + 2)));
      }
      if (d[u.second][flag] > d[v][_flag] + u.first + 1) {
        d[u.second][flag] = d[v][_flag] + u.first + 1;
        que.push(P(d[u.second][flag], u.second + flag * (k + 2)));
      }
    }
  }
  if (min(d[k + 1][0], d[k + 1][1]) == INF)
    cout << -1 << endl;
  else
    cout << min(d[k + 1][0], d[k + 1][1]) << endl;
  /*for(int i = 0;i <= k + 1;i++){
          cout << i << ")" << endl;
          for(int j = 0;j < G[i][0].size();j++){
                  cout << G[i][0][j].second << " ";
          }
          cout << endl;
          for(int j = 0;j < G[i][1].size();j++){
                  cout << G[i][1][j].second << " ";
          }
          cout << endl;
  }*/
  return 0;
}

Fixed submission:
#include <algorithm>
#include <iostream>
#include <queue>
#include <vector>
#define int long long
#define INF 1e+18
using namespace std;

typedef pair<int, int> P;

signed main() {
  int m, n, k;
  bool start = false;
  static int d[200002][2]; // 0:tate 1:yoko
  static vector<P> G[200002][2];
  static vector<P> tate[100000], yoko[100000];
  cin >> m >> n >> k;
  for (int i = 0; i < k + 2; i++) {
    d[i][0] = INF;
    d[i][1] = INF;
  }
  for (int i = 0; i < k; i++) {
    int a, b;
    cin >> a >> b;
    if (a == 1 && b == 1)
      start = true;
    tate[b - 1].push_back(P(a - 1, i + 1));
    yoko[a - 1].push_back(P(b - 1, i + 1));
  }
  tate[0].push_back(P(0, 0));
  yoko[0].push_back(P(0, 0));
  tate[n - 1].push_back(P(m - 1, k + 1));
  yoko[m - 1].push_back(P(n - 1, k + 1));
  for (int i = 0; i < n; i++) {
    if (tate[i].size() <= 1)
      continue;
    sort(tate[i].begin(), tate[i].end());
    for (int j = 0; j < tate[i].size() - 1; j++) {
      G[tate[i][j + 1].second][1].push_back(
          P(tate[i][j + 1].first - tate[i][j].first, tate[i][j].second));
      G[tate[i][j].second][1].push_back(
          P(tate[i][j + 1].first - tate[i][j].first, tate[i][j + 1].second));
    }
  }
  for (int i = 0; i < m; i++) {
    if (yoko[i].size() <= 1)
      continue;
    sort(yoko[i].begin(), yoko[i].end());
    for (int j = 0; j < yoko[i].size() - 1; j++) {
      G[yoko[i][j + 1].second][0].push_back(
          P(yoko[i][j + 1].first - yoko[i][j].first, yoko[i][j].second));
      G[yoko[i][j].second][0].push_back(
          P(yoko[i][j + 1].first - yoko[i][j].first, yoko[i][j + 1].second));
    }
  }
  priority_queue<P, vector<P>, greater<P>> que;
  que.push(P(0, 0));
  d[0][0] = 0;
  if (start) {
    que.push(P(1, k + 2));
    d[0][1] = 1;
  }
  while (!que.empty()) {
    P p = que.top();
    que.pop();
    int v = p.second % (k + 2), flag = p.second / (k + 2),
        _flag = (flag + 1) % 2;
    if (d[v][flag] < p.first)
      continue;
    for (int i = 0; i < G[v][flag].size(); i++) {
      P u = G[v][flag][i];
      if (d[u.second][flag] > d[v][flag] + u.first) {
        d[u.second][flag] = d[v][flag] + u.first;
        que.push(P(d[u.second][flag], u.second + flag * (k + 2)));
      }
      if (d[u.second][_flag] > d[v][flag] + u.first + 1) {
        d[u.second][_flag] = d[v][flag] + u.first + 1;
        que.push(P(d[u.second][_flag], u.second + _flag * (k + 2)));
      }
    }
    for (int i = 0; i < G[v][_flag].size(); i++) {
      P u = G[v][_flag][i];
      if (d[u.second][_flag] > d[v][_flag] + u.first) {
        d[u.second][_flag] = d[v][_flag] + u.first;
        que.push(P(d[u.second][_flag], u.second + _flag * (k + 2)));
      }
      if (d[u.second][flag] > d[v][_flag] + u.first + 1) {
        d[u.second][flag] = d[v][_flag] + u.first + 1;
        que.push(P(d[u.second][flag], u.second + flag * (k + 2)));
      }
    }
  }
  if (min(d[k + 1][0], d[k + 1][1]) == INF)
    cout << -1 << endl;
  else
    cout << min(d[k + 1][0], d[k + 1][1]) << endl;
  /*for(int i = 0;i <= k + 1;i++){
          cout << i << ")" << endl;
          for(int j = 0;j < G[i][0].size();j++){
                  cout << G[i][0][j].second << " ";
          }
          cout << endl;
          for(int j = 0;j < G[i][1].size();j++){
                  cout << G[i][1][j].second << " ";
          }
          cout << endl;
  }*/
  return 0;
}