Алгоритм поиска пути

Question

Я студент и столкнулся с проблемой, которую не могу решить.У меня есть карта Terrain, которая представляет собой массив enum, содержащий целые числа и имя.

enum ETerrainCost
{
    Clear = 1,
    Water = 2,
    Wood  = 3,
    Wall  = 0
};

// Maps of any size are implemented as 2D vectors
using TerrainMap = vector<vector<ETerrainCost>>;

После загрузки карты и добавления координат «начало» и «цель» я запускаю поиск пути с помощью A* но по какой-то причине это неправильно и не игнорирует стены, несмотря на проверку.

inline bool WallCheck(TerrainMap& terrain, unique_ptr<SNode>& node)
{
    if (static_cast<int>(terrain[node->x][node->y]) == 0)
    {
        return true;
    }

    return false;
}

Я смотрел несколько видео, пытался имитировать код psuedo, но до сих пор не могу понять, где онпойдет не так.Вот код поиска:

Эта функция становится владельцем указателей старта и цели, которые передаются из вызывающего кода.Право собственности не возвращается в конце, поэтому начальный и целевой узлы используются.Путь возвращается через опорный параметр.

bool CSearchAStar::FindPath(deque<unique_ptr<SNode>>& openList, deque<unique_ptr<SNode>>& closedList,
                                        TerrainMap& terrain, unique_ptr<SNode>& start, unique_ptr<SNode>& goal,
                                                                            NodeList& path, int& counter)
{
    unique_ptr <SNode> startNode(new SNode);
    unique_ptr <SNode> currentNode(new SNode);
//For .erase() function
deque <unique_ptr <SNode> >::iterator myIt;

//North, East, South, West are the options to move
int rulesNumber = 4;

//Set start and calculate score(heuristic only)
startNode->x = start->x;
startNode->y = start->y;
startNode->parent = 0;
startNode->cost = 0;
startNode->heuristic = utility::NodeHeuristic(startNode, goal);
startNode->score = startNode->heuristic;

//Moved to the openList
openList.push_back(move(startNode));

//If it's the goal, return
if (start->x == goal->x && start->y == goal->y)
{
    return true;
}

while (currentNode->x != goal->x || currentNode->y != goal->y)
{
    //If there is no element to expand, return
    if (openList.empty())
    {
        return false;
    }

    //Take an element from the openList and make 
    //it current to expand it's neighbors
    currentNode = move(openList.front());
    openList.pop_front();

    //tempNode is identical to currentNode, 
    //and will be pushed into the closed list instead of it
    //To be pushed to the closedList
    unique_ptr <SNode> tempNode(new SNode);
    tempNode->x = currentNode->x;
    tempNode->y = currentNode->y;
    tempNode->parent = currentNode->parent;
    tempNode->cost = currentNode->cost;
    tempNode->heuristic = currentNode->heuristic;
    tempNode->score = currentNode->score;

    for (int i = 0; i < rulesNumber; ++i)
    {
        unique_ptr <SNode> neighborNode(new SNode);
        switch (i)
        {
        case 0:
            neighborNode->x = currentNode->x;
            neighborNode->y = currentNode->y + 1;
            neighborNode->cost = utility::NodeCost(terrain, currentNode, neighborNode);
            neighborNode->parent = currentNode.get();
            neighborNode->heuristic = utility::NodeHeuristic(neighborNode, goal);
            neighborNode->score = neighborNode->cost + neighborNode->heuristic;

            //If it's a wall - skip it
            if (utility::WallCheck(terrain, neighborNode))
            {
                break;
            }

            //If it's on one of the lists and it's score is heigher with this route - skip it
            if ((utility::FindOnOpenList(neighborNode, openList, myIt) || utility::FindOnClosedList(neighborNode, closedList, myIt)) 
                && neighborNode->score >= currentNode->score)
            {
                break;
            }

            //If it's on one of the lists and has a lower score with this route - 
            //refer to it from now on and take it from the list
            if (utility::FindOnOpenList(neighborNode, openList, myIt))
            {
                auto temp = myIt;
                neighborNode = move((*myIt));
                openList.erase(temp);

                //Refresh the parent and get the cost changed for the old element
                neighborNode->parent = currentNode.get();
                neighborNode->cost = utility::NodeCost(terrain, currentNode, neighborNode);
                neighborNode->heuristic = utility::NodeHeuristic(neighborNode, goal);
                neighborNode->score = neighborNode->cost + neighborNode->heuristic;
            }
            else if (utility::FindOnClosedList(neighborNode, closedList, myIt))
            {
                break;
            }

            //Push back onto the openList
            openList.push_back(move(neighborNode));

            //Reorder OpenList by score(since either the cost of the node has been updated
            //or it has been pushed onto the OpenList).
            std::sort(openList.begin(), openList.end(), [](unique_ptr<SNode>& lhs,
                unique_ptr<SNode>& rhs) { return lhs->score < rhs->score; });
            counter++;

            break;

то же самое для случаев 1, 2 и 3 и после цикла for:

//Push current to the closedList
    closedList.push_back(move(tempNode));
}


return true;

Извините за ужасно длинный код и сообщение,любая помощь и комментарии даже в отношении стиля кода приветствуются.