condor-pathfinding-grid 0.4.0

//! Dynamic-grid [`GridReplanner`]: Lifelong Planning A* (LPA*).
//!
//! It owns a grid snapshot and request after `initialize`, marks changes through
//! `update_cell` / `update_cost`, then resumes its priority queue at `replan`. Results
//! use the standard discrete invalid/found/no-path contract with per-cell
//! [`traversal_cost`](crate::Grid::traversal_cost), Manhattan keys, and one-step
//! lookahead. Prefer the product-facing
//! [`crate::algorithms::d_star_lite::DStarLite`] entrypoint unless direct LPA*
//! identity is required.
//!
//! Implementation notes: the open set is a **lazy** binary heap (stale keys skipped on pop).
//! Consistent vertices (`g == rhs`) must not re-enter the underconsistent branch. Incomplete
//! compute (iteration cap) forces a cold `initialize` on replan—paths are never reconstructed
//! from a non-converged state.

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

use crate::{
    grid::{Cell, Grid, GridEditError},
    path::Path,
    point::Point,
    replanning::GridReplanner,
    search::{SearchRequest, SearchResult},
};

/// Incremental LPA* [`GridReplanner`] for dynamic weighted grids.
///
/// Owns a grid copy and request; updates mark vertices, `replan` resumes the lazy
/// heap. Backing engine for [`super::d_star_lite::DStarLite`]. Incomplete compute
/// (iteration cap) forces a cold re-initialize — never reconstructs from non-converged state.
pub struct LifelongPlanningAStar {
    initialized: bool,
    grid: Grid,
    request: SearchRequest,
    /// g-values as f64 for LPA* keys (typical grid costs stay exact below 2^53).
    g_costs: Vec<f64>,
    rhs_costs: Vec<f64>,
    queue: BinaryHeap<PriorityEntry>,
    visited_nodes: usize,
}

/// Result of the main LPA* compute loop.
#[derive(Debug, Clone, Copy, PartialEq, Eq)]
enum ComputeOutcome {
    /// Queue drained or top-key ≥ goal-key with consistent goal.
    Converged,
    /// Safety cap hit; search is incomplete (must not return soft success).
    HitIterationCap,
}

impl Default for LifelongPlanningAStar {
    fn default() -> Self {
        Self::new()
    }
}

impl LifelongPlanningAStar {
    /// Creates an uninitialized replanner; call [`GridReplanner::initialize`] before replan.
    #[must_use]
    pub fn new() -> Self {
        Self {
            initialized: false,
            grid: Grid::new(1, 1).expect("grid dimensions are valid"),
            request: SearchRequest::new(Point::new(0, 0), Point::new(0, 0)),
            g_costs: Vec::new(),
            rhs_costs: Vec::new(),
            queue: BinaryHeap::new(),
            visited_nodes: 0,
        }
    }

    fn calculate_key(&self, p: Point) -> [f64; 2] {
        let idx = self.grid.index_of(p).unwrap();
        let g = self.g_costs[idx];
        let rhs = self.rhs_costs[idx];
        let min = g.min(rhs);
        [min + self.heuristic(p), min]
    }

    fn heuristic(&self, p: Point) -> f64 {
        (p.x as f64 - self.request.goal.x as f64).abs()
            + (p.y as f64 - self.request.goal.y as f64).abs()
    }

    fn search_state_is_ready(&self) -> bool {
        let cell_count = self.grid.cell_count();
        self.g_costs.len() == cell_count && self.rhs_costs.len() == cell_count
    }

