The Optimal Path problem

def __init__(self):
  """Set up the sample graph (path_graph) and the node coordinates 
  used by the heuristic."""
  # Adjacency list: node -> list of (neighbor, edge weight)
  self.path_graph = {
    "A": [("B", 3), ("C", 1)],
    "B": [("D", 2), ("E", 2)],
    "C": [("B", 1)],
    "D": [("G", 5)],
    "E": [("F", 2), ("G", 4), ("H", 4)],
    "F": [("G", 1), ("H", 5)],
    "G": [],
    "H": []
  }

  # Node -> (x, y) position, used by A* and greedy to estimate 
  # distance to the goal
  self.coords = {
    "A": (0,0),
    "B": (1,1),
    "C": (0,2),
    "D": (2,0),
    "E": (2,2),
    "F": (3,3),
    "G": (3,1),
    "H": (1,3)
  }

def bfs_path(self, start, goal, debug=False):
        """
        Breadth-first search over self.path_graph, ignoring edge weights.

        Finds the path with the fewest edges (not necessarily the lowest weight).

        Args:
            start: node to begin at.
            goal: node to reach.
            debug: if True, print a step-by-step table of the queue after each expansion.

        Returns:
            (path, cost): path is the list of nodes from start to goal and cost is the
            number of edges in it. Returns (None, inf) if goal is unreachable.
        """
        queue = deque([start])
        visited = {start}
        parent = {start: None}
        rows = []  # debug table rows

        while queue:
            node = queue.popleft()
            if node == goal:
                if debug:
                    rows.append([len(rows) + 1, node, self._fmt_frontier(queue), ", ".join(sorted(visited))])
                break

            for nbr, _ in self.path_graph.get(node, []):
                if nbr not in visited:
                    visited.add(nbr)
                    parent[nbr] = node
                    queue.append(nbr)

            if debug:
                rows.append([len(rows) + 1, node, self._fmt_frontier(queue), ", ".join(sorted(visited))])

        if debug:
            self._print_table(f"BFS {start} -> {goal}", ["Step", "Expanded", "Frontier (queue)", "Visited"], rows)

        if goal not in parent:
            return None, float("inf")

        # Reconstruct path
        path = []
        cur = goal
        while cur is not None:
            path.append(cur)
            cur = parent[cur]
        path.reverse()

        cost = len(path) - 1  # number of edges
        return path, cost

def dijkstra_path(self, start, goal, debug=False):
        """
        Dijkstra's shortest-path search over self.path_graph.

        Always expands the unvisited node with the lowest known distance from start,
        so it finds the minimum total weight path (weights must be non-negative).

        Args:
            start: node to begin at.
            goal: node to reach.
            debug: if True, print a step-by-step table of the frontier after each expansion.

        Returns:
            (path, cost): path is the list of nodes from start to goal and cost is the
            minimum total edge weight. Returns (None, inf) if goal is unreachable.
        """
        pq = [(0, start)]
        dist = {start: 0}
        parent = {start: None}
        visited = set()
        order = []  # expansion order, for the debug table
        rows = []   # debug table rows

        while pq:
            cur_dist, u = heapq.heappop(pq)
            if u in visited:
                continue
            visited.add(u)
            order.append(u)

            if u == goal:
                if debug:
                    frontier = sorted(((v, d) for d, v in pq if v not in visited and d == dist[v]), key=lambda e: (e[1], e[0]))
                    rows.append([len(rows) + 1, u, cur_dist, self._fmt_frontier(frontier), ", ".join(order)])
                break

            for v, w in self.path_graph.get(u, []):
                if v in visited:
                    continue
                new_dist = cur_dist + w
                if new_dist < dist.get(v, float("inf")):
                    dist[v] = new_dist
                    parent[v] = u
                    heapq.heappush(pq, (new_dist, v))

            if debug:
                # Only live entries: not yet expanded and matching the best known distance
                frontier = sorted(((v, d) for d, v in pq if v not in visited and d == dist[v]), key=lambda e: (e[1], e[0]))
                rows.append([len(rows) + 1, u, cur_dist, self._fmt_frontier(frontier), ", ".join(order)])

        if debug:
            self._print_table(f"Dijkstra {start} -> {goal}",
                              ["Step", "Expanded", "g", "Frontier node(g)", "Expanded order"], rows)

        if goal not in parent:
            return None, float("inf")

        # Reconstruct path
        path = []
        cur = goal
        while cur is not None:
            path.append(cur)
            cur = parent[cur]
        path.reverse()

        return path, dist[goal]

