第三个点TLE，其他的点只有3~4ms

#include<bits/stdc++.h>
#define int long long
using namespace std;
int w,h,a[1005][1005],ans=1e18,o,p,dx[]={0,0,1,-1},dy[]={1,-1,0,0};
bool f[1005][1005],f2[1005][1005];
struct node{
	int x[1000005],y[1000005],id[1000005];
	int front,back;
	void pop(){
		front++;
		return;
	}
	void push(int nx,int ny,int nid){
		++back;
		x[back]=nx;
		y[back]=ny;
		id[back]=nid;
		return;
	}
}q,q2;
int bfs2(int x,int y){
	memset(f,0,sizeof(f));
	q.front=1;
	q.back=0;
	q.push(x,y,0);
	while(q.back>=q.front){
		int ax = q.x[q.front];
		int ay = q.y[q.front];
		int id = q.id[q.front];
		if(a[ax][ay]==3){
			o=ax;
			p=ay;
			return id;
		}
		q.pop();
		for(int i = 0;i<4;i++){
			int nx = ax+dx[i];
			int ny = ay+dy[i];
			if(nx<1||ny<1||nx>h||ny>w||f[nx][ny])continue;
			if(a[nx][ny]==1)continue;
			f[nx][ny]=1;
			q.push(nx,ny,id+1);
		}
	} 
}
void bfs1(int x,int y){
	q2.front=1;
	q2.back=0;
	q2.push(x,y,0);
	while(q2.back>=q2.front){
		int ax = q2.x[q2.front];
		int ay = q2.y[q2.front];
		int id = q2.id[q2.front];
		if(a[ax][ay]==4){
			o=ax;
			p=ay;
			ans=min(ans,id+bfs2(ax,ay));
		}
		q2.pop();
		for(int i = 0;i<4;i++){
			int nx = ax+dx[i];
			int ny = ay+dy[i];
			if(nx<1||ny<1||nx>h||ny>w||f2[nx][ny])continue;
			if(a[nx][ny]==1||a[nx][ny]==3)continue;
		    f2[nx][ny]=1;
			q2.push(nx,ny,id+1);
		}
	}
	return;
}
signed main(){
    cin >> w >> h;
    for(int i = 1;i<=h;i++){
    	for(int j = 1;j<=w;j++)scanf("%d",&a[i][j]);
	}
	for(int i = 1;i<=h;i++){
		for(int j = 1;j<=w;j++){
			if(a[i][j]==2){
				bfs1(i,j);
				cout << ans;
				return 0;
			}
		}
	}
    return 0;
}