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

#include <algorithm>
#include <climits>
#include <cmath>
#include <cstdio>
#include <cstdlib>
#include <cstring>
#include <deque>
#include <functional>
#include <iostream>
#include <map>
#include <queue>
#include <set>
#include <sstream>
#include <stack>
#include <string>
#include <utility>
#include <vector>

using namespace std;

#define int long long
#define MP make_pair
#define PB push_back
#define FI first
#define SE second
#define MAX_K 200000
typedef pair<int, pair<int, int>> PPII;
typedef pair<int, int> PII;
static const int INF = 1ll << 60;
static const int dx[] = {
    1,
    -1,
    0,
    0,
};
static const int dy[] = {0, 0, 1, -1};

int M, N, K;
vector<PII> fld, X, Y;
int D[MAX_K + 5];
bool flg = false;

void dijkstra() {
  priority_queue<PPII, vector<PPII>, greater<PPII>> que;
  fill(D, D + MAX_K + 5, INF);
  D[0] = 0;
  que.push(PPII(0, PII(0, 0)));
  while (!que.empty()) {
    PPII p = que.top();
    que.pop();
    int v = p.SE.FI;
    int On = p.SE.SE;
    if (D[v] < p.FI)
      continue;
    if (fld[v].FI == M && fld[v].SE == N)
      break;
    int p1 =
        lower_bound(X.begin(), X.end(), PII(fld[v].FI, fld[v].SE)) - X.begin();
    int p2 =
        lower_bound(Y.begin(), Y.end(), PII(fld[v].SE, fld[v].FI)) - Y.begin();
    for (int i = p1 - 1; i <= p1 + 1; i++) {
      if (p1 == X.size())
        break;
      if (i < 0 || i >= X.size())
        continue;
      int t = lower_bound(fld.begin(), fld.end(), PII(X[i].FI, X[i].SE)) -
              fld.begin();
      PII e = fld[t];
      if (fld[v].FI != e.FI)
        continue;
      if ((!flg) && e.FI == 1 && e.SE == 1)
        continue;
      if (fld[v].FI != e.FI && fld[v].SE != e.SE)
        continue;
      int cost = abs(e.FI - fld[v].FI) + abs(e.SE - fld[v].SE);
      if (On == 1)
        cost++;
      if (D[t] > D[v] + cost) {
        D[t] = D[v] + cost;
        if (On == 1)
          que.push(PPII(D[t], MP(t, 0)));
        else if (On == 0)
          que.push(PPII(D[t], MP(t, 0)));
      }
    }
    for (int i = p2 - 1; i <= p2 + 1; i++) {
      if (flg == false && fld[v].FI == 1 && fld[v].SE == 1)
        break;
      if (p2 == Y.size())
        break;
      if (i < 0 || i >= Y.size())
        continue;
      int t = lower_bound(fld.begin(), fld.end(), PII(Y[i].SE, Y[i].FI)) -
              fld.begin();
      PII e = fld[t];
      if (fld[v].SE != e.SE)
        continue;
      if ((!flg) && e.FI == 1 && e.SE == 1)
        continue;
      if (fld[v].FI != e.FI && fld[v].SE != e.SE)
        continue;
      int cost = abs(e.FI - fld[v].FI) + abs(e.SE - fld[v].SE);
      if (On == 0)
        cost++;
      if (D[t] > D[v] + cost) {
        D[t] = D[v] + cost;
        if (On == 0)
          que.push(PPII(D[t], MP(t, 1)));
        else if (On == 1)
          que.push(PPII(D[t], MP(t, 1)));
      }
    }
  }
}

signed main() {
  cin >> M >> N >> K;
  fld.PB(PII(1, 1));
  fld.PB(PII(M, N));
  X.PB(PII(1, 1));
  Y.PB(PII(1, 1));
  X.PB(PII(M, N));
  Y.PB(PII(N, M));
  for (int i = 0; i < K; ++i) {
    int x, y;
    cin >> x >> y;
    if (x == 1 && y == 1) {
      flg = true;
      continue;
    }
    if (x == M && y == N)
      continue;
    fld.PB(PII(x, y));
    X.PB(PII(x, y));
    Y.PB(PII(y, x));
  }
  sort(fld.begin(), fld.end());
  sort(X.begin(), X.end());
  sort(Y.begin(), Y.end());
  dijkstra();
  int ans = lower_bound(fld.begin(), fld.end(), MP(M, N)) - fld.begin();
  if (D[ans] >= INF)
    cout << -1 << endl;
  else
    cout << D[ans] << endl;
  return 0;
}