def astar_closed_path(self, start, goal, use_manhattan, debug=False):
        """
        A* search over self.path_graph using self.coords for the heuristic, with a closed set.

        Expands the node with the lowest f(n) = g(n) + h(n), where g is the cost so far
        and h is the Manhattan or Euclidean distance to goal. Once a node is expanded it is
        closed and never revisited, so the path is only guaranteed to be minimum-weight if
        the heuristic is consistent. With an inconsistent heuristic (e.g. Manhattan on this
        graph) it can return a costlier path; see astar_path for the version that fixes this.

        Args:
            start: node to begin at.
            goal: node to reach.
            use_manhattan: if True use the Manhattan heuristic, otherwise Euclidean.
            debug: if True, print a step-by-step table of the frontier after each expansion.

        Returns:
            (path, cost): path is the list of nodes from start to goal and cost is its
            total edge weight. Returns (None, inf) if goal is unreachable.
        """
        pq = []
        heapq.heappush(pq, (0, start))  # (f, node)

        g = {start: 0}         # best cost from start to node
        parent = {start: None}
        closed = set()
        order = []  # expansion order, for the debug table
        rows = []   # debug table rows

        def log_step(u):
            # Only live entries: node not closed and f matching its current g + h
            frontier = sorted(((v, f) for f, v in pq
                               if v not in closed and f == g[v] + self._h(v, goal, use_manhattan)),
                              key=lambda e: (e[1], e[0]))
            h = self._h(u, goal, use_manhattan)
            rows.append([len(rows) + 1, u, g[u], h, g[u] + h,
                         self._fmt_frontier(frontier), ", ".join(order)])

        while pq:
            _, u = heapq.heappop(pq)

            if u in closed:
                continue
            closed.add(u)
            order.append(u)

            if u == goal:
                if debug:
                    log_step(u)
                break

            for v, w in self.path_graph.get(u, []):
                if v in closed:
                    continue
                tentative_g = g[u] + w
                if tentative_g < g.get(v, float("inf")):
                    g[v] = tentative_g
                    parent[v] = u
                    if use_manhattan:
                        f = tentative_g + self.manhattan_heuristic(self.coords[v], self.coords[goal])
                    else:
                        f = tentative_g + self.euclidean_heuristic(self.coords[v], self.coords[goal])
                    heapq.heappush(pq, (f, v))

            if debug:
                log_step(u)

        if debug:
            self._print_table(f"A* (closed set) {start} -> {goal}, {'Manhattan' if use_manhattan else 'Euclidean'}",
                              ["Step", "Expanded", "g", "h", "f", "Frontier node(f)", "Closed order"], rows)

        if goal not in parent:
            return None, float("inf")

        # Reconstruct path
        path = []
        cur = goal
        while cur is not None:
            path.append(cur)
            cur = parent[cur]
        path.reverse()

        return path, g[goal]

def astar_reexpansion_path(self, start, goal, use_manhattan, debug=False):
        """
        A* search over self.path_graph using self.coords for the heuristic.

        Expands the node with the lowest f(n) = g(n) + h(n), where g is the cost so far
        and h is the Manhattan or Euclidean distance to goal. Nodes are re-expanded when a
        cheaper path to them is found, so it finds the minimum-weight path as long as the
        heuristic never overestimates the true remaining cost.

        Args:
            start: node to begin at.
            goal: node to reach.
            use_manhattan: if True use the Manhattan heuristic, otherwise Euclidean.
            debug: if True, print a step-by-step table of the frontier after each expansion.

        Returns:
            (path, cost): path is the list of nodes from start to goal and cost is its
            total edge weight. Returns (None, inf) if goal is unreachable.
        """
        pq = []
        heapq.heappush(pq, (0, 0, start))  # (f, g, node)

        g = {start: 0}         # best cost from start to node
        parent = {start: None}
        order = []  # expansion order (a node appears twice if it was re-expanded), for debug
        rows = []   # debug table rows

        def log_step(u):
            # Only live entries: g matches the best known g for that node
            frontier = sorted(((v, f) for f, g_v, v in pq if g_v == g[v]),
                              key=lambda e: (e[1], e[0]))
            h = self._h(u, goal, use_manhattan)
            rows.append([len(rows) + 1, u, g[u], h, g[u] + h,
                         self._fmt_frontier(frontier), ", ".join(order)])

        while pq:
            _, g_u, u = heapq.heappop(pq)

            # Skip stale entries; nodes are re-expanded if a cheaper path is found later,
            # which keeps A* correct even when the heuristic is inconsistent.
            if g_u > g[u]:
                continue
            order.append(u)

            if u == goal:
                if debug:
                    log_step(u)
                break

            for v, w in self.path_graph.get(u, []):
                tentative_g = g[u] + w
                if tentative_g < g.get(v, float("inf")):
                    g[v] = tentative_g
                    parent[v] = u
                    if use_manhattan:
                        f = tentative_g + self.manhattan_heuristic(self.coords[v], self.coords[goal])
                    else:
                        f = tentative_g + self.euclidean_heuristic(self.coords[v], self.coords[goal])
                    heapq.heappush(pq, (f, tentative_g, v))

            if debug:
                log_step(u)

        if debug:
            self._print_table(f"A* {start} -> {goal}, {'Manhattan' if use_manhattan else 'Euclidean'}",
                              ["Step", "Expanded", "g", "h", "f", "Frontier node(f)", "Expanded order"], rows)

        if goal not in parent:
            return None, float("inf")

        # Reconstruct path
        path = []
        cur = goal
        while cur is not None:
            path.append(cur)
            cur = parent[cur]
        path.reverse()

        return path, g[goal]

