a_star Subroutine

   subroutine a_star(graph, source, target, heuristic, path, err)
      !! A* search for one shortest path from `source` to `target`.
      !!
      !! `heuristic(v)` is a **precomputed array** of lower bounds on the
      !! remaining distance from `v` to `target`. An array rather than a
      !! procedure argument is a deliberate portability choice: passing
      !! procedures through an abstract interface is the more general design,
      !! but procedure dummies are exactly the corner of the language where
      !! the compilers PIC targets differ most, and a realistic heuristic
      !! (straight-line distance to the target from stored coordinates) is
      !! computed once per search anyway, so generality buys little here.
      !!
      !! The search settles each vertex once and stops as soon as `target`
      !! settles, so the heuristic must be **consistent** (monotone):
      !! `heuristic(u) - heuristic(v) <= w(u, v)` for every edge. Consistency
      !! implies admissibility, and with it A* returns a path of exactly the
      !! same total cost as `dijkstra`. With `heuristic = 0` - trivially
      !! consistent - it degenerates to Dijkstra exactly, expanding vertices
      !! in the same order and returning the same tie-broken path. Both are
      !! asserted in the test suite.
      !!
      !! `path` is allocated to the vertices from `source` to `target`
      !! inclusive, so `path(1) == source` and `path(size(path)) == target`; a
      !! search with `source == target` yields the single-element path
      !! `[source]`. An unreachable target is **not** an error: `path` comes
      !! back zero-size. Reports `ERROR_VALIDATION` for a malformed graph, an
      !! out-of-range `source` or `target`, a `heuristic` whose length is not
      !! the number of vertices or which holds a negative entry, or a negative
      !! edge weight.
      type(csr_t), intent(in) :: graph
         !! Square weighted adjacency matrix of the graph
      integer(default_int), intent(in) :: source
         !! Start vertex, in `1:n`
      integer(default_int), intent(in) :: target
         !! Goal vertex, in `1:n`
      real(dp), intent(in) :: heuristic(:)
         !! Admissible lower bound on the distance from each vertex to `target`
      integer(default_int), allocatable, intent(out) :: path(:)
         !! Vertices from `source` to `target`, or zero-size if unreachable
      type(error_t), intent(out), optional :: err

      type(heap_t) :: queue
      logical, allocatable :: settled(:)
      real(dp), allocatable :: cost_so_far(:)
      integer(default_int), allocatable :: prev(:)
      integer(default_int) :: n, u, v, k
      integer(int64) :: key
      integer(int32) :: payload
      real(dp) :: cost_u, candidate
      logical :: reached

      allocate (path(0))
      if (.not. check_graph(graph, source, err)) return
      if (.not. check_weights(graph, err)) return

      n = graph%n_rows()
      if (target < 1 .or. target > n) then
         call fail(err, "pic_graph: a_star target vertex is outside 1:n")
         return
      end if
      if (int(size(heuristic), default_int) /= n) then
         call fail(err, "pic_graph: a_star heuristic must have one entry per vertex")
         return
      end if
      do k = 1, n
         if (heuristic(k) < 0.0_dp) then
            call fail(err, "pic_graph: a_star heuristic must be non-negative")
            return
         end if
      end do

      allocate (cost_so_far(n))
      allocate (prev(n))
      allocate (settled(n))
      cost_so_far = GRAPH_INFINITY
      prev = GRAPH_NO_PREDECESSOR
      settled = .false.

      cost_so_far(source) = 0.0_dp
      reached = .false.
      call queue%init(n)
      call queue%push(distance_key(heuristic(source)), int(source, int32))

      do while (.not. queue%is_empty())
         call queue%pop(key, payload)
         u = int(payload, default_int)
         if (settled(u)) cycle
         settled(u) = .true.
         if (u == target) then
            reached = .true.
            exit
         end if
         cost_u = cost_so_far(u)
         do k = graph%row_ptr(u), graph%row_ptr(u + 1) - 1
            v = graph%col_idx(k)
            candidate = cost_u + graph%values(k)
            if (candidate < cost_so_far(v)) then
               cost_so_far(v) = candidate
               prev(v) = u
               call queue%push(distance_key(candidate + heuristic(v)), int(v, int32))
            end if
         end do
      end do

      call queue%destroy()
      if (reached) then
         deallocate (path)
         call trace_back(prev, source, target, path)
      end if
   end subroutine a_star