flatland_pathfinding/

path.rs

//! A* grid pathfinding.

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

use crate::grid::{
    block_circle, cell_center, clear_door_cells, collides_player_at, mark_building_footprint,
    min_walkable_cost, terrain_cost, terrain_is_impassable, world_to_cell, NavWorld, DEFAULT_COST,
    PATH_CLEARANCE_M, PLAYER_RADIUS_M, SEGMENT_SAMPLE_M,
};
use crate::mode::PathMode;
use crate::z_nav::{cell_walkable_for_path_with_ground, goal_z_for};

/// Floor / ceiling for A* node expansions.
///
/// Unreachable goals used to flood a full 256×256 map (~65k cells) and stall
/// the region tick for ~1s — hence a hard ceiling. The hired-worker baseline
/// briefly set a flat 400, which was too tight for town detours (lodging beds
/// → east property plots around Town Market need ~800). Scale with Manhattan
/// cell distance so short hops stay cheap while ~60 m corridor wraps succeed.
const ASTAR_EXPANSIONS_FLOOR: u32 = 800;
const ASTAR_EXPANSIONS_CEILING: u32 = 4_000;
/// Extra expansions per Manhattan cell between start and goal.
const ASTAR_EXPANSIONS_PER_CELL: u32 = 16;

fn max_astar_expansions(start: (i16, i16), goal: (i16, i16)) -> u32 {
    let dx = (start.0 as i32 - goal.0 as i32).unsigned_abs();
    let dy = (start.1 as i32 - goal.1 as i32).unsigned_abs();
    let manhattan = dx.saturating_add(dy);
    let scaled = ASTAR_EXPANSIONS_FLOOR
        .saturating_add(manhattan.saturating_mul(ASTAR_EXPANSIONS_PER_CELL));
    scaled.min(ASTAR_EXPANSIONS_CEILING)
}


/// When start and goal are both on Road/Trail, multiply off-path cell costs so
/// Fastest prefers the corridor (true ETA alone still cuts grassy corners).
const PATH_CORRIDOR_OFFROAD_MULT: u32 = 3;

fn is_path_kind(kind: flatland_protocol::TerrainKindView) -> bool {
    matches!(
        kind,
        flatland_protocol::TerrainKindView::Road | flatland_protocol::TerrainKindView::Trail
    )
}

fn idx_of(x: i16, y: i16, width: i16) -> usize {
    (y as usize) * (width as usize) + (x as usize)
}

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

#[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>,
    /// Minimum walkable cell cost — scales the octile heuristic.
    min_cost: 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 paint_static_nav(world: &NavWorld) -> crate::grid::StaticNavPaint {
    use crate::grid::StaticNavPaint;

    let width = world.world_width_m.max(1.0).ceil() as i16;
    let height = world.world_height_m.max(1.0).ceil() as i16;
    let len = (width as usize) * (height as usize);
    let mut kind_at = vec![flatland_protocol::TerrainKindView::Grass; len];
    let mut elev = vec![0.0f32; len];
    let mut blocked_geometry = vec![false; len];

    // Paint low z_order first so higher z_order wins — matches sim `terrain_zone_at`.
    let mut zone_order: Vec<usize> = (0..world.terrain_zones.len()).collect();
    zone_order.sort_by(|&ia, &ib| {
        let a = &world.terrain_zones[ia];
        let b = &world.terrain_zones[ib];
        a.z_order.cmp(&b.z_order).then(ia.cmp(&ib))
    });
    for &zi in &zone_order {
        let zone = &world.terrain_zones[zi];
        let impassable = terrain_is_impassable(zone.kind, &world.kind_nav);
        // Always paint non-grass kinds so empty/default tables still overwrite
        // underlying bog/water when z_order says the road wins.
        let paint_kind = impassable
            || !matches!(zone.kind, flatland_protocol::TerrainKindView::Grass)
            || zone.elevation != 0.0;
        if !paint_kind {
            continue;
        }
        let x0 = zone.x0.floor().max(0.0) as i16;
        let y0 = zone.y0.floor().max(0.0) as i16;
        let x1 = zone.x1.ceil().min(width as f32) as i16;
        let y1 = zone.y1.ceil().min(height as f32) as i16;
        for y in y0..y1 {
            if y < 0 || y >= height {
                continue;
            }
            for x in x0..x1 {
                if x < 0 || x >= width {
                    continue;
                }
                let cx = x as f32 + 0.5;
                let cy = y as f32 + 0.5;
                if cx < zone.x0 || cx >= zone.x1 || cy < zone.y0 || cy >= zone.y1 {
                    continue;
                }
                let idx = (y as usize) * (width as usize) + (x as usize);
                kind_at[idx] = zone.kind;
                blocked_geometry[idx] = impassable;
                if zone.elevation != 0.0 {
                    elev[idx] = zone.elevation;
                }
            }
        }
    }

    for building in &world.buildings {
        mark_building_footprint(&mut blocked_geometry, width, height, building);
    }
    clear_door_cells(&mut blocked_geometry, width, height, &world.doors);

    StaticNavPaint {
        width,
        height,
        kind_at,
        elev,
        blocked_geometry,
    }
}

