flatland_client_lib/

navigation.rs

//! Client-side pathfinding and auto-navigation toward a map target.

use std::cmp::Ordering;
use std::collections::{BinaryHeap, HashMap};

use flatland_protocol::{
    BuildingView, DoorView, LifeState, ResourceNodeState, TerrainKindView, TerrainZoneView,
};

use crate::game::GameState;

#[derive(Debug, Clone, Copy)]
struct BlockingCircle {
    x: f32,
    y: f32,
    radius_m: f32,
}

#[derive(Debug, Clone)]
struct NavObstacles {
    circles: Vec<BlockingCircle>,
    buildings: Vec<BuildingView>,
}

impl NavObstacles {
    fn from_state(state: &GameState) -> Self {
        let mut circles = Vec::new();
        for node in &state.resource_nodes {
            if node.blocking
                && matches!(
                    node.state,
                    ResourceNodeState::Available | ResourceNodeState::Harvesting
                )
            {
                let radius = if node.blocking_radius_m > 0.0 {
                    node.blocking_radius_m
                } else {
                    0.8
                };
                circles.push(BlockingCircle {
                    x: node.x,
                    y: node.y,
                    radius_m: radius,
                });
            }
        }
        for npc in &state.npcs {
            let alive = npc.life_state != Some(LifeState::Dead)
                && npc.hp_pct.is_none_or(|h| h > 0.0);
            if alive {
                circles.push(BlockingCircle {
                    x: npc.x,
                    y: npc.y,
                    radius_m: 0.55,
                });
            }
        }
        Self {
            circles,
            buildings: state.buildings.clone(),
        }
    }
}

fn collides_player_at(x: f32, y: f32, obstacles: &NavObstacles) -> bool {
    if obstacles.circles.iter().any(|o| circle_overlap(x, y, PLAYER_RADIUS_M, o.x, o.y, o.radius_m)) {
        return true;
    }
    let pad = PLAYER_RADIUS_M;
    obstacles.buildings.iter().any(|b| {
        let hw = b.width_m / 2.0 + pad;
        let hd = b.depth_m / 2.0 + pad;
        x >= b.x - hw && x <= b.x + hw && y >= b.y - hd && y <= b.y + hd
    })
}

fn circle_overlap(ax: f32, ay: f32, ar: f32, bx: f32, by: f32, br: f32) -> bool {
    let dx = ax - bx;
    let dy = ay - by;
    let min_dist = ar + br;
    dx * dx + dy * dy < min_dist * min_dist
}

fn segment_clear_world(
    grid: &NavGrid,
    obstacles: &NavObstacles,
    from: (f32, f32),
    to: (f32, f32),
) -> bool {
    let (fx, fy) = from;
    let (tx, ty) = to;
    let dist = (tx - fx).hypot(ty - fy);
    let steps = (dist / SEGMENT_SAMPLE_M).ceil() as u32 + 1;
    for step in 0..=steps {
        let t = step as f32 / steps as f32;
        let x = fx + (tx - fx) * t;
        let y = fy + (ty - fy) * t;
        if collides_player_at(x, y, obstacles) {
            return false;
        }
    }
    let (cx0, cy0) = world_to_cell(fx, fy);
    let (cx1, cy1) = world_to_cell(tx, ty);
    line_clear(grid, (cx0, cy0), (cx1, cy1))
}

const PLAYER_RADIUS_M: f32 = 0.45;
/// Extra clearance so paths don't hug walls (server collision uses player radius).
const PATH_CLEARANCE_M: f32 = 0.35;
const WAYPOINT_RADIUS_M: f32 = 0.85;
const ARRIVE_RADIUS_M: f32 = 0.75;
const SPRINT_LEG_M: f32 = 4.0;
/// Sample spacing when testing straight segments against circle obstacles.
const SEGMENT_SAMPLE_M: f32 = 0.3;
/// Intent ticks with no progress before attempting a replan.
const STUCK_TICKS_BEFORE_REPLAN: u32 = 6;
/// Consecutive failed/useless replans before giving up.
const MAX_REPLAN_ATTEMPTS: u32 = 5;

#[derive(Debug, Clone)]
pub struct AutoNavigator {
    waypoints: Vec<(f32, f32)>,
    waypoint_index: usize,
    pub goal_x: f32,
    pub goal_y: f32,
    last_progress_x: f32,
    last_progress_y: f32,
    stuck_ticks: u32,
    replan_attempts: u32,
}

