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

#include <cmath>
#include <iostream>
#include <map>
#include <string>
#include <vector>
#define reps(i, n, m) for (int i = int(n); i < int(m); i++)
#define rep(i, n) reps(i, 0, n)
using namespace std;
typedef pair<int, int> P;
const int inf = 500000000;
int sx, sy, gx, gy;
int h, w;
vector<string> vec;
vector<P> mp[5];
vector<int> val[5];
int dp[5][5][1010];
int dist(int x1, int y1, int x2, int y2) { return abs(x1 - x2) + abs(y1 - y2); }
int main() {
  while (cin >> w >> h && h + w) {
    vec.clear();
    rep(i, 5) {
      mp[i].clear();
      val[i].clear();
    }
    rep(i, 5) {
      rep(j, 5) { rep(k, 1010) dp[i][j][k] = inf; }
    }
    rep(i, h) {
      string temp;
      cin >> temp;
      vec.push_back(temp);
      rep(j, w) {
        switch (vec[i][j]) {
        case 'S':
          sx = i;
          sy = j;
          break;
        case 'G':
          gx = i;
          gy = j;
          break;
        case '.':
          break;
        default:
          int num = (int)(vec[i][j] - '1');
          mp[num].push_back(P(i, j));
          break;
        }
      }
    }
    int ansCol = 0;
    int ansDist = inf;
    rep(rest, 4) {
      for (int col = 0; col < 5; col++) {
        rep(i, mp[col].size()) {
          if (rest == 0) {
            dp[rest][col][i] =
                dist(gx, gy, mp[col][i].first, mp[col][i].second);
          } else { // rest<4
            rep(j, mp[(col + 1) % 5].size()) {
              int x1 = mp[col][i].first;
              int y1 = mp[col][i].second;
              int x2 = mp[(col + 1) % 5][j].first;
              int y2 = mp[(col + 1) % 5][j].second;
              int d = dist(x1, y1, x2, y2);
              dp[rest][col][i] =
                  min(dp[rest][col][i], dp[rest - 1][(col + 1) % 5][j] + d);
            }
          }
        }
      }
    }
    rep(col, 5) {
      int d = inf;
      int nc = (col + 1) % 5;
      rep(i, mp[nc].size()) {
        int ds = dist(sx, sy, mp[nc][i].first, mp[nc][i].second);
        d = min(d, dp[3][nc][i] + ds);
      }
      if (d < ansDist) {
        ansDist = d;
        ansCol = col;
      }
    }
    if (ansDist == inf) {
      cout << "NA" << endl;
    } else {
      cout << ansCol + 1 << " " << ansDist << endl;
    }
  }
}

Error: TLE

Fixed submission:

#include <algorithm>
#include <cstring>
#include <iostream>
#include <math.h>
#include <queue>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <time.h>
using namespace std;
#define INF 10000000
int w, h, sy, sx, gy, gx, a, p, dist = 10000000;
int x[5][1001], y[5][1001];
int num[5], d[5000][1002];
char map[1001][1001];
int min(int a, int b) {
  if (a < b) {
    return a;
  } else {
    return b;
  }
}
int max(int a, int b) {
  if (a > b) {
    return a;
  } else {
    return b;
  }
}

int kyori(int y1, int x1, int y2, int x2) {
  return abs(y1 - y2) + abs(x1 - x2);
}
void bfs(int depth, int i, int j) { // star含まずgoal含む
  if (depth == 4) {
    a += d[i * 1000 + j][num[(i + 1) % 5] + 1];
    dist = min(dist, a);
    return;
  } else {
    if (depth == 1) {
      a = d[i * 1000 + j][num[(i + 1) % 5]];
    }
    for (int k = 0; k < num[(i + 1) % 5]; k++) {
      int p = a;
      a += d[i * 1000 + j][k];
      bfs(depth + 1, (i + 1) % 5, k);
      a = p;
    }
  }
}
int main() {
  while (1) {
    int t, color, ans = INF;
    cin >> w >> h;
    if (w + h == 0)
      break;
    for (int i = 0; i < 5; i++) {
      num[i] = 0;
    }
    for (int i = 0; i < h; i++) {
      for (int j = 0; j < w; j++) {
        cin >> map[i][j];
        switch (map[i][j]) {
        case 'S':
          sy = i;
          sx = j;
          break;
        case 'G':
          gy = i;
          gx = j;
          break;
        case '.':
          break;
        default:
          t = (int)(map[i][j] - '1');
          y[t][num[t]] = j;
          x[t][num[t]] = i;
          num[t]++;
          break;
        }
      }
    }
    int flg = 0;
    for (int i = 0; i < 5; i++) {
      if (num[i] == 0)
        flg++;
    }
    if (flg >= 2) {
      cout << "NA" << endl;
      continue;
    }
    for (int i = 0; i < 5; i++) { // i=color
      if (flg == 1 && num[i] != 0)
        continue;
      for (int j = 0; j < num[i]; j++) {
        for (int k = 0; k < num[(i + 1) % 5] + 2; k++) {
          if (k == num[(i + 1) % 5]) { // start
            d[i * 1000 + j][k] = kyori(sy, sx, y[i][j], x[i][j]);
          } else if (k == num[(i + 1) % 5] + 1) { // goal
            d[i * 1000 + j][k] = kyori(gy, gx, y[i][j], x[i][j]);
          } else {
            d[i * 1000 + j][k] =
                kyori(y[(i + 1) % 5][k], x[(i + 1) % 5][k], y[i][j], x[i][j]);
          }
        }
      }
    }
    for (int i = 0; i < 5; i++) {
      p = dist;
      for (int j = 0; j < num[i]; j++) {
        bfs(1, i, j);
      }
      if (p > dist)
        color = i;
    }
    if (color == 0)
      color = 5;
    cout << color << ' ' << dist << endl;
  }
  return 0;
}