raasta 1.0.0

//! Grid-based navigation and A* pathfinding.

use std::cmp::Ordering;
use std::collections::BinaryHeap;

use hisab::Vec2;
use serde::{Deserialize, Serialize};

use crate::error::NavError;

#[cfg(feature = "logging")]
use tracing::instrument;

/// A position on the navigation grid.
#[derive(Debug, Clone, Copy, PartialEq, Eq, Hash, Serialize, Deserialize)]
pub struct GridPos {
    pub x: i32,
    pub y: i32,
}

impl GridPos {
    #[must_use]
    pub fn new(x: i32, y: i32) -> Self {
        Self { x, y }
    }

    /// Manhattan distance to another position.
    #[inline]
    #[must_use]
    pub fn manhattan_distance(self, other: GridPos) -> i32 {
        (self.x - other.x).abs() + (self.y - other.y).abs()
    }

    /// Octile distance (allows diagonal movement).
    #[inline]
    #[must_use]
    pub fn octile_distance(self, other: GridPos) -> f32 {
        let dx = (self.x - other.x).abs() as f32;
        let dy = (self.y - other.y).abs() as f32;
        let (min, max) = if dx < dy { (dx, dy) } else { (dy, dx) };
        max + (std::f32::consts::SQRT_2 - 1.0) * min
    }
}

/// A 2D navigation grid with walkability and movement costs.
///
/// Walkability is stored as a bitset (1 bit per cell) for 8x memory
/// reduction compared to `Vec<bool>`.
#[derive(Debug, Clone, Serialize, Deserialize)]
pub struct NavGrid {
    width: usize,
    height: usize,
    cell_size: f32,
    /// Bit-packed walkability: 1 = walkable, 0 = blocked. Each u64 stores 64 cells.
    walkable: Vec<u64>,
    /// Per-cell movement cost multiplier (1.0 = normal).
    costs: Vec<f32>,
    /// Whether diagonal movement is allowed.
    pub allow_diagonal: bool,
}

impl NavGrid {
    /// Create a new grid where all cells are walkable.
    ///
    /// # Panics
    ///
    /// Panics if `width * height` overflows `usize`.
    #[must_use]
    pub fn new(width: usize, height: usize, cell_size: f32) -> Self {
        Self::try_new(width, height, cell_size).unwrap_or_else(|_| {
            // Saturating fallback: return a minimal 1x1 grid rather than panicking
            Self {
                width: 1,
                height: 1,
                cell_size: cell_size.max(f32::EPSILON),
                walkable: vec![u64::MAX; 1],
                costs: vec![1.0; 1],
                allow_diagonal: true,
            }
        })
    }

    /// Fallible constructor — returns an error instead of panicking on overflow.
    ///
    /// # Errors
    ///
    /// Returns [`NavError::GridOverflow`] if `width * height` overflows `usize`.
    pub fn try_new(width: usize, height: usize, cell_size: f32) -> Result<Self, NavError> {
        if cell_size <= 0.0 {
            return Err(NavError::InvalidCellSize { value: cell_size });
        }
        let len = width
            .checked_mul(height)
            .ok_or(NavError::GridOverflow { width, height })?;
        let bitset_len = len.div_ceil(64);
        Ok(Self {
            width,
            height,
            cell_size,
            walkable: vec![u64::MAX; bitset_len], // all bits set = all walkable
            costs: vec![1.0; len],
            allow_diagonal: true,
        })
    }

    #[must_use]
    pub fn width(&self) -> usize {
        self.width
    }

    #[must_use]
    pub fn height(&self) -> usize {
        self.height
    }

    #[must_use]
    pub fn cell_size(&self) -> f32 {
        self.cell_size
    }

    /// Set whether a cell is walkable.
    pub fn set_walkable(&mut self, x: i32, y: i32, walkable: bool) {
        if let Some(idx) = self.index(x, y) {
            let word = idx / 64;
            let bit = idx % 64;
            if walkable {
                self.walkable[word] |= 1u64 << bit;
            } else {
                self.walkable[word] &= !(1u64 << bit);
            }
        }
    }

    /// Check if a cell is walkable.
    #[inline]
    #[must_use]
    pub fn is_walkable(&self, x: i32, y: i32) -> bool {
        self.index(x, y)
            .map(|idx| (self.walkable[idx / 64] >> (idx % 64)) & 1 == 1)
            .unwrap_or(false)
    }

    /// Set the movement cost for a cell.
    pub fn set_cost(&mut self, x: i32, y: i32, cost: f32) {
        if let Some(idx) = self.index(x, y) {
            self.costs[idx] = cost;
        }
    }

    /// Get the movement cost for a cell.
    #[inline]
    #[must_use]
    pub fn cost(&self, x: i32, y: i32) -> f32 {
        self.index(x, y)
            .map(|idx| self.costs[idx])
            .unwrap_or(f32::INFINITY)
    }

    /// Block a rectangular region of cells.
    pub fn block_rect(&mut self, min_x: i32, min_y: i32, max_x: i32, max_y: i32) {
        for y in min_y..=max_y {
            for x in min_x..=max_x {
                self.set_walkable(x, y, false);
            }
        }
    }

    /// Unblock a rectangular region of cells.
    pub fn unblock_rect(&mut self, min_x: i32, min_y: i32, max_x: i32, max_y: i32) {
        for y in min_y..=max_y {
            for x in min_x..=max_x {
                self.set_walkable(x, y, true);
            }
        }
    }

    /// Block all cells within a circle (world-space center and radius).
    pub fn block_circle(&mut self, center: Vec2, radius: f32) {
        let min = self.world_to_grid(center - Vec2::splat(radius));
        let max = self.world_to_grid(center + Vec2::splat(radius));
        let r_sq = radius * radius;
        for y in min.y..=max.y {
            for x in min.x..=max.x {
                let cell_center = self.grid_to_world(GridPos::new(x, y));
                if cell_center.distance_squared(center) <= r_sq {
                    self.set_walkable(x, y, false);
                }
            }
        }
    }

    /// Unblock all cells within a circle (world-space center and radius).
    pub fn unblock_circle(&mut self, center: Vec2, radius: f32) {
        let min = self.world_to_grid(center - Vec2::splat(radius));
        let max = self.world_to_grid(center + Vec2::splat(radius));
        let r_sq = radius * radius;
        for y in min.y..=max.y {
            for x in min.x..=max.x {
                let cell_center = self.grid_to_world(GridPos::new(x, y));
                if cell_center.distance_squared(center) <= r_sq {
                    self.set_walkable(x, y, true);
                }
            }
        }
    }

    /// Set uniform cost for a rectangular region.
    pub fn set_cost_rect(&mut self, min_x: i32, min_y: i32, max_x: i32, max_y: i32, cost: f32) {
        for y in min_y..=max_y {
            for x in min_x..=max_x {
                self.set_cost(x, y, cost);
            }
        }
    }

    /// Find the nearest walkable cell to the given position.
    ///
    /// Searches outward in a spiral pattern. Returns `None` if no walkable
    /// cell exists in the grid.
    #[must_use]
    pub fn nearest_walkable(&self, pos: GridPos) -> Option<GridPos> {
        if self.is_walkable(pos.x, pos.y) {
            return Some(pos);
        }
        let max_radius = self.width.max(self.height) as i32;
        for r in 1..=max_radius {
            for dx in -r..=r {
                for &dy in &[-r, r] {
                    let candidate = GridPos::new(pos.x + dx, pos.y + dy);
                    if self.is_walkable(candidate.x, candidate.y) {
                        return Some(candidate);
                    }
                }
            }
            for dy in (-r + 1)..r {
                for &dx in &[-r, r] {
                    let candidate = GridPos::new(pos.x + dx, pos.y + dy);
                    if self.is_walkable(candidate.x, candidate.y) {
                        return Some(candidate);
                    }
                }
            }
        }
        None
    }

    /// Reset all cells to walkable with cost 1.0.
    pub fn clear(&mut self) {
        self.walkable.fill(u64::MAX);
        self.costs.fill(1.0);
    }

    /// Convert grid position to world position (center of cell).
    #[must_use]
    pub fn grid_to_world(&self, pos: GridPos) -> Vec2 {
        Vec2::new(
            (pos.x as f32 + 0.5) * self.cell_size,
            (pos.y as f32 + 0.5) * self.cell_size,
        )
    }

    /// Convert world position to grid position.
    #[must_use]
    pub fn world_to_grid(&self, world: Vec2) -> GridPos {
        GridPos::new(
            (world.x / self.cell_size).floor() as i32,
            (world.y / self.cell_size).floor() as i32,
        )
    }

    /// Find a path from `start` to `goal` using A*.
    ///
    /// Returns `None` if no path exists.
    #[cfg_attr(feature = "logging", instrument(skip(self), fields(w = self.width, h = self.height)))]
    #[must_use]
    pub fn find_path(&self, start: GridPos, goal: GridPos) -> Option<Vec<GridPos>> {
        if !self.is_walkable(start.x, start.y) || !self.is_walkable(goal.x, goal.y) {
            return None;
        }
        if start == goal {
            return Some(vec![start]);
        }

        let len = self.width * self.height;
        let mut g_score = vec![f32::INFINITY; len];
        let mut came_from: Vec<Option<usize>> = vec![None; len];
        let mut closed = vec![false; len];
        let mut open = BinaryHeap::new();

        let start_idx = self.index(start.x, start.y)?;
        let goal_idx = self.index(goal.x, goal.y)?;

        g_score[start_idx] = 0.0;
        let h = if self.allow_diagonal {
            start.octile_distance(goal)
        } else {
            start.manhattan_distance(goal) as f32
        };
        open.push(AStarNode {
            idx: start_idx,
            f_score: h,
        });

        let mut neighbors_buf = [(0i32, 0i32, 0.0f32); 8];

        while let Some(current) = open.pop() {
            if current.idx == goal_idx {
                return Some(self.reconstruct_path(&came_from, goal_idx));
            }

            if closed[current.idx] {
                continue;
            }
            closed[current.idx] = true;

            let cx = (current.idx % self.width) as i32;
            let cy = (current.idx / self.width) as i32;
            let count = self.neighbors_into(cx, cy, &mut neighbors_buf);

            for &(nx, ny, move_cost) in &neighbors_buf[..count] {
                let n_idx = self.index_unchecked(nx, ny);
                if closed[n_idx] {
                    continue;
                }
                let tentative_g = g_score[current.idx] + move_cost * self.costs[n_idx];

                if tentative_g < g_score[n_idx] {
                    came_from[n_idx] = Some(current.idx);
                    g_score[n_idx] = tentative_g;
                    let np = GridPos::new(nx, ny);
                    let h = if self.allow_diagonal {
                        np.octile_distance(goal)
                    } else {
                        np.manhattan_distance(goal) as f32
                    };
                    open.push(AStarNode {
                        idx: n_idx,
                        f_score: tentative_g + h,
                    });
                }
            }
        }

        None
    }

