hierarchical_pathfinding 0.5.0

use super::{Cost, Path, PathSegment};
use crate::{grid, neighbors::Neighborhood, Point};

/// A Path that may not be fully calculated yet.
///
/// This struct represents a Path that was generated by the PathCache. Since
/// [`config.cache_paths`](crate::PathCacheConfig::cache_paths) may be set to false, there might by
/// segments of this Path that are not yet calculated. In those cases, since the cost function is
/// required to resolve those sections, it is necessary to call
/// [`safe_next()`](AbstractPath::safe_next) or [`resolve()`](AbstractPath::resolve).
/// Otherwise it is possible to treat this as an Iterator<Point>.
///
/// **Warning: Calling `next()` on an AbstractPath with unknown segments will panic as soon as those
/// segments are reached.**
///
/// **Warning: Keeping an AbstractPath after changing the Grid, or using a different cost function,
/// leads to panics and wrong results.**
///
/// **You have been warned**
#[derive(Debug, Clone)]
pub struct AbstractPath<N: Neighborhood> {
    neighborhood: N,
    total_cost: Cost,
    total_length: usize,
    path: Vec<PathSegment>,
    end: Point,
    current_index: (usize, usize),
    steps_taken: usize,
}

impl<N: Neighborhood> AbstractPath<N> {
    /// Returns the total cost of this Path.
    /// This value is always known and requires no further calculations.
    pub fn cost(&self) -> Cost {
        self.total_cost
    }

    /// Returns the total length of this Path.
    ///
    /// This value is always known and requires no further calculations.
    ///
    /// Note that the [`len`](AbstractPath::len) method is provided by [`ExactSizeIterator`] and tells
    /// the remaining elements, whereas the one stays constant.
    pub fn length(&self) -> usize {
        self.total_length
    }

    /// A variant of [`Iterator::next()`](#impl-Iterator) that can resolve unknown segments
    /// of the Path. Use this method instead of `next()` when
    /// [`config.cache_paths`](crate::PathCacheConfig::cache_paths) is set to `false`.
    pub fn safe_next(&mut self, get_cost: impl FnMut(Point) -> isize) -> Option<Point> {
        self.internal_next(Some(get_cost))
    }
    fn internal_next<F: FnMut(Point) -> isize>(&mut self, get_cost: Option<F>) -> Option<Point> {
        if self.current_index.0 >= self.path.len() {
            return None;
        }
        let mut current = &self.path[self.current_index.0];
        if let PathSegment::Unknown { start, end, .. } = *current {
            let path = grid::a_star_search(
                &self.neighborhood,
                |_| true,
                get_cost.expect("Tried calling next() on a Path that is not fully known. Use safe_next() instead."),
                start,
                end,
                self.neighborhood.heuristic(start, end) * 2
            )
            .unwrap_or_else(|| {
                panic!(
                    "Impossible Path marked as Possible: {:?} -> {:?}",
                    start, end
                )
            });

            self.path[self.current_index.0] = PathSegment::Known(path);
            current = &self.path[self.current_index.0];

            self.current_index.1 = 1; // paths include start and end, but we are already at start
        }

        let path = match current {
            PathSegment::Known(path) => path,
            PathSegment::Unknown { .. } => {
                unreachable!()
            }
        };

        let ret = Some(path[self.current_index.1]);
        self.current_index.1 += 1;
        if self.current_index.1 >= path.len() {
            self.current_index.0 += 1;
            self.current_index.1 = 1;
        }

        self.steps_taken += 1;

        ret
    }

    /// Resolves all unknown sections of the Path.
    ///
    /// if [`config.cache_paths`](crate::PathCacheConfig::cache_paths) is set to true,
    /// then calling this method is similar to calling `path.collect::<Vec<_>>()`.
    pub fn resolve(mut self, mut get_cost: impl FnMut(Point) -> isize) -> Vec<Point> {
        let mut result = Vec::with_capacity(self.len());

        while let Some(pos) = self.safe_next(&mut get_cost) {
            result.push(pos);
        }
        result
    }

