如何调试我的A*算法？附Free Pascal实现代码求助

我照着网上的A*逻辑尝试了好几种实现方式，理论上的逻辑我都理解，但每次运行都失败了。下面是我写的Free Pascal代码：

function getHeuristic(currentXY, targetXY: array of word): word; 
begin 
  getHeuristic:=abs(currentXY[0]-targetXY[0])+abs(currentXY[1]-targetXY[1]); 
end; 

function getPath(startingNodeXY, targetNodeXY: array of word; grid: wordArray3; out pathToControlledCharPtr: word; worldObjIndex: word): wordArray2; 
var 
  openList, closedList: array of array of word; { x/y/g/h/parent x/parent y, total }
  qXYGH: array[0..5] of word; { x/y/g/h/parent x/parent y }
  gridXCnt, gridYCnt: longInt; 
  maxF, q, openListCnt, closedListCnt, parentClosedListCnt, getPathCnt, adjSquNewGScore: word; 
  openListIndexCnt, closedListIndexCnt, qIndexCnt, successorIndexCnt: byte; 
  getMaxF, successorOnClosedList, successorOnOpenList, pathFound: boolean; 
begin 
  { Add the starting square (or node) to the open list. }
  setLength(openList, 6, length(openList)+1); 
  openList[0, 0]:=startingNodeXY[0]; 
  openList[1, 0]:=startingNodeXY[1]; 
  setLength(closedList, 6, 0); 

  { Repeat the following: }
  { D) Stop when you: }
  { Fail to find the target square, and the open list is empty. In this case, there is no path. }
  pathFound:=false; 
  { writeLn('h1'); }
  while length(openList[0])>0 do 
  begin 
    { A) Look for the lowest F cost square on the open list. We refer to this as the current square. }
    maxF:=0; 
    q:=0; 
    getMaxF:=true; 
    for openListCnt:=0 to length(openList[0])-1 do 
    begin 
      //writeLn(formatVal('open list xy {} {}, cnt {}, list max index {}', [openList[0, openListCnt], openList[1, openListCnt], openListCnt, length(openList[0])-1])); 
      { readLnPromptX; }
      if (getMaxF=true) or (maxF>openList[2, openListCnt]+openList[3, openListCnt]) then 
      begin 
        getMaxF:=false; 
        maxF:=openList[2, openListCnt]+openList[3, openListCnt]; 
        q:=openListCnt; 
      end; 
    end; 

    for qIndexCnt:=0 to length(qXYGH)-1 do 
      qXYGH[qIndexCnt]:=openList[qIndexCnt, q]; 

    { B). Switch it to the closed list. }
    setLength(closedList, length(closedList), length(closedList[0])+1); 
    for closedListIndexCnt:=0 to length(closedList)-1 do 
      closedList[closedListIndexCnt, length(closedList[0])-1]:=qXYGH[closedListIndexCnt]; 

    { Remove current square from open list }
    if q<length(openList[0])-1 then 
    begin 
      for openListCnt:=q to length(openList[0])-2 do 
      begin 
        for openListIndexCnt:=0 to length(openList)-1 do 
          openList[openListIndexCnt, openListCnt]:=openList[openListIndexCnt, openListCnt+1]; 
      end; 
    end; 
    setLength(openList, length(openList), length(openList[0])-1); 

    //writeLn(formatVal('q[x] {}, q[y] {}, startingNodeXY x {}, startingNodeXY y {}, targetNodeXY x {}, targetNodeXY y {}', [qXYGH[0], qXYGH[1], startingNodeXY[0], startingNodeXY[1], targetNodeXY[0], targetNodeXY[1]])); 
    { readLnPromptX; }

    { D) Stop when you: }
    { Add the target square to the closed list, in which case the path has been found, or }
    if (qXYGH[0]=targetNodeXY[0]) and (qXYGH[1]=targetNodeXY[1]) then 
    begin 
      pathFound:=true; 
      break; 
    end; 

    { C) For each of the 8 squares adjacent to this current square … }
    for gridXCnt:=qXYGH[0]-1 to qXYGH[0]+1 do 
    begin 
      for gridYCnt:=qXYGH[1]-1 to qXYGH[1]+1 do 
      begin 
        { Adjacent square cannot be the current square }
        if (gridXCnt<>qXYGH[0]) or (gridYCnt<>qXYGH[1]) then 
        begin 
          //writeLn(formatVal('gridXCnt {} gridYCnt {} qXYGH[0] {} qXYGH[1] {}', [gridXCnt, gridYCnt, qXYGH[0], qXYGH[1]])); 
          { readLnPromptX; }

          { Check if successor is on closed list }
          successorOnClosedList:=false; 
          if length(closedList[0])>0 then 
          begin 
            for closedListCnt:=0 to length(closedList[0])-1 do 
            begin 
              if (closedList[0, closedListCnt]=gridXCnt) and (closedList[1, closedListCnt]=gridYCnt) then 
              begin 
                successorOnClosedList:=true; 
                break; 
              end; 
            end; 
          end; 

          { If it is not walkable or if it is on the closed list, ignore it. Otherwise do the following. }
          if (gridXCnt>=0) and (gridXCnt<=length(grid[3])-1) and (gridYCnt>=0) and (gridYCnt<=length(grid[3, 0])-1) and (grid[3, gridXCnt, gridYCnt]=0) and (successorOnClosedList=false) then 
          begin 
            { If it isn’t on the open list, add it to the open list. Make the current square the parent of this square. Record the F, G, and H costs of the square. }
            successorOnOpenList:=false; 
            if length(openList[0])>0 then 
            begin 
              for openListCnt:=0 to length(openList[0])-1 do 
              begin 
                if (openList[0, openListCnt]=gridXCnt) and (openList[1, openListCnt]=gridYCnt) then 
                begin 
                  successorOnOpenList:=true; 
                  break; 
                end; 
              end; 
            end; 

            if successorOnOpenList=false then 
            begin 
              setLength(openList, length(openList), length(openList[0])+1); 
              openList[0, length(openList[0])-1]:=gridXCnt; 
              openList[1, length(openList[0])-1]:=gridYCnt; 
              openList[4, length(openList[0])-1]:=qXYGH[0]; 
              openList[5, length(openList[0])-1]:=qXYGH[1]; 

              if (openList[0, length(openList[0])-1]=qXYGH[0]) or (openList[1, length(openList[0])-1]=qXYGH[1]) then 
              begin 
                openList[2, length(openList[0])-1]:=openList[2, length(openList[0])-1]+10; 
              end 
              else 
              begin 
                openList[2, length(openList[0])-1]:=openList[2, length(openList[0])-1]+14; 
              end; 

              openList[3, length(openList[0])-1]:=getHeuristic([openList[0, length(openList[0])-1], openList[1, length(openList[0])-1]], [targetNodeXY[0], targetNodeXY[1]]); 
            end 
            else 
            begin 
              { If it is on the open list already, check to see if this path to that square is better, using G cost as the measure (check to see if the G score for the adjacent square is lower if we use the current square to get there (adjacent square new G score = current square G score + 10 (if adjacent squre is vertical or horizontal to current square) or +14 (if it is diagonal); if result is lower than adjacent square current G score then this path is better). A lower G cost means that this is a better path. If so, change the parent of the square to the current square, and recalculate the G and F scores of the square. If you are keeping your open list sorted by F score, you may need to resort the list to account for the change. }
              adjSquNewGScore:=openList[2, openListCnt]; 
              if (openList[0, openListCnt]=qXYGH[0]) or (openList[1, openListCnt]=qXYGH[1]) then 
              begin 
                adjSquNewGScore:=adjSquNewGScore+10; 
              end 
              else 
              begin 
                adjSquNewGScore:=adjSquNewGScore+14; 
              end; 

              if adjSquNewGScore<openList[2, openListCnt] then 
              begin 
                openList[4, openListCnt]:=qXYGH[0]; 
                openList[5, openListCnt]:=qXYGH[1]; 
                openList[2, openListCnt]:=adjSquNewGScore; 
              end; 
            end; 
          end; 
        end; 
      end; 
    end; 
  end; 

  { writeLn('h2'); }
  { writeLn(pathFound); }
  { readLnHalt; }

  if pathFound=true then 
  begin 
    { Save the path. Working backwards from the target square, go from each square to its parent square until you reach the starting square. That is your path. }
    closedListCnt:=length(closedList[0])-1; 
    setLength(getPath, 2, 0); 

    { While starting node has not been added to path }
    while (length(getPath[0])=0) or (getPath[0, length(getPath[0])-1]<>startingNodeXY[0]) or (getPath[1, length(getPath[0])-1]<>startingNodeXY[1]) do 
    begin 
      { Add node from closed list to path }
      setLength(getPath, 2, length(getPath[0])+1); 
      getPath[0, length(getPath[0])-1]:=closedList[0, closedListCnt]; 
      getPath[1, length(getPath[0])-1]:=closedList[1, closedListCnt]; 

      { Find next node on closed list with coord matching parent coord of current closed list node }
      for parentClosedListCnt:=length(closedList[0])-1 downto 0 do 
        if (closedList[0, parentClosedListCnt]=closedList[4, closedListCnt]) and (closedList[1, parentClosedListCnt]=closedList[5, closedListCnt]) then 
          break; 
      closedListCnt:=parentClosedListCnt; 
      { if (closedList[0, closedListCnt]=0) and (closedList[1, closedListCnt]=0) then break; }
    end; 

    pathToControlledCharPtr:=length(getPath[0])-1; 
  end; 
end;

我严格遵循了A*的标准步骤：

• 将起始方格（或节点）加入open list。
• 重复执行以下操作：
  1. 在open list中寻找F cost最低的方格，作为当前方格。
  2. 将其移至closed list。
  3. 遍历当前方格的8个相邻方格：
     • 若方格不可行走或已在closed list中，直接忽略。
     • 否则：
       • 若不在open list中，将其加入open list，设置当前方格为其父节点，记录F、G、H cost。
       • 若已在open list中，以G cost为标准判断路径是否更优，若新路径G cost更低则更新父节点及G、F cost。
  4. 终止条件：目标方格加入closed list则找到路径；open list为空则无路径。
• 从目标方格反向回溯至起始方格，生成并保存路径。

我实在找不出哪里出了问题，恳请各位帮忙调试这个实现，找出失败的原因。

内容的提问来源于stack exchange，提问作者Seb0029