flatland_pathfinding/

grid.rs

//! Navigation world snapshot for pathfinding.

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

use crate::mode::PathMode;

pub const PLAYER_RADIUS_M: f32 = 0.45;
/// Extra clearance so paths don't hug walls.
pub const PATH_CLEARANCE_M: f32 = 0.35;
/// Hired-worker nav building-pad dilation. Building footprints are expanded by
/// this much ON TOP of the body radius when painting the hired-worker job-loop
/// nav world, so a sub-tile seam between a building footprint and an adjacent
/// road/terrain cell narrower than the entity can thread comes out IMPASSABLE
/// (A* routes around instead of planning a path the movement layer re-pins on).
/// Default 0 keeps classic/player/static nav unchanged. Meant to seal only the
/// narrowest gaps, so it must stay well under a full cell to avoid sealing
/// legitimate road corridors beside buildings.
pub const WORKER_NAV_BUILDING_CLEARANCE_M: f32 = 0.20;
/// Sample spacing when testing straight segments against circle obstacles.
pub const SEGMENT_SAMPLE_M: f32 = 0.3;
/// Grass baseline A* cost (and Direct walkable cost). Fastest uses `BASE / speed`.
pub const DEFAULT_COST: u16 = 10;

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

/// Per-kind move speed + impassable flag from content (`terrain-kinds.yaml`).
#[derive(Debug, Clone, Copy, PartialEq)]
pub struct TerrainKindNavParams {
    pub move_speed_mult: f32,
    pub impassable: bool,
}

impl Default for TerrainKindNavParams {
    fn default() -> Self {
        Self {
            move_speed_mult: 1.0,
            impassable: false,
        }
    }
}

/// Lookup table for path costs — populated from the terrain kind catalog (no hardcoded speeds).
#[derive(Debug, Clone, Default)]
pub struct TerrainNavTable {
    params: Vec<(TerrainKindView, TerrainKindNavParams)>,
}

impl TerrainNavTable {
    pub fn set(&mut self, kind: TerrainKindView, params: TerrainKindNavParams) {
        if let Some(slot) = self.params.iter_mut().find(|(k, _)| *k == kind) {
            slot.1 = params;
        } else {
            self.params.push((kind, params));
        }
    }

    pub fn get(&self, kind: TerrainKindView) -> TerrainKindNavParams {
        self.params
            .iter()
            .find(|(k, _)| *k == kind)
            .map(|(_, p)| *p)
            .unwrap_or_default()
    }

    pub fn iter(&self) -> impl Iterator<Item = (TerrainKindView, TerrainKindNavParams)> + '_ {
        self.params.iter().copied()
    }

    /// Test / fixture helper: former baked-in speeds so unit tests stay self-contained.
    pub fn unit_test_defaults() -> Self {
        let mut t = Self::default();
        let rows: &[(TerrainKindView, f32, bool)] = &[
            (TerrainKindView::Grass, 1.0, false),
            (TerrainKindView::Dirt, 0.98, false),
            (TerrainKindView::Tilled, 0.95, false),
            (TerrainKindView::Desert, 0.9, false),
            (TerrainKindView::Hill, 0.92, false),
            (TerrainKindView::Trail, 1.10, false),
            (TerrainKindView::Road, 1.50, false),
            (TerrainKindView::Rock, 0.85, true),
            (TerrainKindView::Bog, 0.65, false),
            (TerrainKindView::Beach, 0.94, false),
            (TerrainKindView::ShallowWater, 0.55, false),
            (TerrainKindView::DeepWater, 0.4, true),
        ];
        for &(kind, speed, impassable) in rows {
            t.set(
                kind,
                TerrainKindNavParams {
                    move_speed_mult: speed,
                    impassable,
                },
            );
        }
        t
    }
}

