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

#include <bits/stdc++.h>

#define rep(i, N) for (int i = 0; i < N; i++)
#define rep2(i, N) for (int i = 1; i <= N; i++)
using namespace std;
long long INF = 1e18;
long long mod = 1e9 + 7;

// status unsolved
char block[1010][1010];

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

int wrapx[5] = {-2, -1, 0, 1, 2};
int wrapy[5] = {-2, -1, 0, 1, 2};
int main() {
  long long h, w;
  cin >> h >> w;
  int ch, cw;
  cin >> ch >> cw;
  int dh, dw;
  cin >> dh >> dw;

  int warp = 0;
  rep2(i, h) {
    rep2(k, w) { cin >> block[i][k]; }
  }
  queue<int> q;
  queue<int> Q;
  q.push(ch);
  q.push(cw);
  Q.push(ch);
  Q.push(cw);
  // bool reached[1010][1010];
  long long dist[1010][1010];

  rep2(i, h) rep2(k, w) dist[i][k] = INF;

  dist[ch][cw] = 0;

  while (!q.empty()) {

    // コスト0で行ける範囲にたどり着く
    while (!q.empty()) {
      int nowy = q.front();
      q.pop();
      int nowx = q.front();
      q.pop();
      // if(block[nowy][nowx]=='#')continue;
      Q.push(nowy);
      Q.push(nowx);
      rep(i, 4) {
        int ny = nowy + dy[i];
        int nx = nowx + dx[i];

        if (ny < 1 || ny > h || nx < 1 || nx > w || block[ny][nx] == '#') {
          continue;
        } else if (dist[ny][nx] > dist[nowy][nowx]) {
          dist[ny][nx] = dist[nowy][nowx];

          q.push(ny);
          q.push(nx);
        }
      }
    }
    // コスト+1しないといけない範囲を探索
    while (!Q.empty()) {
      int i = Q.front();
      Q.pop();
      int k = Q.front();
      Q.pop();

      rep(l, 5) {
        rep(p, 5) {
          if (abs(wrapy[l]) + abs(wrapx[p]) <= 1)
            continue;

          int ny = i + wrapy[l];
          int nx = k + wrapx[p];

          if (ny < 1 || ny > h || nx < 1 || nx > w || block[ny][nx] == '#') {
            continue;
          }
          // ワープして行ける範囲でコスト0の幅優先探索
          else if (dist[ny][nx] > dist[i][k] + 1) {
            dist[ny][nx] = dist[i][k] + 1;
            q.push(ny);
            q.push(nx);
          }
        }
      }
    }
  }

  if (dist[dh][dw] == INF) {
    cout << "-1" << endl;
  } else {
    cout << dist[dh][dw] << endl;
  }
  // rep2(i,h){
  //     rep2(k,w){
  //         if(dist[i][k]==INF){
  //             cout<<"INF ";
  //         }
  //         else{
  //             cout<<dist[i][k]<<"    ";
  //         }
  //     }
  //     cout<<endl;
  // }

  return 0;
}
/*


*/


Error: TLE

Fixed submission:

#include <bits/stdc++.h>

#define rep(i, N) for (int i = 0; i < N; i++)
#define rep2(i, N) for (int i = 1; i <= N; i++)
using namespace std;
long long INF = 1e18;
long long mod = 1e9 + 7;

// status unsolved
char block[1010][1010];

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

int wrapx[5] = {-2, -1, 0, 1, 2};
int wrapy[5] = {-2, -1, 0, 1, 2};
int main() {
  long long h, w;
  cin >> h >> w;
  int ch, cw;
  cin >> ch >> cw;
  int dh, dw;
  cin >> dh >> dw;

  int warp = 0;
  rep2(i, h) {
    rep2(k, w) { cin >> block[i][k]; }
  }
  queue<int> q;
  queue<int> Q;
  q.push(ch);
  q.push(cw);

  // bool reached[1010][1010];
  long long dist[1010][1010];

  rep2(i, h) rep2(k, w) dist[i][k] = INF;

  dist[ch][cw] = 0;

  while (!q.empty()) {

    // コスト0で行ける範囲にたどり着く
    while (!q.empty()) {
      int nowy = q.front();
      q.pop();
      int nowx = q.front();
      q.pop();
      if (block[nowy][nowx] == '#')
        continue;
      bool open = true;
      rep(i, 4) {
        int ny = nowy + dy[i];
        int nx = nowx + dx[i];
        if (block[ny][nx] == '#')
          open = false;

        if (ny < 1 || ny > h || nx < 1 || nx > w || block[ny][nx] == '#') {
          continue;
        } else if (dist[ny][nx] > dist[nowy][nowx]) {
          dist[ny][nx] = dist[nowy][nowx];
          q.push(ny);
          q.push(nx);
        }
      }
    }
    // if(open)continue;
    rep2(i, h) {
      rep2(k, w) {
        if (block[i][k] == '#')
          continue;
        // コスト0で行けるマスのみ調べる
        else if (dist[i][k] != INF) {
          // Q.push(i);
          // Q.push(k);
          rep(l, 5) {
            rep(p, 5) {
              if (abs(wrapy[l]) + abs(wrapx[p]) <= 1)
                continue;

              int ny = i + wrapy[l];
              int nx = k + wrapx[p];

              if (ny < 1 || ny > h || nx < 1 || nx > w ||
                  block[ny][nx] == '#') {
                continue;
              } else if (dist[ny][nx] > dist[i][k] + 1) {
                dist[ny][nx] = dist[i][k] + 1;
                q.push(ny);
                q.push(nx);
              }
            }
          }
        }
      }
    }
  }

  if (dist[dh][dw] == INF) {
    cout << "-1" << endl;
  } else {
    cout << dist[dh][dw] << endl;
  }

  return 0;
}
/*


*/