    fn update_vertex(&mut self, p: Point) {
        if !self.search_state_is_ready() {
            return;
        }
        let Some(idx) = self.grid.index_of(p) else {
            return;
        };
        if !self.grid.is_walkable(p) {
            self.rhs_costs[idx] = f64::INFINITY;
        } else if p == self.request.start {
            self.rhs_costs[idx] = 0.0;
        } else {
            let mut min_rhs = f64::INFINITY;
            let cost = self
                .grid
                .traversal_cost(p)
                .expect("walkable point has cost") as f64;
            for pred in self.grid.neighbors4(p) {
                let pred_idx = self.grid.index_of(pred).unwrap();
                min_rhs = min_rhs.min(self.g_costs[pred_idx] + cost);
            }
            self.rhs_costs[idx] = min_rhs;
        }

        if self.g_costs[idx] != self.rhs_costs[idx] {
            self.queue.push(PriorityEntry {
                point: p,
                key: self.calculate_key(p),
            });
        }
    }

    fn search_converged(&self) -> bool {
        if !self.search_state_is_ready() {
            return false;
        }
        let Some(goal_idx) = self.grid.index_of(self.request.goal) else {
            return false;
        };
        if self.g_costs[goal_idx] != self.rhs_costs[goal_idx] {
            return false;
        }
        match self.queue.peek() {
            None => true,
            Some(top) => top.key >= self.calculate_key(self.request.goal),
        }
    }

    fn compute_shortest_path(&mut self) -> ComputeOutcome {
        if !self.search_state_is_ready() {
            return ComputeOutcome::Converged;
        }
        let Some(goal_idx) = self.grid.index_of(self.request.goal) else {
            return ComputeOutcome::Converged;
        };

        let max_iterations = self.grid.cell_count().saturating_mul(64).max(4_096);
        let mut iterations = 0usize;

        while let Some(top) = self.queue.peek() {
            let top_key = top.key;
            let goal_key = self.calculate_key(self.request.goal);

            if top_key >= goal_key && self.rhs_costs[goal_idx] == self.g_costs[goal_idx] {
                return ComputeOutcome::Converged;
            }

            let entry = self.queue.pop().unwrap();
            let u = entry.point;
            let u_idx = self.grid.index_of(u).unwrap();

            if entry.key != self.calculate_key(u) {
                continue;
            }
            if self.g_costs[u_idx] == self.rhs_costs[u_idx] {
                continue;
            }

            iterations += 1;
            if iterations > max_iterations {
                return ComputeOutcome::HitIterationCap;
            }

            self.visited_nodes += 1;

            if self.g_costs[u_idx] > self.rhs_costs[u_idx] {
                // Overconsistent: g ← rhs, then refresh successors.
                self.g_costs[u_idx] = self.rhs_costs[u_idx];
                for s in self.grid.neighbors4(u) {
                    self.update_vertex(s);
                }
            } else {
                // Underconsistent: g ← ∞, then re-evaluate u and successors.
                self.g_costs[u_idx] = f64::INFINITY;
                self.update_vertex(u);
                for s in self.grid.neighbors4(u) {
                    self.update_vertex(s);
                }
            }
        }

        ComputeOutcome::Converged
    }

    fn finish_search(&mut self, outcome: ComputeOutcome) -> SearchResult {
        if !self.initialized {
            return crate::search::not_found(self.visited_nodes);
        }
        crate::search::validate_request(&self.grid, self.request)?;
        let Some(goal_idx) = self.grid.index_of(self.request.goal) else {
            unreachable!("validated goal has a grid index");
        };
        if !self.search_state_is_ready() {
            return crate::search::not_found(self.visited_nodes);
        }

        if outcome == ComputeOutcome::HitIterationCap || !self.search_converged() {
            return crate::search::not_found(self.visited_nodes);
        }

        if self.g_costs[goal_idx] == f64::INFINITY {
            return crate::search::not_found(self.visited_nodes);
        }

        let Some(path) = self.reconstruct_path() else {
            return crate::search::not_found(self.visited_nodes);
        };

        crate::search::found(path, self.visited_nodes)
    }