/// Map catalog id (`road`, `shallow_water`, …) to protocol view.
pub fn terrain_kind_view_from_id(id: &str) -> Option<TerrainKindView> {
    Some(match id {
        "grass" => TerrainKindView::Grass,
        "dirt" => TerrainKindView::Dirt,
        "tilled" => TerrainKindView::Tilled,
        "desert" => TerrainKindView::Desert,
        "hill" => TerrainKindView::Hill,
        "bog" => TerrainKindView::Bog,
        "beach" => TerrainKindView::Beach,
        "shallow_water" => TerrainKindView::ShallowWater,
        "deep_water" => TerrainKindView::DeepWater,
        "trail" => TerrainKindView::Trail,
        "road" => TerrainKindView::Road,
        "rock" => TerrainKindView::Rock,
        _ => return None,
    })
}

/// Terrain + building raster shared across path queries on the same [`NavWorld`].
///
/// Painting thousands of authored zones onto a full outdoor grid dominated tick cost
/// when every A* rebuilt from scratch. Dynamic circles / z-band filters still apply
/// per query on top of this layer.
#[derive(Debug, Clone)]
pub struct StaticNavPaint {
    pub width: i16,
    pub height: i16,
    pub kind_at: Vec<TerrainKindView>,
    pub elev: Vec<f32>,
    /// Terrain impassable + building footprints with doors cleared.
    pub blocked_geometry: Vec<bool>,
}

/// Static + dynamic obstacles for one path query.
#[derive(Debug, Clone)]
pub struct NavWorld {
    pub world_width_m: f32,
    pub world_height_m: f32,
    pub terrain_zones: Vec<TerrainZoneView>,
    pub z_platforms: Vec<ZPlatformView>,
    pub z_transitions: Vec<ZTransitionView>,
    pub buildings: Vec<BuildingView>,
    pub doors: Vec<DoorView>,
    pub circles: Vec<NavBlockingCircle>,
    /// Kind → speed / impassable from content catalog.
    pub kind_nav: TerrainNavTable,
    /// Extra building-footprint pad (m) BEYOND `PLAYER_RADIUS_M` applied when
    /// painting this world. Hired-worker job-loop A* sets it to seal sub-tile
    /// seams between a building footprint and adjacent road/terrain (a gap the
    /// body can't thread) while broad road corridors stay open. 0.0 for others.
    pub worker_building_clearance_m: f32,
    /// Lazily painted zone/building layer; clones of this world share the Arc.
    static_paint: std::sync::Arc<std::sync::OnceLock<StaticNavPaint>>,
}

impl NavWorld {
    /// Fresh world with an empty static-paint cache (call after mutating geometry).
    pub fn new(
        world_width_m: f32,
        world_height_m: f32,
        terrain_zones: Vec<TerrainZoneView>,
        z_platforms: Vec<ZPlatformView>,
        z_transitions: Vec<ZTransitionView>,
        buildings: Vec<BuildingView>,
        doors: Vec<DoorView>,
        circles: Vec<NavBlockingCircle>,
        kind_nav: TerrainNavTable,
    ) -> Self {
        Self {
            world_width_m,
            world_height_m,
            terrain_zones,
            z_platforms,
            z_transitions,
            buildings,
            doors,
            circles,
            kind_nav,
            worker_building_clearance_m: 0.0,
            static_paint: std::sync::Arc::new(std::sync::OnceLock::new()),
        }
    }

    pub fn elevation_at(&self, x: f32, y: f32) -> f32 {
        terrain_at(&self.terrain_zones, x, y)
            .map(|z| z.elevation)
            .unwrap_or(0.0)
    }

    pub fn terrain_kind_at(&self, x: f32, y: f32) -> TerrainKindView {
        terrain_at(&self.terrain_zones, x, y)
            .map(|z| z.kind)
            .unwrap_or(TerrainKindView::Grass)
    }

    /// Lazily paint / return the static terrain+building raster for this world.
    pub(crate) fn ensure_static_paint(
        &self,
        init: impl FnOnce() -> StaticNavPaint,
    ) -> &StaticNavPaint {
        self.static_paint.get_or_init(init)
    }

