Problem p00528 (C++) — buggy submission, verdict: Time Limit Exceeded

#include <algorithm>
#include <iostream>
#include <queue>
#include <unordered_set>
#include <vector>

using namespace std;

#define INF (1e18)

typedef pair<long long, long long> P;
typedef pair<long long, P> P2;

int main() {
  int M, N, K;
  cin >> M >> N >> K;

  vector<P2> sw;
  sw.push_back(P2(0, P(1, 1)));

  for (int i = 1; i <= K; ++i) {
    long long a, b;
    cin >> a >> b;
    sw.push_back(P2(i, P(a, b)));
  }

  sw.push_back(P2(K + 1, P(M, N)));

  vector<vector<P>> to[2];
  to[0].resize(K + 2);
  to[1].resize(K + 2);

  sort(sw.begin(), sw.end(), [](P2 &lhs, P2 &rhs) {
    return (lhs.second.first == rhs.second.first)
               ? ((lhs.second.second == rhs.second.second)
                      ? (lhs.first < rhs.first)
                      : (lhs.second.second < rhs.second.second))
               : (lhs.second.first < rhs.second.first);
  });

  for (int i = 1; i <= K + 1; ++i) {
    if (sw[i - 1].second.first == sw[i].second.first) {
      long long d = sw[i].second.second - sw[i - 1].second.second;
      to[0][sw[i - 1].first].push_back(P(sw[i].first, d));
      if (sw[i - 1].first != 0)
        to[0][sw[i].first].push_back(P(sw[i - 1].first, d));
    }
  }

  sort(sw.begin(), sw.end(), [](P2 &lhs, P2 &rhs) {
    return (lhs.second.second == rhs.second.second)
               ? ((lhs.second.first == rhs.second.first)
                      ? (lhs.first < rhs.first)
                      : (lhs.second.first < rhs.second.first))
               : (lhs.second.second < rhs.second.second);
  });

  for (int i = 1; i <= K + 1; ++i) {
    if (sw[i - 1].second.second == sw[i].second.second) {
      long long d = sw[i].second.first - sw[i - 1].second.first;
      to[1][sw[i - 1].first].push_back(P(sw[i].first, d));
      if (sw[i - 1].first != 0)
        to[1][sw[i].first].push_back(P(sw[i - 1].first, d));
    }
  }

  sort(sw.begin(), sw.end());

  priority_queue<P2, vector<P2>, greater<P2>> q;
  vector<long long> dist[2];
  dist[0].resize(K + 2, INF);
  dist[1].resize(K + 2, INF);
  unordered_set<long long> visited[2];
  q.push(P2(0, P(0, 0))); // dist, from, sw
  dist[0][0] = 0;
  visited[1].insert(0);
  while (!q.empty()) {
    P2 pp = q.top();
    q.pop();
    long long now_dist = pp.first;
    long long now_from = pp.second.first;
    long long now_sw = pp.second.second;
    if (sw[now_sw].second.first == M && sw[now_sw].second.second == N) {
      dist[now_from][K + 1] = now_dist;
      break;
    }
    if (visited[now_from].count(now_sw) != 0 ||
        dist[now_from][now_sw] < now_dist)
      continue;
    visited[now_from].insert(now_sw);

    for (int i = 0; i < (int)to[now_from][now_sw].size(); ++i) {
      long long nxt_sw = to[now_from][now_sw][i].first;
      long long nxt_dist = to[now_from][now_sw][i].second + now_dist;
      if (visited[now_from].count(nxt_sw) != 0 ||
          dist[now_from][nxt_sw] <= nxt_dist)
        continue;
      dist[now_from][nxt_sw] = nxt_dist;
      q.push(P2(nxt_dist, P(now_from, nxt_sw)));
    }
    if (now_sw != 0) {
      for (int i = 0; i < (int)to[(now_from + 1) % 2][now_sw].size(); ++i) {
        long long nxt_sw = to[(now_from + 1) % 2][now_sw][i].first;
        long long nxt_dist =
            to[(now_from + 1) % 2][now_sw][i].second + now_dist + 1;
        if (visited[(now_from + 1) % 2].count(nxt_sw) != 0 ||
            dist[(now_from + 1) % 2][nxt_sw] <= nxt_dist)
          continue;
        dist[(now_from + 1) % 2][nxt_sw] = nxt_dist;
        q.push(P2(nxt_dist, P((now_from + 1) % 2, nxt_sw)));
      }
    }
  }

  long long ans = min(dist[0][K + 1], dist[1][K + 1]);
  if (ans == INF)
    ans = -1;

  cout << ans << endl;

  return 0;
}