impl AutoNavigator {
    pub fn plan(state: &GameState, goal_x: f32, goal_y: f32) -> Option<Self> {
        let (px, py) = state.player_position();
        let path = find_path(state, px, py, goal_x, goal_y)?;
        if path.is_empty() {
            return None;
        }
        Some(Self {
            waypoints: path,
            waypoint_index: 0,
            goal_x,
            goal_y,
            last_progress_x: px,
            last_progress_y: py,
            stuck_ticks: 0,
            replan_attempts: 0,
        })
    }

    pub fn active(&self) -> bool {
        self.waypoint_index < self.waypoints.len()
    }

    /// Recompute path from current position (e.g. after getting stuck on an obstacle).
    /// Returns `false` only after several failed attempts — keeps trying around buildings.
    pub fn replan(&mut self, state: &GameState) -> bool {
        let (px, py) = state.player_position();
        if let Some(path) = find_path(state, px, py, self.goal_x, self.goal_y) {
            if !path.is_empty() {
                let changed = path != self.waypoints;
                self.waypoints = path;
                self.waypoint_index = 0;
                self.stuck_ticks = 0;
                self.last_progress_x = px;
                self.last_progress_y = py;
                if changed {
                    self.replan_attempts = 0;
                } else {
                    self.replan_attempts = self.replan_attempts.saturating_add(1);
                }
                return self.replan_attempts < MAX_REPLAN_ATTEMPTS;
            }
        }
        self.replan_attempts = self.replan_attempts.saturating_add(1);
        self.replan_attempts < MAX_REPLAN_ATTEMPTS
    }

    /// Returns true when movement has not progressed and a replan is advised.
    pub fn note_progress(&mut self, px: f32, py: f32) -> bool {
        if (px - self.last_progress_x).hypot(py - self.last_progress_y) > 0.25 {
            self.last_progress_x = px;
            self.last_progress_y = py;
            self.stuck_ticks = 0;
            self.replan_attempts = 0;
            return false;
        }
        self.stuck_ticks = self.stuck_ticks.saturating_add(1);
        self.stuck_ticks >= STUCK_TICKS_BEFORE_REPLAN
    }

    /// Movement intent toward the next waypoint. `None` when finished.
    pub fn steer(&mut self, px: f32, py: f32) -> Option<(f32, f32, bool)> {
        while self.waypoint_index < self.waypoints.len() {
            let (wx, wy) = self.waypoints[self.waypoint_index];
            let dx = wx - px;
            let dy = wy - py;
            let dist = (dx * dx + dy * dy).sqrt();
            if dist < WAYPOINT_RADIUS_M {
                self.waypoint_index += 1;
                continue;
            }
            let forward = (dy / dist).clamp(-1.0, 1.0);
            let strafe = (dx / dist).clamp(-1.0, 1.0);
            let sprint = dist > SPRINT_LEG_M;
            return Some((forward, strafe, sprint));
        }
        let goal_dist = (self.goal_x - px).hypot(self.goal_y - py);
        if goal_dist > ARRIVE_RADIUS_M {
            let forward = ((self.goal_y - py) / goal_dist).clamp(-1.0, 1.0);
            let strafe = ((self.goal_x - px) / goal_dist).clamp(-1.0, 1.0);
            return Some((forward, strafe, goal_dist > SPRINT_LEG_M));
        }
        None
    }
}

#[derive(Clone, Copy, Eq, PartialEq)]
struct OpenNode {
    f: u32,
    g: u32,
    x: i16,
    y: i16,
}

impl Ord for OpenNode {
    fn cmp(&self, other: &Self) -> Ordering {
        other.f.cmp(&self.f).then_with(|| other.g.cmp(&self.g))
    }
}

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

struct NavGrid {
    width: i16,
    height: i16,
    blocked: Vec<bool>,
    cost: Vec<u16>,
}

impl NavGrid {
    fn idx(&self, x: i16, y: i16) -> usize {
        (y as usize) * (self.width as usize) + (x as usize)
    }

    fn in_bounds(&self, x: i16, y: i16) -> bool {
        x >= 0 && y >= 0 && x < self.width && y < self.height
    }

    fn is_walkable(&self, x: i16, y: i16) -> bool {
        self.in_bounds(x, y) && !self.blocked[self.idx(x, y)]
    }

    fn move_cost(&self, x: i16, y: i16) -> u32 {
        self.cost[self.idx(x, y)] as u32
    }

    fn set_blocked(&mut self, x: i16, y: i16, blocked: bool) {
        if self.in_bounds(x, y) {
            let idx = self.idx(x, y);
            self.blocked[idx] = blocked;
        }
    }
}

