Problem p00860 (C++)

Buggy submission (status: Time Limit Exceeded):
#include <algorithm>
#include <array>
#include <cassert>
#include <cctype>
#include <iostream>
#include <map>
#include <queue>
#include <unordered_set>
#include <vector>
#define repeat(i, n) for (int i = 0; (i) < (n); ++(i))
#define repeat_from(i, m, n) for (int i = (m); (i) < (n); ++(i))
#define repeat_reverse(i, n) for (int i = (n)-1; (i) >= 0; --(i))
#define repeat_from_reverse(i, m, n) for (int i = (n)-1; (i) >= (m); --(i))
#define dump(x) cerr << #x << " = " << (x) << endl
#define debug(x)                                                               \
  cerr << #x << " = " << (x) << " (L" << __LINE__ << ")"                       \
       << " " << __FILE__ << endl
typedef long long ll;
using namespace std;

struct point_t {
  int y, x;
};
point_t operator+(point_t const &a, point_t const &b) {
  return (point_t){a.y + b.y, a.x + b.x};
}
bool operator==(point_t const &a, point_t const &b) {
  return make_pair(a.y, a.x) == make_pair(b.y, b.x);
}
bool operator<(point_t const &a, point_t const &b) {
  return make_pair(a.y, a.x) < make_pair(b.y, b.x);
}
static point_t dp[5] = {{-1, 0}, {1, 0}, {0, 1}, {0, -1}, {0, 0}};

struct state_t {
  array<point_t, 3> a;
  int cost;
  int dist;
};
bool operator<(state_t const &a, state_t const &b) {
  return 2 * a.cost + a.dist > 2 * b.cost + b.dist;
}
uint32_t encode(array<point_t, 3> const &s) {
  uint32_t q = 0;
  for (point_t p : s) {
    q = (q * 19) + p.y;
    q = (q * 16) + p.x;
  }
  return q;
}
bool is_valid_move(int i, int j, array<point_t, 3> const &s,
                   array<point_t, 3> const &t) {
  if (t[i] == t[j])
    return false;
  if (t[i] == s[j] and t[j] == s[i])
    return false;
  return true;
}