fn build_grid(
    world: &NavWorld,
    player_z: f32,
    goal_z: f32,
    mode: PathMode,
    from_x: f32,
    from_y: f32,
    to_x: f32,
    to_y: f32,
) -> NavGrid {
    let t_paint = std::time::Instant::now();
    let paint = world.ensure_static_paint(|| paint_static_nav(world));
    let paint_ms = t_paint.elapsed().as_secs_f32() * 1000.0;
    // Cold paint on dense maps (thousands of zones) can take tens of ms once;
    // subsequent finds reuse the Arc cache and stay cheap.
    if paint_ms > 30.0 {
        eprintln!(
            "[diag] static_nav_paint {paint_ms:.1}ms zones={} (world {:.0}x{:.0}) — cached for later finds",
            world.terrain_zones.len(),
            world.world_width_m,
            world.world_height_m,
        );
    }

    let width = paint.width;
    let height = paint.height;
    let min_cost = min_walkable_cost(mode, &world.terrain_zones, &world.kind_nav);

    let mut cost: Vec<u16> = paint
        .kind_at
        .iter()
        .map(|kind| terrain_cost(*kind, mode, &world.kind_nav))
        .collect();
    let mut blocked = paint.blocked_geometry.clone();
    for (b, c) in blocked.iter_mut().zip(cost.iter()) {
        if *c == u16::MAX {
            *b = true;
        }
    }

    // When both endpoints sit on Road/Trail, inflate off-path costs so Fastest
    // stays on the corridor instead of cutting slow corners through grass/bog.
    if mode == PathMode::Fastest {
        let (sx, sy) = world_to_cell(from_x, from_y);
        let (gx, gy) = world_to_cell(to_x, to_y);
        let start_path = grid_in_bounds(sx, sy, width, height)
            && is_path_kind(paint.kind_at[idx_of(sx, sy, width)]);
        let goal_path = grid_in_bounds(gx, gy, width, height)
            && is_path_kind(paint.kind_at[idx_of(gx, gy, width)]);
        if start_path && goal_path {
            for ((cell_cost, kind), is_blocked) in cost
                .iter_mut()
                .zip(paint.kind_at.iter())
                .zip(blocked.iter())
            {
                if *is_blocked || is_path_kind(*kind) {
                    continue;
                }
                let boosted = (*cell_cost as u32).saturating_mul(PATH_CORRIDOR_OFFROAD_MULT);
                *cell_cost = boosted.min((u16::MAX - 1) as u32) as u16;
            }
        }
    }

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

    for circle in &world.circles {
        block_circle(
            &mut grid.blocked,
            grid.width,
            grid.height,
            circle.x,
            circle.y,
            circle.radius_m,
        );
    }

    // Z-band walkability is O(width*height × platforms); skip when no layers.
    // Must use painted `elev` — calling elevation_at per cell re-scans all zones
    // and stalls ~1s on large outdoor maps once any interior z_platform is set.
    if !world.z_platforms.is_empty() || !world.z_transitions.is_empty() {
        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 idx = (y as usize) * (width as usize) + (x as usize);
                if !cell_walkable_for_path_with_ground(
                    world,
                    cx,
                    cy,
                    player_z,
                    goal_z,
                    paint.elev[idx],
                ) {
                    grid.set_blocked(x, y, true);
                }
            }
        }
    }

    grid
}

/// Paint the static terrain/building raster now so the first gameplay `find_path`
/// does not stall a live tick (~40–120 ms on dense outdoor maps).
pub fn prewarm_static_paint(world: &NavWorld) {
    let _ = world.ensure_static_paint(|| paint_static_nav(world));
}