fn terrain_at(zones: &[TerrainZoneView], x: f32, y: f32) -> TerrainKindView {
    for zone in zones {
        if x >= zone.x0 && x < zone.x1 && y >= zone.y0 && y < zone.y1 {
            return zone.kind;
        }
    }
    TerrainKindView::Grass
}

fn terrain_cost(kind: TerrainKindView) -> u16 {
    match kind {
        TerrainKindView::Grass => 10,
        TerrainKindView::Hill => 14,
        TerrainKindView::Bog => 25,
        TerrainKindView::ShallowWater => 40,
        TerrainKindView::DeepWater => u16::MAX,
    }
}

fn block_circle(grid: &mut NavGrid, cx: f32, cy: f32, radius_m: f32) {
    let block_r = radius_m + PLAYER_RADIUS_M + PATH_CLEARANCE_M;
    let r = block_r.ceil() as i16;
    let ix = cx.floor() as i16;
    let iy = cy.floor() as i16;
    for dy in -r..=r {
        for dx in -r..=r {
            let x = ix + dx;
            let y = iy + dy;
            if !grid.in_bounds(x, y) {
                continue;
            }
            let cell_cx = x as f32 + 0.5;
            let cell_cy = y as f32 + 0.5;
            if (cell_cx - cx).hypot(cell_cy - cy) <= block_r {
                grid.set_blocked(x, y, true);
            }
        }
    }
}

/// Block building AABB inflated by player radius so paths match server collision.
fn mark_building_footprint(grid: &mut NavGrid, building: &BuildingView) {
    let pad = PLAYER_RADIUS_M + PATH_CLEARANCE_M;
    let hw = building.width_m / 2.0;
    let hd = building.depth_m / 2.0;
    let x0 = (building.x - hw - pad).floor() as i16;
    let y0 = (building.y - hd - pad).floor() as i16;
    let x1 = (building.x + hw + pad).ceil() as i16 - 1;
    let y1 = (building.y + hd + pad).ceil() as i16 - 1;
    if x1 < x0 || y1 < y0 {
        return;
    }
    for x in x0..=x1 {
        for y in y0..=y1 {
            grid.set_blocked(x, y, true);
        }
    }
}

fn clear_door_cells(grid: &mut NavGrid, doors: &[DoorView]) {
    // Outdoor pathfinding treats buildings as solid. Only clear a small doorway
    // corridor when the door is open so paths can enter if the goal is inside.
    for door in doors {
        if door.open {
            let (x, y) = world_to_cell(door.x, door.y);
            for dx in -1i16..=1 {
                for dy in -1i16..=1 {
                    if dx.abs() + dy.abs() <= 1 {
                        grid.set_blocked(x + dx, y + dy, false);
                    }
                }
            }
        }
    }
}

fn build_grid(state: &GameState, obstacles: &NavObstacles) -> NavGrid {
    let width = state.world_width_m.max(1.0).ceil() as i16;
    let height = state.world_height_m.max(1.0).ceil() as i16;
    let len = (width as usize) * (height as usize);
    let mut blocked = vec![false; len];
    let mut cost = vec![10u16; len];

    for y in 0..height {
        for x in 0..width {
            let cx = x as f32 + 0.5;
            let cy = y as f32 + 0.5;
            let kind = terrain_at(&state.terrain_zones, cx, cy);
            let tc = terrain_cost(kind);
            let idx = (y as usize) * (width as usize) + (x as usize);
            cost[idx] = tc;
            if tc == u16::MAX {
                blocked[idx] = true;
            }
        }
    }

    let mut grid = NavGrid {
        width,
        height,
        blocked,
        cost,
    };

    for circle in &obstacles.circles {
        block_circle(&mut grid, circle.x, circle.y, circle.radius_m);
    }

    for building in &state.buildings {
        mark_building_footprint(&mut grid, building);
    }

    clear_door_cells(&mut grid, &state.doors);

    grid
}

fn world_to_cell(x: f32, y: f32) -> (i16, i16) {
    (x.floor() as i16, y.floor() as i16)
}

fn cell_center(x: i16, y: i16) -> (f32, f32) {
    (x as f32 + 0.5, y as f32 + 0.5)
}

fn heuristic(ax: i16, ay: i16, bx: i16, by: i16) -> u32 {
    let dx = (ax - bx).unsigned_abs() as u32;
    let dy = (ay - by).unsigned_abs() as u32;
    let diag = dx.min(dy);
    let straight = dx.max(dy) - diag;
    diag * 14 + straight * 10
}

