///|
/// Performs A* search to find the shortest path from `start` to `goal`
/// using a configurable heuristic.
///
/// A* combines Dijkstra's algorithm with a heuristic function that
/// estimates the remaining distance to the goal. This allows A* to
/// focus the search toward the goal, typically expanding fewer nodes
/// than Dijkstra while still finding optimal paths (if the heuristic
/// is admissible).
///
/// # Heuristic
/// The heuristic function `h(current, goal)` must be deterministic,
/// non-negative, zero at the goal, and **admissible** (never overestimate the
/// true cost) for A* to guarantee optimality. Non-finite and negative values
/// are defensively treated as zero.
/// Common choices:
/// - `manhattan` for four-way movement
/// - `chebyshev` or `octile` for eight-way movement
/// - `euclidean` for any-direction movement
///
/// # Movement
/// The `options.movement` field controls whether four-way or eight-way
/// neighbors are generated, including appropriate move costs.
///
/// # Errors
/// Returns a failure result if:
/// - `start` or `goal` is out of bounds
/// - `start` or `goal` is blocked
/// - No path exists
pub fn astar(
  grid : Grid,
  start : Point,
  goal : Point,
  options : SearchOptions,
) -> SearchResult {
  // --- Input validation ---
  if !grid.is_valid_search(start, goal) {
    return SearchResult::failure(0, 0)
  }

  // --- Trivial case ---
  if start == goal {
    let path = Array::new()
    path.push(start)
    return SearchResult::success(path, 0.0, 1, 0)
  }

  let size = grid.size()
  let start_idx = start.y * grid.width + start.x
  let goal_idx = goal.y * grid.width + goal.x

  let infinity = 1.0e300

  // g_score: actual cost from start to node
  let g_score = Array::make(size, infinity)
  g_score[start_idx] = 0.0

  // Best queued f-score for lazy deletion of stale heap entries.
  let f_score = Array::make(size, infinity)

  // Parent tracking
  let parent = Array::make(size, -1)

  // Select neighbor generation function based on movement mode
  let get_neighbors = match options.movement {
    Movement::FourWay => four_way_neighbors_with_cost
    Movement::EightWay => eight_way_neighbors_with_cost
  }

  // Min-heap priority queue: stores (f_score, node_index)
  // f_score = g_score + h_score
  let heap = MinHeap::new()
  let h_start = heuristic_value(options, start, goal)
  f_score[start_idx] = h_start
  heap.push(f_score[start_idx], start_idx)

  let discovered = Array::make(size, false)
  discovered[start_idx] = true
  let mut nodes_visited = 1
  let mut nodes_expanded = 0
  let mut found = false

  while !heap.is_empty() {
    match heap.pop() {
      None => break
      Some((current_f, current_idx)) => {
        // Lazy deletion: skip stale entries
        if current_f > f_score[current_idx] {
          continue
        }
        let cx = current_idx % grid.width
        let cy = current_idx / grid.width
        let current_point = Point::new(cx, cy)

        if current_idx == goal_idx {
          found = true
          break
        }
        nodes_expanded = nodes_expanded + 1

        let neighbors = get_neighbors(grid, current_point)

        for i = 0; i < neighbors.length(); i = i + 1 {
          let (n, edge_cost) = neighbors[i]
          let n_idx = n.y * grid.width + n.x
          let tentative_g = g_score[current_idx] + edge_cost

          if tentative_g < g_score[n_idx] {
            g_score[n_idx] = tentative_g
            parent[n_idx] = current_idx
            let h = heuristic_value(options, n, goal)
            let f = tentative_g + h
            f_score[n_idx] = f
            heap.push(f, n_idx)
            if !discovered[n_idx] {
              discovered[n_idx] = true
              nodes_visited = nodes_visited + 1
            }
          }
        }
      }
    }
  }

  if !found {
    return SearchResult::failure(nodes_visited, nodes_expanded)
  }

  let path = reconstruct_path(parent, start_idx, goal_idx, grid.width)
  SearchResult::success(path, g_score[goal_idx], nodes_visited, nodes_expanded)
}

///|
fn heuristic_value(
  options : SearchOptions,
  point : Point,
  goal : Point,
) -> Double {
  if point == goal {
    return 0.0
  }
  let value = (options.heuristic)(point, goal)
  if value < 0.0 || value.is_nan() || value.is_inf() {
    0.0
  } else {
    value
  }
}