Problem p00737 (C++)

Buggy submission (status: Time Limit Exceeded):
#include <algorithm>
#include <cstdlib>
#include <deque>
#include <iomanip>
#include <iostream>
#include <list>
#include <map>
#include <math.h>
#include <queue>
#include <set>
#include <stack>
#include <stdio.h>
#include <string>
#include <time.h>
#include <vector>
#define REP(i, N) for (ll i = 0; i < N; ++i)
#define FOR(i, a, b) for (ll i = a; i < b; ++i)
#define ALL(a) (a).begin(), (a).end()
#define pb push_back
#define INF 1000000000
#define MOD 1000000007
using namespace std;
typedef long long ll;
typedef pair<ll, ll> P;
int dx[4] = {1, 0, -1, 0};
int dy[4] = {0, 1, 0, -1};
int qx[8] = {1, 1, 0, -1, -1, -1, 0, 1};
int qy[8] = {0, 1, 1, 1, 0, -1, -1, -1};

int main(void) {
  while (true) {
    int w, h;
    cin >> w >> h;
    if (w == 0 && h == 0)
      break;
    vector<vector<int>> ins(h, vector<int>(w));
    vector<vector<vector<int>>> d(h,
                                  vector<vector<int>>(w, vector<int>(4, INF)));
    vector<int> c(4);
    REP(i, h) { REP(j, w) cin >> ins[i][j]; }
    REP(i, 4) cin >> c[i];
    priority_queue<pair<ll, pair<P, ll>>> q;
    pair<P, ll> np = pair<P, ll>(P(0, 0), 1);
    REP(i, 100000) {
      if (d[np.first.first][np.first.second][np.second] == 0)
        break;
      d[np.first.first][np.first.second][np.second] = 0;
      q.push(pair<ll, pair<P, ll>>(
          0, pair<P, ll>(P(np.first.first, np.first.second), np.second)));
      if (ins[np.first.first][np.first.second] == 4)
        break;
      ll nh = np.first.first;
      ll nw = np.first.second;
      ll nr = np.second;
      switch (ins[np.first.first][np.first.second]) {
      case 0: {
        if (np.second == 0) {
          --nh;
        }
        if (np.second == 1) {
          ++nw;
        }
        if (np.second == 2) {
          ++nh;
        }
        if (np.second == 3) {
          --nw;
        }
        break;
      }
      case 1: {
        nr = (nr + 1) % 4;
        if (np.second == 0) {
          ++nw;
        }
        if (np.second == 1) {
          ++nh;
        }
        if (np.second == 2) {
          --nw;
        }
        if (np.second == 3) {
          --nh;
        }
        break;
      }
      case 2: {
        nr = (nr + 2) % 4;
        if (np.second == 0) {
          ++nh;
        }
        if (np.second == 1) {
          --nw;
        }
        if (np.second == 2) {
          --nh;
        }
        if (np.second == 3) {
          ++nw;
        }
        break;
      }
      case 3: {
        nr = (nr + 3) % 4;
        if (np.second == 0) {
          --nw;
        }
        if (np.second == 1) {
          --nh;
        }
        if (np.second == 2) {
          ++nw;
        }
        if (np.second == 3) {
          ++nh;
        }
        break;
      }
      }
      if (nh >= 0 && nh < h && nw >= 0 && nw < w) {
        np.first.first = nh;
        np.first.second = nw;
        np.second = nr;
      } else
        break;
    }
    ll ans = INF;
    // pair<bool, bool> hoge = pair<bool, bool>(false, false);
    while (!q.empty()) {
      pair<ll, pair<P, ll>> p = q.top();
      q.pop();
      if (d[p.second.first.first][p.second.first.second][p.second.second] !=
          p.first)
        continue;
      // if(p.second.first == P(h - 1, w - 1)) {
      //	if(p.second.second == 1) hoge.first = true;
      //	if(p.second.second == 2) hoge.second = true;
      // }
      // if(hoge.first && hoge.second) break;
      REP(i, 4) {
        ll nh = p.second.first.first + dx[i];
        ll nw = p.second.first.second + dy[i];
        if (nh >= 0 && nh < h && nw >= 0 && nw < w) {
          ll cost;
          if (p.second.second == 0) {
            switch (i) {
            case 0: {
              if (ins[p.second.first.first][p.second.first.second] == 2)
                cost = 0;
              else
                cost = c[2];
              break;
            }
            case 1: {
              if (ins[p.second.first.first][p.second.first.second] == 1)
                cost = 0;
              else
                cost = c[1];
              break;
            }
            case 2: {
              if (ins[p.second.first.first][p.second.first.second] == 0)
                cost = 0;
              else
                cost = c[0];
              break;
            }
            case 3: {
              if (ins[p.second.first.first][p.second.first.second] == 3)
                cost = 0;
              else
                cost = c[3];
              break;
            }
            }
          }
          if (p.second.second == 1) {
            switch (i) {
            case 0: {
              if (ins[p.second.first.first][p.second.first.second] == 1)
                cost = 0;
              else
                cost = c[1];
              break;
            }
            case 1: {
              if (ins[p.second.first.first][p.second.first.second] == 0)
                cost = 0;
              else
                cost = c[0];
              break;
            }
            case 2: {
              if (ins[p.second.first.first][p.second.first.second] == 3)
                cost = 0;
              else
                cost = c[3];
              break;
            }
            case 3: {
              if (ins[p.second.first.first][p.second.first.second] == 2)
                cost = 0;
              else
                cost = c[2];
              break;
            }
            }
          }
          if (p.second.second == 2) {
            switch (i) {
            case 0: {
              if (ins[p.second.first.first][p.second.first.second] == 0)
                cost = 0;
              else
                cost = c[0];
              break;
            }
            case 1: {
              if (ins[p.second.first.first][p.second.first.second] == 3)
                cost = 0;
              else
                cost = c[3];
              break;
            }
            case 2: {
              if (ins[p.second.first.first][p.second.first.second] == 2)
                cost = 0;
              else
                cost = c[2];
              break;
            }
            case 3: {
              if (ins[p.second.first.first][p.second.first.second] == 1)
                cost = 0;
              else
                cost = c[1];
              break;
            }
            }
          }
          if (p.second.second == 3) {
            switch (i) {
            case 0: {
              if (ins[p.second.first.first][p.second.first.second] == 3)
                cost = 0;
              else
                cost = c[3];
              break;
            }
            case 1: {
              if (ins[p.second.first.first][p.second.first.second] == 2)
                cost = 0;
              else
                cost = c[2];
              break;
            }
            case 2: {
              if (ins[p.second.first.first][p.second.first.second] == 1)
                cost = 0;
              else
                cost = c[1];
              break;
            }
            case 3: {
              if (ins[p.second.first.first][p.second.first.second] == 0)
                cost = 0;
              else
                cost = c[0];
              break;
            }
            }
          }
          switch (i) {
          case 0: {
            if (d[nh][nw][2] > d[p.second.first.first][p.second.first.second]
                                [p.second.second] +
                                   cost) {
              d[nh][nw][2] = d[p.second.first.first][p.second.first.second]
                              [p.second.second] +
                             cost;
              q.push(pair<ll, pair<P, ll>>(d[nh][nw][2],
                                           pair<P, ll>(P(nh, nw), 2)));
            }
            break;
          }
          case 1: {
            if (d[nh][nw][1] > d[p.second.first.first][p.second.first.second]
                                [p.second.second] +
                                   cost) {
              d[nh][nw][1] = d[p.second.first.first][p.second.first.second]
                              [p.second.second] +
                             cost;
              q.push(pair<ll, pair<P, ll>>(d[nh][nw][1],
                                           pair<P, ll>(P(nh, nw), 1)));
            }
            break;
          }
          case 2: {
            if (d[nh][nw][0] > d[p.second.first.first][p.second.first.second]
                                [p.second.second] +
                                   cost) {
              d[nh][nw][0] = d[p.second.first.first][p.second.first.second]
                              [p.second.second] +
                             cost;
              q.push(pair<ll, pair<P, ll>>(d[nh][nw][0],
                                           pair<P, ll>(P(nh, nw), 0)));
            }
            break;
          }
          case 3: {
            if (d[nh][nw][3] > d[p.second.first.first][p.second.first.second]
                                [p.second.second] +
                                   cost) {
              d[nh][nw][3] = d[p.second.first.first][p.second.first.second]
                              [p.second.second] +
                             cost;
              q.push(pair<ll, pair<P, ll>>(d[nh][nw][3],
                                           pair<P, ll>(P(nh, nw), 3)));
            }
            break;
          }
          }
        }
      }
    }
    REP(i, 4) { ans = min(ans, (ll)d[h - 1][w - 1][i]); }
    cout << ans << endl;
  }
}