    fn reconstruct_path(&self) -> Option<Path> {
        if !self.search_state_is_ready() {
            return None;
        }
        if !self.grid.is_walkable(self.request.start) || !self.grid.is_walkable(self.request.goal) {
            return None;
        }

        let mut steps = Vec::new();
        let mut current = self.request.goal;
        steps.push(current);
        let mut seen = BTreeSet::from([current]);

        let mut total_cost: usize = 0;

        while current != self.request.start {
            let mut best_neighbor = None;
            let mut min_candidate_cost = f64::INFINITY;
            let step_cost_raw = self.grid.traversal_cost(current)?;
            let step_cost = step_cost_raw as f64;

            for neighbor in self.grid.neighbors4(current) {
                let neighbor_idx = self.grid.index_of(neighbor).unwrap();
                let candidate_cost = self.g_costs[neighbor_idx] + step_cost;
                if candidate_cost < min_candidate_cost {
                    min_candidate_cost = candidate_cost;
                    best_neighbor = Some(neighbor);
                }
            }

            let next = best_neighbor?;
            if !seen.insert(next) {
                return None;
            }
            total_cost = total_cost.saturating_add(step_cost_raw);
            current = next;
            steps.push(current);
        }
        steps.reverse();
        Some(
            Path::from_steps_with_cost(steps, total_cost)
                .expect("path contains at least one point"),
        )
    }
}

impl GridReplanner for LifelongPlanningAStar {
    fn name(&self) -> &'static str {
        "lpa-star"
    }

    fn initialize(&mut self, grid: &Grid, request: SearchRequest) -> SearchResult {
        crate::search::validate_request(grid, request)?;
        self.initialized = true;
        self.grid = grid.clone();
        self.request = request;
        self.g_costs.clear();
        self.rhs_costs.clear();
        self.queue.clear();
        self.visited_nodes = 0;

        let start_idx = self
            .grid
            .index_of(request.start)
            .expect("validated start has a grid index");
        let n = self.grid.cell_count();
        self.g_costs = vec![f64::INFINITY; n];
        self.rhs_costs = vec![f64::INFINITY; n];

        self.rhs_costs[start_idx] = 0.0;
        if request.start == request.goal {
            self.g_costs[start_idx] = 0.0;
            return crate::search::found(
                Path::from_steps(vec![request.start]).expect("path contains at least one point"),
                1,
            );
        }

        self.queue.push(PriorityEntry {
            point: request.start,
            key: self.calculate_key(request.start),
        });

        let outcome = self.compute_shortest_path();
        self.finish_search(outcome)
    }

    fn update_cell(&mut self, point: Point, cell: Cell) {
        if self.grid.set_cell(point, cell).is_err() {
            return;
        }
        self.update_vertex(point);
        for neighbor in self.grid.neighbors4(point) {
            self.update_vertex(neighbor);
        }
    }

    fn update_cost(&mut self, point: Point, cost: usize) -> Result<(), GridEditError> {
        self.grid.set_traversal_cost(point, cost)?;
        self.update_vertex(point);
        for neighbor in self.grid.neighbors4(point) {
            self.update_vertex(neighbor);
        }
        Ok(())
    }

    fn replan(&mut self) -> SearchResult {
        if !self.initialized {
            return crate::search::not_found(0);
        }
        self.visited_nodes = 0;
        let outcome = self.compute_shortest_path();
        let result = self.finish_search(outcome);
        if result.as_ref().is_ok_and(|outcome| outcome.is_found()) {
            return result;
        }

        let grid = self.grid.clone();
        let request = self.request;
        self.initialize(&grid, request)
    }
}

#[derive(Debug)]
struct PriorityEntry {
    point: Point,
    key: [f64; 2],
}

impl PartialEq for PriorityEntry {
    fn eq(&self, other: &Self) -> bool {
        self.key == other.key
    }
}

impl Eq for PriorityEntry {}

impl Ord for PriorityEntry {
    fn cmp(&self, other: &Self) -> Ordering {
        for i in 0..2 {
            if self.key[i] < other.key[i] {
                return Ordering::Greater;
            }
            if self.key[i] > other.key[i] {
                return Ordering::Less;
            }
        }
        Ordering::Equal
    }
}

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