    /// Find a path from `start` to `goal`, returning a partial path to the
    /// closest reachable point if the goal is unreachable.
    ///
    /// Returns `None` only if the start is not walkable.
    #[cfg_attr(feature = "logging", instrument(skip(self), fields(w = self.width, h = self.height)))]
    #[must_use]
    pub fn find_path_partial(&self, start: GridPos, goal: GridPos) -> Option<Vec<GridPos>> {
        if !self.is_walkable(start.x, start.y) {
            return None;
        }
        if start == goal {
            return Some(vec![start]);
        }
        // If goal is walkable, try the normal path first
        if self.is_walkable(goal.x, goal.y)
            && let Some(path) = self.find_path(start, goal)
        {
            return Some(path);
        }

        // Run A* from start, tracking the explored node closest to goal
        let len = self.width * self.height;
        let mut g_score = vec![f32::INFINITY; len];
        let mut came_from: Vec<Option<usize>> = vec![None; len];
        let mut closed = vec![false; len];
        let mut open = BinaryHeap::new();

        let start_idx = self.index(start.x, start.y)?;

        g_score[start_idx] = 0.0;
        let h = if self.allow_diagonal {
            start.octile_distance(goal)
        } else {
            start.manhattan_distance(goal) as f32
        };
        open.push(AStarNode {
            idx: start_idx,
            f_score: h,
        });

        let mut best_idx = start_idx;
        let mut best_h = h;

        let mut neighbors_buf = [(0i32, 0i32, 0.0f32); 8];

        while let Some(current) = open.pop() {
            if closed[current.idx] {
                continue;
            }
            closed[current.idx] = true;

            // Track the node closest to goal by heuristic
            let cx = (current.idx % self.width) as i32;
            let cy = (current.idx / self.width) as i32;
            let current_pos = GridPos::new(cx, cy);
            let current_h = if self.allow_diagonal {
                current_pos.octile_distance(goal)
            } else {
                current_pos.manhattan_distance(goal) as f32
            };
            if current_h < best_h {
                best_h = current_h;
                best_idx = current.idx;
            }

            let count = self.neighbors_into(cx, cy, &mut neighbors_buf);

            for &(nx, ny, move_cost) in &neighbors_buf[..count] {
                let n_idx = self.index_unchecked(nx, ny);
                if closed[n_idx] {
                    continue;
                }
                let tentative_g = g_score[current.idx] + move_cost * self.costs[n_idx];

                if tentative_g < g_score[n_idx] {
                    came_from[n_idx] = Some(current.idx);
                    g_score[n_idx] = tentative_g;
                    let np = GridPos::new(nx, ny);
                    let h = if self.allow_diagonal {
                        np.octile_distance(goal)
                    } else {
                        np.manhattan_distance(goal) as f32
                    };
                    open.push(AStarNode {
                        idx: n_idx,
                        f_score: tentative_g + h,
                    });
                }
            }
        }

        // Reconstruct path to the closest explored node
        Some(self.reconstruct_path(&came_from, best_idx))
    }

    /// Find a path using Jump Point Search (JPS).
    ///
    /// JPS is significantly faster than A* on uniform-cost grids with diagonal
    /// movement. It prunes symmetric paths by "jumping" along straight lines
    /// and only expanding nodes where forced neighbors exist.
    ///
    /// **Requirements:** `allow_diagonal` must be `true` and all walkable cells
    /// should have uniform cost (1.0). Falls back to standard A* if these
    /// conditions are not met.
    #[cfg_attr(feature = "logging", instrument(skip(self)))]
    #[must_use]
    pub fn find_path_jps(&self, start: GridPos, goal: GridPos) -> Option<Vec<GridPos>> {
        // JPS requires diagonal movement
        if !self.allow_diagonal {
            return self.find_path(start, goal);
        }
        if !self.is_walkable(start.x, start.y) || !self.is_walkable(goal.x, goal.y) {
            return None;
        }
        if start == goal {
            return Some(vec![start]);
        }

        let len = self.width * self.height;
        let mut g_score = vec![f32::INFINITY; len];
        let mut came_from: Vec<Option<usize>> = vec![None; len];
        let mut closed = vec![false; len];
        let mut open = BinaryHeap::new();

        let start_idx = self.index(start.x, start.y)?;
        let goal_idx = self.index(goal.x, goal.y)?;

        g_score[start_idx] = 0.0;
        open.push(AStarNode {
            idx: start_idx,
            f_score: start.octile_distance(goal),
        });

        while let Some(current) = open.pop() {
            if current.idx == goal_idx {
                return Some(self.reconstruct_jps_path(&came_from, goal_idx));
            }

            if closed[current.idx] {
                continue;
            }
            closed[current.idx] = true;

            let cx = (current.idx % self.width) as i32;
            let cy = (current.idx / self.width) as i32;
            let current_pos = GridPos::new(cx, cy);

            // Determine search directions from parent
            let directions = if let Some(parent_idx) = came_from[current.idx] {
                let px = (parent_idx % self.width) as i32;
                let py = (parent_idx / self.width) as i32;
                let dx = (cx - px).signum();
                let dy = (cy - py).signum();
                self.jps_prune_directions(cx, cy, dx, dy)
            } else {
                // Start node: expand all 8 directions
                [
                    (1, 0),
                    (-1, 0),
                    (0, 1),
                    (0, -1),
                    (1, 1),
                    (1, -1),
                    (-1, 1),
                    (-1, -1),
                ]
            };

            for (dx, dy) in directions {
                if dx == 0 && dy == 0 {
                    continue;
                }
                if let Some(jump_pos) = self.jump(cx, cy, dx, dy, goal) {
                    let j_idx = self.index_unchecked(jump_pos.x, jump_pos.y);
                    if closed[j_idx] {
                        continue;
                    }

                    let dist = current_pos.octile_distance(jump_pos);
                    let tentative_g = g_score[current.idx] + dist;

                    if tentative_g < g_score[j_idx] {
                        g_score[j_idx] = tentative_g;
                        came_from[j_idx] = Some(current.idx);
                        let h = jump_pos.octile_distance(goal);
                        open.push(AStarNode {
                            idx: j_idx,
                            f_score: tentative_g + h,
                        });
                    }
                }
            }
        }

        None
    }

    /// Jump in direction (dx, dy) from (x, y). Returns the jump point if found.
    ///
    /// Iterative implementation to avoid stack overflow on large grids.
    #[inline]
    fn jump(&self, x: i32, y: i32, dx: i32, dy: i32, goal: GridPos) -> Option<GridPos> {
        let mut cx = x + dx;
        let mut cy = y + dy;

        loop {
            if !self.is_walkable(cx, cy) {
                return None;
            }

            if cx == goal.x && cy == goal.y {
                return Some(GridPos::new(cx, cy));
            }

            // Check for forced neighbors
            if dx != 0 && dy != 0 {
                // Diagonal move: forced neighbor if blocked cardinal but open diagonal-adjacent
                if (!self.is_walkable(cx - dx, cy) && self.is_walkable(cx - dx, cy + dy))
                    || (!self.is_walkable(cx, cy - dy) && self.is_walkable(cx + dx, cy - dy))
                {
                    return Some(GridPos::new(cx, cy));
                }
                // Diagonal: must also check cardinal sub-jumps (these are straight-line, safe to recurse)
                if self.jump_straight(cx, cy, dx, 0, goal).is_some()
                    || self.jump_straight(cx, cy, 0, dy, goal).is_some()
                {
                    return Some(GridPos::new(cx, cy));
                }
            } else if dx != 0 {
                // Horizontal move
                if (!self.is_walkable(cx, cy + 1) && self.is_walkable(cx + dx, cy + 1))
                    || (!self.is_walkable(cx, cy - 1) && self.is_walkable(cx + dx, cy - 1))
                {
                    return Some(GridPos::new(cx, cy));
                }
            } else {
                // Vertical move
                if (!self.is_walkable(cx + 1, cy) && self.is_walkable(cx + 1, cy + dy))
                    || (!self.is_walkable(cx - 1, cy) && self.is_walkable(cx - 1, cy + dy))
                {
                    return Some(GridPos::new(cx, cy));
                }
            }

            // Continue jumping
            // Diagonal requires both cardinal cells walkable (no corner-cutting)
            if dx != 0
                && dy != 0
                && (!self.is_walkable(cx + dx, cy) || !self.is_walkable(cx, cy + dy))
            {
                return None;
            }
            cx += dx;
            cy += dy;
        }
    }

    /// Straight-line (cardinal) jump — iterative helper for diagonal jump's sub-searches.
    #[inline]
    fn jump_straight(&self, x: i32, y: i32, dx: i32, dy: i32, goal: GridPos) -> Option<GridPos> {
        debug_assert!(
            (dx == 0) != (dy == 0),
            "jump_straight must be cardinal only"
        );
        let mut cx = x + dx;
        let mut cy = y + dy;

        loop {
            if !self.is_walkable(cx, cy) {
                return None;
            }
            if cx == goal.x && cy == goal.y {
                return Some(GridPos::new(cx, cy));
            }
            if dx != 0 {
                if (!self.is_walkable(cx, cy + 1) && self.is_walkable(cx + dx, cy + 1))
                    || (!self.is_walkable(cx, cy - 1) && self.is_walkable(cx + dx, cy - 1))
                {
                    return Some(GridPos::new(cx, cy));
                }
            } else if (!self.is_walkable(cx + 1, cy) && self.is_walkable(cx + 1, cy + dy))
                || (!self.is_walkable(cx - 1, cy) && self.is_walkable(cx - 1, cy + dy))
            {
                return Some(GridPos::new(cx, cy));
            }
            cx += dx;
            cy += dy;
        }
    }

    /// Prune directions for JPS based on parent direction.
    /// Returns up to 8 (dx, dy) pairs; unused slots are (0, 0).
    fn jps_prune_directions(&self, x: i32, y: i32, dx: i32, dy: i32) -> [(i32, i32); 8] {
        let mut dirs = [(0i32, 0i32); 8];
        let mut count = 0;

        if dx != 0 && dy != 0 {
            // Diagonal: natural neighbors are (dx,0), (0,dy), (dx,dy)
            dirs[count] = (dx, 0);
            count += 1;
            dirs[count] = (0, dy);
            count += 1;
            dirs[count] = (dx, dy);
            count += 1;

            // Forced neighbors from blocked cardinals
            if !self.is_walkable(x - dx, y) {
                dirs[count] = (-dx, dy);
                count += 1;
            }
            if !self.is_walkable(x, y - dy) {
                dirs[count] = (dx, -dy);
                count += 1;
            }
        } else if dx != 0 {
            // Horizontal: natural neighbor is (dx, 0)
            dirs[count] = (dx, 0);
            count += 1;

            // Forced: if cell above/below is blocked, diagonal becomes forced
            if !self.is_walkable(x, y + 1) {
                dirs[count] = (dx, 1);
                count += 1;
            }
            if !self.is_walkable(x, y - 1) {
                dirs[count] = (dx, -1);
                count += 1;
            }
        } else if dy != 0 {
            // Vertical: natural neighbor is (0, dy)
            dirs[count] = (0, dy);
            count += 1;

            // Forced
            if !self.is_walkable(x + 1, y) {
                dirs[count] = (1, dy);
                count += 1;
            }
            if !self.is_walkable(x - 1, y) {
                dirs[count] = (-1, dy);
                count += 1;
            }
        }

        let _ = count; // suppress unused warning
        dirs
    }

    /// Reconstruct path from JPS came_from, inserting intermediate cells
    /// along jumps so the path is contiguous.
    fn reconstruct_jps_path(&self, came_from: &[Option<usize>], goal_idx: usize) -> Vec<GridPos> {
        // First collect jump points in reverse
        let mut jump_points = Vec::new();
        let mut current = goal_idx;
        loop {
            let x = (current % self.width) as i32;
            let y = (current / self.width) as i32;
            jump_points.push(GridPos::new(x, y));
            match came_from[current] {
                Some(prev) => current = prev,
                None => break,
            }
        }
        jump_points.reverse();

        if jump_points.len() <= 1 {
            return jump_points;
        }

        // Interpolate between consecutive jump points
        let mut path = Vec::with_capacity(jump_points.len() * 4);
        path.push(jump_points[0]);

        for w in jump_points.windows(2) {
            let from = w[0];
            let to = w[1];
            let dx = (to.x - from.x).signum();
            let dy = (to.y - from.y).signum();
            let mut cx = from.x;
            let mut cy = from.y;
            while cx != to.x || cy != to.y {
                cx += dx;
                cy += dy;
                path.push(GridPos::new(cx, cy));
            }
        }

        path
    }