fn line_clear(grid: &NavGrid, from: (i16, i16), to: (i16, i16)) -> bool {
    let (mut x0, mut y0) = from;
    let (x1, y1) = to;
    let dx = (x1 - x0).abs();
    let dy = (y1 - y0).abs();
    let sx = if x0 < x1 { 1 } else { -1 };
    let sy = if y0 < y1 { 1 } else { -1 };
    let mut err = dx - dy;
    loop {
        if !grid.is_walkable(x0, y0) {
            return false;
        }
        if x0 == x1 && y0 == y1 {
            break;
        }
        let e2 = err * 2;
        if e2 > -dy {
            err -= dy;
            x0 += sx;
        }
        if e2 < dx {
            err += dx;
            y0 += sy;
        }
    }
    true
}

fn find_path(state: &GameState, from_x: f32, from_y: f32, to_x: f32, to_y: f32) -> Option<Vec<(f32, f32)>> {
    let obstacles = NavObstacles::from_state(state);
    let grid = build_grid(state, &obstacles);
    let (sx, sy) = world_to_cell(from_x, from_y);
    let (gx, gy) = world_to_cell(to_x, to_y);

    if !grid.in_bounds(sx, sy) || !grid.in_bounds(gx, gy) {
        return None;
    }

    let mut goal_x = gx;
    let mut goal_y = gy;
    if !grid.is_walkable(goal_x, goal_y) {
        let mut found = None;
        'search: for radius in 1..=16i16 {
            for dy in -radius..=radius {
                for dx in -radius..=radius {
                    if dx.abs() != radius && dy.abs() != radius {
                        continue;
                    }
                    let x = gx + dx;
                    let y = gy + dy;
                    if grid.is_walkable(x, y) {
                        found = Some((x, y));
                        break 'search;
                    }
                }
            }
        }
        let (x, y) = found?;
        goal_x = x;
        goal_y = y;
    }

    let goal_key = (goal_x, goal_y);

    // If start is inside a blocked cell (snapped), find nearest walkable.
    let mut start_x = sx;
    let mut start_y = sy;
    if !grid.is_walkable(start_x, start_y) {
        let mut found = None;
        'start: for radius in 1..=16i16 {
            for dy in -radius..=radius {
                for dx in -radius..=radius {
                    if dx.abs() != radius && dy.abs() != radius {
                        continue;
                    }
                    let x = sx + dx;
                    let y = sy + dy;
                    if grid.is_walkable(x, y) {
                        found = Some((x, y));
                        break 'start;
                    }
                }
            }
        }
        let (x, y) = found?;
        start_x = x;
        start_y = y;
    }
    let start_key = (start_x, start_y);

    if start_key == goal_key {
        return Some(vec![cell_center(goal_x, goal_y)]);
    }

    let mut open = BinaryHeap::new();
    let mut g_score: HashMap<(i16, i16), u32> = HashMap::new();
    let mut came_from: HashMap<(i16, i16), (i16, i16)> = HashMap::new();

    g_score.insert(start_key, 0);
    open.push(OpenNode {
        f: heuristic(start_x, start_y, goal_x, goal_y),
        g: 0,
        x: start_x,
        y: start_y,
    });

    const NEIGHBORS: [(i16, i16, u32); 8] = [
        (1, 0, 10),
        (-1, 0, 10),
        (0, 1, 10),
        (0, -1, 10),
        (1, 1, 14),
        (1, -1, 14),
        (-1, 1, 14),
        (-1, -1, 14),
    ];

    while let Some(current) = open.pop() {
        if (current.x, current.y) == goal_key {
            return Some(simplify_path(
                &grid,
                &obstacles,
                &came_from,
                start_key,
                goal_key,
                cell_center(goal_x, goal_y),
            ));
        }
        let Some(&best_g) = g_score.get(&(current.x, current.y)) else {
            continue;
        };
        if current.g > best_g {
            continue;
        }

        for (dx, dy, step_base) in NEIGHBORS {
            let nx = current.x + dx;
            let ny = current.y + dy;
            if !grid.is_walkable(nx, ny) {
                continue;
            }
            if dx != 0 && dy != 0 {
                if !grid.is_walkable(current.x + dx, current.y)
                    || !grid.is_walkable(current.x, current.y + dy)
                {
                    continue;
                }
            }
            let from = cell_center(current.x, current.y);
            let to = cell_center(nx, ny);
            if !segment_clear_world(&grid, &obstacles, from, to) {
                continue;
            }
            let step = step_base * grid.move_cost(nx, ny) / 10;
            let tentative = best_g + step;
            let key = (nx, ny);
            if tentative >= *g_score.get(&key).unwrap_or(&u32::MAX) {
                continue;
            }
            came_from.insert(key, (current.x, current.y));
            g_score.insert(key, tentative);
            open.push(OpenNode {
                f: tentative + heuristic(nx, ny, goal_x, goal_y),
                g: tentative,
                x: nx,
                y: ny,
            });
        }
    }

    None
}

