Создание A* поиска с помощью PHP

у меня есть карта, хранящаяся как многомерный массив ($map[row][col]), и я хочу создать путь из точки A в точку B.

поскольку у меня могут быть некоторые препятствия с поворотами, углами и т.д., я хочу использовать поиск A* для вычисления самого быстрого пути.

f(x) = g(x) + h(x)

и у меня есть все эти значения. g(x) - стоимость перемещения (и она сохраняется на карте); h(x) - линейное расстояние между A и B.

итак, у меня есть все необходимое, но у меня вопрос: как мне все организовать?
У меня нет необходимости проверять альтернативные пути, так как квадрат на карте может быть проходимым или нет, поэтому, когда я достигаю цели, он должен быть самым коротким.

как мне все организовать?
Я пробовал с многомерным массивом, но я теряюсь... :(

EDIT
я разработал некоторый код, это довольно большая стена текста :)

//$start = array(28, 19), $end = array(14, 19)
//$this->map->map is a multidimensional array, everything has a cost of 1, except for 
//blocking squares that cost 99
//$this->map->map == $this->radar
//blocking square at 23-17, 22-18, 22-19, 22-20, 23-21, 19-17, 20-18,20-19,20-20,19-21
//they are like 2 specular mustache :P
function createPath($start, $end)
{
    $found = false;
    $temp  = $this->cost($start, $end);

    foreach($temp as $t){
        if($t['cost'] == $this->map->map[$end[0]][$end[1]]) $found = true;
        $this->costStack[$t['cost']][] = array('grid' => $t['grid'], 'dir' => $t['dir']);
    }

    ksort($this->costStack);

    if(!$found) {
        foreach($this->costStack as $k => $stack){
            foreach($stack as $kn => $node){
                $curNode = $node['grid'];
                unset($this->costStack[$k][$kn]);
                break;
            }

            if(!count($this->costStack[$k])) unset($this->costStack[$k]);
            break;
        }
        $this->createPath($curNode, $end);
    }
}

function cost($current, $target)
{
    $return = array();

    //$AIM  = array('n' => array(-1,  0),'e' => array( 0,  1),'s' => array( 1,  0),'w' => array( 0, -1));
    foreach($this->AIM as $direction => $offset){
        $position[0] = $current[0] + $offset[0];
        $position[1] = $current[1] + $offset[1];

        //radar is a copy of the map
        if ( $this->radar[$position[0]][$position[1]] == 'V') continue;
        else $this->radar[$position[0]][$position[1]] =  'V';

        $h = (int) $this->distance($position, $target);
        $g = $this->map->map[$position[0]][$position[1]];

        $return[] = array('grid' => $position,
                          'dir'  => $direction,
                          'cost' => $h + $g);
    }

    return $return;
}

я надеюсь, что вы можете понять все, я пытался быть ясным, насколько это возможно.
Наконец-то я могу добраться до места назначения, расширяя только более дешевые узлы, но теперь у меня проблема.
Как я могу превратить это в направление? Я должен хранить стек заказов (т.е. n, n, e etc etc etc), как я могу определить путь внутри этих значений?