def greedy_path(self, start, goal, use_manhattan, debug=False):
        """
        Greedy best-first search over self.path_graph.

        Uses f(n) = h(n) only: always expands the node that looks closest to goal by the
        Manhattan or Euclidean heuristic, ignoring the cost so far. Fast, but the path
        found is not guaranteed to be the cheapest.

        Args:
            start: node to begin at.
            goal: node to reach.
            use_manhattan: if True use the Manhattan heuristic, otherwise Euclidean.
            debug: if True, print a step-by-step table of the frontier after each expansion.

        Returns:
            (path, cost): path is the list of nodes from start to goal and cost is the
            actual total edge weight along that path. Returns (None, inf) if goal is
            unreachable.
        """
        pq = []
        if use_manhattan:
            heapq.heappush(pq, (self.manhattan_heuristic(self.coords[start], self.coords[goal]), start))
        else:
            heapq.heappush(pq, (self.euclidean_heuristic(self.coords[start], self.coords[goal]), start))

        parent = {start: None}
        visited = set()
        order = []  # expansion order, for the debug table
        rows = []   # debug table rows

        def log_step(u):
            # Distinct nodes still waiting, lowest h first
            frontier = sorted({(v, h) for h, v in pq if v not in visited}, key=lambda e: (e[1], e[0]))
            rows.append([len(rows) + 1, u, self._h(u, goal, use_manhattan),
                         self._fmt_frontier(frontier), ", ".join(order)])

        # We'll compute cost by summing edges along the reconstructed path
        # (Alternatively, track g[] as well, but GBFS doesn't use it for priorities.)
        # For simplicity we reconstruct first then sum.
        while pq:
            _, u = heapq.heappop(pq)
            if u in visited:
                continue
            visited.add(u)
            order.append(u)

            if u == goal:
                if debug:
                    log_step(u)
                break

            for v, w in self.path_graph.get(u, []):
                if v in visited:
                    continue
                # If we reached v via u first, set parent.
                # (We don't do "relaxation" based on g because GBFS doesn't optimize g.)
                if v not in parent:
                    parent[v] = u

                if use_manhattan:
                    heapq.heappush(pq, (self.manhattan_heuristic(self.coords[v], self.coords[goal]), v))
                else:
                    heapq.heappush(pq, (self.euclidean_heuristic(self.coords[v], self.coords[goal]), v))

            if debug:
                log_step(u)

        if debug:
            self._print_table(f"Greedy {start} -> {goal}, {'Manhattan' if use_manhattan else 'Euclidean'}",
                              ["Step", "Expanded", "h", "Frontier node(h)", "Expanded order"], rows)

        if goal not in parent:
            return None, float("inf")

        # Reconstruct path
        path = []
        cur = goal
        while cur is not None:
            path.append(cur)
            cur = parent[cur]
        path.reverse()

        # Sum actual weights along path
        # Build a quick lookup of weights from u to v
        weight_lookup = {}
        for u in self.path_graph:
            for v, w in self.path_graph[u]:
                weight_lookup[(u, v)] = w

        total_cost = 0
        for i in range(len(path) - 1):
            total_cost += weight_lookup[(path[i], path[i+1])]

        return path, total_cost