Tiled Map и волновой алгоритм

void find_path(int N,int M)
{
    struct t{
        int x, y;
    };
    t Q[10000], S, G;
    int i_st=0, i_end=1, t, i, j, a[100][100]={0};
    for(i=0; i<N; i++)
        for(j=0; j<M; j++)
        {
            if(wayToPacMan[i][j]=='G') {G.x=i; G.y=j;}
            if(wayToPacMan[i][j]=='S') {S.x=i; S.y=j;}
        }
    a[G.x][G.y]=1;
    Q[0].x=G.x; Q[0].y=G.y;
    while(i_st<i_end)
    {
        if(a[Q[i_st].x-1][Q[i_st].y]==0 && wayToPacMan[Q[i_st].x-1][Q[i_st].y]!='#')
        {
            Q[i_end].x=Q[i_st].x-1; Q[i_end++].y=Q[i_st].y;
            a[Q[i_st].x-1][Q[i_st].y]=a[Q[i_st].x][Q[i_st].y]+1;
        }
        if(a[Q[i_st].x+1][Q[i_st].y]==0 && wayToPacMan[Q[i_st].x+1][Q[i_st].y]!='#')
        {
            Q[i_end].x=Q[i_st].x+1; Q[i_end++].y=Q[i_st].y;
            a[Q[i_st].x+1][Q[i_st].y]=a[Q[i_st].x][Q[i_st].y]+1;
        }
        if(a[Q[i_st].x][Q[i_st].y-1]==0 && wayToPacMan[Q[i_st].x][Q[i_st].y-1]!='#')
        {
            Q[i_end].x=Q[i_st].x; Q[i_end++].y=Q[i_st].y-1;
            a[Q[i_st].x][Q[i_st].y-1]=a[Q[i_st].x][Q[i_st].y]+1;
        }
        if(a[Q[i_st].x][Q[i_st].y+1]==0 && wayToPacMan[Q[i_st].x][Q[i_st].y+1]!='#')
        {
            Q[i_end].x=Q[i_st].x; Q[i_end++].y=Q[i_st].y+1;
            a[Q[i_st].x][Q[i_st].y+1]=a[Q[i_st].x][Q[i_st].y]+1;
        }
        i_st++;
    }
    if(a[S.x][S.y]==0)
        cout<<"No way"<<endl;
    else
    {
        t=a[S.x][S.y];
        while(t>1)
        {
            if(a[S.x-1][S.y]==t-1)
            {
                wayToPacMan[S.x-1][S.y]='+';
                S.x--; t--;
            }
            else
            if(a[S.x+1][S.y]==t-1)
            {
                wayToPacMan[S.x+1][S.y]='+';
                S.x++; t--;
            }
            else
            if(a[S.x][S.y-1]==t-1)
            {
                wayToPacMan[S.x][S.y-1]='+';
                S.y--; t--;
            }
            else
            if(a[S.x][S.y+1]==t-1)
            {
                wayToPacMan[S.x][S.y+1]='+';
                S.y++; t--;
            }
        }
    }
}