    /// Check line-of-sight between two grid cells using Bresenham's line.
    ///
    /// Returns `true` if all cells along the line are walkable.
    #[must_use]
    pub fn has_line_of_sight(&self, from: GridPos, to: GridPos) -> bool {
        let mut x = from.x;
        let mut y = from.y;
        let dx = (to.x - from.x).abs();
        let dy = (to.y - from.y).abs();
        let sx = if from.x < to.x { 1 } else { -1 };
        let sy = if from.y < to.y { 1 } else { -1 };
        let mut err = dx - dy;

        loop {
            if !self.is_walkable(x, y) {
                return false;
            }
            if x == to.x && y == to.y {
                return true;
            }
            let e2 = 2 * err;
            if e2 > -dy {
                err -= dy;
                x += sx;
            }
            if e2 < dx {
                err += dx;
                y += sy;
            }
        }
    }

    /// Find a path using Theta\* (any-angle pathfinding).
    ///
    /// Like A\*, but checks line-of-sight to the parent's parent. If LOS
    /// exists, skips the intermediate node, producing smoother paths
    /// without post-processing.
    #[cfg_attr(feature = "logging", instrument(skip(self)))]
    #[must_use]
    pub fn find_path_theta(&self, start: GridPos, goal: GridPos) -> Option<Vec<GridPos>> {
        if !self.is_walkable(start.x, start.y) || !self.is_walkable(goal.x, goal.y) {
            return None;
        }
        if start == goal {
            return Some(vec![start]);
        }

        let len = self.width * self.height;
        let mut g_score = vec![f32::INFINITY; len];
        let mut came_from: Vec<Option<usize>> = vec![None; len];
        let mut closed = vec![false; len];
        let mut open = BinaryHeap::new();

        let start_idx = self.index(start.x, start.y)?;
        let goal_idx = self.index(goal.x, goal.y)?;

        g_score[start_idx] = 0.0;
        let h = start.octile_distance(goal);
        open.push(AStarNode {
            idx: start_idx,
            f_score: h,
        });

        let mut neighbors_buf = [(0i32, 0i32, 0.0f32); 8];

        while let Some(current) = open.pop() {
            if current.idx == goal_idx {
                return Some(self.reconstruct_path(&came_from, goal_idx));
            }

            if closed[current.idx] {
                continue;
            }
            closed[current.idx] = true;

            let cx = (current.idx % self.width) as i32;
            let cy = (current.idx / self.width) as i32;
            let count = self.neighbors_into(cx, cy, &mut neighbors_buf);

            for &(nx, ny, _) in &neighbors_buf[..count] {
                let n_idx = self.index_unchecked(nx, ny);
                if closed[n_idx] {
                    continue;
                }

                let neighbor_pos = GridPos::new(nx, ny);

                // Theta* key insight: check LOS from parent of current to neighbor
                let (source_idx, source_g) = if let Some(parent_idx) = came_from[current.idx] {
                    let px = (parent_idx % self.width) as i32;
                    let py = (parent_idx / self.width) as i32;
                    let parent_pos = GridPos::new(px, py);
                    if self.has_line_of_sight(parent_pos, neighbor_pos) {
                        (parent_idx, g_score[parent_idx])
                    } else {
                        (current.idx, g_score[current.idx])
                    }
                } else {
                    (current.idx, g_score[current.idx])
                };

                let source_pos = GridPos::new(
                    (source_idx % self.width) as i32,
                    (source_idx / self.width) as i32,
                );
                let edge_cost = if source_idx == current.idx {
                    // Regular A* step — adjacent cell
                    source_pos.octile_distance(neighbor_pos) * self.costs[n_idx]
                } else {
                    // LOS shortcut — sum costs along the line
                    self.line_cost(source_pos, neighbor_pos)
                };
                let tentative_g = source_g + edge_cost;

                if tentative_g < g_score[n_idx] {
                    came_from[n_idx] = Some(source_idx);
                    g_score[n_idx] = tentative_g;
                    let h = neighbor_pos.octile_distance(goal);
                    open.push(AStarNode {
                        idx: n_idx,
                        f_score: tentative_g + h,
                    });
                }
            }
        }

        None
    }

    /// Find a path using Lazy Theta\* (any-angle with deferred LOS checks).
    ///
    /// Like Theta\*, produces smooth any-angle paths, but defers line-of-sight
    /// checks until a node is expanded rather than when it's generated.
    /// This typically requires fewer LOS checks than standard Theta\*.
    #[cfg_attr(feature = "logging", instrument(skip(self)))]
    #[must_use]
    pub fn find_path_lazy_theta(&self, start: GridPos, goal: GridPos) -> Option<Vec<GridPos>> {
        if !self.is_walkable(start.x, start.y) || !self.is_walkable(goal.x, goal.y) {
            return None;
        }
        if start == goal {
            return Some(vec![start]);
        }

        let len = self.width * self.height;
        let mut g_score = vec![f32::INFINITY; len];
        // parent stores the index of the node's parent in the path
        // In lazy theta*, we optimistically set parent to grandparent
        let mut parent: Vec<Option<usize>> = vec![None; len];
        let mut closed = vec![false; len];
        let mut open = BinaryHeap::new();

        let start_idx = self.index(start.x, start.y)?;
        let goal_idx = self.index(goal.x, goal.y)?;

        g_score[start_idx] = 0.0;
        let h = start.octile_distance(goal);
        open.push(AStarNode {
            idx: start_idx,
            f_score: h,
        });

        let mut neighbors_buf = [(0i32, 0i32, 0.0f32); 8];

        while let Some(current) = open.pop() {
            if current.idx == goal_idx {
                return Some(self.reconstruct_path(&parent, goal_idx));
            }

            if closed[current.idx] {
                continue;
            }

            let cx = (current.idx % self.width) as i32;
            let cy = (current.idx / self.width) as i32;
            let current_pos = GridPos::new(cx, cy);

            // LAZY THETA* KEY: check LOS now (at expansion time)
            // If current's parent has LOS to current, keep the parent link.
            // Otherwise, revert to best adjacent neighbor as parent (like A*).
            if let Some(pp_idx) = parent[current.idx] {
                let pp_pos =
                    GridPos::new((pp_idx % self.width) as i32, (pp_idx / self.width) as i32);
                if !self.has_line_of_sight(pp_pos, current_pos) {
                    // No LOS — find the best actual neighbor as parent
                    let mut best_g = f32::INFINITY;
                    let mut best_parent = None;
                    let count = self.neighbors_into(cx, cy, &mut neighbors_buf);
                    for &(nx, ny, move_cost) in &neighbors_buf[..count] {
                        let n_idx = self.index_unchecked(nx, ny);
                        if closed[n_idx] {
                            let g = g_score[n_idx] + move_cost * self.costs[n_idx];
                            if g < best_g {
                                best_g = g;
                                best_parent = Some(n_idx);
                            }
                        }
                    }
                    if let Some(bp) = best_parent {
                        parent[current.idx] = Some(bp);
                        g_score[current.idx] = best_g;
                    }
                }
            }

            closed[current.idx] = true;
            let current_g = g_score[current.idx];

            // Expand neighbors
            let count = self.neighbors_into(cx, cy, &mut neighbors_buf);
            for &(nx, ny, _) in &neighbors_buf[..count] {
                let n_idx = self.index_unchecked(nx, ny);
                if closed[n_idx] {
                    continue;
                }

                let neighbor_pos = GridPos::new(nx, ny);

                // Optimistically assume LOS from current's parent to neighbor
                let (source_idx, source_g) = if let Some(pp_idx) = parent[current.idx] {
                    (pp_idx, g_score[pp_idx])
                } else {
                    (current.idx, current_g)
                };

                let source_pos = GridPos::new(
                    (source_idx % self.width) as i32,
                    (source_idx / self.width) as i32,
                );

                // Cost: Euclidean distance from source to neighbor
                let tentative_g = source_g + source_pos.octile_distance(neighbor_pos);

                // Also compute normal A* cost through current
                let normal_g =
                    current_g + current_pos.octile_distance(neighbor_pos) * self.costs[n_idx];

                if tentative_g < g_score[n_idx] {
                    parent[n_idx] = Some(source_idx);
                    g_score[n_idx] = tentative_g;
                    let h = neighbor_pos.octile_distance(goal);
                    open.push(AStarNode {
                        idx: n_idx,
                        f_score: tentative_g + h,
                    });
                } else if normal_g < g_score[n_idx] {
                    parent[n_idx] = Some(current.idx);
                    g_score[n_idx] = normal_g;
                    let h = neighbor_pos.octile_distance(goal);
                    open.push(AStarNode {
                        idx: n_idx,
                        f_score: normal_g + h,
                    });
                }
            }
        }

        None
    }

    /// Generate a flow field toward the given goal.
    ///
    /// Returns a grid-sized Vec of direction vectors `(dx, dy)` where
    /// `(0, 0)` means the goal cell or unreachable.
    #[cfg_attr(feature = "logging", instrument(skip(self)))]
    #[must_use]
    pub fn flow_field(&self, goal: GridPos) -> Vec<(i32, i32)> {
        let len = self.width * self.height;
        let mut dist = vec![f32::INFINITY; len];
        let mut directions = vec![(0i32, 0i32); len];

        let goal_idx = match self.index(goal.x, goal.y) {
            Some(idx) => idx,
            None => return directions,
        };

        // Dijkstra from goal
        dist[goal_idx] = 0.0;
        let mut closed = vec![false; len];
        let mut queue = BinaryHeap::new();
        queue.push(AStarNode {
            idx: goal_idx,
            f_score: 0.0,
        });

        let mut neighbors_buf = [(0i32, 0i32, 0.0f32); 8];

        while let Some(current) = queue.pop() {
            if closed[current.idx] {
                continue;
            }
            closed[current.idx] = true;

            let cx = (current.idx % self.width) as i32;
            let cy = (current.idx / self.width) as i32;
            let count = self.neighbors_into(cx, cy, &mut neighbors_buf);

            for &(nx, ny, move_cost) in &neighbors_buf[..count] {
                let n_idx = self.index_unchecked(nx, ny);
                if closed[n_idx] {
                    continue;
                }
                let new_dist = dist[current.idx] + move_cost * self.costs[n_idx];
                if new_dist < dist[n_idx] {
                    dist[n_idx] = new_dist;
                    queue.push(AStarNode {
                        idx: n_idx,
                        f_score: new_dist,
                    });
                }
            }
        }

        // Compute directions: each cell points toward its lowest-cost neighbor
        for y in 0..self.height as i32 {
            for x in 0..self.width as i32 {
                let idx = self.index_unchecked(x, y);
                if idx == goal_idx || dist[idx] == f32::INFINITY {
                    continue;
                }
                let mut best_dir = (0i32, 0i32);
                let mut best_dist = dist[idx];
                let count = self.neighbors_into(x, y, &mut neighbors_buf);
                for &(nx, ny, _) in &neighbors_buf[..count] {
                    let n_idx = self.index_unchecked(nx, ny);
                    if dist[n_idx] < best_dist {
                        best_dist = dist[n_idx];
                        best_dir = (nx - x, ny - y);
                    }
                }
                directions[idx] = best_dir;
            }
        }

        directions
    }

    /// Sum movement costs along a Bresenham line from `from` to `to`.
    /// Used by Theta\* for cost-aware LOS shortcuts.
    fn line_cost(&self, from: GridPos, to: GridPos) -> f32 {
        let mut cost = 0.0f32;
        let mut x = from.x;
        let mut y = from.y;
        let dx = (to.x - from.x).abs();
        let dy = (to.y - from.y).abs();
        let sx = if from.x < to.x { 1 } else { -1 };
        let sy = if from.y < to.y { 1 } else { -1 };
        let mut err = dx - dy;

        loop {
            if x == to.x && y == to.y {
                break;
            }
            let e2 = 2 * err;
            let mut moved_x = false;
            let mut moved_y = false;
            if e2 > -dy {
                err -= dy;
                x += sx;
                moved_x = true;
            }
            if e2 < dx {
                err += dx;
                y += sy;
                moved_y = true;
            }
            let step_dist = if moved_x && moved_y {
                std::f32::consts::SQRT_2
            } else {
                1.0
            };
            let cell_cost = self.index(x, y).map(|idx| self.costs[idx]).unwrap_or(1.0);
            cost += step_dist * cell_cost;
        }

        cost
    }