    pub(crate) fn new(neighborhood: N, end: Point) -> AbstractPath<N> {
        AbstractPath {
            neighborhood,
            total_cost: 0,
            total_length: 0,
            path: vec![],
            end,
            current_index: (0, 1),
            steps_taken: 0,
        }
    }

    pub(crate) fn from_known_path(neighborhood: N, path: Path<Point>) -> AbstractPath<N> {
        let end = path[path.len() - 1];
        AbstractPath {
            total_cost: path.cost(),
            total_length: path.len() - 1,
            path: vec![PathSegment::Known(path)],
            ..AbstractPath::new(neighborhood, end)
        }
    }

    pub(crate) fn add_path_segment(&mut self, path: PathSegment) -> &mut Self {
        assert!(
            self.end == path.start(),
            "Added disconnected PathSegment: expected {:?}, got {:?}",
            self.end,
            path.start()
        );
        self.total_cost += path.cost();
        self.total_length += path.len() - 1;
        self.end = path.end();
        self.path.push(path);
        self
    }

    pub(crate) fn add_path(&mut self, path: Path<Point>) -> &mut Self {
        assert!(
            self.end == path[0],
            "Added disconnected Path: expected {:?}, got {:?}",
            self.end,
            path[0]
        );
        self.total_cost += path.cost();
        self.total_length += path.len() - 1;
        self.end = path[path.len() - 1];
        self.path.push(PathSegment::Known(path));
        self
    }

    #[allow(dead_code)]
    pub(crate) fn add_node(&mut self, node: Point, cost: Cost, len: usize) -> &mut Self {
        self.path.push(PathSegment::Unknown {
            start: self.end,
            end: node,
            cost,
            len,
        });
        self.total_cost += cost;
        self.total_length += len;
        self.end = node;
        self
    }
}

