Problem p01401 (C++)

Buggy submission (status: Runtime Error):
#include <algorithm>
#include <cstdlib>
#include <cstring>
#include <iostream>
#include <map>
#include <queue>
#include <vector>
using namespace std;
#define fr first
#define sc second
typedef pair<int, int> P;
P s, g;
typedef pair<P, P> Q;
int h, w, sx, sy, gx, gy, c, dx[] = {0, 1, 0, -1}, dy[] = {1, 0, -1, 0},
                             num[105][105];
bool used[25][105][105];
char wara[100];
int bfs();
int main() {
  while (cin >> w >> h && w || h) {
    memset(num, 0, sizeof(num));
    for (int i = 0; i < h; i++) {
      for (int j = 0; j < w; j++) {
        cin >> wara;
        if (strcmp(wara, "S") == 0)
          s = P(i, j);
        else if (strcmp(wara, "G") == 0)
          g = P(i, j);
        else if (strcmp(wara, ".") != 0) {
          c = max(c, atoi(wara));
          num[i][j] = atoi(wara);
        }
      }
    }
    num[g.fr][g.sc] = c + 1;
    cout << bfs() << endl;
    c = 0;
  }
}
int bfs() {
  memset(used, false, sizeof(used));
  queue<Q> que;
  que.push(Q(s, P(1, 0)));
  used[1][s.fr][s.sc] = true;
  while (!que.empty()) {
    Q q = que.front();
    que.pop();
    if (q.sc.fr == c + 1 && q.fr == g)
      return q.sc.sc;
    if (num[q.fr.fr][q.fr.sc] == q.sc.fr &&
        !used[q.sc.fr + 1][q.fr.fr][q.fr.sc]) {
      used[q.sc.fr + 1][q.fr.fr][q.fr.sc] = true;
      q.sc.fr++;
    }
    for (int i = 0; i < 4; i++) {
      int nx = q.fr.fr + dx[i], ny = q.fr.sc + dy[i];
      bool flg = true;
      if (nx >= 0 && nx < h && ny >= 0 && ny < w && !used[q.sc.fr][nx][ny]) {
        used[q.sc.fr][nx][ny] = true;
        que.push(Q(P(nx, ny), P(q.sc.fr + (flg ? 0 : 1), q.sc.sc + 1)));
      }
    }
  }
  return -1;
}

Fixed submission:
#include <algorithm>
#include <cstdlib>
#include <cstring>
#include <iostream>
#include <map>
#include <queue>
#include <vector>
using namespace std;
#define fr first
#define sc second
typedef pair<int, int> P;
P s, g;
typedef pair<P, P> Q;
int h, w, sx, sy, gx, gy, c, dx[] = {0, 1, 0, -1}, dy[] = {1, 0, -1, 0},
                             num[105][105];
bool used[2500][105][105];
char wara[100];
int bfs();
int main() {
  while (cin >> w >> h && w || h) {
    memset(num, 0, sizeof(num));
    for (int i = 0; i < h; i++) {
      for (int j = 0; j < w; j++) {
        cin >> wara;
        if (strcmp(wara, "S") == 0)
          s = P(i, j);
        else if (strcmp(wara, "G") == 0)
          g = P(i, j);
        else if (strcmp(wara, ".") != 0) {
          c = max(c, atoi(wara));
          num[i][j] = atoi(wara);
        }
      }
    }
    num[g.fr][g.sc] = c + 1;
    cout << bfs() << endl;
    c = 0;
  }
}
int bfs() {
  memset(used, false, sizeof(used));
  queue<Q> que;
  que.push(Q(s, P(1, 0)));
  used[1][s.fr][s.sc] = true;
  while (!que.empty()) {
    Q q = que.front();
    que.pop();
    if (q.sc.fr == c + 1 && q.fr == g)
      return q.sc.sc;
    if (num[q.fr.fr][q.fr.sc] == q.sc.fr &&
        !used[q.sc.fr + 1][q.fr.fr][q.fr.sc]) {
      used[q.sc.fr + 1][q.fr.fr][q.fr.sc] = true;
      q.sc.fr++;
    }
    for (int i = 0; i < 4; i++) {
      int nx = q.fr.fr + dx[i], ny = q.fr.sc + dy[i];
      bool flg = true;
      if (nx >= 0 && nx < h && ny >= 0 && ny < w && !used[q.sc.fr][nx][ny]) {
        used[q.sc.fr][nx][ny] = true;
        que.push(Q(P(nx, ny), P(q.sc.fr + (flg ? 0 : 1), q.sc.sc + 1)));
      }
    }
  }
  return -1;
}