后面6个点tle和re了，求助各位大佬

#include <cstdio>
#include <algorithm>
#include <queue>
#include <iostream>
using namespace std;
int m,vis[500][500]={0},d[5]={-1,0,1,0,-1},T[500][500]={0};
struct node
{
	int x,y,t;
};
int inmap(int x,int y)
{
	return x>=0&&y>=0;
}
void BFS()
{
	queue<node> q;
	q.push({0,0,0});
	while(!q.empty())
	{
		node now=q.front();
		for(int i=0;i<=3;i++)
		{
			if(inmap(now.x+d[i],now.y+d[i+1])&&((!vis[now.x+d[i]][now.y+d[i+1]])||(vis[now.x+d[i]][now.y+d[i+1]]&&T[now.x+d[i]][now.y+d[i+1]]-now.t>1)))
			{
				int nx=now.x+d[i];
				int ny=now.y+d[i+1];
				vis[now.x][now.y]=1;
				vis[0][0]=1;
				q.push({nx,ny,now.t+1});
				if(!vis[nx][ny])
				{
					printf("%d",now.t+1);
					return;
				}
			}
		}
			q.pop();
	}
	printf("-1");
}
struct stu
{
	int x,y,t;
}h[10000];
bool cmp(stu x,stu y)
{
	return x.t>y.t;
}
int main()
{
	int i,j; 
	scanf("%d",&m);
	for(i=0;i<m;i++)
	{
		scanf("%d%d%d",&h[i].x,&h[i].y,&h[i].t);
	}
	sort(h,h+m,cmp);
	for(i=0;i<m;i++)
	{
		vis[h[i].x][h[i].y]=1,vis[h[i].x+1][h[i].y]=1,vis[h[i].x][h[i].y+1]=1;
		T[h[i].x][h[i].y]=h[i].t,T[h[i].x+1][h[i].y]=h[i].t,T[h[i].x][h[i].y+1]=h[i].t;
		if(h[i].x>=1)
		{
			vis[h[i].x-1][h[i].y]=1;
			T[h[i].x-1][h[i].y]=h[i].t;
		}
		if(h[i].y>=1)
		{
			vis[h[i].x][h[i].y-1]=1;
			T[h[i].x][h[i].y-1]=h[i].t;
		}
	}
	
	BFS();
	return 0;
}