Problem p00528 (C++)

Buggy submission (status: Runtime Error):
#include <bits/stdc++.h>
using namespace std;
#define rep(i, n) for (int i = 0; i < n; ++i)
typedef long long ll;
struct Point {
  int x, y, id;
};
bool linkx(Point &p1, Point &p2) {
  return p1.y == p2.y ? p1.x < p2.x : p1.y < p2.y;
}
bool linky(Point &p1, Point &p2) {
  return p1.x == p2.x ? p1.y < p2.y : p1.x < p2.x;
}

#define all(a) a.begin(), a.end()

typedef pair<ll, int> edge; // cost,to id
#define INF LLONG_MAX

#define SIZE 200010

int main(void) {
  int M, N, K, X, Y;
  cin >> M >> N >> K;
  vector<edge> g[SIZE];
  vector<Point> vp(K + 1);
  vector<ll> dp(SIZE);
  rep(i, K) {
    cin >> X >> Y;
    vp[i] = {X, Y, i};
  }
  vp[K] = {M, N, K};

  // push switch
  rep(i, K) {
    g[vp[i].id * 2].push_back({1, vp[i].id * 2 + 1});
    g[vp[i].id * 2 + 1].push_back({1, vp[i].id * 2});
  }

  // linkx
  sort(all(vp), linkx);
  rep(i, K) {
    if (vp[i].y == vp[i + 1].y) {
      Point p1 = vp[i], p2 = vp[i + 1];
      int cost = abs(p1.x - p2.x);
      g[p1.id * 2].push_back({cost, p2.id * 2});
      g[p2.id * 2].push_back({cost, p1.id * 2});
    }
  }

  // linky
  sort(all(vp), linky);
  if (vp[0].x != 1) {
    cout << -1 << endl;
    return 0;
  }

  rep(i, K) {
    if (vp[i].x == vp[i + 1].x) {
      Point p1 = vp[i], p2 = vp[i + 1];
      int cost = abs(p1.y - p2.y);
      g[p1.id * 2 + 1].push_back({cost, p2.id * 2 + 1});
      g[p2.id * 2 + 1].push_back({cost, p1.id * 2 + 1});
    }
  }

  rep(i, K * 2 + 2) dp[i] = INF;
  priority_queue<edge, vector<edge>, greater<edge>> q;
  dp[vp[0].id * 2 + 1] = vp[0].y - 1;
  q.push({dp[vp[0].id * 2 + 1], vp[0].id * 2 + 1});

  while (q.size()) {
    edge u = q.top();
    q.pop();
    int id = u.second;
    if (id >= 2 * K) {
      cout << u.first << endl;
      return 0;
    }
    if (dp[id] < u.first)
      continue;
    for (auto e : g[id]) {
      if (dp[e.second] > dp[id] + e.first) {
        dp[e.second] = dp[id] + e.first;
        q.push({dp[e.second], e.second});
      }
    }
  }
  cout << -1 << endl;
  return 0;
}

Fixed submission:
#include <bits/stdc++.h>
using namespace std;
#define rep(i, n) for (int i = 0; i < n; ++i)
typedef long long ll;
struct Point {
  int x, y, id;
};
bool linkx(Point &p1, Point &p2) {
  return p1.y == p2.y ? p1.x < p2.x : p1.y < p2.y;
}
bool linky(Point &p1, Point &p2) {
  return p1.x == p2.x ? p1.y < p2.y : p1.x < p2.x;
}

#define all(a) a.begin(), a.end()

typedef pair<ll, int> edge; // cost,to id
#define INF LLONG_MAX

#define SIZE 400010

int main(void) {
  int M, N, K, X, Y;
  cin >> M >> N >> K;
  vector<edge> g[SIZE];
  vector<Point> vp(K + 1);
  vector<ll> dp(SIZE);
  rep(i, K) {
    cin >> X >> Y;
    vp[i] = {X, Y, i};
  }
  vp[K] = {M, N, K};

  // push switch
  rep(i, K) {
    g[vp[i].id * 2].push_back({1, vp[i].id * 2 + 1});
    g[vp[i].id * 2 + 1].push_back({1, vp[i].id * 2});
  }

  // linkx
  sort(all(vp), linkx);
  rep(i, K) {
    if (vp[i].y == vp[i + 1].y) {
      Point p1 = vp[i], p2 = vp[i + 1];
      int cost = abs(p1.x - p2.x);
      g[p1.id * 2].push_back({cost, p2.id * 2});
      g[p2.id * 2].push_back({cost, p1.id * 2});
    }
  }

  // linky
  sort(all(vp), linky);
  if (vp[0].x != 1) {
    cout << -1 << endl;
    return 0;
  }

  rep(i, K) {
    if (vp[i].x == vp[i + 1].x) {
      Point p1 = vp[i], p2 = vp[i + 1];
      int cost = abs(p1.y - p2.y);
      g[p1.id * 2 + 1].push_back({cost, p2.id * 2 + 1});
      g[p2.id * 2 + 1].push_back({cost, p1.id * 2 + 1});
    }
  }

  rep(i, K * 2 + 2) dp[i] = INF;
  priority_queue<edge, vector<edge>, greater<edge>> q;
  dp[vp[0].id * 2 + 1] = vp[0].y - 1;
  q.push({dp[vp[0].id * 2 + 1], vp[0].id * 2 + 1});

  while (q.size()) {
    edge u = q.top();
    q.pop();
    int id = u.second;
    if (id >= 2 * K) {
      cout << u.first << endl;
      return 0;
    }
    if (dp[id] < u.first)
      continue;
    for (auto e : g[id]) {
      if (dp[e.second] > dp[id] + e.first) {
        dp[e.second] = dp[id] + e.first;
        q.push({dp[e.second], e.second});
      }
    }
  }
  cout << -1 << endl;
  return 0;
}