int main() {
  while (true) {
    int w, h, n;
    cin >> w >> h >> n;
    cin.ignore();
    if (w == 0 and h == 0 and n == 0)
      break;
    vector<string> c(h);
    repeat(i, h) getline(cin, c[i]);

    assert(1 <= n and n <= 3);
    array<point_t, 3> start, goal;
    repeat(y, h) repeat(x, w) {
      if (islower(c[y][x])) {
        start[c[y][x] - 'a'] = {y, x};
      } else if (isupper(c[y][x])) {
        goal[c[y][x] - 'A'] = {y, x};
      }
    }
    if (n == 1 or n == 2) {
      assert(4 <= w);
      string w = c.back();
      c.push_back(w);
      // c
      c.back()[2] = '.';
      start[2] = goal[2] = (point_t){h, 2};
      if (n == 1) {
        // b
        c.back()[1] = '.';
        start[1] = goal[1] = (point_t){h, 1};
      }
      c.push_back(w);
      h += 2;
    }

    vector<vector<int>> dist[3]; // from goal
    repeat(i, 3) {
      dist[i].resize(h, vector<int>(w, 1000000007));
      // bfs
      vector<point_t> cur, nxt;
      nxt.push_back(goal[i]);
      dist[i][goal[i].y][goal[i].x] = 0;
      int l = 1;
      while (not nxt.empty()) {
        cur.clear();
        cur.swap(nxt);
        for (auto p : cur) {
          repeat(j, 4) {
            auto q = p + dp[j];
            if (c[q.y][q.x] == '#')
              continue;
            if (l < dist[i][q.y][q.x]) {
              dist[i][q.y][q.x] = l;
              nxt.push_back(q);
            }
          }
        }
        ++l;
      }
    }

    map<point_t, int> ix;
    repeat(y, h) repeat(x, w) {
      if (c[y][x] != '#') {
        int i = ix.size();
        ix[(point_t){y, x}] = i;
      }
    }
    vector<vector<vector<bool>>> used(
        ix.size(), vector<vector<bool>>(ix.size(), vector<bool>(ix.size())));

    priority_queue<state_t> que;
    {
      state_t initial = {start, 0, 0};
      repeat(i, 3) initial.dist += dist[i][start[i].y][start[i].x];
      used[ix[start[0]]][ix[start[1]]][ix[start[2]]] = true;
      que.push(initial);
    }
    while (not que.empty()) {
      state_t st = que.top();
      que.pop();
      array<point_t, 3> &s = st.a;
      if (s == goal) {
        cout << st.cost << endl;
        break;
      }
      state_t tt = st;
      tt.cost += 1;
      array<point_t, 3> &t = tt.a;
      repeat(i, 5) { // a
        t[0] = s[0] + dp[i];
        if (c[t[0].y][t[0].x] == '#')
          continue;
        repeat(j, 5) { // b
          t[1] = s[1] + dp[j];
          if (c[t[1].y][t[1].x] == '#')
            continue;
          if (not is_valid_move(0, 1, s, t))
            continue;
          repeat(k, 5) { // c
            t[2] = s[2] + dp[k];
            if (c[t[2].y][t[2].x] == '#')
              continue;
            if (not is_valid_move(1, 2, s, t))
              continue;
            if (not is_valid_move(2, 0, s, t))
              continue;
            if (used[ix[t[0]]][ix[t[1]]][ix[t[2]]])
              continue;
            used[ix[t[0]]][ix[t[1]]][ix[t[2]]] = true;
            tt.dist = 0;
            repeat(i, 3) tt.dist = max(tt.dist, dist[i][t[i].y][t[i].x]);
            que.push(tt);
          }
        }
      }
    }
  }
  return 0;
}

Fixed submission:
#include <algorithm>
#include <array>
#include <cassert>
#include <cctype>
#include <iostream>
#include <map>
#include <queue>
#include <unordered_set>
#include <vector>
#define repeat(i, n) for (int i = 0; (i) < (n); ++(i))
#define repeat_from(i, m, n) for (int i = (m); (i) < (n); ++(i))
#define repeat_reverse(i, n) for (int i = (n)-1; (i) >= 0; --(i))
#define repeat_from_reverse(i, m, n) for (int i = (n)-1; (i) >= (m); --(i))
#define dump(x) cerr << #x << " = " << (x) << endl
#define debug(x)                                                               \
  cerr << #x << " = " << (x) << " (L" << __LINE__ << ")"                       \
       << " " << __FILE__ << endl
typedef long long ll;
using namespace std;

struct point_t {
  int y, x;
};
point_t operator+(point_t const &a, point_t const &b) {
  return (point_t){a.y + b.y, a.x + b.x};
}
bool operator==(point_t const &a, point_t const &b) {
  return make_pair(a.y, a.x) == make_pair(b.y, b.x);
}
bool operator<(point_t const &a, point_t const &b) {
  return make_pair(a.y, a.x) < make_pair(b.y, b.x);
}
static point_t dp[5] = {{-1, 0}, {1, 0}, {0, 1}, {0, -1}, {0, 0}};

struct state_t {
  array<point_t, 3> a;
  int cost;
  int dist;
};
bool operator<(state_t const &a, state_t const &b) {
  return 1.3 * a.cost + a.dist > 1.3 * b.cost + b.dist;
}
uint32_t encode(array<point_t, 3> const &s) {
  uint32_t q = 0;
  for (point_t p : s) {
    q = (q * 19) + p.y;
    q = (q * 16) + p.x;
  }
  return q;
}
bool is_valid_move(int i, int j, array<point_t, 3> const &s,
                   array<point_t, 3> const &t) {
  if (t[i] == t[j])
    return false;
  if (t[i] == s[j] and t[j] == s[i])
    return false;
  return true;
}