    #[inline]
    fn index(&self, x: i32, y: i32) -> Option<usize> {
        if x >= 0 && y >= 0 && (x as usize) < self.width && (y as usize) < self.height {
            Some(y as usize * self.width + x as usize)
        } else {
            None
        }
    }

    /// Index for coordinates known to be in-bounds (from `neighbors_into` or
    /// bounded iteration). Debug-asserts validity; returns 0 in release if
    /// the invariant is somehow broken (callers already filter via `closed`).
    #[inline]
    fn index_unchecked(&self, x: i32, y: i32) -> usize {
        debug_assert!(
            x >= 0 && y >= 0 && (x as usize) < self.width && (y as usize) < self.height,
            "index_unchecked called with out-of-bounds ({x}, {y})"
        );
        y as usize * self.width + x as usize
    }

    /// Write walkable neighbors into `buf` and return the count.
    /// Zero-allocation hot-path alternative to returning a Vec.
    #[inline]
    fn neighbors_into(&self, x: i32, y: i32, buf: &mut [(i32, i32, f32); 8]) -> usize {
        const CARDINAL: [(i32, i32); 4] = [(0, 1), (0, -1), (1, 0), (-1, 0)];
        const DIAGONAL: [(i32, i32); 4] = [(1, 1), (1, -1), (-1, 1), (-1, -1)];

        let mut count = 0;
        for (dx, dy) in CARDINAL {
            let nx = x + dx;
            let ny = y + dy;
            if self.is_walkable(nx, ny) {
                buf[count] = (nx, ny, 1.0);
                count += 1;
            }
        }
        if self.allow_diagonal {
            for (dx, dy) in DIAGONAL {
                let nx = x + dx;
                let ny = y + dy;
                // Diagonal requires both adjacent cardinal cells walkable (no corner-cutting)
                if self.is_walkable(nx, ny)
                    && self.is_walkable(x + dx, y)
                    && self.is_walkable(x, y + dy)
                {
                    buf[count] = (nx, ny, std::f32::consts::SQRT_2);
                    count += 1;
                }
            }
        }
        count
    }

    /// Find a path using fringe search.
    ///
    /// An IDA\*-like algorithm that iteratively deepens with a threshold.
    /// Uses a flat list instead of a priority queue, which can be more
    /// cache-friendly than A\* on large grids.
    #[cfg_attr(feature = "logging", instrument(skip(self)))]
    #[must_use]
    pub fn find_path_fringe(&self, start: GridPos, goal: GridPos) -> Option<Vec<GridPos>> {
        if !self.is_walkable(start.x, start.y) || !self.is_walkable(goal.x, goal.y) {
            return None;
        }
        if start == goal {
            return Some(vec![start]);
        }

        let len = self.width * self.height;
        let start_idx = self.index(start.x, start.y)?;
        let goal_idx = self.index(goal.x, goal.y)?;

        let mut g_score = vec![f32::INFINITY; len];
        let mut came_from: Vec<Option<usize>> = vec![None; len];

        g_score[start_idx] = 0.0;

        let h_start = if self.allow_diagonal {
            start.octile_distance(goal)
        } else {
            start.manhattan_distance(goal) as f32
        };

        let mut threshold = h_start;
        let mut fringe = vec![start_idx];
        let mut neighbors_buf = [(0i32, 0i32, 0.0f32); 8];

        loop {
            let mut next_threshold = f32::INFINITY;
            let mut next_fringe: Vec<usize> = Vec::new();
            let current_fringe = std::mem::take(&mut fringe);

            if current_fringe.is_empty() {
                return None;
            }

            for &node_idx in &current_fringe {
                let nx = (node_idx % self.width) as i32;
                let ny = (node_idx / self.width) as i32;
                let node_pos = GridPos::new(nx, ny);
                let h = if self.allow_diagonal {
                    node_pos.octile_distance(goal)
                } else {
                    node_pos.manhattan_distance(goal) as f32
                };
                let f = g_score[node_idx] + h;

                if f > threshold {
                    next_fringe.push(node_idx);
                    if f < next_threshold {
                        next_threshold = f;
                    }
                    continue;
                }

                if node_idx == goal_idx {
                    return Some(self.reconstruct_path(&came_from, goal_idx));
                }

                let count = self.neighbors_into(nx, ny, &mut neighbors_buf);
                for &(cnx, cny, move_cost) in &neighbors_buf[..count] {
                    let n_idx = self.index_unchecked(cnx, cny);
                    let tentative_g = g_score[node_idx] + move_cost * self.costs[n_idx];

                    if tentative_g < g_score[n_idx] {
                        g_score[n_idx] = tentative_g;
                        came_from[n_idx] = Some(node_idx);
                        next_fringe.push(n_idx);
                    }
                }
            }

            fringe = next_fringe;

            if fringe.is_empty() {
                return None;
            }

            threshold = next_threshold;
        }
    }

    /// Find a path using weighted A\* (focal search).
    ///
    /// Uses an inflated heuristic (`weight * h(n)`) to find paths faster
    /// at the cost of bounded suboptimality. A weight of 1.0 gives optimal
    /// A\*; higher weights find paths faster but up to `weight` times longer.
    ///
    /// `weight` is clamped to minimum 1.0.
    #[cfg_attr(feature = "logging", instrument(skip(self)))]
    #[must_use]
    pub fn find_path_weighted(
        &self,
        start: GridPos,
        goal: GridPos,
        weight: f32,
    ) -> Option<Vec<GridPos>> {
        let weight = weight.max(1.0);

        if !self.is_walkable(start.x, start.y) || !self.is_walkable(goal.x, goal.y) {
            return None;
        }
        if start == goal {
            return Some(vec![start]);
        }

        let len = self.width * self.height;
        let mut g_score = vec![f32::INFINITY; len];
        let mut came_from: Vec<Option<usize>> = vec![None; len];
        let mut closed = vec![false; len];
        let mut open = BinaryHeap::new();

        let start_idx = self.index(start.x, start.y)?;
        let goal_idx = self.index(goal.x, goal.y)?;

        g_score[start_idx] = 0.0;
        let h = if self.allow_diagonal {
            start.octile_distance(goal)
        } else {
            start.manhattan_distance(goal) as f32
        };
        open.push(AStarNode {
            idx: start_idx,
            f_score: weight * h,
        });

        let mut neighbors_buf = [(0i32, 0i32, 0.0f32); 8];

        while let Some(current) = open.pop() {
            if current.idx == goal_idx {
                return Some(self.reconstruct_path(&came_from, goal_idx));
            }

            if closed[current.idx] {
                continue;
            }
            closed[current.idx] = true;

            let cx = (current.idx % self.width) as i32;
            let cy = (current.idx / self.width) as i32;
            let count = self.neighbors_into(cx, cy, &mut neighbors_buf);

            for &(nx, ny, move_cost) in &neighbors_buf[..count] {
                let n_idx = self.index_unchecked(nx, ny);
                if closed[n_idx] {
                    continue;
                }
                let tentative_g = g_score[current.idx] + move_cost * self.costs[n_idx];

                if tentative_g < g_score[n_idx] {
                    came_from[n_idx] = Some(current.idx);
                    g_score[n_idx] = tentative_g;
                    let np = GridPos::new(nx, ny);
                    let h = if self.allow_diagonal {
                        np.octile_distance(goal)
                    } else {
                        np.manhattan_distance(goal) as f32
                    };
                    open.push(AStarNode {
                        idx: n_idx,
                        f_score: tentative_g + weight * h,
                    });
                }
            }
        }

        None
    }

    fn reconstruct_path(&self, came_from: &[Option<usize>], goal_idx: usize) -> Vec<GridPos> {
        let mut path = Vec::new();
        let mut current = goal_idx;
        loop {
            let x = (current % self.width) as i32;
            let y = (current / self.width) as i32;
            path.push(GridPos::new(x, y));
            match came_from[current] {
                Some(prev) => current = prev,
                None => break,
            }
        }
        path.reverse();
        path
    }

    /// Compute connected-component IDs for all walkable cells.
    ///
    /// Returns a grid-sized Vec where each cell contains its component ID.
    /// Unwalkable cells get `u32::MAX`. Two cells with the same ID are
    /// mutually reachable. Use to reject unreachable pathfinding queries instantly.
    #[cfg_attr(feature = "logging", instrument(skip(self)))]
    #[must_use]
    pub fn connected_components(&self) -> Vec<u32> {
        use std::collections::VecDeque;

        let len = self.width * self.height;
        let mut component = vec![u32::MAX; len];
        let mut current_id: u32 = 0;
        let mut queue = VecDeque::new();
        let mut neighbors_buf = [(0i32, 0i32, 0.0f32); 8];

        for start in 0..len {
            // Skip unwalkable or already-assigned cells
            let sx = (start % self.width) as i32;
            let sy = (start / self.width) as i32;
            if !self.is_walkable(sx, sy) || component[start] != u32::MAX {
                continue;
            }

            // BFS flood-fill from this cell
            component[start] = current_id;
            queue.push_back(start);

            while let Some(idx) = queue.pop_front() {
                let cx = (idx % self.width) as i32;
                let cy = (idx / self.width) as i32;
                let count = self.neighbors_into(cx, cy, &mut neighbors_buf);

                for &(nx, ny, _) in &neighbors_buf[..count] {
                    let n_idx = self.index_unchecked(nx, ny);
                    if component[n_idx] == u32::MAX && self.is_walkable(nx, ny) {
                        component[n_idx] = current_id;
                        queue.push_back(n_idx);
                    }
                }
            }

            current_id = current_id.saturating_add(1);
        }

        component
    }

