flatland-pathfinding 0.2.26

//! Path session — follow waypoints toward a goal (client auto-nav + worker travel steps).

use crate::grid::NavWorld;
use crate::path::find_path_with_goal_z;
use crate::z_nav::{goal_z_for, transition_vertical, Z_LEVEL_TOLERANCE};

pub const WAYPOINT_RADIUS_M: f32 = 0.85;
pub const ARRIVE_RADIUS_M: f32 = 0.75;
const SPRINT_LEG_M: f32 = 4.0;
const STUCK_TICKS_BEFORE_REPLAN: u32 = 6;
const MAX_REPLAN_ATTEMPTS: u32 = 5;

/// Dexterity contribution to move speed (display DEX 1–100 scale).
pub const DEX_MOVE_COEFF: f32 = 0.003;

/// Effective horizontal speed (m/s) for travel ETA and worker movement.
pub fn effective_move_speed_mps(base_speed: f32, dex_display: f32, encumbrance_mult: f32) -> f32 {
    base_speed * encumbrance_mult * (1.0 + DEX_MOVE_COEFF * dex_display.max(0.0))
}

#[derive(Debug, Clone, Copy, PartialEq)]
pub struct PathSteer {
    pub forward: f32,
    pub strafe: f32,
    pub vertical: f32,
    pub sprint: bool,
}

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

impl PathSession {
    pub fn plan(
        world: &NavWorld,
        from_x: f32,
        from_y: f32,
        from_z: f32,
        goal_x: f32,
        goal_y: f32,
    ) -> Option<Self> {
        let goal_z = goal_z_for(world, goal_x, goal_y, from_z);
        Self::plan_with_goal_z(world, from_x, from_y, from_z, goal_x, goal_y, goal_z)
    }

    pub fn plan_with_goal_z(
        world: &NavWorld,
        from_x: f32,
        from_y: f32,
        from_z: f32,
        goal_x: f32,
        goal_y: f32,
        goal_z: f32,
    ) -> Option<Self> {
        let path = find_path_with_goal_z(world, from_x, from_y, from_z, goal_x, goal_y, goal_z)?;
        if path.is_empty() {
            return None;
        }
        Some(Self {
            waypoints: path,
            waypoint_index: 0,
            goal_x,
            goal_y,
            goal_z,
            last_progress_x: from_x,
            last_progress_y: from_y,
            stuck_ticks: 0,
            replan_attempts: 0,
        })
    }

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

    pub fn waypoints(&self) -> &[(f32, f32)] {
        &self.waypoints
    }

    pub fn replan(&mut self, world: &NavWorld, px: f32, py: f32, pz: f32) -> bool {
        if let Some(path) =
            find_path_with_goal_z(world, px, py, pz, self.goal_x, self.goal_y, self.goal_z)
        {
            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
    }

    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
    }

    pub fn steer(&mut self, px: f32, py: f32, pz: f32, world: &NavWorld) -> Option<PathSteer> {
        let vertical_climb = transition_vertical(world, px, py, pz, self.goal_z);
        if vertical_climb.abs() > f32::EPSILON {
            return Some(PathSteer {
                forward: 0.0,
                strafe: 0.0,
                vertical: vertical_climb,
                sprint: false,
            });
        }
        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(PathSteer {
                forward,
                strafe,
                vertical: 0.0,
                sprint,
            });
        }
        let goal_dist = (self.goal_x - px).hypot(self.goal_y - py);
        if goal_dist > ARRIVE_RADIUS_M || (pz - self.goal_z).abs() > Z_LEVEL_TOLERANCE {
            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(PathSteer {
                    forward,
                    strafe,
                    vertical: 0.0,
                    sprint: goal_dist > SPRINT_LEG_M,
                });
            }
            let vz = transition_vertical(world, px, py, pz, self.goal_z);
            if vz.abs() > f32::EPSILON {
                return Some(PathSteer {
                    forward: 0.0,
                    strafe: 0.0,
                    vertical: vz,
                    sprint: false,
                });
            }
        }
        None
    }

    /// Remaining path length in meters (rough estimate).
    pub fn remaining_distance_m(&self, px: f32, py: f32) -> f32 {
        let mut total = 0.0;
        let mut last = (px, py);
        if self.waypoint_index < self.waypoints.len() {
            for &(wx, wy) in &self.waypoints[self.waypoint_index..] {
                total += (wx - last.0).hypot(wy - last.1);
                last = (wx, wy);
            }
        }
        total + (self.goal_x - last.0).hypot(self.goal_y - last.1)
    }
}

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

    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 replan_gives_up_when_goal_becomes_unreachable() {
        let world = open_world();
        let mut session =
            PathSession::plan_with_goal_z(&world, 10.0, 10.0, 0.0, 20.0, 10.0, 0.0).expect("path");
        // Wall off the goal after the first plan.
        let mut blocked = open_world();
        blocked.terrain_zones.push(flatland_protocol::TerrainZoneView {
            id: "wall".into(),
            x0: 14.0,
            y0: 0.0,
            x1: 18.0,
            y1: 64.0,
            kind: flatland_protocol::TerrainKindView::DeepWater,
            elevation: 0.0,
            glyph: None,
            color: None,
            tile_id: None,
            z_order: 0,
        });
        let mut gave_up = false;
        for _ in 0..MAX_REPLAN_ATTEMPTS + 2 {
            if !session.replan(&blocked, 10.0, 10.0, 0.0) {
                gave_up = true;
                break;
            }
        }
        assert!(gave_up, "replan must eventually return false for unreachable goals");
    }
}