hierarchical_pathfinding 0.3.6

use super::path_segment::{PathSegment, PathSegment::*};
use crate::{
	generics::{grid::a_star_search, Cost, Path},
	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`](super::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 undefined behavior and panics.**
///
/// **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),
}

#[allow(dead_code)]
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.
	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`](super::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(&mut self, get_cost: Option<impl FnMut(Point) -> isize>) -> Option<Point> {
		if self.current_index.0 >= self.path.len() {
			return None;
		}
		let mut current = &self.path[self.current_index.0];
		if let Unknown { start, end, .. } = *current {
			let path = a_star_search(
				|p| self.neighborhood.get_all_neighbors(p),
				get_cost.expect("Tried calling next() on a Path that is not fully known. Use safe_next() instead."),
				start,
				end,
				|p| self.neighborhood.heuristic(p, end),
			)
			.unwrap_or_else(|| {
				panic!(
					"Impossible Path marked as Possible: {:?} -> {:?}",
					start, end
				)
			});

			self.path[self.current_index.0] = 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 {
			Known(path) => path,
			Unknown { .. } => panic!("Internal Error #1 in AbstractPath. Please report this"),
		};

		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;
		}

		// detect an early finish, since Node Approximation might cause 2 unnecessary Steps
		if ret == Some(self.end) {
			self.current_index.0 = self.path.len();
		}

		ret
	}

	/// Resolves all unknown sections of the Path.
	///
	/// if [`config.cache_paths`](super::PathCacheConfig::cache_paths) is set to true,
	/// then calling this method is similar to calling `path.collect::<Vec<Point>>()`.
	///
	/// The return value is a [`Path`] from the [`generics`](crate::generics) module, which is
	/// essentially a Vec<Point>, but with a `cost` member, since this path is consumed by
	/// `resolve`
	pub fn resolve(mut self, mut get_cost: impl FnMut(Point) -> isize) -> Path<Point> {
		let mut result = Vec::with_capacity(self.total_length);

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

		Path::new(result, self.total_cost)
	}

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

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

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

	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();
		self.end = path.end();
		self.path.push(path);
		self
	}

	pub(crate) fn add_path(&mut self, path: Path<Point>) -> &mut Self {
		self.total_cost += path.cost();
		self.total_length += path.len();
		self.end = path[path.len() - 1];
		self.path.push(Known(path));
		self
	}

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

	pub(crate) fn set_goal(&mut self, goal: Point) {
		self.end = goal;
	}
}

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`](super::PathCacheConfig::cache_paths)
	/// is set to false. Use [`safe_next`](AbstractPath::safe_next) in those cases.
	fn next(&mut self) -> Option<Point> {
		// this is necessary because with just 'self.internal_next(None)' the compiler can't infer the type of 'impl FnMut...'
		#[allow(unused_assignments)]
		let mut arg = Some(dummy_cost_fn);
		arg = None;
		self.internal_next(arg)
	}
}

fn dummy_cost_fn(_: Point) -> isize {
	0
}