#[cfg(test)]
mod tests {
    use super::LifelongPlanningAStar;
    use crate::{Cell, Grid, GridSearchError, Point, SearchRequest, replanning::GridReplanner};

    #[test]
    fn blocking_the_only_bridge_cell_removes_the_path() {
        let grid = Grid::new(3, 1).expect("grid dimensions are valid");
        let request = SearchRequest::new(Point::new(0, 0), Point::new(2, 0));
        let mut replanner = LifelongPlanningAStar::new();

        let initial = replanner.initialize(&grid, request);
        assert!(initial.as_ref().expect("valid search request").is_found());
        assert_eq!(
            initial.as_ref().expect("valid search request").cost(),
            Some(2)
        );

        replanner.update_cell(Point::new(1, 0), Cell::Blocked);
        let repaired = replanner.replan();
        assert!(!repaired.as_ref().expect("valid search request").is_found());
    }

    #[test]
    fn blocking_the_goal_removes_the_path() {
        let grid = Grid::new(3, 1).expect("grid dimensions are valid");
        let request = SearchRequest::new(Point::new(0, 0), Point::new(2, 0));
        let mut replanner = LifelongPlanningAStar::new();

        let initial = replanner.initialize(&grid, request);
        assert!(initial.as_ref().expect("valid search request").is_found());

        replanner.update_cell(Point::new(2, 0), Cell::Blocked);
        let repaired = replanner.replan();
        assert_eq!(
            repaired,
            Err(GridSearchError::InvalidGoal {
                point: Point::new(2, 0),
            })
        );
    }

    #[test]
    fn reopening_the_start_restores_the_path() {
        let grid = Grid::new(3, 1).expect("grid dimensions are valid");
        let request = SearchRequest::new(Point::new(0, 0), Point::new(2, 0));
        let mut replanner = LifelongPlanningAStar::new();

        let initial = replanner.initialize(&grid, request);
        assert!(initial.as_ref().expect("valid search request").is_found());

        replanner.update_cell(Point::new(0, 0), Cell::Blocked);
        let blocked = replanner.replan();
        assert_eq!(
            blocked,
            Err(GridSearchError::InvalidStart {
                point: Point::new(0, 0),
            })
        );

        replanner.update_cell(Point::new(0, 0), Cell::Open);
        let reopened = replanner.replan();
        assert!(reopened.as_ref().expect("valid search request").is_found());
        assert_eq!(
            reopened.as_ref().expect("valid search request").cost(),
            Some(2)
        );
    }

    #[test]
    fn unblocking_a_start_blocked_at_init_recovers_via_update_then_replan() {
        let mut grid = Grid::new(3, 1).expect("grid dimensions are valid");
        grid.set_cell(Point::new(0, 0), Cell::Blocked)
            .expect("valid grid edit");
        let request = SearchRequest::new(Point::new(0, 0), Point::new(2, 0));
        let mut replanner = LifelongPlanningAStar::new();

        let initial = replanner.initialize(&grid, request);
        assert_eq!(
            initial,
            Err(GridSearchError::InvalidStart {
                point: Point::new(0, 0),
            })
        );

        grid.set_cell(Point::new(0, 0), Cell::Open)
            .expect("valid grid edit");
        let recovered = replanner.initialize(&grid, request);
        assert!(recovered.as_ref().expect("valid search request").is_found());
        assert_eq!(
            recovered.as_ref().expect("valid search request").cost(),
            Some(2)
        );
    }