fn heuristic(ax: i16, ay: i16, bx: i16, by: i16, min_cost: u16) -> 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;
    // Octile distance assumes DEFAULT_COST=10 orthogonal / 14 diagonal; scale by min walkable.
    let octile = diag * 14 + straight * 10;
    octile * (min_cost as u32) / (DEFAULT_COST as u32)
}

fn line_clear(grid: &NavGrid, from: (i16, i16), to: (i16, i16)) -> bool {
    let (mut x0, mut y0) = from;
    let (x1, y1) = to;
    let max_end_cost = grid
        .move_cost(from.0, from.1)
        .max(grid.move_cost(to.0, to.1));
    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;
        }
        // Do not collapse Fastest detours through slower terrain (e.g. road → bog shortcut).
        if grid.move_cost(x0, y0) > max_end_cost {
            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 simplify_path(
    grid: &NavGrid,
    world: &NavWorld,
    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, world, 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, world, &out) {
        return out;
    }

    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, world: &NavWorld, path: &[(f32, f32)]) -> bool {
    path.windows(2)
        .all(|w| segment_clear_world(grid, world, w[0], w[1]))
}

/// Set `FLATLAND_NAV_DEBUG=1` to print why a corridor may be sealed (kinds/costs/blocked).
fn eprintln_nav_corridor_debug(
    world: &NavWorld,
    grid: &NavGrid,
    mode: PathMode,
    from_x: f32,
    from_y: f32,
    to_x: f32,
    to_y: f32,
    sx: i16,
    sy: i16,
    gx: i16,
    gy: i16,
) {
    let sk = world.terrain_kind_at(from_x, from_y);
    let gk = world.terrain_kind_at(to_x, to_y);
    eprintln!(
        "[nav-debug] mode={mode:?} start=({from_x:.1},{from_y:.1})→cell({sx},{sy}) kind={sk:?} cost={} walk={} | goal=({to_x:.1},{to_y:.1})→cell({gx},{gy}) kind={gk:?} cost={} walk={} | buildings={} circles={} kind_nav_rows={}",
        if grid.in_bounds(sx, sy) {
            grid.move_cost(sx, sy)
        } else {
            0
        },
        grid.in_bounds(sx, sy) && grid.is_walkable(sx, sy),
        if grid.in_bounds(gx, gy) {
            grid.move_cost(gx, gy)
        } else {
            0
        },
        grid.in_bounds(gx, gy) && grid.is_walkable(gx, gy),
        world.buildings.len(),
        world.circles.len(),
        world.kind_nav.iter().count(),
    );
    // Sample the axis-aligned corridor (straight cell line) for sealed pavement.
    let mut x0 = sx;
    let mut y0 = sy;
    let x1 = gx;
    let y1 = gy;
    let dx = (x1 - x0).abs();
    let dy = (y1 - y0).abs();
    let sx_step: i16 = if x0 < x1 { 1 } else { -1 };
    let sy_step: i16 = if y0 < y1 { 1 } else { -1 };
    let mut err = dx - dy;
    let mut blocked_n = 0u32;
    let mut samples = 0u32;
    loop {
        samples += 1;
        if grid.in_bounds(x0, y0) {
            let walk = grid.is_walkable(x0, y0);
            let cost = grid.move_cost(x0, y0);
            let (cx, cy) = cell_center(x0, y0);
            let kind = world.terrain_kind_at(cx, cy);
            if !walk {
                blocked_n += 1;
                if blocked_n <= 12 {
                    let hit_b = world.buildings.iter().find(|b| {
                        let pad = PLAYER_RADIUS_M;
                        let hw = b.width_m / 2.0 + pad;
                        let hd = b.depth_m / 2.0 + pad;
                        cx >= b.x - hw && cx <= b.x + hw && cy >= b.y - hd && cy <= b.y + hd
                    });
                    let hit_c = world.circles.iter().find(|o| {
                        let r = o.radius_m + PLAYER_RADIUS_M + PATH_CLEARANCE_M;
                        (cx - o.x).hypot(cy - o.y) <= r
                    });
                    eprintln!(
                        "[nav-debug]   BLOCKED cell({x0},{y0}) kind={kind:?} cost={cost} building={} circle={}",
                        hit_b.map(|b| b.id.as_str()).unwrap_or("-"),
                        hit_c
                            .map(|c| format!("({:.1},{:.1})", c.x, c.y))
                            .unwrap_or_else(|| "-".into()),
                    );
                }
            }
        }
        if x0 == x1 && y0 == y1 {
            break;
        }
        let e2 = err * 2;
        if e2 > -dy {
            err -= dy;
            x0 += sx_step;
        }
        if e2 < dx {
            err += dx;
            y0 += sy_step;
        }
        if samples > 512 {
            break;
        }
    }
    eprintln!(
        "[nav-debug] straight-line samples={samples} blocked={blocked_n} (if blocked>0 A* cannot stay on the line)"
    );
}