    /// Share another world's static paint cache (same terrain/buildings; circles may differ).
    pub fn adopt_static_paint_from(&mut self, other: &NavWorld) {
        self.static_paint = std::sync::Arc::clone(&other.static_paint);
    }

    /// True when the zone/building raster has already been painted.
    pub fn static_paint_ready(&self) -> bool {
        self.static_paint.get().is_some()
    }
}

fn terrain_at(zones: &[TerrainZoneView], x: f32, y: f32) -> Option<&TerrainZoneView> {
    // Match sim `WorldSegment::terrain_zone_at`: highest z_order wins; tie → later list index.
    zones
        .iter()
        .enumerate()
        .filter(|(_, zone)| x >= zone.x0 && x < zone.x1 && y >= zone.y0 && y < zone.y1)
        .max_by(|(ia, a), (ib, b)| a.z_order.cmp(&b.z_order).then(ia.cmp(ib)))
        .map(|(_, z)| z)
}

pub fn terrain_move_speed_mult(kind: TerrainKindView, table: &TerrainNavTable) -> f32 {
    table.get(kind).move_speed_mult.max(0.01)
}

pub fn terrain_is_impassable(kind: TerrainKindView, table: &TerrainNavTable) -> bool {
    table.get(kind).impassable
}

/// A* cell cost for `mode`. Impassable → `u16::MAX` (blocked).
pub fn terrain_cost(kind: TerrainKindView, mode: PathMode, table: &TerrainNavTable) -> u16 {
    if terrain_is_impassable(kind, table) {
        return u16::MAX;
    }
    match mode {
        PathMode::Direct => DEFAULT_COST,
        PathMode::Fastest => {
            let speed = terrain_move_speed_mult(kind, table);
            let c = (DEFAULT_COST as f32 / speed).round();
            c.clamp(1.0, (u16::MAX - 1) as f32) as u16
        }
    }
}

/// Minimum walkable cost for `mode` given zones present (admissible A* scale).
pub fn min_walkable_cost(
    mode: PathMode,
    zones: &[TerrainZoneView],
    table: &TerrainNavTable,
) -> u16 {
    match mode {
        PathMode::Direct => DEFAULT_COST,
        PathMode::Fastest => {
            let mut min_c = DEFAULT_COST;
            for z in zones {
                let c = terrain_cost(z.kind, PathMode::Fastest, table);
                if c != u16::MAX && c < min_c {
                    min_c = c;
                }
            }
            min_c
        }
    }
}

pub fn collides_player_at(x: f32, y: f32, world: &NavWorld) -> bool {
    if world
        .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;
    world.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
}

pub(crate) fn block_circle(
    blocked: &mut [bool],
    width: i16,
    height: i16,
    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 cell_x = ix + dx;
            let cell_y = iy + dy;
            if cell_x < 0 || cell_y < 0 || cell_x >= width || cell_y >= height {
                continue;
            }
            let cell_cx = cell_x as f32 + 0.5;
            let cell_cy = cell_y as f32 + 0.5;
            if (cell_cx - cx).hypot(cell_cy - cy) <= block_r {
                let idx = (cell_y as usize) * (width as usize) + (cell_x as usize);
                blocked[idx] = true;
            }
        }
    }
}

pub(crate) fn mark_building_footprint(
    blocked: &mut [bool],
    width: i16,
    height: i16,
    building: &BuildingView,
    extra_clearance_m: f32,
) {
    // Body-radius pad (worker can't step onto the footprint) plus, when the
    // hire-worker world requests it, a sub-cell seam-clearance pad. The seam
    // between a building footprint and an adjacent road/terrain cell is narrower
    // than the entity; adding the clearance ensures the immediately-adjacent
    // walkable cell center is no longer treated as routable when the gap is too
    // tight to thread. Keep it small so broad road corridors beside buildings
    // (which the player/other world builds leave walkable) are not sealed.
    let pad = PLAYER_RADIUS_M + extra_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 {
            if x >= 0 && y >= 0 && x < width && y < height {
                let idx = (y as usize) * (width as usize) + (x as usize);
                blocked[idx] = true;
            }
        }
    }
}