    #[test]
    fn unblocking_a_goal_blocked_at_init_recovers_via_update_then_replan() {
        let mut grid = Grid::new(3, 1).expect("grid dimensions are valid");
        grid.set_cell(Point::new(2, 0), Cell::Blocked)
            .expect("valid grid edit");
        let request = SearchRequest::new(Point::new(0, 0), Point::new(2, 0));
        let mut replanner = LifelongPlanningAStar::new();

        let initial = replanner.initialize(&grid, request);
        assert_eq!(
            initial,
            Err(GridSearchError::InvalidGoal {
                point: Point::new(2, 0),
            })
        );

        grid.set_cell(Point::new(2, 0), Cell::Open)
            .expect("valid grid edit");
        let recovered = replanner.initialize(&grid, request);
        assert!(recovered.as_ref().expect("valid search request").is_found());
        assert_eq!(
            recovered.as_ref().expect("valid search request").cost(),
            Some(2)
        );
    }

    #[test]
    fn reports_saturated_cost_on_large_traversal_cost() {
        let mut grid = Grid::new(3, 1).expect("grid dimensions are valid");
        assert_eq!(
            grid.set_traversal_cost(Point::new(1, 0), usize::MAX),
            Ok(())
        );

        let request = SearchRequest::new(Point::new(0, 0), Point::new(2, 0));
        let mut replanner = LifelongPlanningAStar::new();

        let initial = replanner.initialize(&grid, request);
        assert!(initial.as_ref().expect("valid search request").is_found());
        assert_eq!(
            initial.as_ref().expect("valid search request").cost(),
            Some(usize::MAX)
        );
        assert_eq!(
            initial
                .as_ref()
                .expect("valid search request")
                .path()
                .expect("path should exist")
                .cost(),
            usize::MAX
        );
    }

    #[test]
    fn cold_init_open_field_with_mid_wall_matches_astar() {
        use crate::{AStar, Pathfinder};

        let mut grid = Grid::new(24, 24).expect("grid");
        for k in 0..3 {
            grid.set_cell(Point::new(12, 8 + k), Cell::Blocked)
                .expect("valid grid edit");
        }
        let request = SearchRequest::new(Point::new(2, 9), Point::new(20, 9));
        let mut lpa = LifelongPlanningAStar::new();
        let cold = lpa.initialize(&grid, request);
        let astar = AStar.search(&grid, request);
        assert!(
            cold.as_ref().expect("valid search request").is_found()
                && astar.as_ref().expect("valid search request").is_found()
        );
        assert_eq!(
            cold.as_ref().expect("valid search request").cost(),
            astar.as_ref().expect("valid search request").cost()
        );
        let cap = grid.cell_count().saturating_mul(64);
        assert!(
            cold.as_ref()
                .expect("valid search request")
                .stats()
                .visited_nodes
                < cap,
            "must not thrash to iteration cap (visited={})",
            cold.as_ref()
                .expect("valid search request")
                .stats()
                .visited_nodes
        );
    }

    #[test]
    fn incremental_replan_after_local_wall_matches_astar() {
        use crate::{AStar, Pathfinder};

        let mut grid = Grid::new(32, 32).expect("grid");
        let request = SearchRequest::new(Point::new(2, 16), Point::new(28, 16));
        let mut lpa = LifelongPlanningAStar::new();
        let init = lpa.initialize(&grid, request);
        assert!(init.as_ref().expect("valid search request").is_found());
        assert_eq!(
            init.as_ref().expect("valid search request").cost(),
            AStar
                .search(&grid, request)
                .as_ref()
                .expect("valid search request")
                .cost()
        );

        for k in 0..5 {
            let p = Point::new(16, 14 + k);
            grid.set_cell(p, Cell::Blocked).expect("valid grid edit");
            lpa.update_cell(p, Cell::Blocked);
        }
        let repaired = lpa.replan();
        let restart = AStar.search(&grid, request);
        assert!(
            repaired.as_ref().expect("valid search request").is_found()
                && restart.as_ref().expect("valid search request").is_found()
        );
        assert_eq!(
            repaired.as_ref().expect("valid search request").cost(),
            restart.as_ref().expect("valid search request").cost()
        );
        let cap = grid.cell_count().saturating_mul(64);
        assert!(
            repaired
                .as_ref()
                .expect("valid search request")
                .stats()
                .visited_nodes
                < cap
        );
    }
}