///|
/// Performs BFS (Breadth-First Search) to find the shortest path
/// on an unweighted grid from `start` to `goal`.
///
/// BFS ignores cell weights (all unblocked cells have cost 1) and
/// finds the path with the fewest steps. Uses four-way movement.
///
/// Returns a `SearchResult` with the path, total steps, and statistics.
///
/// # Errors
/// Returns a failure result if:
/// - `start` or `goal` is out of bounds
/// - `start` or `goal` is blocked
/// - No path exists from start to goal
pub fn bfs(grid : Grid, start : Point, goal : Point) -> SearchResult {
  // --- Input validation ---
  if !grid.is_valid_search(start, goal) {
    return SearchResult::failure(0, 0)
  }

  // --- Trivial case: start == goal ---
  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

  // Visited tracking
  let visited = Array::make(size, false)
  visited[start_idx] = true

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

  // Simple queue using two-pointer approach (efficient ring-buffer-like)
  let queue = Array::new()
  queue.push(start_idx)
  let mut head = 0

  let mut nodes_visited = 1
  let mut nodes_expanded = 0

  let mut found = false

  while head < queue.length() {
    let current_idx = queue[head]
    head = head + 1

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

    // Convert index to point for neighbor generation
    let cx = current_idx % grid.width
    let cy = current_idx / grid.width
    let neighbors = four_way_neighbors(grid, Point::new(cx, cy))

    for i = 0; i < neighbors.length(); i = i + 1 {
      let n = neighbors[i]
      let n_idx = n.y * grid.width + n.x
      if !visited[n_idx] {
        visited[n_idx] = true
        parent[n_idx] = current_idx
        queue.push(n_idx)
        nodes_visited = nodes_visited + 1
      }
    }
  }

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

  // --- Reconstruct path ---
  let path = reconstruct_path(parent, start_idx, goal_idx, grid.width)
  let total_cost = (path.length() - 1).to_double()

  SearchResult::success(path, total_cost, nodes_visited, nodes_expanded)
}

///|
/// Reconstructs the path as Points from start to goal (inclusive).
///
/// Traces parent pointers backwards from goal to start,
/// then reverses to get the forward path.
///
/// Includes a safety limit to prevent infinite loops from
/// corrupted parent data (should never trigger in correct usage).
fn reconstruct_path(
  parent : Array[Int],
  start_idx : Int,
  goal_idx : Int,
  width : Int,
) -> Array[Point] {
  let max_steps = parent.length()
  let reversed = Array::new()
  let mut current = goal_idx
  reversed.push(current)
  let mut steps = 0
  while current != start_idx && steps < max_steps {
    current = parent[current]
    // Guard against -1 (no parent) or invalid parent
    if current < 0 || current >= parent.length() {
      break
    }
    reversed.push(current)
    steps = steps + 1
  }

  // Build forward path (start -> goal) as Points
  let path = Array::new()
  let len = reversed.length()
  let mut i = len - 1
  while i >= 0 {
    let idx = reversed[i]
    let x = idx % width
    let y = idx / width
    path.push(Point::new(x, y))
    i = i - 1
  }
  path
}