后面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;
}