Волновой алгоритм поиска пути на PHP

На учебе по методам оптимизации дали список тем для курсача, я выбрал волновой алгоритм поиска пути.

Собственно сам код с комментариям:

<?php
$map[50][50];

echo "<h2>Start</h2>";
echo "<table>";
for($u1=1; $u1<=50; $u1++)
{
    echo "<tr>";
    for($u2=1; $u2<=50; $u2++)
    {
    $val = round(rand(0,10));
    if($val<2)
    {
    $map[$u1][$u2] = 1;
    echo "<td style=\"width:10px; height: 10px; background-color: #0f0\"></td>";
    }
    else
    {
    $map[$u1][$u2] = 0;
    echo "<td style=\"width:10px; height: 10px; background-color: #f00\"></td>";
    }
    }
    echo "</tr>";
}
echo "<table>";

function SearchWay($x, $y, $x_to, $y_to, $map)
{
    $size =50;
    $matrix[$size][$size][3];

    $step;
    $added=true;
    $result=true;

    for($i=0;$i<$size;$i++)
    {
        for($j=0;$j<$size;$j++)
        {
            if($map[$i][$j]!=0)
            {
                $matrix[$i][$j][0] = -2;// занято
            }
            else
            {
                $matrix[$i][$j][0] = -1;// Мы еще нигде не были
            }
        }
    }
    $matrix[$x_to][$y_to][0]= 0;// До финиша ноль шагов - от него будем разбегаться
    $step = 0; // Изначально мы сделали ноль шагов
   
    // Пока вершины добаляются и мы не дошли до старта

    while($added && $matrix[$x][$y][0]==-1)
    {
        $added = false;// Пока что ничего не добавили
        $step++;// Увеличиваем число шагов

        for($i=0;$i<$size;$i++)// Пробегаем по всей карте
        {
            for($j=0;$j<$size;$j++)
            {
                // Если (i, j) была добавлена на предыдущем шаге
                // Пробегаем по всем четырем сторонам
                if($matrix[$i][$j][0]== $step-1)
                {
                    $i2;
                    $j2;

                    $i2=$i+1;$j2=$j;
                    // Если не вышли за пределы карты -  обрабатываем
                    if($i2>=0 && $j2>=0 && $i2<$size && $j2<$size)
                    {
                        // Если ($i2, $j2) уже добавлено или непроходимо, то не обрабатываем
                         if($matrix[$i2][$j2][0]==-1 && $matrix[$i2][$j2][0]!=-2)
                         {
                             $matrix[$i2][$j2][0]= $step; // Добав-
                             $matrix[$i2][$j2][1]= $i; // ля-
                             $matrix[$i2][$j2][2]= $j; // ем
                             $added = true; // Что-то добавили
                         }
                    }
                    $i2=$i-1;$j2=$j;
                    // Если не вышли за пределы карты -  обрабатываем
                    if($i2>=0 && $j2>=0 && $i2<$size && $j2<$size)
                    {
                        // Если ($i2, $j2) уже добавлено или непроходимо, то не обрабатываем
                        if($matrix[$i2][$j2][0]==-1 && $matrix[$i2][$j2][0]!=-2)
                        {
                            $matrix[$i2][$j2][0]= $step; // Добав-
                            $matrix[$i2][$j2][1]= $i; // ля-
                            $matrix[$i2][$j2][2]= $j; // ем
                            $added = true; // Что-то добавили
                        }
                    }
                    $i2=$i;$j2=$j+1;
                    // Если не вышли за пределы карты -  обрабатываем
                    if($i2>=0 && $j2>=0 && $i2<$size && $j2<$size)
                    {
                        // Если ($i2, $j2) уже добавлено или непроходимо, то не обрабатываем
                        if($matrix[$i2][$j2][0]==-1 && $matrix[$i2][$j2][0]!=-2)
                        {
                            $matrix[$i2][$j2][0]= $step; // Добав-
                            $matrix[$i2][$j2][1]= $i; // ля-
                            $matrix[$i2][$j2][2]= $j; // ем
                            $added = true; // Что-то добавили
                        }
                    }
                    $i2=$i;$j2=$j-1;
                    // Если не вышли за пределы карты -  обрабатываем
                    if($i2>=0 && $j2>=0 && $i2<$size && $j2<$size)
                    {
                        // Если ($i2, $j2) уже добавлено или непроходимо, то не обрабатываем
                        if($matrix[$i2][$j2][0]==-1 && $matrix[$i2][$j2][0]!=-2)
                        {
                            $matrix[$i2][$j2][0]= $step; // Добав-
                            $matrix[$i2][$j2][1]= $i; // ля-
                            $matrix[$i2][$j2][2]= $j; // ем
                            $added = true; // Что-то добавили
                        }
                    }
                }
            }
        }
    }

    if($matrix[$x][$y][0]== -1)
    {
        $result = false; // то пути не существует
    }


    if($result)
    {
        $i2=$x;
        $j2=$y;

        while($matrix[$i2][$j2][0]!=0)
        {   

            $li = $matrix[$i2][$j2][1];
            $lj = $matrix[$i2][$j2][2];
            $i2=$li;
            $j2=$lj;
            $map[$i2][$j2] = 7;
        }
    }
    return $map;
}

$st_x = 10;
$st_y = 10;

$en_x = 40;
$en_y = 30;

//Обнуляем в изначальной карте начальные и конечные координаты
$map[$st_x][$st_y] = 0;
$map[$en_x][$en_y] = 0;

$map2 = SearchWay(10, 10, 40, 30, $map);

echo "<h2>Result</h2><br/><br/>";

echo "<table>";
for($u1=1; $u1<=50; $u1++)
{
    echo "<tr>";
    for($u2=1; $u2<=50; $u2++)
    {

    //echo $map2[$u1][$u2]." ";
   
    if($map2[$u1][$u2]==1)echo "<td style=\"width:10px; height: 10px; background-color: #0f0\"></td>";
    if($map2[$u1][$u2]==0)echo "<td style=\"width:10px; height: 10px; background-color: #f00\"></td>";
    if($map2[$u1][$u2]==7)echo "<td style=\"width:10px; height: 10px; background-color: #00f\"></td>";

    }
    echo "</tr>";
}
echo "</table>";
?>

За основу взял найденный в гугле пример реализации на C++ выдрал функцию переписал под php и все заработало. Добавил только передачу массива с начальной картой в функцию SearchWay. На основе этого алгоритма вроде как основана игра UFO 1 и 2 части. Результат будет таким: