题目描述：
推箱子是一个很经典的游戏。今天我们来玩一个简单版本。在一个M*N的房间里有一个箱子和一个搬运工，搬运工的工作就是把箱子推到指定的位置,注意，搬运工只能推箱子而不能拉箱子，因此如果箱子被推到一个角上(如图2)那么箱子就不能再被移动了，如果箱子被推到一面墙上，那么箱子只能沿着墙移动。
现在给定房间的结构，箱子的位置，搬运工的位置和箱子要被推去的位置，请你计算出搬运工至少要推动箱子多少格。
输入格式：
输入数据的第一行是一个整数T(1<=T<=20)，代表测试数据的数量。然后是T组测试数据，每组测试数据的第一行是两个正整数M,N(2<=M,N<=7)，代表房间的大小，然后是一个M行N列的矩阵，代表房间的布局，其中0代表空的地板，1 代表墙，2 代表箱子的起始位置，3 代表箱子要被推去的位置，4 代表搬运工的起始位置。
输出格式：
对于每组测试数据，输出搬运工最少需要推动箱子多少格才能帮箱子推到指定位置，如果不能推到指定位置则输出 -1。
样例输入：
1 5 5 0 3 0 0 0 1 0 1 4 0 0 0 1 0 0 1 0 2 0 0 0 0 0 0 0
样例输出：
4
我的代码
#include<bits/stdc++.h>
using namespace std;
const int N=7;
struct Node{
int bx,by,px,py,step;
Node(){}
Node(int bx,int by,int px,int py,int step){
this->bx=bx;
this->by=by;
this->px=px;
this->py=py;
this->step=step;
}
};
int n,m,sx,sy,dx,dy,px,py,a[N][N],s[N][N][N][N],nxtx[]={1,-1,0,0},nxty[]={0,0,1,-1};
int bfs(){
deque<Node>q;
memset(s,-1,sizeof(s));
q.push_back(Node(sx,sy,px,py,0));
s[sx][sy][px][py]=0;
int nx,ny,tx,ty,w;
bool flag;
while(q.size()){
Node now=q.front();
q.pop_front();
if(now.step>s[now.bx][now.by][now.px][now.py]){
continue;
}
w=s[now.bx][now.by][now.px][now.py];
if(now.bx==dx && now.by==dy){
return w;
}
for(int i=0;i<4;i++){
nx=now.px+nxtx[i];
ny=now.py+nxty[i];
tx=now.bx;
ty=now.by;
flag=0;
if(nx<0 || ny<0 || nx>=n || ny>=m || a[nx][ny]==1){
continue;
}
if(nx==now.bx && ny==now.by){
tx+=nxtx[i];
ty+=nxty[i];
if(tx<0 || ty<0 || tx>=n || ty>=m || a[tx][ty]==1){
continue;
}
flag=1;
}
w+=flag;
if(s[tx][ty][nx][ny]==-1 || s[tx][ty][nx][ny]>w){
s[tx][ty][nx][ny]=w;
if(flag){
q.push_back(Node(tx,ty,nx,ny,w));
}
else{
q.push_front(Node(tx,ty,nx,ny,w));
}
}
w-=flag;
}
}
return -1;
}
void solve(){
cin>>n>>m;
for(int i=0;i<n;i++){
for(int j=0;j<m;j++){
cin>>a[i][j];
if(a[i][j]==2){
sx=i;
sy=j;
}
else if(a[i][j]==3){
dx=i;
dy=j;
}
else if(a[i][j]==4){
px=i;
py=j;
}
}
}
cout<<bfs();
}
int main(){
int t;
cin>>t;
while(t--){
solve();
}
return 0;
}