impl<N: Neighborhood> Iterator for AbstractPath<N> {
    type Item = Point;
    /// See [`Iterator::next`]
    ///
    /// ## Panics
    /// Panics if a segment of the Path is not known because [`config.cache_paths`](crate::PathCacheConfig::cache_paths)
    /// is set to `false`. Use [`safe_next`](AbstractPath::safe_next) in those cases.
    fn next(&mut self) -> Option<Point> {
        self.internal_next::<fn((usize, usize)) -> isize>(None)
    }
    fn size_hint(&self) -> (usize, Option<usize>) {
        let len = self.total_length - self.steps_taken;
        (len, Some(len))
    }
    fn count(self) -> usize {
        self.len()
    }
    fn last(self) -> Option<Point> {
        Some(self.end)
    }
    fn nth(&mut self, step: usize) -> Option<Point> {
        self.current_index.1 += step;
        while self.current_index.0 < self.path.len() {
            let current_len = self.path[self.current_index.0].len();
            if self.current_index.1 < current_len {
                break;
            } else {
                self.current_index.1 -= current_len - 1;
                self.current_index.0 += 1;
            }
        }
        self.next()
    }
}
impl<N: Neighborhood> ExactSizeIterator for AbstractPath<N> {}
impl<N: Neighborhood> std::iter::FusedIterator for AbstractPath<N> {}

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

    #[test]
    fn next() {
        let neigh = crate::neighbors::ManhattanNeighborhood::new(100, 100);
        let mut path = AbstractPath::from_known_path(
            neigh,
            Path::from_slice(&[(99, 99), (0, 0), (1, 1), (2, 2), (3, 3)], 3),
        );
        path.add_path(Path::from_slice(&[(3, 3), (4, 4), (5, 5)], 5));
        assert_eq!(path.next(), Some((0, 0)));
        assert_eq!(path.next(), Some((1, 1)));
        assert_eq!(path.next(), Some((2, 2)));
        assert_eq!(path.next(), Some((3, 3)));
        assert_eq!(path.next(), Some((4, 4)));
        assert_eq!(path.next(), Some((5, 5)));
        assert_eq!(path.next(), None);
        assert_eq!(path.next(), None);
        assert_eq!(path.next(), None);
    }

    #[test]
    fn nth() {
        let neigh = crate::neighbors::ManhattanNeighborhood::new(100, 100);
        let mut path = AbstractPath::from_known_path(
            neigh,
            Path::from_slice(
                &[(99, 99), (0, 0), (1, 1), (2, 2), (3, 3), (4, 4), (5, 5)],
                3,
            ),
        );
        {
            let mut path = path.clone();
            assert_eq!(path.nth(3), Some((3, 3)));
            assert_eq!(path.next(), Some((4, 4)));
            assert_eq!(path.nth(1), None);
        }

        path.add_path(Path::from_slice(&[(5, 5), (6, 6), (7, 7)], 5));

        {
            let mut path = path.clone();
            assert_eq!(path.nth(3), Some((3, 3)));
            assert_eq!(path.nth(3), Some((7, 7)));
            assert_eq!(path.nth(3), None);
        }
        {
            let mut path = path.clone();
            assert_eq!(path.nth(7), Some((7, 7)));
        }
        assert_eq!(path.nth(100), None);
    }

    #[test]
    fn size_hint_and_len() {
        let neigh = crate::neighbors::ManhattanNeighborhood::new(100, 100);
        let mut path = AbstractPath::from_known_path(
            neigh,
            Path::from_slice(&[(99, 99), (0, 0), (1, 1), (2, 2), (3, 3)], 3),
        );
        path.add_path(Path::from_slice(&[(3, 3), (4, 4), (5, 5)], 5));
        assert_eq!(path.size_hint(), (6, Some(6)));
        assert_eq!(path.len(), 6);
        assert_eq!(path.len(), path.length());
        assert_eq!(path.next(), Some((0, 0)));

        assert_eq!(path.size_hint(), (5, Some(5)));
        assert_eq!(path.len(), 5);
        assert_eq!(path.next(), Some((1, 1)));

        assert_eq!(path.size_hint(), (4, Some(4)));
        assert_eq!(path.len(), 4);
        assert_eq!(path.next(), Some((2, 2)));

        assert_eq!(path.size_hint(), (3, Some(3)));
        assert_eq!(path.len(), 3);
        assert_eq!(path.next(), Some((3, 3)));

        assert_eq!(path.size_hint(), (2, Some(2)));
        assert_eq!(path.len(), 2);
        assert_eq!(path.next(), Some((4, 4)));

        assert_eq!(path.size_hint(), (1, Some(1)));
        assert_eq!(path.len(), 1);
        assert_eq!(path.next(), Some((5, 5)));

        assert_eq!(path.size_hint(), (0, Some(0)));
        assert_eq!(path.len(), 0);
        assert_eq!(path.next(), None);

        assert_eq!(path.len(), 0);
        assert_eq!(path.size_hint(), (0, Some(0)));
    }

    #[test]
    fn count_and_last() {
        let neigh = crate::neighbors::ManhattanNeighborhood::new(100, 100);
        let mut path = AbstractPath::from_known_path(
            neigh,
            Path::from_slice(&[(99, 99), (0, 0), (1, 1), (2, 2), (3, 3)], 3),
        );
        path.add_path(Path::from_slice(&[(3, 3), (4, 4), (5, 5)], 5));

        let mut cnt = 0;
        let mut last = (99, 99);
        for p in path.clone() {
            cnt += 1;
            last = p;
        }
        assert_eq!(path.clone().count(), cnt);
        assert_eq!(path.last(), Some(last));
    }

    #[test]
    fn disconnected_segment_on_long_paths() {
        use crate::prelude::*;
        for w in (1..5).map(|w| w * 23) {
            println!("w: {:?}", w);
            let pathfinding = PathCache::new(
                (w, w),
                |_| 1,
                MooreNeighborhood::new(w, w),
                Default::default(),
            );
            let path = pathfinding.find_path((0, 0), (w - 1, w - 1), |_| 1);
            assert!(path.is_some());

            let pathfinding = crate::PathCache::new(
                (w, w),
                |_| 1,
                ManhattanNeighborhood::new(w, w),
                Default::default(),
            );
            let path = pathfinding.find_path((0, 0), (w - 1, w - 1), |_| 1);
            assert!(path.is_some());
        }
    }
}