Кратчайший путь

public void FindWave(int startX, int startY, int targetX, int targetY)
        {
            bool add = true;
            int[,] cMap = new int[Map.GetLength(0), Map.GetLength(1)];
            int x, y, step = 0;
            for (y = 0; y < Map.GetLength(0); y++)
                for (x = 0; x < Map.GetLength(1); x++)
                {
                    if (Map[y, x] == 0)
                        cMap[y, x] = -2;//индикатор стены
                    else
                        cMap[y, x] = -1;//индикатор еще не ступали сюда
                }
            cMap[targetX, targetY] = 0;//Начинаем с финиша
            while (add == true)
            {
                add = false;
                for (y = 0; y < Map.GetLength(1); y++)
                    for (x = 0; x < Map.GetLength(0); x++)
                    {
                        if (cMap[x, y] == step)
                        {
 
                            //Ставим значение шага+1 в соседние ячейки (если они проходимы)
                            if (y - 1 >= 0 && cMap[x, y - 1] != -2 && cMap[x, y - 1] == -1)
                                cMap[x, y - 1] = step + 1;
                            if (x - 1 >= 0 && cMap[x - 1, y] != -2 && cMap[x - 1, y] == -1)
                                cMap[x - 1, y] = step + 1;
 
                            if (y + 1 < Map.GetLength(1) && cMap[x, y + 1] != -2 && cMap[x, y + 1] == -1)
                                cMap[x, y + 1] = step + 1;
                            if (x + 1 < Map.GetLength(0) && cMap[x + 1, y] != -2 && cMap[x + 1, y] == -1)
                                cMap[x + 1, y] = step + 1;
                        }
                    }
                step++;
 
                add = true;
                if (cMap[startY, startX] == -1)//решение найдено
                    add = true;
                if (step > Map.GetLength(0) * Map.GetLength(1))//решение не найдено
                    add = false;
            }
}