fn segment_clear_world(grid: &NavGrid, world: &NavWorld, 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, world) {
            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))
}

/// Plan a path from `(from_x, from_y, from_z)` to `(to_x, to_y)` using goal z inferred from `from_z`.
pub fn find_path(
    world: &NavWorld,
    from_x: f32,
    from_y: f32,
    from_z: f32,
    to_x: f32,
    to_y: f32,
    mode: PathMode,
) -> Option<Vec<(f32, f32)>> {
    let to_z = goal_z_for(world, to_x, to_y, from_z);
    find_path_with_goal_z(world, from_x, from_y, from_z, to_x, to_y, to_z, mode)
}

pub fn find_path_with_goal_z(
    world: &NavWorld,
    from_x: f32,
    from_y: f32,
    from_z: f32,
    to_x: f32,
    to_y: f32,
    to_z: f32,
    mode: PathMode,
) -> Option<Vec<(f32, f32)>> {
    let grid = build_grid(world, from_z, to_z, mode, from_x, from_y, to_x, to_y);
    let (sx, sy) = world_to_cell(from_x, from_y);
    let (gx, gy) = world_to_cell(to_x, to_y);

    let nav_debug = std::env::var_os("FLATLAND_NAV_DEBUG").is_some();
    if nav_debug {
        eprintln_nav_corridor_debug(
            world, &grid, mode, from_x, from_y, to_x, to_y, sx, sy, gx, gy,
        );
    }

    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);

    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();
    let h_scale = grid.min_cost;

    g_score.insert(start_key, 0);
    open.push(OpenNode {
        f: heuristic(start_x, start_y, goal_x, goal_y, h_scale),
        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),
    ];

    let mut expansions = 0u32;
    while let Some(current) = open.pop() {
        if (current.x, current.y) == goal_key {
            let path = simplify_path(
                &grid,
                world,
                &came_from,
                start_key,
                goal_key,
                cell_center(goal_x, goal_y),
            );
            if std::env::var_os("FLATLAND_NAV_DEBUG").is_some() {
                let max_dev = path
                    .iter()
                    .map(|(x, y)| {
                        // Distance from the start→goal chord.
                        let (ax, ay) = (from_x, from_y);
                        let (bx, by) = (to_x, to_y);
                        let abx = bx - ax;
                        let aby = by - ay;
                        let ab2 = abx * abx + aby * aby;
                        if ab2 < 1e-6 {
                            return 0.0;
                        }
                        let t = ((x - ax) * abx + (y - ay) * aby) / ab2;
                        let t = t.clamp(0.0, 1.0);
                        let px = ax + abx * t;
                        let py = ay + aby * t;
                        (x - px).hypot(y - py)
                    })
                    .fold(0.0f32, f32::max);
                eprintln!(
                    "[nav-debug] path waypoints={} max_dev_from_chord={max_dev:.2}m first={:?} last={:?}",
                    path.len(),
                    path.first(),
                    path.last()
                );
            }
            return Some(path);
        }
        let Some(&best_g) = g_score.get(&(current.x, current.y)) else {
            continue;
        };
        if current.g > best_g {
            continue;
        }
        expansions = expansions.saturating_add(1);
        if expansions > max_astar_expansions(start_key, goal_key) {
            return None;
        }

        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;
                }
            }
            // Obstacles are already stamped into `blocked` (circles + buildings).
            // Per-edge world sampling here used to dominate failed long-range searches.
            let step = step_base * grid.move_cost(nx, ny) / (DEFAULT_COST as u32);
            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, h_scale),
                g: tentative,
                x: nx,
                y: ny,
            });
        }
    }

    None
}

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

    fn open_world() -> NavWorld {
        NavWorld::new(
            64.0,
            64.0,
            vec![],
            vec![],
            vec![],
            vec![],
            vec![],
            vec![],
            crate::grid::TerrainNavTable::unit_test_defaults(),
        )
    }

    #[test]
    fn path_on_open_field() {
        let world = open_world();
        let path = find_path(&world, 10.0, 10.0, 0.0, 20.0, 15.0, PathMode::Fastest).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_building_footprint() {
        let mut world = open_world();
        world.buildings.push(flatland_protocol::BuildingView {
            id: "storage".into(),
            label: "Storage".into(),
            x: 15.0,
            y: 12.0,
            width_m: 6.0,
            depth_m: 4.0,
            interior_blueprint: None,
            tags: vec![],
            market_boundary_zone_ids: vec![],
            market_max_volume: None,
            wall_set: None,
            roof_set: None,
        });
        let path = find_path(&world, 10.0, 12.0, 0.0, 20.0, 12.0, PathMode::Fastest)
            .expect("path around building");
        for (x, y) in &path {
            assert!(
                !collides_player_at(*x, *y, &world),
                "path must not cut through building at ({x},{y})"
            );
        }
    }

    #[test]
    fn path_routes_around_blocking_tree() {
        let mut world = open_world();
        world.circles.push(crate::grid::NavBlockingCircle {
            x: 15.0,
            y: 12.0,
            radius_m: 0.8,
        });
        let path = find_path(&world, 10.0, 12.0, 0.0, 20.0, 12.0, PathMode::Fastest)
            .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 unreachable_goal_fails_fast_on_large_map() {
        use std::time::Instant;
        let mut world = NavWorld::new(
            256.0,
            256.0,
            vec![],
            vec![],
            vec![],
            vec![],
            vec![],
            vec![],
            crate::grid::TerrainNavTable::unit_test_defaults(),
        );
        // Full-height deep-water barrier splits the map so A* would flood otherwise.
        world
            .terrain_zones
            .push(flatland_protocol::TerrainZoneView {
                id: "moat".into(),
                x0: 120.0,
                y0: 0.0,
                x1: 136.0,
                y1: 256.0,
                kind: flatland_protocol::TerrainKindView::DeepWater,
                elevation: 0.0,
                glyph: None,
                color: None,
                tile_id: None,
                z_order: 0,
                channel_start_tick: None,
                channel_end_tick: None,
            });
        let start = Instant::now();
        let path = find_path(&world, 40.0, 40.0, 0.0, 200.0, 200.0, PathMode::Fastest);
        let elapsed_ms = start.elapsed().as_secs_f32() * 1000.0;
        assert!(path.is_none(), "moat should make goal unreachable");
        assert!(
            elapsed_ms < 150.0,
            "unreachable search must stay under 150ms (debug), took {elapsed_ms:.1}ms"
        );
    }

    #[test]
    fn default_cost_overlay_flood_stays_fast() {
        use std::time::Instant;
        let mut world = NavWorld::new(
            256.0,
            256.0,
            vec![],
            vec![],
            vec![],
            vec![],
            vec![],
            vec![],
            crate::grid::TerrainNavTable::unit_test_defaults(),
        );
        // Mimic an interior entry flooding `terrain_zones` with cheap (Grass-cost)
        // overlay rects. Painting these is a no-op for nav, so build_grid must skip
        // them instead of costing O(zone cells) per zone (~1s stall).
        for i in 0..600i16 {
            world
                .terrain_zones
                .push(flatland_protocol::TerrainZoneView {
                    id: format!("rt:{i}").into(),
                    x0: 0.0,
                    y0: 0.0,
                    x1: 256.0,
                    y1: 256.0,
                    kind: flatland_protocol::TerrainKindView::Grass,
                    elevation: 0.0,
                    glyph: None,
                    color: None,
                    tile_id: None,
                    z_order: 0,
                    channel_start_tick: None,
                    channel_end_tick: None,
                });
        }
        let start = Instant::now();
        let path = find_path(&world, 40.0, 40.0, 0.0, 200.0, 200.0, PathMode::Fastest);
        let elapsed_ms = start.elapsed().as_secs_f32() * 1000.0;
        assert!(path.is_some(), "open field must remain reachable");
        assert!(
            elapsed_ms < 150.0,
            "default-cost overlay flood must stay under 150ms (debug), took {elapsed_ms:.1}ms"
        );
    }

    #[test]
    fn z_platform_with_many_zones_stays_fast() {
        use std::time::Instant;
        // Reproduce the building-enter stall: outdoor-sized map + many terrain zones +
        // a single interior z_platform. Per-cell elevation_at would be O(cells × zones)
        // (~1s); painted elev must keep this fast.
        let mut world = NavWorld::new(
            512.0,
            256.0,
            vec![],
            vec![flatland_protocol::ZPlatformView {
                id: "floor_0".into(),
                z: 0.0,
                x0: 0.0,
                y0: 0.0,
                x1: 16.0,
                y1: 16.0,
            }],
            vec![],
            vec![],
            vec![],
            vec![],
            crate::grid::TerrainNavTable::unit_test_defaults(),
        );
        for i in 0..400i16 {
            world
                .terrain_zones
                .push(flatland_protocol::TerrainZoneView {
                    id: format!("zone:{i}").into(),
                    x0: (i % 32) as f32 * 8.0,
                    y0: (i / 32) as f32 * 8.0,
                    x1: (i % 32) as f32 * 8.0 + 8.0,
                    y1: (i / 32) as f32 * 8.0 + 8.0,
                    kind: flatland_protocol::TerrainKindView::Dirt,
                    elevation: 0.0,
                    glyph: None,
                    color: None,
                    tile_id: None,
                    z_order: 0,
                    channel_start_tick: None,
                    channel_end_tick: None,
                });
        }
        let start = Instant::now();
        let path = find_path(&world, 4.0, 4.0, 0.0, 12.0, 12.0, PathMode::Fastest);
        let elapsed_ms = start.elapsed().as_secs_f32() * 1000.0;
        assert!(path.is_some(), "interior cells must remain reachable");
        assert!(
            elapsed_ms < 200.0,
            "z_platform + many zones must stay under 200ms (debug), took {elapsed_ms:.1}ms"
        );
    }

    fn zone(
        id: &str,
        x0: f32,
        y0: f32,
        x1: f32,
        y1: f32,
        kind: flatland_protocol::TerrainKindView,
    ) -> flatland_protocol::TerrainZoneView {
        flatland_protocol::TerrainZoneView {
            id: id.into(),
            x0,
            y0,
            x1,
            y1,
            kind,
            elevation: 0.0,
            glyph: None,
            color: None,
            tile_id: None,
            z_order: 0,
            channel_start_tick: None,
            channel_end_tick: None,
        }
    }

    #[test]
    fn fastest_prefers_road_detour_over_bog() {
        // Long bog on the straight line; short northern road bypass — Fastest ETA prefers road.
        let mut world = open_world();
        world.terrain_zones.push(zone(
            "bog",
            14.0,
            10.0,
            50.0,
            22.0,
            flatland_protocol::TerrainKindView::Bog,
        ));
        world.terrain_zones.push(zone(
            "road",
            10.0,
            24.0,
            54.0,
            28.0,
            flatland_protocol::TerrainKindView::Road,
        ));
        let path = find_path(&world, 12.0, 16.0, 0.0, 52.0, 16.0, PathMode::Fastest)
            .expect("fastest path");
        let max_y = path.iter().map(|p| p.1).fold(f32::NEG_INFINITY, f32::max);
        assert!(
            max_y > 23.0,
            "Fastest should climb onto the road (max_y={max_y}), path={path:?}"
        );
    }

    #[test]
    fn direct_crosses_bog_when_shorter() {
        let mut world = open_world();
        world.terrain_zones.push(zone(
            "bog",
            14.0,
            10.0,
            50.0,
            22.0,
            flatland_protocol::TerrainKindView::Bog,
        ));
        world.terrain_zones.push(zone(
            "road",
            10.0,
            24.0,
            54.0,
            28.0,
            flatland_protocol::TerrainKindView::Road,
        ));
        let path =
            find_path(&world, 12.0, 16.0, 0.0, 52.0, 16.0, PathMode::Direct).expect("direct path");
        let max_y = path.iter().map(|p| p.1).fold(f32::NEG_INFINITY, f32::max);
        assert!(
            max_y < 23.0,
            "Direct should stay near the straight line through bog (max_y={max_y})"
        );
    }

    #[test]
    fn highest_z_order_wins_road_over_bog() {
        // Sim movement uses max z_order; pathfinding must match so roads aren't
        // treated as the older bog/water underneath.
        let mut world = open_world();
        world.terrain_zones.push(zone(
            "bog",
            10.0,
            10.0,
            54.0,
            22.0,
            flatland_protocol::TerrainKindView::Bog,
        ));
        world.terrain_zones.last_mut().unwrap().z_order = 0;
        world.terrain_zones.push(zone(
            "road",
            10.0,
            14.0,
            54.0,
            18.0,
            flatland_protocol::TerrainKindView::Road,
        ));
        world.terrain_zones.last_mut().unwrap().z_order = 10;
        let path = find_path(&world, 12.0, 16.0, 0.0, 52.0, 16.0, PathMode::Fastest).expect("path");
        let max_dev = path
            .iter()
            .map(|p| (p.1 - 16.0).abs())
            .fold(0.0f32, f32::max);
        assert!(
            max_dev < 3.0,
            "path should stay on the road strip (max |y-16|={max_dev}), path={path:?}"
        );
    }

    #[test]
    fn fastest_stays_on_road_when_endpoints_on_road() {
        // Narrow road with grass on both sides — geometric shortcut leaves the road;
        // corridor bias must keep Fastest on the pavement when both ends are on-road.
        let mut world = open_world();
        world.terrain_zones.push(zone(
            "road",
            10.0,
            15.0,
            54.0,
            17.0,
            flatland_protocol::TerrainKindView::Road,
        ));
        world.terrain_zones.last_mut().unwrap().z_order = 5;
        let path = find_path(&world, 12.0, 16.0, 0.0, 52.0, 16.0, PathMode::Fastest).expect("path");
        let max_dev = path
            .iter()
            .map(|p| (p.1 - 16.0).abs())
            .fold(0.0f32, f32::max);
        assert!(
            max_dev < 2.5,
            "path should hug the road strip (max |y-16|={max_dev}), path={path:?}"
        );
    }

    #[test]
    fn building_clearance_does_not_seal_adjacent_road() {
        // Repro: West Storage-style pad used to block road cells the player can walk
        // (collides_player_at uses PLAYER_RADIUS only), forcing a grass detour.
        let mut world = open_world();
        world.world_width_m = 200.0;
        world.world_height_m = 130.0;
        world.terrain_zones.push(zone(
            "road",
            159.0,
            106.0,
            176.0,
            107.0,
            flatland_protocol::TerrainKindView::Road,
        ));
        world.terrain_zones.last_mut().unwrap().z_order = 10;
        world.buildings.push(flatland_protocol::BuildingView {
            id: "town_storage_west".into(),
            label: "West Storage".into(),
            x: 164.0,
            y: 102.0,
            width_m: 8.0,
            depth_m: 6.5,
            interior_blueprint: None,
            tags: vec![],
            market_boundary_zone_ids: vec![],
            market_max_volume: None,
            wall_set: None,
            roof_set: None,
        });
        // Road cell centers under the old over-padded footprint must stay walkable.
        assert!(
            !collides_player_at(164.0, 106.5, &world),
            "runtime collision must allow the road beside the building"
        );
        let path = find_path(&world, 172.2, 106.5, 0.0, 159.5, 106.5, PathMode::Fastest)
            .expect("path along road");
        let max_dev = path
            .iter()
            .map(|p| (p.1 - 106.5).abs())
            .fold(0.0f32, f32::max);
        assert!(
            max_dev < 1.5,
            "must stay on the road, not detour south (max |y-106.5|={max_dev}), path={path:?}"
        );
    }

    #[test]
    fn eta_costs_match_speed_table() {
        use crate::grid::{terrain_cost, terrain_move_speed_mult, TerrainNavTable};
        let table = TerrainNavTable::unit_test_defaults();
        let road_speed = terrain_move_speed_mult(flatland_protocol::TerrainKindView::Road, &table);
        let expected = (DEFAULT_COST as f32 / road_speed).round() as u16;
        assert_eq!(
            terrain_cost(
                flatland_protocol::TerrainKindView::Road,
                PathMode::Fastest,
                &table
            ),
            expected
        );
        assert_eq!(
            terrain_cost(
                flatland_protocol::TerrainKindView::Bog,
                PathMode::Direct,
                &table
            ),
            DEFAULT_COST
        );
        assert_eq!(
            terrain_cost(
                flatland_protocol::TerrainKindView::DeepWater,
                PathMode::Direct,
                &table
            ),
            u16::MAX
        );
    }
}