机器人，BFS

Problem Description
Dr.Kong设计了一个可以前进或后退机器人，该机器人在每个位置i会得到一个移动步数的指令Ki (i=1,2…N),聪明的机器人自己会判断是要前进Ki步还是后退Ki步。

例如：给定指令序列(3 3 1 2 5)，表示机器人在第1个位置时，可以前进3步到第4个位置，此时后退是不起作用的，出界；机器人在第2个位置时，可以前进3步到第5个位置，此时后退是不起作用的，出界；机器人在第3个位置时，可以前进1步到第4个位置，也可以后退1步到第2个位置等等。

你认为，对给定的两个位置 A,B, 聪明的机器人从A位置走到B位置至少要判断几次？
Input
【标准输入】
第一行: M 表示以下有M组测试数据（0<M<=8）
接下来每组有两行数据
头一行：N A B ( 1≤ N≤ 50, 1≤A,B ≤N )
下一行: K1 K2…..Kn ( 0<=Ki<=N )
Output
【标准输出】
输出有M行，第i行为第i组测试数据的最少判断次数, 若无法到达，则输出-1。
Sample Input
2
5 1 5
3 3 1 2 5
8 5 3
1 2 1 5 3 1 1 1
Sample Output
3
-1

#include <stdio.h>
#include <stdlib.h>
//6个方向的搜索。
//最终一定会有结果，
///用pre指针，指向每一个前一个，动作状态。 
int step[52]={0};
int count[10000]={0};
int Q[10000]={0};
int visited[10000]={0};
int head,tail;
void push(int x)
{
    Q[tail++] = x;
}
int pop()
{
    return Q[head++];
}

int Qempty()
{
    if( head == tail )
        return 1;
    return 0;
}
int main(void)
{
    int ncases,A,B,N,move,s,next,i;
    scanf("%d",&ncases);
    while(ncases--)
    {
        scanf("%d%d%d",&N,&A,&B);
        for( i=1; i<=N; i++ )
            scanf("%d",&step[i]);
            
        push(A);
        visited[A] = 1;
        
        while( !Qempty() )
        {
            s= pop();
            if(s == B)
                break;
            move = step[s];
            //起始的位置加上，移动数组里面的值，下一次移动的距离。 
            //两个方向的搜索。 
            next = s + move;
            if( next <=B && visited[next] == 0 )
            {
                push(next);
                visited[next] = 1;
                count[next] = count[s] + 1;
            }
            
            next = s - move;
            if( next >= 1 && visited[next] == 0 )
            {
                push(next);
                visited[next] = 1;
                count[next] = count[s] + 1;
            }
        }
        if( s == B )
            printf("%d\n",count[s]);
        else
            printf("-1\n");
    }

return 0;
}