fn simplify_path(
    grid: &NavGrid,
    obstacles: &NavObstacles,
    came_from: &HashMap<(i16, i16), (i16, i16)>,
    start: (i16, i16),
    goal: (i16, i16),
    goal_center: (f32, f32),
) -> Vec<(f32, f32)> {
    let mut cells = vec![goal];
    let mut current = goal;
    while current != start {
        let Some(&prev) = came_from.get(&current) else {
            break;
        };
        cells.push(prev);
        current = prev;
    }
    cells.reverse();

    if cells.is_empty() {
        return vec![goal_center];
    }

    let mut waypoints: Vec<(i16, i16)> = Vec::new();
    let mut anchor = 0usize;
    waypoints.push(cells[0]);
    for i in 1..cells.len() {
        if i + 1 < cells.len() {
            let from = cell_center(cells[anchor].0, cells[anchor].1);
            let to = if cells[i + 1] == goal {
                goal_center
            } else {
                cell_center(cells[i + 1].0, cells[i + 1].1)
            };
            if line_clear(grid, cells[anchor], cells[i + 1])
                && segment_clear_world(grid, obstacles, from, to)
            {
                continue;
            }
        }
        waypoints.push(cells[i]);
        anchor = i;
    }

    let mut out: Vec<(f32, f32)> = waypoints
        .iter()
        .map(|&(x, y)| cell_center(x, y))
        .collect();
    if let Some(last) = out.last_mut() {
        *last = goal_center;
    }

    if path_segments_clear(&grid, &obstacles, &out) {
        return out;
    }

    // Simplification shortcut would cut through a circle obstacle — keep full cell path.
    let mut fallback: Vec<(f32, f32)> = cells
        .iter()
        .map(|&(x, y)| cell_center(x, y))
        .collect();
    if let Some(last) = fallback.last_mut() {
        *last = goal_center;
    }
    fallback
}

fn path_segments_clear(grid: &NavGrid, obstacles: &NavObstacles, path: &[(f32, f32)]) -> bool {
    path.windows(2).all(|w| segment_clear_world(grid, obstacles, w[0], w[1]))
}

#[cfg(test)]
mod tests {
    use super::*;
    use flatland_protocol::{EntityState, Transform, Velocity2D, WorldCoord};

    fn empty_state() -> GameState {
        GameState {
            session_id: Default::default(),
            entity_id: 1,
            character_id: None,
            tick: 0,
            chunk_rev: 0,
            content_rev: 0,
            entities: vec![],
            player: Some(EntityState {
                id: 1,
                transform: Transform {
                    position: WorldCoord::surface(10.0, 10.0),
                    yaw: 0.0,
                    velocity: Velocity2D { vx: 0.0, vy: 0.0 },
                },
                label: "p".into(),
                vitals: None,
                attributes: None,
                skills: None,
                inside_building: None,
            }),
            resource_nodes: vec![],
            ground_drops: vec![],
            placed_containers: vec![],
            buildings: vec![],
            doors: vec![],
            npcs: vec![],
            blueprints: vec![],
            world_width_m: 64.0,
            world_height_m: 64.0,
            terrain_zones: vec![],
            world_clock: Default::default(),
            inventory: Default::default(),
            inventory_hints: Default::default(),
            logs: Default::default(),
            intents_sent: 0,
            ticks_received: 0,
            connected: true,
            disconnect_reason: None,
            show_stats: false,
            show_craft_menu: false,
            craft_menu_index: 0,
            craft_batch_quantity: 1,
            show_inventory_menu: false,
            inventory_menu_index: 0,
            show_move_picker: false,
            show_rename_prompt: false,
            rename_buffer: String::new(),
            move_picker_index: 0,
            move_picker: None,
            combat_target: None,
            combat_target_label: None,
            in_combat: false,
            auto_attack: false,
            combat_has_los: false,
            attack_cd_ticks: 0,
            gcd_ticks: 0,
            weapon_ability_id: String::new(),
            mainhand_template_id: None,
            mainhand_label: None,
            worn: std::collections::BTreeMap::new(),
            carry_mass: 0.0,
            carry_mass_max: 0.0,
            encumbrance: flatland_protocol::EncumbranceState::Light,
            inventory_stacks: Vec::new(),
            combat_target_detail: None,
            cast_progress: None,
            ability_cooldowns: vec![],
            blocking_active: false,
            max_target_slots: 1,
            combat_slots: vec![],
            rotation_presets: vec![],
            show_loadout_menu: false,
            show_rotation_editor: false,
            loadout_menu_index: 0,
            rotation_editor: Default::default(),
            harvest_in_progress: false,
            harvest_started_at: None,
            pending_craft_ack: None,
        }
    }