Error: TLE

Fixed submission:

#include <algorithm>
#include <iostream>
#include <queue>
#include <unordered_set>
#include <vector>

using namespace std;

#define INF (1e18)

typedef pair<long long, long long> P;
typedef pair<long long, P> P2;

int main() {
  long long M, N, K;
  cin >> M >> N >> K;

  vector<P> x, y;
  vector<P> sw;
  x.push_back(P(1, 0));
  y.push_back(P(1, 0));
  sw.push_back(P(1, 1));
  long long cnt = 1;
  for (int i = 0; i < K; ++i) {
    long long a, b;
    cin >> a >> b;
    if (a == M && b == N)
      continue;
    x.push_back(P(a, cnt));
    y.push_back(P(b, cnt));
    sw.push_back(P(a, b));
    ++cnt;
  }
  x.push_back(P(M, cnt));
  y.push_back(P(N, cnt));
  sw.push_back(P(M, N));

  sort(x.begin(), x.end());
  sort(y.begin(), y.end());

  vector<vector<P>> to[2]; // 0... NS , 1... EW
  to[0].resize(cnt + 1);
  to[1].resize(cnt + 1);
  for (int i = 1; i <= cnt; ++i) {
    if (x[i - 1].first == x[i].first) {
      long long dis = sw[x[i].second].second - sw[x[i - 1].second].second;
      to[0][x[i - 1].second].push_back(P(x[i].second, dis));
      to[0][x[i].second].push_back(P(x[i - 1].second, dis));
    }
    if (y[i - 1].first == y[i].first) {
      long long dis = sw[y[i].second].first - sw[y[i - 1].second].first;
      to[1][y[i - 1].second].push_back(P(y[i].second, dis));
      to[1][y[i].second].push_back(P(y[i - 1].second, dis));
    }
  }

  unordered_set<long long> visited;
  priority_queue<P2> q;
  vector<long long> dist[2];
  dist[0].resize(cnt + 1, INF);
  dist[1].resize(cnt + 1, INF);
  q.push(P2(0, P(0, 0))); // now_dist, now_from, now_sw
  visited.insert(0);
  while (!q.empty()) {
    P2 pp = q.top();
    q.pop();
    long long now_dist = pp.first;
    long long now_from = pp.second.first;
    long long now_sw = pp.second.second;
    if (now_sw == cnt)
      break;
    if (dist[now_from][now_sw] < now_dist)
      continue;
    visited.insert(now_sw);
    for (int i = 0; i < (int)to[now_from][now_sw].size(); ++i) {
      int nxt_sw = to[now_from][now_sw][i].first;
      int nxt_dist = now_dist + to[now_from][now_sw][i].second;
      if (visited.count(nxt_sw) != 0 || dist[now_from][nxt_sw] <= nxt_dist)
        continue;
      dist[now_from][nxt_sw] = nxt_dist;
      q.push(P2(nxt_dist, P(now_from, nxt_sw)));
    }
    for (int i = 0; i < (int)to[(now_from + 1) % 2][now_sw].size(); ++i) {
      int nxt_sw = to[(now_from + 1) % 2][now_sw][i].first;
      int nxt_dist = now_dist + to[(now_from + 1) % 2][now_sw][i].second + 1;
      if (visited.count(nxt_sw) != 0 ||
          dist[(now_from + 1) % 2][nxt_sw] <= nxt_dist)
        continue;
      dist[(now_from + 1) % 2][nxt_sw] = nxt_dist;
      q.push(P2(nxt_dist, P((now_from + 1) % 2, nxt_sw)));
    }
    if (dist[(now_from + 1) % 2][now_sw] < now_dist + 1) {
      dist[(now_from + 1) % 2][now_sw] = now_dist + 1;
      q.push(P2(now_dist + 1, P((now_from + 1) % 2, now_sw)));
    }
  }

  long long ans = min(dist[0][cnt], dist[1][cnt]);
  if (ans == INF)
    ans = -1;

  cout << ans << endl;

  return 0;
}