A 算法实现常见陷阱：解决节点探索不足的问题

# 原始代码片段中的错误
while not openSet.isEmpty():
    current = openSet.dequeue()

    # ... 其他逻辑 ...

    # 错误之处：始终使用 start_node 探索邻居
    for neighbour in find_neighbors(start_node, graph): 
        # ... 后续处理 ...

import heapq

# 假设 PriorityQueue 是一个简单的基于 heapq 的实现
class PriorityQueue:
    def __init__(self):
        self._queue = []
        self._entry_finder = {} # 映射条目到其在队列中的位置 (用于高效更新)
        self._counter = 0 # 唯一计数器，用于处理优先级相同的情况

    def enequeue(self, priority, item): # 注意：原始代码中使用的是 enequeue，此处保留
        if item in self._entry_finder:
            self.remove(item)
        count = self._counter
        self._counter += 1
        entry = [priority, count, item]
        self._entry_finder[item] = entry
        heapq.heappush(self._queue, entry)

    def dequeue(self):
        while self._queue:
            priority, count, item = heapq.heappop(self._queue)
            if item is not None:
                del self._entry_finder[item]
                return item
        raise KeyError('dequeue from an empty priority queue')

    def remove(self, item):
        entry = self._entry_finder.pop(item)
        entry[-1] = None # 标记为删除，不从堆中实际移除，惰性删除

    def isEmpty(self):
        return not self._entry_finder

    def contains(self, item):
        return item in self._entry_finder

# 启发函数（例如：曼哈顿距离）
def heuristic(node_a, node_b):
    x1, y1 = node_a
    x2, y2 = node_b
    return abs(x1 - x2) + abs(y1 - y2)

# 查找邻居节点函数
def find_neighbors(node, graph):
    x, y = node
    neighbors = []

    # 假设 graph 是一个包含所有有效节点坐标的集合或字典
    # 这里的 graph 应该包含所有可通行的坐标点

    potential_neighbors = [
        (x + 1, y), # 右
        (x - 1, y), # 左
        (x, y + 1), # 下
        (x, y - 1)  # 上
    ]

    for neighbor_coord in potential_neighbors:
        if neighbor_coord in graph: # 检查邻居是否在图中（可通行区域）
            neighbors.append(neighbor_coord)
    return neighbors

# 路径回溯函数
def RetracePath(cameFrom, end_node):
    path = []
    current = end_node
    while current in cameFrom:
        path.append(current)
        current = cameFrom[current]
    path.append(current) # 添加起始节点
    return path[::-1] # 反转路径以从起点到终点

def AStar(start_node, end_node, graph):
    openSet = PriorityQueue()
    openSet.enequeue(0, start_node) # 初始fCost为0，因为是起点

    infinity = float("inf")

    gCost = {} # 从起点到当前节点的实际代价
    fCost = {} # 从起点经过当前节点到终点的总预估代价
    cameFrom = {} # 记录路径，每个节点的前一个节点

    # 初始化所有节点的gCost和fCost为无穷大
    for node in graph: # 遍历图中所有节点进行初始化
        gCost[node] = infinity
        fCost[node] = infinity

    gCost[start_node] = 0
    fCost[start_node] = heuristic(start_node, end_node)

    while not openSet.isEmpty():
        current = openSet.dequeue() # 获取fCost最小的节点

        if current == end_node:
            print(f"Goal reached! Path: {RetracePath(cameFrom, end_node)}")
            return RetracePath(cameFrom, end_node)

        # 核心修正：探索 current 节点的邻居，而不是 start_node 的邻居
        for neighbour in find_neighbors(current, graph): 
            # 假设每一步的代价为1
            tempGCost = gCost[current] + 1 

            if tempGCost < gCost[neighbour]:
                cameFrom[neighbour] = current
                gCost[neighbour] = tempGCost
                fCost[neighbour] = tempGCost + heuristic(neighbour, end_node)

                if not openSet.contains(neighbour):
                    openSet.enequeue(fCost[neighbour], neighbour)

        # 调试输出，可以根据需要保留或删除
        # print(f"Came from: {cameFrom}\nCurrent: {current}")

    print("No path found.")
    return False # 如果openSet为空，表示没有找到路径