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

#include <bits/stdc++.h>
using namespace std;

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

  using LL = long long;

  struct Switch {
    int id, x, y, left, right, down, up;
    Switch(int i = -1, int x = -1, int y = -1)
        : id(i), x(x), y(y), left(i), right(i), down(i), up(i) {}
  };

  using Node = tuple<LL, int, int>;
  priority_queue<Node, vector<Node>, greater<Node>> q;
  int S = 0, G = K + 1;
  vector<Switch> sw(G + 1);
  unordered_map<int, vector<int>> X, Y;

  X[1].push_back(S);
  Y[1].push_back(S);
  X[M].push_back(G);
  Y[N].push_back(G);
  sw[S] = Switch(S, 1, 1);
  q.emplace(0, S, 0);
  for (int i = 1; i <= K; ++i) {
    int x, y;
    cin >> x >> y;
    if (x == M && y == N)
      continue;
    if (x == 1 && y == 1) {
      q.emplace(1, S, 1);
      continue;
    }
    sw[i] = Switch(i, x, y);
    X[x].push_back(i);
    Y[y].push_back(i);
  }
  sw[G] = Switch(G, M, N);

  for (auto &p : X)
    sort(begin(p.second), end(p.second),
         [&](int l, int r) { return sw[l].y < sw[r].y; });
  for (auto &p : Y)
    sort(begin(p.second), end(p.second),
         [&](int l, int r) { return sw[l].x < sw[r].x; });
  for (auto &p : X) {
    auto &v = p.second;
    for (int i = 1; i < v.size(); ++i) {
      if (v[i - 1] != S)
        sw[v[i]].down = v[i - 1];
      sw[v[i - 1]].up = v[i];
    }
  }
  for (auto &p : Y) {
    auto &v = p.second;
    for (int i = 1; i < v.size(); ++i) {
      if (v[i - 1] != S)
        sw[v[i]].left = v[i - 1];
      sw[v[i - 1]].right = v[i];
    }
  }

  vector<vector<LL>> d(G + 1, vector<LL>(2, -1));
  while (!q.empty()) {
    LL t;
    int i, s;
    tie(t, i, s) = q.top();
    q.pop();

    if (~d[i][s])
      continue;
    d[i][s] = t;

    if (i == G)
      break;

    if (i != S)
      if (!~d[i][!s])
        q.emplace(t + 1, i, !s);

    if (s) {
      if (!~d[sw[i].left][s])
        q.emplace(t + sw[i].x - sw[sw[i].left].x, sw[i].left, s);
      if (!~d[sw[i].right][s])
        q.emplace(t + sw[sw[i].right].x - sw[i].x, sw[i].right, s);
    } else {
      if (!~d[sw[i].down][s])
        q.emplace(t + sw[i].y - sw[sw[i].down].y, sw[i].down, s);
      if (!~d[sw[i].up][s])
        q.emplace(t + sw[sw[i].up].y - sw[i].y, sw[i].up, s);
    }
  }

  cout << max(d[G][0], d[G][1]) << endl;
}

Error: TLE

Fixed submission:

#include <bits/stdc++.h>
using namespace std;

int main() {
  using LL = long long;

  struct Switch {
    int id, x, y;
  };

  int M, N, K;
  cin >> M >> N >> K;
  vector<Switch> sw(K);
  unordered_map<int, vector<int>> X, Y;
  using Node = tuple<LL, int, int>;
  priority_queue<Node, vector<Node>, greater<Node>> q;
  q.emplace(0, K, 0);
  for (int i = 0; i < K; ++i) {
    sw[i].id = i;
    cin >> sw[i].x >> sw[i].y;
    if (sw[i].x == 1 && sw[i].y == 1)
      q.emplace(1, K, 1);
    if (sw[i].x == 1 && sw[i].y == 1)
      continue;
    if (sw[i].x == M && sw[i].y == N)
      continue;
    X[sw[i].x].push_back(i);
    Y[sw[i].y].push_back(i);
  }
  sw.push_back({K, 1, 1});
  sw.push_back({K + 1, M, N});
  X[1].push_back(K);
  Y[1].push_back(K);
  X[M].push_back(K + 1);
  Y[N].push_back(K + 1);
  for (auto &i : X)
    sort(begin(i.second), end(i.second),
         [&](int l, int r) { return sw[l].y < sw[r].y; });
  for (auto &i : Y)
    sort(begin(i.second), end(i.second),
         [&](int l, int r) { return sw[l].x < sw[r].x; });

  vector<vector<LL>> d(K + 2, vector<LL>(2, -1));
  while (!q.empty()) {
    LL t;
    int i, s;
    tie(t, i, s) = q.top();
    q.pop();

    if (~d[i][s])
      continue;
    d[i][s] = t;

    if (i == K + 1)
      break;

    if (i != K)
      if (!~d[i][!s])
        q.emplace(t + 1, i, !s);

    if (s) {
      auto &v = Y[sw[i].y];
      auto j = lower_bound(begin(v), end(v), sw[i].x,
                           [sw](auto i, auto v) { return sw[i].x < v; });
      if (j != begin(v))
        if (!~d[*(j - 1)][s])
          q.emplace(t + sw[i].x - sw[*(j - 1)].x, *(j - 1), s);
      if (j + 1 != end(v))
        if (!~d[*(j + 1)][s])
          q.emplace(t + sw[*(j + 1)].x - sw[i].x, *(j + 1), s);
    } else {
      auto &v = X[sw[i].x];
      auto j = lower_bound(begin(v), end(v), sw[i].y,
                           [sw](auto i, auto v) { return sw[i].y < v; });
      if (j != begin(v))
        if (!~d[*(j - 1)][s])
          q.emplace(t + sw[i].y - sw[*(j - 1)].y, *(j - 1), s);
      if (j + 1 != end(v))
        if (!~d[*(j - 1)][s])
          q.emplace(t + sw[*(j + 1)].y - sw[i].y, *(j + 1), s);
    }
  }

  cout << max(d[K + 1][0], d[K + 1][1]) << endl;
}