    /// Find a path using bidirectional A*.
    ///
    /// Searches from both start and goal simultaneously. Terminates when
    /// the two search frontiers meet, typically expanding fewer nodes
    /// than unidirectional A*.
    #[cfg_attr(feature = "logging", instrument(skip(self)))]
    #[must_use]
    pub fn find_path_bidirectional(&self, start: GridPos, goal: GridPos) -> Option<Vec<GridPos>> {
        if !self.is_walkable(start.x, start.y) || !self.is_walkable(goal.x, goal.y) {
            return None;
        }
        if start == goal {
            return Some(vec![start]);
        }

        let len = self.width * self.height;
        let start_idx = self.index(start.x, start.y)?;
        let goal_idx = self.index(goal.x, goal.y)?;

        let mut g_fwd = vec![f32::INFINITY; len];
        let mut g_bwd = vec![f32::INFINITY; len];
        let mut came_from_fwd: Vec<Option<usize>> = vec![None; len];
        let mut came_from_bwd: Vec<Option<usize>> = vec![None; len];
        let mut closed_fwd = vec![false; len];
        let mut closed_bwd = vec![false; len];
        let mut open_fwd = BinaryHeap::new();
        let mut open_bwd = BinaryHeap::new();

        g_fwd[start_idx] = 0.0;
        g_bwd[goal_idx] = 0.0;

        let h_start = if self.allow_diagonal {
            start.octile_distance(goal)
        } else {
            start.manhattan_distance(goal) as f32
        };
        open_fwd.push(AStarNode {
            idx: start_idx,
            f_score: h_start,
        });
        open_bwd.push(AStarNode {
            idx: goal_idx,
            f_score: h_start,
        });

        let mut mu = f32::INFINITY;
        let mut best_meeting: Option<usize> = None;
        let mut neighbors_buf = [(0i32, 0i32, 0.0f32); 8];

        loop {
            // Check termination: both open sets exhausted or min f-scores sum >= mu
            let min_f_fwd = open_fwd.peek().map(|n| n.f_score);
            let min_f_bwd = open_bwd.peek().map(|n| n.f_score);

            match (min_f_fwd, min_f_bwd) {
                (None, None) => break,
                (Some(f_f), Some(f_b)) => {
                    if f_f + f_b >= mu {
                        break;
                    }
                }
                (Some(f_f), None) => {
                    if f_f >= mu {
                        break;
                    }
                }
                (None, Some(f_b)) => {
                    if f_b >= mu {
                        break;
                    }
                }
            }

            // Expand forward
            if let Some(current) = open_fwd.pop()
                && !closed_fwd[current.idx]
            {
                closed_fwd[current.idx] = true;

                let cx = (current.idx % self.width) as i32;
                let cy = (current.idx / self.width) as i32;
                let count = self.neighbors_into(cx, cy, &mut neighbors_buf);

                for &(nx, ny, move_cost) in &neighbors_buf[..count] {
                    let n_idx = self.index_unchecked(nx, ny);
                    if closed_fwd[n_idx] {
                        continue;
                    }
                    let tentative_g = g_fwd[current.idx] + move_cost * self.costs[n_idx];
                    if tentative_g < g_fwd[n_idx] {
                        came_from_fwd[n_idx] = Some(current.idx);
                        g_fwd[n_idx] = tentative_g;
                        let np = GridPos::new(nx, ny);
                        let h = if self.allow_diagonal {
                            np.octile_distance(goal)
                        } else {
                            np.manhattan_distance(goal) as f32
                        };
                        open_fwd.push(AStarNode {
                            idx: n_idx,
                            f_score: tentative_g + h,
                        });
                    }
                    // Check if backward search has reached this node
                    if closed_bwd[n_idx] {
                        let total = g_fwd[n_idx] + g_bwd[n_idx];
                        if total < mu {
                            mu = total;
                            best_meeting = Some(n_idx);
                        }
                    }
                }
            }

            // Expand backward
            if let Some(current) = open_bwd.pop()
                && !closed_bwd[current.idx]
            {
                closed_bwd[current.idx] = true;

                let cx = (current.idx % self.width) as i32;
                let cy = (current.idx / self.width) as i32;
                let count = self.neighbors_into(cx, cy, &mut neighbors_buf);

                for &(nx, ny, move_cost) in &neighbors_buf[..count] {
                    let n_idx = self.index_unchecked(nx, ny);
                    if closed_bwd[n_idx] {
                        continue;
                    }
                    let tentative_g = g_bwd[current.idx] + move_cost * self.costs[n_idx];
                    if tentative_g < g_bwd[n_idx] {
                        came_from_bwd[n_idx] = Some(current.idx);
                        g_bwd[n_idx] = tentative_g;
                        let np = GridPos::new(nx, ny);
                        let h = if self.allow_diagonal {
                            np.octile_distance(start)
                        } else {
                            np.manhattan_distance(start) as f32
                        };
                        open_bwd.push(AStarNode {
                            idx: n_idx,
                            f_score: tentative_g + h,
                        });
                    }
                    // Check if forward search has reached this node
                    if closed_fwd[n_idx] {
                        let total = g_fwd[n_idx] + g_bwd[n_idx];
                        if total < mu {
                            mu = total;
                            best_meeting = Some(n_idx);
                        }
                    }
                }
            }
        }

        let meeting = best_meeting?;

        // Reconstruct: forward path from start to meeting
        let mut fwd_path = Vec::new();
        let mut current = meeting;
        loop {
            fwd_path.push(current);
            match came_from_fwd[current] {
                Some(prev) => current = prev,
                None => break,
            }
        }
        fwd_path.reverse();

        // Backward path from meeting to goal (follow came_from_bwd)
        current = meeting;
        while let Some(next) = came_from_bwd[current] {
            current = next;
            fwd_path.push(current);
        }

        // Convert indices to GridPos
        let path: Vec<GridPos> = fwd_path
            .iter()
            .map(|&idx| GridPos::new((idx % self.width) as i32, (idx / self.width) as i32))
            .collect();

        Some(path)
    }
}

/// A* open-set node (min-heap by f_score).
#[derive(Clone, Copy)]
struct AStarNode {
    idx: usize,
    f_score: f32,
}

impl PartialEq for AStarNode {
    fn eq(&self, other: &Self) -> bool {
        self.f_score == other.f_score
    }
}

impl Eq for AStarNode {}

impl PartialOrd for AStarNode {
    fn partial_cmp(&self, other: &Self) -> Option<Ordering> {
        Some(self.cmp(other))
    }
}

impl Ord for AStarNode {
    fn cmp(&self, other: &Self) -> Ordering {
        // Reversed for min-heap (BinaryHeap is max-heap)
        other
            .f_score
            .partial_cmp(&self.f_score)
            .unwrap_or(Ordering::Equal)
    }
}

// ---------------------------------------------------------------------------
// Tests
// ---------------------------------------------------------------------------

#[cfg(test)]
mod tests {
    use super::*;

    #[test]
    fn grid_pos_manhattan() {
        let a = GridPos::new(0, 0);
        let b = GridPos::new(3, 4);
        assert_eq!(a.manhattan_distance(b), 7);
    }

    #[test]
    fn grid_pos_octile() {
        let a = GridPos::new(0, 0);
        let b = GridPos::new(3, 3);
        let d = a.octile_distance(b);
        // 3 diagonal moves: 3 * sqrt(2) ≈ 4.24
        assert!((d - 3.0 * std::f32::consts::SQRT_2).abs() < 0.01);
    }

    #[test]
    fn empty_grid_path() {
        let grid = NavGrid::new(5, 5, 1.0);
        let path = grid.find_path(GridPos::new(0, 0), GridPos::new(4, 4));
        assert!(path.is_some());
        let path = path.unwrap();
        assert_eq!(*path.first().unwrap(), GridPos::new(0, 0));
        assert_eq!(*path.last().unwrap(), GridPos::new(4, 4));
    }

    #[test]
    fn same_start_goal() {
        let grid = NavGrid::new(5, 5, 1.0);
        let path = grid.find_path(GridPos::new(2, 2), GridPos::new(2, 2));
        assert_eq!(path, Some(vec![GridPos::new(2, 2)]));
    }

    #[test]
    fn blocked_path() {
        let mut grid = NavGrid::new(5, 1, 1.0);
        grid.set_walkable(2, 0, false);
        let path = grid.find_path(GridPos::new(0, 0), GridPos::new(4, 0));
        assert!(path.is_none());
    }

    #[test]
    fn path_around_wall() {
        let mut grid = NavGrid::new(5, 5, 1.0);
        // Wall across middle
        for x in 0..4 {
            grid.set_walkable(x, 2, false);
        }
        let path = grid.find_path(GridPos::new(0, 0), GridPos::new(0, 4));
        assert!(path.is_some());
        let path = path.unwrap();
        assert_eq!(*path.last().unwrap(), GridPos::new(0, 4));
    }

    #[test]
    fn blocked_start() {
        let mut grid = NavGrid::new(5, 5, 1.0);
        grid.set_walkable(0, 0, false);
        let path = grid.find_path(GridPos::new(0, 0), GridPos::new(4, 4));
        assert!(path.is_none());
    }

    #[test]
    fn blocked_goal() {
        let mut grid = NavGrid::new(5, 5, 1.0);
        grid.set_walkable(4, 4, false);
        let path = grid.find_path(GridPos::new(0, 0), GridPos::new(4, 4));
        assert!(path.is_none());
    }

    #[test]
    fn cardinal_only() {
        let mut grid = NavGrid::new(3, 3, 1.0);
        grid.allow_diagonal = false;
        let path = grid.find_path(GridPos::new(0, 0), GridPos::new(2, 2));
        assert!(path.is_some());
        let path = path.unwrap();
        // All moves should be cardinal (no diagonals)
        for w in path.windows(2) {
            let dx = (w[1].x - w[0].x).abs();
            let dy = (w[1].y - w[0].y).abs();
            assert!(dx + dy == 1, "expected cardinal move, got dx={dx} dy={dy}");
        }
    }

    #[test]
    fn movement_cost() {
        let mut grid = NavGrid::new(5, 1, 1.0);
        grid.allow_diagonal = false;
        grid.set_cost(2, 0, 100.0); // expensive cell
        // Path should still go through it (only option in 1D)
        let path = grid.find_path(GridPos::new(0, 0), GridPos::new(4, 0));
        assert!(path.is_some());
    }

    #[test]
    fn grid_to_world_conversion() {
        let grid = NavGrid::new(10, 10, 2.0);
        let w = grid.grid_to_world(GridPos::new(3, 4));
        assert!((w.x - 7.0).abs() < f32::EPSILON);
        assert!((w.y - 9.0).abs() < f32::EPSILON);
    }

    #[test]
    fn world_to_grid_conversion() {
        let grid = NavGrid::new(10, 10, 2.0);
        let pos = grid.world_to_grid(Vec2::new(7.5, 9.5));
        assert_eq!(pos, GridPos::new(3, 4));
    }

    #[test]
    fn walkable_default() {
        let grid = NavGrid::new(5, 5, 1.0);
        assert!(grid.is_walkable(0, 0));
        assert!(grid.is_walkable(4, 4));
        assert!(!grid.is_walkable(-1, 0)); // out of bounds
        assert!(!grid.is_walkable(5, 0)); // out of bounds
    }

    #[test]
    fn cost_default() {
        let grid = NavGrid::new(5, 5, 1.0);
        assert!((grid.cost(0, 0) - 1.0).abs() < f32::EPSILON);
        assert!(grid.cost(-1, 0).is_infinite()); // out of bounds
    }

    #[test]
    fn flow_field_basic() {
        let grid = NavGrid::new(5, 5, 1.0);
        let field = grid.flow_field(GridPos::new(4, 4));
        // Cell (0,0) should point toward (4,4) — direction should be (1,1) diagonal
        let (dx, dy) = field[0]; // index for (0,0)
        assert!(
            dx > 0 && dy > 0,
            "expected positive direction, got ({dx},{dy})"
        );
        // Goal cell should be (0,0)
        let goal_idx = 4 * 5 + 4; // y=4, x=4
        assert_eq!(field[goal_idx], (0, 0));
    }

    #[test]
    fn flow_field_blocked() {
        let mut grid = NavGrid::new(3, 1, 1.0);
        grid.set_walkable(1, 0, false);
        let field = grid.flow_field(GridPos::new(2, 0));
        // Cell (0,0) is unreachable — direction should be (0,0)
        assert_eq!(field[0], (0, 0));
    }

    #[test]
    fn no_corner_cutting() {
        // Diagonal should not cut through corners
        let mut grid = NavGrid::new(3, 3, 1.0);
        grid.set_walkable(1, 0, false);
        grid.set_walkable(0, 1, false);
        // (0,0) to (1,1) diagonal should be blocked because adjacent cardinals are blocked
        let path = grid.find_path(GridPos::new(0, 0), GridPos::new(1, 1));
        assert!(path.is_none());
    }

    #[test]
    fn large_grid_path() {
        let grid = NavGrid::new(100, 100, 1.0);
        let path = grid.find_path(GridPos::new(0, 0), GridPos::new(99, 99));
        assert!(path.is_some());
    }

    #[test]
    fn grid_dimensions() {
        let grid = NavGrid::new(10, 20, 0.5);
        assert_eq!(grid.width(), 10);
        assert_eq!(grid.height(), 20);
        assert!((grid.cell_size() - 0.5).abs() < f32::EPSILON);
    }

    #[test]
    fn grid_1x1() {
        let grid = NavGrid::new(1, 1, 1.0);
        let path = grid.find_path(GridPos::new(0, 0), GridPos::new(0, 0));
        assert_eq!(path, Some(vec![GridPos::new(0, 0)]));
    }

    #[test]
    fn out_of_bounds_start_goal() {
        let grid = NavGrid::new(5, 5, 1.0);
        assert!(
            grid.find_path(GridPos::new(-1, 0), GridPos::new(4, 4))
                .is_none()
        );
        assert!(
            grid.find_path(GridPos::new(0, 0), GridPos::new(5, 5))
                .is_none()
        );
    }