Error: TLE

Fixed submission:

#include <algorithm>
#include <climits>
#include <cmath>
#include <cstdio>
#include <cstdlib>
#include <cstring>
#include <deque>
#include <functional>
#include <iostream>
#include <map>
#include <queue>
#include <set>
#include <sstream>
#include <stack>
#include <string>
#include <utility>
#include <vector>

using namespace std;

#define int long long
#define MP make_pair
#define PB push_back
#define FI first
#define SE second
#define MAX_K 200000
typedef pair<int, pair<int, int>> PPII;
typedef pair<int, int> PII;
static const int INF = 1ll << 60;
static const int dx[] = {
    1,
    -1,
    0,
    0,
};
static const int dy[] = {0, 0, 1, -1};

int M, N, K;
vector<PII> fld, X, Y;
int D[MAX_K + 5];
bool flg = false;

void dijkstra() {
  priority_queue<PPII, vector<PPII>> que;
  fill(D, D + MAX_K, INF);
  D[0] = 0;
  que.push(PPII(0, PII(0, 0)));
  if (flg)
    que.push(PPII(1, PII(0, 1)));
  while (!que.empty()) {
    PPII p = que.top();
    que.pop();
    int v = p.SE.FI;
    if (D[v] < p.FI)
      continue;
    int p1 = find(X.begin(), X.end(), PII(fld[v].FI, fld[v].SE)) - X.begin();
    int p2 = find(Y.begin(), Y.end(), PII(fld[v].SE, fld[v].FI)) - Y.begin();
    for (int i = p1 - 1; i <= p1 + 1; i++) {
      if (i < 0)
        continue;
      int t = find(fld.begin(), fld.end(), PII(X[i].FI, X[i].SE)) - fld.begin();
      PII e = fld[t];
      int On = p.SE.SE;
      if (fld[v].FI != e.FI)
        continue;
      int cost = abs(e.FI - fld[v].FI) + abs(e.SE - fld[v].SE);
      if (On == true)
        cost++;
      if (D[t] > D[v] + cost) {
        D[t] = D[v] + cost;
        if (On == true)
          que.push(PPII(D[t], PII(t, !On)));
        else
          que.push(PPII(D[t], PII(t, On)));
      }
    }
    for (int i = p2 - 1; i <= p2 + 1; i++) {
      if (i < 0)
        continue;
      int t = find(fld.begin(), fld.end(), PII(Y[i].SE, Y[i].FI)) - fld.begin();
      PII e = fld[t];
      int On = p.SE.SE;
      if (fld[v].SE != e.SE)
        continue;
      int cost = abs(e.FI - fld[v].FI) + abs(e.SE - fld[v].SE);
      if (On == false)
        cost++;
      if (D[t] > D[v] + cost) {
        D[t] = D[v] + cost;
        if (On == false)
          que.push(PPII(D[t], PII(t, !On)));
        else
          que.push(PPII(D[t], PII(t, On)));
      }
    }
  }
}

signed main() {
  int cnt = 0;
  cin >> M >> N >> K;
  fld.PB(PII(1, 1));
  fld.PB(PII(M, N));
  X.PB(PII(1, 1));
  Y.PB(PII(1, 1));
  X.PB(PII(M, N));
  Y.PB(PII(N, M));
  for (int i = 0; i < K; ++i) {
    int x, y;
    cin >> x >> y;
    if (x == 1 && y == 1)
      continue;
    if (x == M && y == N)
      continue;
    fld.PB(PII(x, y));
    X.PB(PII(x, y));
    Y.PB(PII(y, x));
  }
  sort(fld.begin(), fld.end());
  sort(X.begin(), X.end());
  sort(Y.begin(), Y.end());
  dijkstra();
  // for(int i=0;i<10;++i)cout<<D[i]<<endl;
  int ans = find(fld.begin(), fld.end(), MP(M, N)) - fld.begin();
  if (D[ans] >= INF)
    cout << -1 << endl;
  else
    cout << D[ans] << endl;
  return 0;
}