Fixed submission:
#include <algorithm>
#include <cstdlib>
#include <deque>
#include <iomanip>
#include <iostream>
#include <list>
#include <map>
#include <math.h>
#include <queue>
#include <set>
#include <stack>
#include <stdio.h>
#include <string>
#include <time.h>
#include <vector>
#define REP(i, N) for (ll i = 0; i < N; ++i)
#define FOR(i, a, b) for (ll i = a; i < b; ++i)
#define ALL(a) (a).begin(), (a).end()
#define pb push_back
#define INF 1000000000
#define MOD 1000000007
using namespace std;
typedef long long ll;
typedef pair<ll, ll> P;
int dx[4] = {1, 0, -1, 0};
int dy[4] = {0, 1, 0, -1};
int qx[8] = {1, 1, 0, -1, -1, -1, 0, 1};
int qy[8] = {0, 1, 1, 1, 0, -1, -1, -1};

int main(void) {
  while (true) {
    int w, h;
    cin >> w >> h;
    if (w == 0 && h == 0)
      break;
    vector<vector<int>> ins(h, vector<int>(w));
    vector<vector<vector<int>>> d(h,
                                  vector<vector<int>>(w, vector<int>(4, INF)));
    vector<int> c(4);
    REP(i, h) { REP(j, w) cin >> ins[i][j]; }
    REP(i, 4) cin >> c[i];
    priority_queue<pair<ll, pair<pair<ll, ll>, ll>>,
                   vector<pair<ll, pair<pair<ll, ll>, ll>>>,
                   greater<pair<ll, pair<pair<ll, ll>, ll>>>>
        q;
    pair<P, ll> np = pair<P, ll>(P(0, 0), 1);
    REP(i, 100000) {
      if (d[np.first.first][np.first.second][np.second] == 0)
        break;
      d[np.first.first][np.first.second][np.second] = 0;
      q.push(pair<ll, pair<P, ll>>(
          0, pair<P, ll>(P(np.first.first, np.first.second), np.second)));
      if (ins[np.first.first][np.first.second] == 4)
        break;
      ll nh = np.first.first;
      ll nw = np.first.second;
      ll nr = np.second;
      switch (ins[np.first.first][np.first.second]) {
      case 0: {
        if (np.second == 0) {
          --nh;
        }
        if (np.second == 1) {
          ++nw;
        }
        if (np.second == 2) {
          ++nh;
        }
        if (np.second == 3) {
          --nw;
        }
        break;
      }
      case 1: {
        nr = (nr + 1) % 4;
        if (np.second == 0) {
          ++nw;
        }
        if (np.second == 1) {
          ++nh;
        }
        if (np.second == 2) {
          --nw;
        }
        if (np.second == 3) {
          --nh;
        }
        break;
      }
      case 2: {
        nr = (nr + 2) % 4;
        if (np.second == 0) {
          ++nh;
        }
        if (np.second == 1) {
          --nw;
        }
        if (np.second == 2) {
          --nh;
        }
        if (np.second == 3) {
          ++nw;
        }
        break;
      }
      case 3: {
        nr = (nr + 3) % 4;
        if (np.second == 0) {
          --nw;
        }
        if (np.second == 1) {
          --nh;
        }
        if (np.second == 2) {
          ++nw;
        }
        if (np.second == 3) {
          ++nh;
        }
        break;
      }
      }
      if (nh >= 0 && nh < h && nw >= 0 && nw < w) {
        np.first.first = nh;
        np.first.second = nw;
        np.second = nr;
      } else
        break;
    }
    ll ans = INF;
    // pair<bool, bool> hoge = pair<bool, bool>(false, false);
    while (!q.empty()) {
      pair<ll, pair<P, ll>> p = q.top();
      q.pop();
      if (d[p.second.first.first][p.second.first.second][p.second.second] !=
          p.first)
        continue;
      // if(p.second.first == P(h - 1, w - 1)) {
      //	if(p.second.second == 1) hoge.first = true;
      //	if(p.second.second == 2) hoge.second = true;
      // }
      // if(hoge.first && hoge.second) break;
      REP(i, 4) {
        ll nh = p.second.first.first + dx[i];
        ll nw = p.second.first.second + dy[i];
        if (nh >= 0 && nh < h && nw >= 0 && nw < w) {
          ll cost;
          if (p.second.second == 0) {
            switch (i) {
            case 0: {
              if (ins[p.second.first.first][p.second.first.second] == 2)
                cost = 0;
              else
                cost = c[2];
              break;
            }
            case 1: {
              if (ins[p.second.first.first][p.second.first.second] == 1)
                cost = 0;
              else
                cost = c[1];
              break;
            }
            case 2: {
              if (ins[p.second.first.first][p.second.first.second] == 0)
                cost = 0;
              else
                cost = c[0];
              break;
            }
            case 3: {
              if (ins[p.second.first.first][p.second.first.second] == 3)
                cost = 0;
              else
                cost = c[3];
              break;
            }
            }
          }
          if (p.second.second == 1) {
            switch (i) {
            case 0: {
              if (ins[p.second.first.first][p.second.first.second] == 1)
                cost = 0;
              else
                cost = c[1];
              break;
            }
            case 1: {
              if (ins[p.second.first.first][p.second.first.second] == 0)
                cost = 0;
              else
                cost = c[0];
              break;
            }
            case 2: {
              if (ins[p.second.first.first][p.second.first.second] == 3)
                cost = 0;
              else
                cost = c[3];
              break;
            }
            case 3: {
              if (ins[p.second.first.first][p.second.first.second] == 2)
                cost = 0;
              else
                cost = c[2];
              break;
            }
            }
          }
          if (p.second.second == 2) {
            switch (i) {
            case 0: {
              if (ins[p.second.first.first][p.second.first.second] == 0)
                cost = 0;
              else
                cost = c[0];
              break;
            }
            case 1: {
              if (ins[p.second.first.first][p.second.first.second] == 3)
                cost = 0;
              else
                cost = c[3];
              break;
            }
            case 2: {
              if (ins[p.second.first.first][p.second.first.second] == 2)
                cost = 0;
              else
                cost = c[2];
              break;
            }
            case 3: {
              if (ins[p.second.first.first][p.second.first.second] == 1)
                cost = 0;
              else
                cost = c[1];
              break;
            }
            }
          }
          if (p.second.second == 3) {
            switch (i) {
            case 0: {
              if (ins[p.second.first.first][p.second.first.second] == 3)
                cost = 0;
              else
                cost = c[3];
              break;
            }
            case 1: {
              if (ins[p.second.first.first][p.second.first.second] == 2)
                cost = 0;
              else
                cost = c[2];
              break;
            }
            case 2: {
              if (ins[p.second.first.first][p.second.first.second] == 1)
                cost = 0;
              else
                cost = c[1];
              break;
            }
            case 3: {
              if (ins[p.second.first.first][p.second.first.second] == 0)
                cost = 0;
              else
                cost = c[0];
              break;
            }
            }
          }
          switch (i) {
          case 0: {
            if (d[nh][nw][2] > d[p.second.first.first][p.second.first.second]
                                [p.second.second] +
                                   cost) {
              d[nh][nw][2] = d[p.second.first.first][p.second.first.second]
                              [p.second.second] +
                             cost;
              q.push(pair<ll, pair<P, ll>>(d[nh][nw][2],
                                           pair<P, ll>(P(nh, nw), 2)));
            }
            break;
          }
          case 1: {
            if (d[nh][nw][1] > d[p.second.first.first][p.second.first.second]
                                [p.second.second] +
                                   cost) {
              d[nh][nw][1] = d[p.second.first.first][p.second.first.second]
                              [p.second.second] +
                             cost;
              q.push(pair<ll, pair<P, ll>>(d[nh][nw][1],
                                           pair<P, ll>(P(nh, nw), 1)));
            }
            break;
          }
          case 2: {
            if (d[nh][nw][0] > d[p.second.first.first][p.second.first.second]
                                [p.second.second] +
                                   cost) {
              d[nh][nw][0] = d[p.second.first.first][p.second.first.second]
                              [p.second.second] +
                             cost;
              q.push(pair<ll, pair<P, ll>>(d[nh][nw][0],
                                           pair<P, ll>(P(nh, nw), 0)));
            }
            break;
          }
          case 3: {
            if (d[nh][nw][3] > d[p.second.first.first][p.second.first.second]
                                [p.second.second] +
                                   cost) {
              d[nh][nw][3] = d[p.second.first.first][p.second.first.second]
                              [p.second.second] +
                             cost;
              q.push(pair<ll, pair<P, ll>>(d[nh][nw][3],
                                           pair<P, ll>(P(nh, nw), 3)));
            }
            break;
          }
          }
        }
      }
    }
    REP(i, 4) { ans = min(ans, (ll)d[h - 1][w - 1][i]); }
    cout << ans << endl;
  }
}