    #[test]
    fn path_on_open_field() {
        let state = empty_state();
        let path = find_path(&state, 10.0, 10.0, 20.0, 15.0).expect("path");
        assert!(!path.is_empty());
        let last = *path.last().unwrap();
        assert!((last.0 - 20.5).abs() < 1.0);
        assert!((last.1 - 15.5).abs() < 1.0);
    }

    #[test]
    fn path_routes_around_blocking_tree() {
        let mut state = empty_state();
        state.resource_nodes.push(flatland_protocol::ResourceNodeView {
            id: "oak".into(),
            label: "Oak".into(),
            x: 15.0,
            y: 12.0,
            z: 0.0,
            item_template: "oak_log".into(),
            state: ResourceNodeState::Available,
            blocking: true,
            blocking_radius_m: 0.8,
        });
        let path = find_path(&state, 10.0, 12.0, 20.0, 12.0).expect("path around tree");
        for (x, y) in &path {
            let near_tree = (*x - 15.0).abs() < 1.0 && (*y - 12.0).abs() < 1.0;
            assert!(!near_tree, "path should not cut through tree at ({x},{y})");
        }
    }

    #[test]
    fn path_segments_avoid_tree_corner_cut() {
        let mut state = empty_state();
        state.resource_nodes.push(flatland_protocol::ResourceNodeView {
            id: "oak".into(),
            label: "Oak".into(),
            x: 15.0,
            y: 12.0,
            z: 0.0,
            item_template: "oak_log".into(),
            state: ResourceNodeState::Available,
            blocking: true,
            blocking_radius_m: 0.8,
        });
        let path = find_path(&state, 10.0, 11.0, 20.0, 13.0).expect("diagonal path");
        let obstacles = NavObstacles::from_state(&state);
        let grid = build_grid(&state, &obstacles);
        assert!(
            path_segments_clear(&grid, &obstacles, &path),
            "every leg must clear tree collision: {path:?}"
        );
    }

    #[test]
    fn path_routes_around_building_with_clearance() {
        let mut state = empty_state();
        // 6x6 building centered at (20, 20) — blocks [17,23] x [17,23] before pad.
        state.buildings.push(flatland_protocol::BuildingView {
            id: "hut".into(),
            label: "Hut".into(),
            x: 20.0,
            y: 20.0,
            width_m: 6.0,
            depth_m: 6.0,
            interior_origin_x: 0.0,
            interior_origin_y: 0.0,
            tags: vec![],
        });
        let path = find_path(&state, 10.0, 20.0, 30.0, 20.0).expect("path around building");
        assert!(!path.is_empty());
        let pad = PLAYER_RADIUS_M + PATH_CLEARANCE_M;
        let x0 = 20.0 - 3.0 - pad;
        let x1 = 20.0 + 3.0 + pad;
        let y0 = 20.0 - 3.0 - pad;
        let y1 = 20.0 + 3.0 + pad;
        for (x, y) in &path {
            let inside = *x >= x0 && *x <= x1 && *y >= y0 && *y <= y1;
            assert!(
                !inside,
                "path waypoint ({x},{y}) intersects inflated building footprint"
            );
        }
        let last = *path.last().unwrap();
        assert!((last.0 - 30.0).abs() < 2.0, "should reach far side");
    }

    #[test]
    fn auto_navigator_finishes_near_goal() {
        let state = empty_state();
        let mut nav = AutoNavigator::plan(&state, 14.0, 12.0).expect("plan");
        let (fx, fy) = state.player_position();
        let steer = nav.steer(fx, fy).expect("steer");
        assert!(steer.0.abs() + steer.1.abs() > 0.0);
    }
}