    #[test]
    fn flow_field_out_of_bounds_goal() {
        let grid = NavGrid::new(5, 5, 1.0);
        let field = grid.flow_field(GridPos::new(-1, -1));
        // All directions should be (0,0) — no valid goal
        assert!(field.iter().all(|&d| d == (0, 0)));
    }

    #[test]
    fn manhattan_distance_symmetry() {
        let a = GridPos::new(2, 3);
        let b = GridPos::new(7, 1);
        assert_eq!(a.manhattan_distance(b), b.manhattan_distance(a));
    }

    #[test]
    fn octile_distance_symmetry() {
        let a = GridPos::new(0, 0);
        let b = GridPos::new(5, 3);
        assert!((a.octile_distance(b) - b.octile_distance(a)).abs() < f32::EPSILON);
    }

    #[test]
    fn set_walkable_out_of_bounds() {
        let mut grid = NavGrid::new(5, 5, 1.0);
        // Should not panic
        grid.set_walkable(-1, 0, false);
        grid.set_walkable(0, 100, false);
    }

    #[test]
    fn set_cost_out_of_bounds() {
        let mut grid = NavGrid::new(5, 5, 1.0);
        // Should not panic
        grid.set_cost(-1, 0, 5.0);
        grid.set_cost(0, 100, 5.0);
    }

    #[test]
    fn gridpos_serde_roundtrip() {
        let pos = GridPos::new(42, -7);
        let json = serde_json::to_string(&pos).unwrap();
        let deserialized: GridPos = serde_json::from_str(&json).unwrap();
        assert_eq!(deserialized, pos);
    }

    #[test]
    fn cost_affects_path_choice() {
        // 3x1 grid with diagonal off: two paths exist in a 3x3 grid
        let mut grid = NavGrid::new(3, 3, 1.0);
        grid.allow_diagonal = false;
        // Make middle row expensive except the center
        grid.set_cost(0, 1, 100.0);
        grid.set_cost(2, 1, 100.0);
        let path = grid.find_path(GridPos::new(0, 0), GridPos::new(2, 2));
        assert!(path.is_some());
        let path = path.unwrap();
        // Path should prefer going through (1,1) which has default cost
        assert!(path.contains(&GridPos::new(1, 1)));
    }

    #[test]
    fn navgrid_serde_roundtrip() {
        let mut grid = NavGrid::new(5, 5, 2.0);
        grid.set_walkable(2, 2, false);
        grid.set_cost(1, 1, 3.0);
        grid.allow_diagonal = false;

        let json = serde_json::to_string(&grid).unwrap();
        let deserialized: NavGrid = serde_json::from_str(&json).unwrap();

        assert_eq!(deserialized.width(), 5);
        assert_eq!(deserialized.height(), 5);
        assert!((deserialized.cell_size() - 2.0).abs() < f32::EPSILON);
        assert!(!deserialized.is_walkable(2, 2));
        assert!((deserialized.cost(1, 1) - 3.0).abs() < f32::EPSILON);
        assert!(!deserialized.allow_diagonal);
    }

    #[test]
    fn grid_overflow_saturates_to_1x1() {
        let grid = NavGrid::new(usize::MAX, 2, 1.0);
        assert_eq!(grid.width(), 1);
        assert_eq!(grid.height(), 1);
    }

    #[test]
    fn try_new_overflow_returns_error() {
        let result = NavGrid::try_new(usize::MAX, 2, 1.0);
        assert!(result.is_err());
    }

    #[test]
    fn try_new_success() {
        let result = NavGrid::try_new(5, 5, 1.0);
        assert!(result.is_ok());
        assert_eq!(result.unwrap().width(), 5);
    }

    // --- JPS tests ---

    #[test]
    fn jps_empty_grid() {
        let grid = NavGrid::new(10, 10, 1.0);
        let path = grid.find_path_jps(GridPos::new(0, 0), GridPos::new(9, 9));
        assert!(path.is_some());
        let path = path.unwrap();
        assert_eq!(*path.first().unwrap(), GridPos::new(0, 0));
        assert_eq!(*path.last().unwrap(), GridPos::new(9, 9));
    }

    #[test]
    fn jps_same_start_goal() {
        let grid = NavGrid::new(5, 5, 1.0);
        let path = grid.find_path_jps(GridPos::new(2, 2), GridPos::new(2, 2));
        assert_eq!(path, Some(vec![GridPos::new(2, 2)]));
    }

    #[test]
    fn jps_blocked_start() {
        let mut grid = NavGrid::new(5, 5, 1.0);
        grid.set_walkable(0, 0, false);
        assert!(
            grid.find_path_jps(GridPos::new(0, 0), GridPos::new(4, 4))
                .is_none()
        );
    }

    #[test]
    fn jps_blocked_goal() {
        let mut grid = NavGrid::new(5, 5, 1.0);
        grid.set_walkable(4, 4, false);
        assert!(
            grid.find_path_jps(GridPos::new(0, 0), GridPos::new(4, 4))
                .is_none()
        );
    }

    #[test]
    fn jps_path_around_wall() {
        let mut grid = NavGrid::new(10, 10, 1.0);
        // Wall across middle
        for x in 0..8 {
            grid.set_walkable(x, 5, false);
        }
        let path = grid.find_path_jps(GridPos::new(0, 0), GridPos::new(0, 9));
        assert!(path.is_some());
        let path = path.unwrap();
        assert_eq!(*path.last().unwrap(), GridPos::new(0, 9));
        // All waypoints should be walkable
        for p in &path {
            assert!(
                grid.is_walkable(p.x, p.y),
                "unwalkable cell in path: ({}, {})",
                p.x,
                p.y
            );
        }
    }

    #[test]
    fn jps_no_path() {
        let mut grid = NavGrid::new(5, 1, 1.0);
        grid.set_walkable(2, 0, false);
        // 1D grid with a wall — no way around
        assert!(
            grid.find_path_jps(GridPos::new(0, 0), GridPos::new(4, 0))
                .is_none()
        );
    }

    #[test]
    fn jps_matches_astar_result() {
        // JPS should find a path of equal or shorter length than A*
        let mut grid = NavGrid::new(20, 20, 1.0);
        for y in 0..15 {
            grid.set_walkable(10, y, false);
        }
        let astar_path = grid
            .find_path(GridPos::new(0, 0), GridPos::new(19, 19))
            .unwrap();
        let jps_path = grid
            .find_path_jps(GridPos::new(0, 0), GridPos::new(19, 19))
            .unwrap();

        // Same endpoints
        assert_eq!(jps_path.first(), astar_path.first());
        assert_eq!(jps_path.last(), astar_path.last());

        // JPS path should be contiguous (each step is adjacent)
        for w in jps_path.windows(2) {
            let dx = (w[1].x - w[0].x).abs();
            let dy = (w[1].y - w[0].y).abs();
            assert!(
                dx <= 1 && dy <= 1 && (dx + dy) > 0,
                "non-adjacent step: ({},{}) -> ({},{})",
                w[0].x,
                w[0].y,
                w[1].x,
                w[1].y
            );
        }
    }

    #[test]
    fn jps_large_grid() {
        let grid = NavGrid::new(100, 100, 1.0);
        let path = grid.find_path_jps(GridPos::new(0, 0), GridPos::new(99, 99));
        assert!(path.is_some());
    }

    #[test]
    fn jps_fallback_cardinal_only() {
        // JPS falls back to A* when diagonal is disabled
        let mut grid = NavGrid::new(5, 5, 1.0);
        grid.allow_diagonal = false;
        let path = grid.find_path_jps(GridPos::new(0, 0), GridPos::new(4, 4));
        assert!(path.is_some());
        // Verify all moves are cardinal
        let path = path.unwrap();
        for w in path.windows(2) {
            let dx = (w[1].x - w[0].x).abs();
            let dy = (w[1].y - w[0].y).abs();
            assert!(dx + dy == 1);
        }
    }

    // --- Dynamic obstacle tests ---

    #[test]
    fn block_rect() {
        let mut grid = NavGrid::new(10, 10, 1.0);
        grid.block_rect(2, 2, 4, 4);
        assert!(!grid.is_walkable(2, 2));
        assert!(!grid.is_walkable(3, 3));
        assert!(!grid.is_walkable(4, 4));
        assert!(grid.is_walkable(1, 1));
        assert!(grid.is_walkable(5, 5));
    }

    #[test]
    fn unblock_rect() {
        let mut grid = NavGrid::new(10, 10, 1.0);
        grid.block_rect(0, 0, 9, 9);
        grid.unblock_rect(3, 3, 6, 6);
        assert!(!grid.is_walkable(0, 0));
        assert!(grid.is_walkable(4, 4));
    }

    #[test]
    fn block_circle() {
        let mut grid = NavGrid::new(10, 10, 1.0);
        grid.block_circle(Vec2::new(5.0, 5.0), 2.0);
        // Center should be blocked
        assert!(!grid.is_walkable(5, 5));
        assert!(!grid.is_walkable(4, 5));
        // Far corner should be fine
        assert!(grid.is_walkable(0, 0));
    }

    #[test]
    fn unblock_circle() {
        let mut grid = NavGrid::new(10, 10, 1.0);
        grid.block_rect(0, 0, 9, 9);
        grid.unblock_circle(Vec2::new(5.0, 5.0), 2.0);
        assert!(grid.is_walkable(5, 5));
        assert!(!grid.is_walkable(0, 0));
    }

    #[test]
    fn set_cost_rect() {
        let mut grid = NavGrid::new(10, 10, 1.0);
        grid.set_cost_rect(2, 2, 4, 4, 5.0);
        assert!((grid.cost(3, 3) - 5.0).abs() < f32::EPSILON);
        assert!((grid.cost(0, 0) - 1.0).abs() < f32::EPSILON);
    }

    #[test]
    fn clear_grid() {
        let mut grid = NavGrid::new(10, 10, 1.0);
        grid.block_rect(0, 0, 9, 9);
        grid.set_cost_rect(0, 0, 9, 9, 99.0);
        grid.clear();
        assert!(grid.is_walkable(5, 5));
        assert!((grid.cost(5, 5) - 1.0).abs() < f32::EPSILON);
    }

    #[test]
    fn dynamic_obstacle_repath() {
        let mut grid = NavGrid::new(10, 10, 1.0);
        // Path exists
        assert!(
            grid.find_path(GridPos::new(0, 0), GridPos::new(9, 9))
                .is_some()
        );
        // Block the way
        grid.block_rect(0, 5, 9, 5);
        // No path through solid wall
        assert!(
            grid.find_path(GridPos::new(0, 0), GridPos::new(0, 9))
                .is_none()
        );
        // Open a gap
        grid.set_walkable(5, 5, true);
        // Path exists again
        assert!(
            grid.find_path(GridPos::new(0, 0), GridPos::new(0, 9))
                .is_some()
        );
    }

    #[test]
    fn block_rect_out_of_bounds() {
        let mut grid = NavGrid::new(5, 5, 1.0);
        // Partially out of bounds — should not panic
        grid.block_rect(-2, -2, 1, 1);
        assert!(!grid.is_walkable(0, 0));
        assert!(!grid.is_walkable(1, 1));
    }

    #[test]
    fn jps_contiguous_path_with_obstacles() {
        let mut grid = NavGrid::new(15, 15, 1.0);
        // Create an L-shaped wall
        for y in 0..10 {
            grid.set_walkable(7, y, false);
        }
        for x in 7..15 {
            grid.set_walkable(x, 10, false);
        }
        let path = grid.find_path_jps(GridPos::new(0, 0), GridPos::new(14, 14));
        assert!(path.is_some());
        let path = path.unwrap();
        for p in &path {
            assert!(grid.is_walkable(p.x, p.y));
        }
        for w in path.windows(2) {
            let dx = (w[1].x - w[0].x).abs();
            let dy = (w[1].y - w[0].y).abs();
            assert!(dx <= 1 && dy <= 1 && (dx + dy) > 0);
        }
    }