int main() {
  while (true) {
    int w, h, n;
    cin >> w >> h >> n;
    cin.ignore();
    if (w == 0 and h == 0 and n == 0)
      break;
    vector<string> c(h);
    repeat(i, h) getline(cin, c[i]);

    assert(1 <= n and n <= 3);
    array<point_t, 3> start, goal;
    repeat(y, h) repeat(x, w) {
      if (islower(c[y][x])) {
        start[c[y][x] - 'a'] = {y, x};
      } else if (isupper(c[y][x])) {
        goal[c[y][x] - 'A'] = {y, x};
      }
    }
    if (n == 1 or n == 2) {
      assert(4 <= w);
      string w = c.back();
      c.push_back(w);
      // c
      c.back()[2] = '.';
      start[2] = goal[2] = (point_t){h, 2};
      if (n == 1) {
        // b
        c.back()[1] = '.';
        start[1] = goal[1] = (point_t){h, 1};
      }
      c.push_back(w);
      h += 2;
    }

    vector<vector<int>> dist[3]; // from goal
    repeat(i, 3) {
      dist[i].resize(h, vector<int>(w, 1000000007));
      // bfs
      vector<point_t> cur, nxt;
      nxt.push_back(goal[i]);
      dist[i][goal[i].y][goal[i].x] = 0;
      int l = 1;
      while (not nxt.empty()) {
        cur.clear();
        cur.swap(nxt);
        for (auto p : cur) {
          repeat(j, 4) {
            auto q = p + dp[j];
            if (c[q.y][q.x] == '#')
              continue;
            if (l < dist[i][q.y][q.x]) {
              dist[i][q.y][q.x] = l;
              nxt.push_back(q);
            }
          }
        }
        ++l;
      }
    }

    map<point_t, int> ix;
    repeat(y, h) repeat(x, w) {
      if (c[y][x] != '#') {
        int i = ix.size();
        ix[(point_t){y, x}] = i;
      }
    }
    vector<vector<vector<bool>>> used(
        ix.size(), vector<vector<bool>>(ix.size(), vector<bool>(ix.size())));

    priority_queue<state_t> que;
    {
      state_t initial = {start, 0, 0};
      repeat(i, 3) initial.dist += dist[i][start[i].y][start[i].x];
      used[ix[start[0]]][ix[start[1]]][ix[start[2]]] = true;
      que.push(initial);
    }
    while (not que.empty()) {
      state_t st = que.top();
      que.pop();
      array<point_t, 3> &s = st.a;
      if (s == goal) {
        cout << st.cost << endl;
        break;
      }
      state_t tt = st;
      tt.cost += 1;
      array<point_t, 3> &t = tt.a;
      repeat(i, 5) { // a
        t[0] = s[0] + dp[i];
        if (c[t[0].y][t[0].x] == '#')
          continue;
        repeat(j, 5) { // b
          t[1] = s[1] + dp[j];
          if (c[t[1].y][t[1].x] == '#')
            continue;
          if (not is_valid_move(0, 1, s, t))
            continue;
          repeat(k, 5) { // c
            t[2] = s[2] + dp[k];
            if (c[t[2].y][t[2].x] == '#')
              continue;
            if (not is_valid_move(1, 2, s, t))
              continue;
            if (not is_valid_move(2, 0, s, t))
              continue;
            if (used[ix[t[0]]][ix[t[1]]][ix[t[2]]])
              continue;
            used[ix[t[0]]][ix[t[1]]][ix[t[2]]] = true;
            tt.dist = 0;
            repeat(i, 3) tt.dist = max(tt.dist, dist[i][t[i].y][t[i].x]);
            que.push(tt);
          }
        }
      }
    }
  }
  return 0;
}