Не работает алгоритм Astar (возможно, неверное эвристическое значение?)

Question

Я пытался следовать псевдокоду * в вики, и у меня возникли проблемы при создании метода эвристического значения, поэтому я просто использовал его в качестве кратчайшего пути, но код продолжает добавлять начальный узел в закрытый набор.

public uint TotalNodes = 0;
        uint[][] Neighbors = null;
        uint[][] Weights = null;

        public Graph(uint v)
        {
            TotalNodes = v + 1;
            Neighbors = new uint[TotalNodes][];
            Weights = new uint[TotalNodes][];

            for (int i = 1; i < TotalNodes; i++)
            {
                for (int j = 1; j < TotalNodes; j++)
                {
                    Neighbors[i] = new uint[TotalNodes];
                    Weights[i] = new uint[TotalNodes];
                }
            }
        }

        public void AddNeighbor(uint parent, uint neighbor, uint distance)
        {
            Neighbors[parent][neighbor] = neighbor;
            Weights[parent][neighbor] = distance;
        }

        public void RunAstarAlgorithm(uint start, uint goal)
        {
            uint[] closedSet = new uint[TotalNodes];
            uint[] openSet = new uint[TotalNodes];
            uint[] g_score = new uint[TotalNodes];
            uint[] f_score = new uint[TotalNodes];
            uint openSetNodes = 0, closedSetNodes = 0;
            uint currentNode = 0;
            uint tentativePathCost = 0;

            openSet[0] = start;
            for (uint i = 0; i < TotalNodes; i++)
            {
                if (i == start) f_score[i] = 0;
                    else f_score[i] = uint.MaxValue;
            }

            f_score[start] = g_score[start] + (uint)HeuristicCostEstimate(start,goal);

            while (openSet.Length != 0)
            {
                for (uint i = 1; i < TotalNodes; i++)
                {
                    if (f_score[i] == f_score.Min()) currentNode = i;
                }

                if (currentNode == goal)
                {
                    ReconstructPath(closedSet, currentNode);
                    return;
                }

                for (uint i = 0; i < TotalNodes; i++) if (openSet[i] == currentNode) openSet[i] = 0;
                closedSet[closedSetNodes] = currentNode;
                closedSetNodes++;


                foreach (uint neighbor in Neighbors[currentNode])
                {
                    for (uint i = 0; i < TotalNodes; i++)
                    {
                        if (closedSet.Contains(neighbor)) continue;
                    }
                    tentativePathCost += g_score[currentNode] + Weights[currentNode][neighbor];

                    if (!openSet.Contains(neighbor) || tentativePathCost < g_score[neighbor])
                    {
                        g_score[neighbor] = tentativePathCost;
                        f_score[neighbor] = g_score[neighbor] + (uint)HeuristicCostEstimate(neighbor,goal);
                        if (!openSet.Contains(neighbor))
                        {
                            openSet[openSetNodes] = neighbor;
                            openSetNodes++;
                        }
                    }
                }
            }
        }

Найти эвристическое значение:

public int HeuristicCostEstimate(uint start, uint goal)
        {
            Queue<uint> queue = new Queue<uint>();
            queue.Enqueue(start);

            bool[] visitedNodes = new bool[TotalNodes];
            int[] distances = new int[TotalNodes];
            for (uint i = 0; i < TotalNodes; i++) distances[i] = -1;
            distances[start] = 0;

            while(queue.Count > 0)
            {
                uint node = queue.Dequeue();           
                if (node == goal) break;

                foreach(uint neighbor in Neighbors[node])
                {
                    if (neighbor == 0) continue;
                    if(distances[neighbor] == -1)
                    {
                        distances[neighbor] = distances[node] + 1;
                        queue.Enqueue(neighbor);
                    }
                }
            }
            return distances[goal];
        }

Итак, у меня есть этот график: 7 графов узлов и, например,Я хочу перейти от 1 до 5, поэтому путь должен быть 1-2-3-6-5, верно?

Мой график представлен в неровных массивах (игнорируйте первое (0) место, я начинаю с1 для распечатки)

массив соседей (первая строка представляет узлы, столбцы представляют их соседей:

   (0)(1)(2)(3)(4)(5)(6)(7)
(0) 0  0  0  0  0  0  0  0
(1) 0  0  1  0  0  0  0  1
(2) 0  2  0  2  2  0  0  0
(3) 0  0  3  0  3  0  3  0
(4) 0  0  4  4  0  4  0  0
(5) 0  0  0  0  5  0  5  0
(6) 0  0  0  6  0  6  0  6
(7) 0  7  0  0  0  0  7  0 

массив масс (расстояние) (столбцы представляют их соседей и каково расстояниеим):

   (0)(1)(2)(3)(4)(5)(6)(7)
(0) 0  0  0  0  0  0  0  0
(1) 0  0  2  0  0  0  0  5
(2) 0  2  0  3  7  0  0  0
(3) 0  0  3  0  4  0  3  0
(4) 0  0  7  4  0  6  0  0
(5) 0  0  0  0  5  0  1  0
(6) 0  0  0  2  0  1  0 10
(7) 0  5  0  0  0  0 10  0 

Итак, как я могу достичь кратчайшего пути с помощью алгоритма A *?