Problem p00223 (C++)

Buggy submission (status: Runtime Error):
#include <algorithm>
#include <bitset>
#include <cstdio>
#include <cstdlib>
#include <iostream>
#include <iterator>
#include <map>
#include <math.h>
#include <queue>
#include <set>
#include <sstream>
#include <string>
#include <time.h>
#include <vector>
using namespace std;
#define FOR(I, F, N) for (int I = F; I < (int)(N); I++)
#define rep(i, n) FOR(i, 0, n)
#define FIN(V) cout << V << endl
#define pb push_back
#define INF (1 << 30)

template <typename T> void remove(vector<T> &vector, unsigned int index) {
  vector.erase(vector.begin() + index);
}

typedef pair<int, int> P;
typedef long long ll;
typedef priority_queue<int> pq;
int StrToInt(string);
string IntToStr(int);
int dx[4] = {1, -1, 0, 0};
int dy[4] = {0, 0, 1, -1};

int H, W;
// maps[y][x];
int state[55][55];
int maps[55][55][55][55];

struct Data {
  P t, k;
  int cost;
  Data(P _t, P _k, int c) {
    t = _t;
    k = _k;
    cost = c;
  }
  Data() {}
};

int bfs(Data d) {
  queue<Data> que;
  que.push(d);
  int ans = -1;
  while (que.size()) {
    Data d = que.front();
    que.pop();
    P taka = d.t;
    P kazu = d.k;
    int cost = d.cost;
    if (cost >= 100)
      break;

    if (maps[taka.first][taka.second][kazu.first][kazu.second] < cost)
      continue;
    maps[taka.first][taka.second][kazu.first][kazu.second] = cost;

    if (taka.first == kazu.first && taka.second == kazu.second) {
      ans = cost;
      break;
    }
    for (int i = 0; i < 4; i++) {
      int ntx, nty, nkx, nky;
      ntx = taka.first + dx[i];
      nty = taka.second + dy[i];
      nkx = kazu.first - dx[i];
      nky = kazu.second - dy[i];
      if (ntx <= -1 || nty <= -1) {
        ntx = taka.first;
        nty = taka.second;
      }
      if (nkx <= -1 || nky <= -1) {
        nkx = kazu.first;
        nky = kazu.second;
      }
      if (state[nky][nkx] != 0) {
        nkx = kazu.first;
        nky = kazu.second;
      }
      if (state[nty][ntx] != 0) {
        ntx = taka.first;
        nty = taka.second;
      }
      /// cout << ntx << "," << nty << " " << nkx << "," << nky << endl;
      que.push(Data(P(ntx, nty), P(nkx, nky), cost + 1));
    }
  }
  return ans;
}

int main(void) {

  while (cin >> W >> H, W | H) {
    int x, y;
    fill_n(*state, 55 * 55, -1);
    fill_n(***maps, 55 * 55 * 55 * 55, INF);
    cin >> x >> y;
    x--;
    y--;
    P taka = P(x, y);
    cin >> x >> y;
    x--;
    y--;
    P kazu = P(x, y);
    rep(i, H) rep(j, W) { cin >> state[i][j]; }
    int ans = bfs(Data(taka, kazu, 0));
    if (ans < 0 || ans >= 100) {
      cout << "NA" << endl;
      continue;
    }
    cout << ans << endl;
  }

  return 0;
}

int StrToInt(string s) {
  stringstream ss;
  ss << s;
  int val;
  ss >> val;
  return val;
}
string IntToStr(int i) {
  stringstream ss;
  ss << i;
  return ss.str();
}

Fixed submission:
#include <algorithm>
#include <bitset>
#include <cstdio>
#include <cstdlib>
#include <iostream>
#include <iterator>
#include <map>
#include <math.h>
#include <queue>
#include <set>
#include <sstream>
#include <string>
#include <time.h>
#include <vector>
using namespace std;
#define FOR(I, F, N) for (int I = F; I < (int)(N); I++)
#define rep(i, n) FOR(i, 0, n)
#define FIN(V) cout << V << endl
#define pb push_back
#define INF (1 << 30)

template <typename T> void remove(vector<T> &vector, unsigned int index) {
  vector.erase(vector.begin() + index);
}

typedef pair<int, int> P;
typedef long long ll;
typedef priority_queue<int> pq;
int StrToInt(string);
string IntToStr(int);
int dx[4] = {1, -1, 0, 0};
int dy[4] = {0, 0, 1, -1};

int H, W;
// maps[y][x];
int state[55][55];
int maps[55][55][55][55];

struct Data {
  P t, k;
  int cost;
  Data(P _t, P _k, int c) {
    t = _t;
    k = _k;
    cost = c;
  }
  Data() {}
};

int bfs(Data d) {
  queue<Data> que;
  que.push(d);
  int ans = -1;
  while (que.size()) {
    Data d = que.front();
    que.pop();
    P taka = d.t;
    P kazu = d.k;
    int cost = d.cost;
    if (cost >= 100)
      break;

    if (maps[taka.first][taka.second][kazu.first][kazu.second] <= cost)
      continue;
    maps[taka.first][taka.second][kazu.first][kazu.second] = cost;

    if (taka.first == kazu.first && taka.second == kazu.second) {
      ans = cost;
      break;
    }
    for (int i = 0; i < 4; i++) {
      int ntx, nty, nkx, nky;
      ntx = taka.first + dx[i];
      nty = taka.second + dy[i];
      nkx = kazu.first - dx[i];
      nky = kazu.second - dy[i];
      if (ntx <= -1 || nty <= -1) {
        ntx = taka.first;
        nty = taka.second;
      }
      if (nkx <= -1 || nky <= -1) {
        nkx = kazu.first;
        nky = kazu.second;
      }
      if (state[nky][nkx] != 0) {
        nkx = kazu.first;
        nky = kazu.second;
      }
      if (state[nty][ntx] != 0) {
        ntx = taka.first;
        nty = taka.second;
      }
      /// cout << ntx << "," << nty << " " << nkx << "," << nky << endl;
      que.push(Data(P(ntx, nty), P(nkx, nky), cost + 1));
    }
  }
  return ans;
}

int main(void) {

  while (cin >> W >> H, W | H) {
    int x, y;
    fill_n(*state, 55 * 55, -1);
    fill_n(***maps, 55 * 55 * 55 * 55, INF);
    cin >> x >> y;
    x--;
    y--;
    P taka = P(x, y);
    cin >> x >> y;
    x--;
    y--;
    P kazu = P(x, y);
    rep(i, H) rep(j, W) { cin >> state[i][j]; }
    int ans = bfs(Data(taka, kazu, 0));
    if (ans < 0 || ans >= 100) {
      cout << "NA" << endl;
      continue;
    }
    cout << ans << endl;
  }

  return 0;
}

int StrToInt(string s) {
  stringstream ss;
  ss << s;
  int val;
  ss >> val;
  return val;
}
string IntToStr(int i) {
  stringstream ss;
  ss << i;
  return ss.str();
}