pub(crate) fn clear_door_cells(blocked: &mut [bool], width: i16, height: i16, doors: &[DoorView]) {
    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 {
                        let cx = x + dx;
                        let cy = y + dy;
                        if cx >= 0 && cy >= 0 && cx < width && cy < height {
                            let idx = (cy as usize) * (width as usize) + (cx as usize);
                            blocked[idx] = false;
                        }
                    }
                }
            }
        }
    }
}

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

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

/// Default nav radius when a placed prop has `blocking: true` but radius 0 in data.
pub const DEFAULT_PLACED_PROP_BLOCK_RADIUS_M: f32 = 0.7;

/// Blocking circles for placed props that have `blocking` on the item template (AOI view).
pub fn circles_from_placed_containers(
    containers: &[flatland_protocol::PlacedContainerView],
) -> Vec<NavBlockingCircle> {
    containers
        .iter()
        .filter(|c| c.blocking)
        .map(|c| {
            let radius_m = if c.blocking_radius_m > 0.0 {
                c.blocking_radius_m
            } else {
                DEFAULT_PLACED_PROP_BLOCK_RADIUS_M
            };
            NavBlockingCircle {
                x: c.x,
                y: c.y,
                radius_m,
            }
        })
        .collect()
}

/// Outdoor travel goal snapped clear of buildings, trees, NPCs, and placed props.
pub fn snap_nav_goal(world: &NavWorld, gx: f32, gy: f32) -> (f32, f32) {
    if !nav_position_blocked(world, gx, gy, PATH_CLEARANCE_M) {
        return (gx, gy);
    }
    for step in 1..=40 {
        let d = step as f32 * 0.35;
        for (dx, dy) in [
            (0.0, -d),
            (0.0, d),
            (d, 0.0),
            (-d, 0.0),
            (d, -d),
            (d, d),
            (-d, -d),
            (-d, d),
        ] {
            let nx = gx + dx;
            let ny = gy + dy;
            if !nav_position_blocked(world, nx, ny, PATH_CLEARANCE_M) {
                return (nx, ny);
            }
        }
    }
    (gx, gy)
}

fn nav_position_blocked(world: &NavWorld, x: f32, y: f32, extra_pad_m: f32) -> bool {
    let body = PLAYER_RADIUS_M + extra_pad_m.max(0.0);
    if world
        .circles
        .iter()
        .any(|o| circle_overlap(x, y, body, o.x, o.y, o.radius_m + extra_pad_m.max(0.0)))
    {
        return true;
    }
    world.buildings.iter().any(|b| {
        let hw = b.width_m / 2.0 + body;
        let hd = b.depth_m / 2.0 + body;
        x >= b.x - hw && x <= b.x + hw && y >= b.y - hd && y <= b.y + hd
    })
}

/// Build blocking circles from protocol views (resource nodes + living NPCs).
pub fn circles_from_views(
    resource_nodes: &[flatland_protocol::ResourceNodeView],
    npcs: &[flatland_protocol::NpcView],
) -> Vec<NavBlockingCircle> {
    let mut circles = Vec::new();
    for node in 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(NavBlockingCircle {
                x: node.x,
                y: node.y,
                radius_m: radius,
            });
        }
    }
    for npc in npcs {
        let alive = npc.life_state != Some(LifeState::Dead) && npc.hp_pct.is_none_or(|h| h > 0.0);
        if alive {
            circles.push(NavBlockingCircle {
                x: npc.x,
                y: npc.y,
                radius_m: 0.55,
            });
        }
    }
    circles
}