flatland-pathfinding 0.2.36

//! 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,
    terrain_cost, world_to_cell, NavWorld, SEGMENT_SAMPLE_M,
};
use crate::z_nav::{cell_walkable_for_path_with_ground, goal_z_for};

/// Hard cap on A* node expansions. Unreachable goals used to flood a full 256×256
/// map (~65k cells × world collision samples) and stall the region tick for ~1s.
const MAX_ASTAR_EXPANSIONS: u32 = 8_000;
/// Default walk cost per cell (grass). Zones with this cost cannot change the grid.
const DEFAULT_COST: u16 = 10;

#[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 build_grid(world: &NavWorld, player_z: f32, goal_z: f32) -> NavGrid {
    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 blocked = vec![false; len];
    // Default grass cost; paint zone rects instead of O(cells × zones) probes.
    let mut cost = vec![10u16; len];
    // Ground elevation painted once so the z-band pass is O(cells × platforms),
    // not O(cells × terrain_zones) via per-cell elevation_at scans.
    let mut elev = vec![0.0f32; len];

    // Paint later zones first so earlier list entries win (matches terrain_kind_at).
    // Skip zones that change neither cost nor elevation — pure no-ops that cost
    // O(zone cells). Interior floor/overlay zones are normally default-cost.
    for zone in world.terrain_zones.iter().rev() {
        let tc = terrain_cost(zone.kind);
        let paint_cost = tc != DEFAULT_COST;
        let paint_elev = zone.elevation != 0.0;
        if !paint_cost && !paint_elev {
            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);
                if paint_cost {
                    cost[idx] = tc;
                    blocked[idx] = tc == u16::MAX;
                }
                if paint_elev {
                    elev[idx] = zone.elevation;
                }
            }
        }
    }

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

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

    for building in &world.buildings {
        mark_building_footprint(&mut grid.blocked, grid.width, grid.height, building);
    }

    clear_door_cells(&mut grid.blocked, grid.width, grid.height, &world.doors);

    // 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,
                    elev[idx],
                ) {
                    grid.set_blocked(x, y, true);
                }
            }
        }
    }

    grid
}

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 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]))
}

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,
) -> 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)
}

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,
) -> Option<Vec<(f32, f32)>> {
    let t_grid = std::time::Instant::now();
    let grid = build_grid(world, from_z, to_z);
    let grid_ms = t_grid.elapsed().as_secs_f32() * 1000.0;
    if grid_ms > 30.0 {
        let mut non_default = 0usize;
        let mut max_area: u64 = 0;
        let mut max_span_kind = String::new();
        for z in &world.terrain_zones {
            if terrain_cost(z.kind) == DEFAULT_COST {
                continue;
            }
            non_default += 1;
            let w = (z.x1 - z.x0).max(0.0) as u64;
            let h = (z.y1 - z.y0).max(0.0) as u64;
            let a = w.saturating_mul(h);
            if a > max_area {
                max_area = a;
                max_span_kind = format!("{:?}", z.kind);
            }
        }
        eprintln!(
            "[diag] build_grid {grid_ms:.1}ms zones={} circles={} buildings={} non_default_zones={} max_zone_area={} max_spans={} (world {:.0}x{:.0})",
            world.terrain_zones.len(), world.circles.len(), world.buildings.len(), non_default, max_area, max_span_kind, world.world_width_m, world.world_height_m,
        );
    }
    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);

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

    let mut expansions = 0u32;
    while let Some(current) = open.pop() {
        if (current.x, current.y) == goal_key {
            return Some(simplify_path(
                &grid,
                world,
                &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;
        }
        expansions = expansions.saturating_add(1);
        if expansions > MAX_ASTAR_EXPANSIONS {
            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) / 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
}

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

    fn open_world() -> NavWorld {
        NavWorld {
            world_width_m: 64.0,
            world_height_m: 64.0,
            terrain_zones: vec![],
            z_platforms: vec![],
            z_transitions: vec![],
            buildings: vec![],
            doors: vec![],
            circles: vec![],
        }
    }

    #[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).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).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).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 {
            world_width_m: 256.0,
            world_height_m: 256.0,
            terrain_zones: vec![],
            z_platforms: vec![],
            z_transitions: vec![],
            buildings: vec![],
            doors: vec![],
            circles: vec![],
        };
        // 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);
        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 {
            world_width_m: 256.0,
            world_height_m: 256.0,
            terrain_zones: vec![],
            z_platforms: vec![],
            z_transitions: vec![],
            buildings: vec![],
            doors: vec![],
            circles: vec![],
        };
        // 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);
        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 {
            world_width_m: 512.0,
            world_height_m: 256.0,
            terrain_zones: vec![],
            z_platforms: vec![flatland_protocol::ZPlatformView {
                id: "floor_0".into(),
                z: 0.0,
                x0: 0.0,
                y0: 0.0,
                x1: 16.0,
                y1: 16.0,
            }],
            z_transitions: vec![],
            buildings: vec![],
            doors: vec![],
            circles: vec![],
        };
        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);
        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"
        );
    }
}