    // --- Nav query tests ---

    #[test]
    fn nearest_walkable_already_walkable() {
        let grid = NavGrid::new(10, 10, 1.0);
        assert_eq!(
            grid.nearest_walkable(GridPos::new(5, 5)),
            Some(GridPos::new(5, 5))
        );
    }

    #[test]
    fn nearest_walkable_blocked() {
        let mut grid = NavGrid::new(10, 10, 1.0);
        grid.set_walkable(5, 5, false);
        let nearest = grid.nearest_walkable(GridPos::new(5, 5));
        assert!(nearest.is_some());
        let n = nearest.unwrap();
        // Should be adjacent
        let dist = (n.x - 5).abs() + (n.y - 5).abs();
        assert!(dist <= 2);
    }

    #[test]
    fn nearest_walkable_all_blocked() {
        let mut grid = NavGrid::new(3, 3, 1.0);
        grid.block_rect(0, 0, 2, 2);
        assert!(grid.nearest_walkable(GridPos::new(1, 1)).is_none());
    }

    // --- Theta* / LOS tests ---

    #[test]
    fn los_open_grid() {
        let grid = NavGrid::new(10, 10, 1.0);
        assert!(grid.has_line_of_sight(GridPos::new(0, 0), GridPos::new(9, 9)));
    }

    #[test]
    fn los_blocked_by_wall() {
        let mut grid = NavGrid::new(10, 10, 1.0);
        grid.set_walkable(5, 5, false);
        assert!(!grid.has_line_of_sight(GridPos::new(0, 0), GridPos::new(9, 9)));
    }

    #[test]
    fn los_same_cell() {
        let grid = NavGrid::new(5, 5, 1.0);
        assert!(grid.has_line_of_sight(GridPos::new(2, 2), GridPos::new(2, 2)));
    }

    #[test]
    fn los_horizontal() {
        let grid = NavGrid::new(10, 10, 1.0);
        assert!(grid.has_line_of_sight(GridPos::new(0, 5), GridPos::new(9, 5)));
    }

    #[test]
    fn theta_open_grid() {
        let grid = NavGrid::new(10, 10, 1.0);
        let path = grid.find_path_theta(GridPos::new(0, 0), GridPos::new(9, 9));
        assert!(path.is_some());
        let path = path.unwrap();
        assert_eq!(*path.first().unwrap(), GridPos::new(0, 0));
        assert_eq!(*path.last().unwrap(), GridPos::new(9, 9));
    }

    #[test]
    fn theta_same_start_goal() {
        let grid = NavGrid::new(5, 5, 1.0);
        let path = grid.find_path_theta(GridPos::new(2, 2), GridPos::new(2, 2));
        assert_eq!(path, Some(vec![GridPos::new(2, 2)]));
    }

    #[test]
    fn theta_blocked() {
        let mut grid = NavGrid::new(5, 1, 1.0);
        grid.set_walkable(2, 0, false);
        assert!(
            grid.find_path_theta(GridPos::new(0, 0), GridPos::new(4, 0))
                .is_none()
        );
    }

    #[test]
    fn theta_fewer_waypoints_than_astar() {
        // On an open grid, Theta* should produce fewer waypoints
        // because it can skip intermediate nodes via LOS
        let grid = NavGrid::new(20, 20, 1.0);
        let astar = grid
            .find_path(GridPos::new(0, 0), GridPos::new(19, 19))
            .unwrap();
        let theta = grid
            .find_path_theta(GridPos::new(0, 0), GridPos::new(19, 19))
            .unwrap();
        // Theta* path should be equal or shorter in waypoint count
        assert!(theta.len() <= astar.len());
    }

    #[test]
    fn theta_with_wall() {
        let mut grid = NavGrid::new(20, 20, 1.0);
        for y in 0..18 {
            grid.set_walkable(10, y, false);
        }
        let path = grid.find_path_theta(GridPos::new(0, 0), GridPos::new(19, 19));
        assert!(path.is_some());
        let path = path.unwrap();
        for p in &path {
            assert!(grid.is_walkable(p.x, p.y));
        }
    }

    #[test]
    fn theta_respects_movement_costs() {
        // 10x3 grid with expensive vertical stripe at x=5.
        // If Theta* respects costs, it should avoid cells in column 5.
        let mut grid = NavGrid::new(10, 3, 1.0);
        for y in 0..3 {
            grid.set_cost(5, y, 100.0);
        }
        let path = grid.find_path_theta(GridPos::new(0, 1), GridPos::new(9, 1));
        assert!(path.is_some());
        let path = path.unwrap();
        assert_eq!(*path.first().unwrap(), GridPos::new(0, 1));
        assert_eq!(*path.last().unwrap(), GridPos::new(9, 1));
        // All waypoints should be walkable
        for p in &path {
            assert!(grid.is_walkable(p.x, p.y));
        }
        // The path should avoid the expensive column 5 at y=1
        // (may pass through (5,0) or (5,2) which are cheaper routes)
        let passes_through_5_1 = path.contains(&GridPos::new(5, 1));
        // With a cost of 100x, the path should detour around column 5 via row 0 or 2
        assert!(
            !passes_through_5_1,
            "Theta* should avoid expensive cell (5,1)"
        );
    }

    #[test]
    fn flow_field_with_costs() {
        let mut grid = NavGrid::new(5, 1, 1.0);
        grid.allow_diagonal = false;
        // Make cell (2,0) expensive
        grid.set_cost(2, 0, 10.0);
        let field = grid.flow_field(GridPos::new(4, 0));
        // Cell (1,0) should still point toward goal (positive dx)
        let idx = 1; // (1, 0)
        assert_eq!(field[idx].0, 1);
    }

    #[test]
    fn jps_large_grid_with_maze() {
        // Stress test: 200x200 grid with maze-like walls
        let mut grid = NavGrid::new(200, 200, 1.0);
        for y in (0..200).step_by(4) {
            for x in 0..198 {
                grid.set_walkable(x, y, false);
            }
        }
        for y in (2..200).step_by(4) {
            for x in 2..200 {
                grid.set_walkable(x, y, false);
            }
        }
        let path = grid.find_path_jps(GridPos::new(0, 1), GridPos::new(199, 199));
        // Either finds a path or correctly returns None — just shouldn't stack overflow
        if let Some(path) = &path {
            for p in path {
                assert!(grid.is_walkable(p.x, p.y));
            }
            for w in path.windows(2) {
                let dx = (w[1].x - w[0].x).abs();
                let dy = (w[1].y - w[0].y).abs();
                assert!(dx <= 1 && dy <= 1 && (dx + dy) > 0);
            }
        }
    }

    #[test]
    fn partial_path_blocked_goal() {
        let mut grid = NavGrid::new(10, 10, 1.0);
        grid.set_walkable(9, 9, false);
        let path = grid.find_path_partial(GridPos::new(0, 0), GridPos::new(9, 9));
        assert!(path.is_some());
        let path = path.unwrap();
        // Should reach a cell near (9,9)
        let last = path.last().unwrap();
        let dist = last.octile_distance(GridPos::new(9, 9));
        assert!(dist < 3.0);
    }

    #[test]
    fn partial_path_unreachable_island() {
        let mut grid = NavGrid::new(10, 10, 1.0);
        // Wall off the right side completely
        for y in 0..10 {
            grid.set_walkable(5, y, false);
        }
        let path = grid.find_path_partial(GridPos::new(0, 0), GridPos::new(9, 9));
        assert!(path.is_some());
        let path = path.unwrap();
        let last = path.last().unwrap();
        // Should end near x=4 (closest reachable to goal)
        assert!(last.x <= 4);
    }

    #[test]
    fn partial_path_reachable_returns_full() {
        let grid = NavGrid::new(10, 10, 1.0);
        let partial = grid.find_path_partial(GridPos::new(0, 0), GridPos::new(9, 9));
        let full = grid.find_path(GridPos::new(0, 0), GridPos::new(9, 9));
        assert_eq!(partial, full);
    }

    #[test]
    fn partial_path_unwalkable_start() {
        let mut grid = NavGrid::new(10, 10, 1.0);
        grid.set_walkable(0, 0, false);
        assert!(
            grid.find_path_partial(GridPos::new(0, 0), GridPos::new(9, 9))
                .is_none()
        );
    }

    // --- Lazy Theta* tests ---

    #[test]
    fn lazy_theta_basic() {
        let grid = NavGrid::new(10, 10, 1.0);
        let path = grid.find_path_lazy_theta(GridPos::new(0, 0), GridPos::new(9, 9));
        assert!(path.is_some());
        let path = path.unwrap();
        assert_eq!(*path.first().unwrap(), GridPos::new(0, 0));
        assert_eq!(*path.last().unwrap(), GridPos::new(9, 9));
    }

    #[test]
    fn lazy_theta_same_start_goal() {
        let grid = NavGrid::new(10, 10, 1.0);
        let path = grid.find_path_lazy_theta(GridPos::new(5, 5), GridPos::new(5, 5));
        assert_eq!(path, Some(vec![GridPos::new(5, 5)]));
    }

    #[test]
    fn lazy_theta_unwalkable() {
        let mut grid = NavGrid::new(10, 10, 1.0);
        grid.set_walkable(0, 0, false);
        assert!(
            grid.find_path_lazy_theta(GridPos::new(0, 0), GridPos::new(9, 9))
                .is_none()
        );
    }

    #[test]
    fn lazy_theta_blocked() {
        let mut grid = NavGrid::new(10, 1, 1.0);
        grid.set_walkable(5, 0, false);
        assert!(
            grid.find_path_lazy_theta(GridPos::new(0, 0), GridPos::new(9, 0))
                .is_none()
        );
    }

    #[test]
    fn lazy_theta_around_obstacle() {
        let mut grid = NavGrid::new(10, 10, 1.0);
        for y in 0..8 {
            grid.set_walkable(5, y, false);
        }
        let path = grid.find_path_lazy_theta(GridPos::new(0, 0), GridPos::new(9, 0));
        assert!(path.is_some());
        let path = path.unwrap();
        assert_eq!(*path.last().unwrap(), GridPos::new(9, 0));
    }

    #[test]
    fn lazy_theta_fewer_waypoints_than_astar() {
        // On an open grid, lazy theta* should produce fewer waypoints
        // (more direct) than regular A*
        let grid = NavGrid::new(20, 20, 1.0);
        let astar = grid
            .find_path(GridPos::new(0, 0), GridPos::new(19, 0))
            .unwrap();
        let theta = grid
            .find_path_lazy_theta(GridPos::new(0, 0), GridPos::new(19, 0))
            .unwrap();
        // Theta* should have same or fewer waypoints
        assert!(theta.len() <= astar.len());
    }

    // --- Fringe search tests ---

    #[test]
    fn fringe_empty_grid() {
        let grid = NavGrid::new(10, 10, 1.0);
        let path = grid.find_path_fringe(GridPos::new(0, 0), GridPos::new(9, 9));
        assert!(path.is_some());
        let path = path.unwrap();
        assert_eq!(*path.first().unwrap(), GridPos::new(0, 0));
        assert_eq!(*path.last().unwrap(), GridPos::new(9, 9));
    }

    #[test]
    fn fringe_same_start_goal() {
        let grid = NavGrid::new(5, 5, 1.0);
        let path = grid.find_path_fringe(GridPos::new(2, 2), GridPos::new(2, 2));
        assert_eq!(path, Some(vec![GridPos::new(2, 2)]));
    }

    #[test]
    fn fringe_blocked_start() {
        let mut grid = NavGrid::new(5, 5, 1.0);
        grid.set_walkable(0, 0, false);
        assert!(
            grid.find_path_fringe(GridPos::new(0, 0), GridPos::new(4, 4))
                .is_none()
        );
    }

