import heapq
from math import sqrt

class Node:
    def __init__(self, position, parent=None):
        self.position = position  # 节点的坐标 (x, y)
        self.parent = parent     # 父节点
        self.g = 0               # 从起点到当前节点的实际代价
        self.h = 0               # 启发式估计代价
        self.f = 0               # 总代价 f = g + h

    def __eq__(self, other):
        return self.position == other.position

    def __lt__(self, other):
        return self.f < other.f

def heuristic(a, b):
    """启发式函数：计算曼哈顿距离"""
    return abs(a[0] - b[0]) + abs(a[1] - b[1])

def a_star(grid, start, end):
    """A*算法实现"""
    open_list = []  # 优先队列，存储待探索的节点
    closed_list = set()  # 存储已探索的节点

    start_node = Node(start)
    end_node = Node(end)

    heapq.heappush(open_list, start_node)

    while open_list:
        current_node = heapq.heappop(open_list)
        closed_list.add(current_node.position)

        # 如果找到目标节点，返回路径
        if current_node == end_node:
            path = []
            while current_node:
                path.append(current_node.position)
                current_node = current_node.parent
            return path[::-1]  # 反转路径，从起点到终点

        # 遍历当前节点的邻居
        for neighbor in [(0, -1), (0, 1), (-1, 0), (1, 0)]:  # 上下左右四个方向
            neighbor_position = (current_node.position[0] + neighbor[0], current_node.position[1] + neighbor[1])

            # 检查邻居是否在地图范围内且不是障碍物
            if (neighbor_position[0] < 0 or neighbor_position[0] >= len(grid) or
                neighbor_position[1] < 0 or neighbor_position[1] >= len(grid[0]) or
                grid[neighbor_position[0]][neighbor_position[1]] == 1):
                continue

            # 创建邻居节点
            neighbor_node = Node(neighbor_position, current_node)

            # 如果邻居节点已经探索过，跳过
            if neighbor_node.position in closed_list:
                continue

            # 计算 g, h, f 值
            neighbor_node.g = current_node.g + 1
            neighbor_node.h = heuristic(neighbor_node.position, end_node.position)
            neighbor_node.f = neighbor_node.g + neighbor_node.h

            # 如果邻居节点已经在 open_list 中且新代价更高，跳过
            if any(neighbor_node == open_node and neighbor_node.g > open_node.g for open_node in open_list):
                continue

            # 将邻居节点加入 open_list
            heapq.heappush(open_list, neighbor_node)

    return None  # 如果没有找到路径，返回 None

# 示例地图 (0 表示可通行，1 表示障碍物)
grid = [
    [0, 0, 0, 0, 1, 0],
    [0, 1, 1, 0, 1, 0],
    [0, 0, 0, 0, 1, 0],
    [0, 1, 0, 1, 1, 0],
    [0, 0, 0, 0, 0, 0]
]

# 起点和终点
start = (0, 0)
end = (4, 5)

# 运行A*算法
path = a_star(grid, start, end)
if path:
    print("找到路径:", path)
else:
    print("未找到路径")