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

#include <algorithm>
#include <cmath>
#include <ctime>
#include <deque>
#include <iostream>
#include <limits>
#include <map>
#include <queue>
#include <set>
#include <stack>
#include <stdio.h>
#include <string>
#include <tuple>
#include <vector>
using namespace std;
#define int long long int
#define rep(i, n) for (int i = 0; i < (int)(n); i++)
#define dup(x, y) (((x) + (y)-1) / (y))
int gcd(int a, int b) { return b ? gcd(b, a % b) : a; }
int lcm(int a, int b) { return a / gcd(a, b) * b; }
template <class T> inline bool chmin(T &a, T b) {
  if (a > b) {
    a = b;
    return true;
  }
  return false;
}
template <class T> inline bool chmax(T &a, T b) {
  if (a < b) {
    a = b;
    return true;
  }
  return false;
}
typedef pair<int, int> P;
const int MOD = 1e9 + 7;
// const int MOD = 998244353;
const int INF = 1e18;
const long double PI = (acos(-1));

const int dy[4] = {1, 0, -1, 0};
const int dx[4] = {0, 1, 0, -1};

signed main() {
  int H, W;
  int sh, sw, gh, gw;
  cin >> H >> W >> sh >> sw >> gh >> gw;
  vector<string> S(H);
  rep(i, H) cin >> S[i];
  sh--;
  sw--;
  gh--;
  gw--;
  deque<P> q;
  q.push_back(P(sh, sw));
  vector<vector<int>> d(H, vector<int>(W, INF));
  d[sh][sw] = 0;
  while (q.size()) {
    P p = q.front();
    q.pop_front();
    rep(i, 4) {
      int nh = p.first + dx[i];
      int nw = p.second + dy[i];
      int nd = d[p.first][p.second];
      if (nh < 0 || nh >= H || nw < 0 || nw >= W)
        continue;
      if (S[nh][nw] == '#')
        continue;
      if (d[nh][nw] <= nd)
        continue;
      q.push_front(P(nh, nw));
      d[nh][nw] = nd;
    }

    rep(i, 5) rep(j, 5) {
      if (abs(i - 2) + abs(j - 2) < 2)
        continue;
      int nh = p.first + i - 2;
      int nw = p.second + j - 2;
      int nd = d[p.first][p.second] + 1;
      if (nh < 0 || nh >= H || nw < 0 || nw >= W)
        continue;
      if (S[nh][nw] == '#')
        continue;
      if (d[nh][nw] <= nd)
        continue;
      q.push_back(P(nh, nw));
      d[nh][nw] = nd;
    }
  }
  if (d[gh][gw] == INF)
    cout << -1;
  else
    cout << d[gh][gw];
}

Error: TLE

Fixed submission:

#include <algorithm>
#include <cmath>
#include <ctime>
#include <deque>
#include <iostream>
#include <limits>
#include <map>
#include <queue>
#include <set>
#include <stack>
#include <stdio.h>
#include <string>
#include <tuple>
#include <vector>
using namespace std;
// #define int long long int
#define rep(i, n) for (int i = 0; i < (int)(n); i++)
#define dup(x, y) (((x) + (y)-1) / (y))
int gcd(int a, int b) { return b ? gcd(b, a % b) : a; }
int lcm(int a, int b) { return a / gcd(a, b) * b; }
template <class T> inline bool chmin(T &a, T b) {
  if (a > b) {
    a = b;
    return true;
  }
  return false;
}
template <class T> inline bool chmax(T &a, T b) {
  if (a < b) {
    a = b;
    return true;
  }
  return false;
}
typedef pair<int, int> P;
const int MOD = 1e9 + 7;
// const int MOD = 998244353;
// const int INF = 1e18;
const int INF = 1e9;
const long double PI = (acos(-1));

const int dy[4] = {1, 0, -1, 0};
const int dx[4] = {0, 1, 0, -1};

signed main() {
  int H, W;
  int sh, sw, gh, gw;
  cin >> H >> W >> sh >> sw >> gh >> gw;
  vector<string> S(H);
  rep(i, H) cin >> S[i];
  sh--;
  sw--;
  gh--;
  gw--;
  queue<P> q;
  q.push(P(sh, sw));
  vector<vector<int>> d(H, vector<int>(W, INF));
  d[sh][sw] = 0;
  vector<vector<bool>> A(H, vector<bool>(W, false));
  while (q.size()) {
    P p = q.front();
    q.pop();
    A[p.first][p.second] = false;
    int nd = d[p.first][p.second];
    rep(i, 4) {
      int nh = p.first + dx[i];
      int nw = p.second + dy[i];
      if (nh < 0 || nh >= H || nw < 0 || nw >= W)
        continue;
      if (S[nh][nw] == '#')
        continue;
      if (d[nh][nw] <= nd)
        continue;
      if (A[nh][nw] == false) {
        q.push(P(nh, nw));
        A[nh][nw] = true;
      }
      d[nh][nw] = nd;
    }
    nd++;
    rep(i, 5) rep(j, 5) {
      if (abs(i - 2) + abs(j - 2) < 2)
        continue;
      int nh = p.first + i - 2;
      int nw = p.second + j - 2;
      if (nh < 0 || nh >= H || nw < 0 || nw >= W)
        continue;
      if (S[nh][nw] == '#')
        continue;
      if (d[nh][nw] <= nd)
        continue;
      if (A[nh][nw] == false) {
        q.push(P(nh, nw));
        A[nh][nw] = true;
      }
      d[nh][nw] = nd;
    }
  }
  if (d[gh][gw] == INF)
    cout << -1;
  else
    cout << d[gh][gw];
}