    #[test]
    fn fringe_blocked_goal() {
        let mut grid = NavGrid::new(5, 5, 1.0);
        grid.set_walkable(4, 4, false);
        assert!(
            grid.find_path_fringe(GridPos::new(0, 0), GridPos::new(4, 4))
                .is_none()
        );
    }

    #[test]
    fn fringe_blocked_no_path() {
        let mut grid = NavGrid::new(5, 1, 1.0);
        grid.set_walkable(2, 0, false);
        assert!(
            grid.find_path_fringe(GridPos::new(0, 0), GridPos::new(4, 0))
                .is_none()
        );
    }

    #[test]
    fn fringe_around_obstacle() {
        let mut grid = NavGrid::new(10, 10, 1.0);
        for y in 0..8 {
            grid.set_walkable(5, y, false);
        }
        let path = grid.find_path_fringe(GridPos::new(0, 0), GridPos::new(9, 0));
        assert!(path.is_some());
        let path = path.unwrap();
        assert_eq!(*path.last().unwrap(), GridPos::new(9, 0));
        for p in &path {
            assert!(
                grid.is_walkable(p.x, p.y),
                "unwalkable cell in fringe path: ({}, {})",
                p.x,
                p.y
            );
        }
    }

    #[test]
    fn fringe_matches_astar_endpoints() {
        let mut grid = NavGrid::new(20, 20, 1.0);
        for y in 0..15 {
            grid.set_walkable(10, y, false);
        }
        let astar_path = grid
            .find_path(GridPos::new(0, 0), GridPos::new(19, 19))
            .unwrap();
        let fringe_path = grid
            .find_path_fringe(GridPos::new(0, 0), GridPos::new(19, 19))
            .unwrap();

        assert_eq!(fringe_path.first(), astar_path.first());
        assert_eq!(fringe_path.last(), astar_path.last());

        // Fringe path should be contiguous
        for w in fringe_path.windows(2) {
            let dx = (w[1].x - w[0].x).abs();
            let dy = (w[1].y - w[0].y).abs();
            assert!(
                dx <= 1 && dy <= 1 && (dx + dy) > 0,
                "non-adjacent step in fringe path: ({},{}) -> ({},{})",
                w[0].x,
                w[0].y,
                w[1].x,
                w[1].y
            );
        }
    }

    // --- Weighted A* tests ---

    #[test]
    fn weighted_empty_grid() {
        let grid = NavGrid::new(10, 10, 1.0);
        let path = grid.find_path_weighted(GridPos::new(0, 0), GridPos::new(9, 9), 2.0);
        assert!(path.is_some());
        let path = path.unwrap();
        assert_eq!(*path.first().unwrap(), GridPos::new(0, 0));
        assert_eq!(*path.last().unwrap(), GridPos::new(9, 9));
    }

    #[test]
    fn weighted_same_start_goal() {
        let grid = NavGrid::new(5, 5, 1.0);
        let path = grid.find_path_weighted(GridPos::new(2, 2), GridPos::new(2, 2), 1.5);
        assert_eq!(path, Some(vec![GridPos::new(2, 2)]));
    }

    #[test]
    fn weighted_blocked_start() {
        let mut grid = NavGrid::new(5, 5, 1.0);
        grid.set_walkable(0, 0, false);
        assert!(
            grid.find_path_weighted(GridPos::new(0, 0), GridPos::new(4, 4), 1.0)
                .is_none()
        );
    }

    #[test]
    fn weighted_blocked_goal() {
        let mut grid = NavGrid::new(5, 5, 1.0);
        grid.set_walkable(4, 4, false);
        assert!(
            grid.find_path_weighted(GridPos::new(0, 0), GridPos::new(4, 4), 1.0)
                .is_none()
        );
    }

    #[test]
    fn weighted_blocked_no_path() {
        let mut grid = NavGrid::new(5, 1, 1.0);
        grid.set_walkable(2, 0, false);
        assert!(
            grid.find_path_weighted(GridPos::new(0, 0), GridPos::new(4, 0), 3.0)
                .is_none()
        );
    }

    #[test]
    fn weighted_1_0_matches_astar() {
        // Weight 1.0 should produce the same optimal path as A*
        let mut grid = NavGrid::new(20, 20, 1.0);
        for y in 0..15 {
            grid.set_walkable(10, y, false);
        }
        let astar_path = grid
            .find_path(GridPos::new(0, 0), GridPos::new(19, 19))
            .unwrap();
        let weighted_path = grid
            .find_path_weighted(GridPos::new(0, 0), GridPos::new(19, 19), 1.0)
            .unwrap();

        assert_eq!(astar_path, weighted_path);
    }

    #[test]
    fn weighted_higher_weight_finds_path() {
        // Higher weight should still find a valid path (possibly suboptimal)
        let mut grid = NavGrid::new(20, 20, 1.0);
        for y in 0..15 {
            grid.set_walkable(10, y, false);
        }
        let path = grid
            .find_path_weighted(GridPos::new(0, 0), GridPos::new(19, 19), 5.0)
            .unwrap();

        assert_eq!(*path.first().unwrap(), GridPos::new(0, 0));
        assert_eq!(*path.last().unwrap(), GridPos::new(19, 19));

        // Path should be contiguous
        for w in path.windows(2) {
            let dx = (w[1].x - w[0].x).abs();
            let dy = (w[1].y - w[0].y).abs();
            assert!(
                dx <= 1 && dy <= 1 && (dx + dy) > 0,
                "non-adjacent step in weighted path: ({},{}) -> ({},{})",
                w[0].x,
                w[0].y,
                w[1].x,
                w[1].y
            );
        }
    }

    #[test]
    fn weighted_clamps_below_1() {
        // Weight below 1.0 should be clamped to 1.0 (same as A*)
        let grid = NavGrid::new(10, 10, 1.0);
        let path_low = grid
            .find_path_weighted(GridPos::new(0, 0), GridPos::new(9, 9), 0.5)
            .unwrap();
        let path_one = grid
            .find_path_weighted(GridPos::new(0, 0), GridPos::new(9, 9), 1.0)
            .unwrap();
        assert_eq!(path_low, path_one);
    }

    // --- Connected components tests ---

    #[test]
    fn connected_components_all_walkable() {
        let grid = NavGrid::new(5, 5, 1.0);
        let comps = grid.connected_components();
        // All walkable cells should have the same component ID
        let first_id = comps[0];
        assert_ne!(first_id, u32::MAX);
        for &c in &comps {
            assert_eq!(c, first_id);
        }
    }

    #[test]
    fn connected_components_two_islands() {
        let mut grid = NavGrid::new(5, 5, 1.0);
        // Wall across the middle row
        for x in 0..5 {
            grid.set_walkable(x, 2, false);
        }
        let comps = grid.connected_components();
        // Check unwalkable cells
        for x in 0..5 {
            let idx = 2 * 5 + x as usize;
            assert_eq!(comps[idx], u32::MAX);
        }
        // Top and bottom should be different components
        let top_id = comps[0]; // (0,0)
        let bottom_id = comps[3 * 5]; // (0,3)
        assert_ne!(top_id, u32::MAX);
        assert_ne!(bottom_id, u32::MAX);
        assert_ne!(top_id, bottom_id);
    }

    #[test]
    fn connected_components_unwalkable_cells_get_max() {
        let mut grid = NavGrid::new(3, 3, 1.0);
        grid.set_walkable(1, 1, false);
        let comps = grid.connected_components();
        let center_idx = 4; // (1,1) in a 3-wide grid
        assert_eq!(comps[center_idx], u32::MAX);
    }

    #[test]
    fn connected_components_empty_grid() {
        let mut grid = NavGrid::new(3, 3, 1.0);
        // Block all cells
        for y in 0..3 {
            for x in 0..3 {
                grid.set_walkable(x, y, false);
            }
        }
        let comps = grid.connected_components();
        for &c in &comps {
            assert_eq!(c, u32::MAX);
        }
    }

    // --- Bidirectional A* tests ---

    #[test]
    fn bidir_basic_path() {
        let grid = NavGrid::new(10, 10, 1.0);
        let path = grid.find_path_bidirectional(GridPos::new(0, 0), GridPos::new(9, 9));
        assert!(path.is_some());
        let path = path.unwrap();
        assert_eq!(*path.first().unwrap(), GridPos::new(0, 0));
        assert_eq!(*path.last().unwrap(), GridPos::new(9, 9));
    }

    #[test]
    fn bidir_same_start_goal() {
        let grid = NavGrid::new(5, 5, 1.0);
        let path = grid.find_path_bidirectional(GridPos::new(2, 2), GridPos::new(2, 2));
        assert_eq!(path, Some(vec![GridPos::new(2, 2)]));
    }

    #[test]
    fn bidir_blocked() {
        let mut grid = NavGrid::new(5, 1, 1.0);
        grid.set_walkable(2, 0, false);
        let path = grid.find_path_bidirectional(GridPos::new(0, 0), GridPos::new(4, 0));
        assert!(path.is_none());
    }

    #[test]
    fn bidir_around_obstacle() {
        let mut grid = NavGrid::new(10, 10, 1.0);
        for y in 0..8 {
            grid.set_walkable(5, y, false);
        }
        let path = grid.find_path_bidirectional(GridPos::new(0, 0), GridPos::new(9, 0));
        assert!(path.is_some());
        let path = path.unwrap();
        assert_eq!(*path.first().unwrap(), GridPos::new(0, 0));
        assert_eq!(*path.last().unwrap(), GridPos::new(9, 0));
        // All cells in path should be walkable
        for p in &path {
            assert!(grid.is_walkable(p.x, p.y));
        }
    }

    #[test]
    fn bidir_matches_regular_astar() {
        let mut grid = NavGrid::new(20, 20, 1.0);
        for y in 0..15 {
            grid.set_walkable(10, y, false);
        }
        let astar_path = grid
            .find_path(GridPos::new(0, 0), GridPos::new(19, 19))
            .unwrap();
        let bidir_path = grid
            .find_path_bidirectional(GridPos::new(0, 0), GridPos::new(19, 19))
            .unwrap();

        // Same endpoints
        assert_eq!(bidir_path.first(), astar_path.first());
        assert_eq!(bidir_path.last(), astar_path.last());

        // Bidirectional path should be contiguous
        for w in bidir_path.windows(2) {
            let dx = (w[1].x - w[0].x).abs();
            let dy = (w[1].y - w[0].y).abs();
            assert!(
                dx <= 1 && dy <= 1 && (dx + dy) > 0,
                "non-adjacent step: ({},{}) -> ({},{})",
                w[0].x,
                w[0].y,
                w[1].x,
                w[1].y
            );
        }

        // Path lengths should be equal (both optimal)
        assert_eq!(astar_path.len(), bidir_path.len());
    }

    #[test]
    fn bidir_blocked_start() {
        let mut grid = NavGrid::new(5, 5, 1.0);
        grid.set_walkable(0, 0, false);
        assert!(
            grid.find_path_bidirectional(GridPos::new(0, 0), GridPos::new(4, 4))
                .is_none()
        );
    }

    #[test]
    fn bidir_blocked_goal() {
        let mut grid = NavGrid::new(5, 5, 1.0);
        grid.set_walkable(4, 4, false);
        assert!(
            grid.find_path_bidirectional(GridPos::new(0, 0), GridPos::new(4, 4))
                .is_none()
        );
    }

    #[test]
    fn cell_size_zero_returns_error() {
        let result = NavGrid::try_new(10, 10, 0.0);
        assert!(matches!(result, Err(NavError::InvalidCellSize { .. })));
    }

    #[test]
    fn new_overflow_saturates() {
        let grid = NavGrid::new(usize::MAX, usize::MAX, 1.0);
        assert_eq!(grid.width(), 1);
        assert_eq!(grid.height(), 1);
    }

    #[test]
    fn try_new_cell_size_negative() {
        let result = NavGrid::try_new(10, 10, -1.0);
        assert!(result.is_err());
    }
}