axiolid-route 0.3.6

//! Many-target shortest paths and the farthest point of a subregion (#186).
//!
//! # The map
//!
//! [`DistanceMap`] runs one Dijkstra from every target at once over the
//! visibility graph, so each graph vertex knows its distance to the nearest
//! target and the next vertex on the way. A query point then needs only the
//! vertices it sees: its distance is the least `|x - v| + d(v)` over them.
//! Visibility is decided exactly, as for [`crate::shortest_path`]; lengths
//! are sums of square roots in binary64.
//!
//! # The farthest point, bracketed
//!
//! The distance `D` to the nearest target is 1-Lipschitz along any segment
//! that stays in the free space: `D(y) <= D(a) + |y - a|`. So the free space
//! is cut into triangles that each lie inside it -- a constrained
//! triangulation whose constraints are every wall and barrier -- and on a
//! triangle `T` with a point `a` of known distance,
//!
//! ```text
//! max over T of D  <=  D(a) + (greatest distance from a to a corner of T).
//! ```
//!
//! Every distance evaluated at a point of the subregion is a lower bound on
//! the maximum. Triangles are split at their edge midpoints, the one with
//! the greatest upper bound first, and dropped once their upper bound falls
//! below the best lower one, until the two are within the tolerance or the
//! cell budget runs out. The result is an interval that contains the true
//! maximum: widened by the rounding of the lengths, and of the midpoints,
//! which leave gaps between cells no wider than a few ulps.
//!
//! Anchors -- the points whose distance bounds a cell -- are only ever
//! points proven inside the cell's original triangle, exactly: a rounded
//! midpoint on a wall may fall just outside the free space, and its
//! distance says nothing about the triangle. Nor may an anchor lie on a
//! barrier: a point on a barrier is seen from both sides, so its distance
//! is the nearer side's, and bounds nothing on the farther.

use core::cmp::Ordering;
use std::collections::{BinaryHeap, HashMap};

use axiolid_contracts::Sign;
use axiolid_core::Point2;
use axiolid_overlay::{Polygon, Ring};
use axiolid_triangulate::{triangulate, Constraint};

use crate::graph::{self, Graph, Side};
use crate::{
    contains, crosses, dedup_points, obstacle_segments, ring_edges, side, validate_region, within,
    Route, RouteError, Unreachable, MAX_VERTICES,
};

/// Cells [`farthest_point`] refines at most.
pub const MAX_CELLS: usize = 20_000;

/// Why no distance map was built.
#[derive(Debug, Clone, Copy, PartialEq)]
#[non_exhaustive]
pub enum MapError {
    /// The region, a barrier or a target is malformed, or the input is
    /// over budget ([`RouteError::TooManyVertices`], whose lower bound is
    /// zero here: there is no single pair of endpoints).
    Route(RouteError),
    /// No targets were given.
    NoTargets,
    /// A target lies outside the free-space region.
    TargetOutside {
        /// Its index in the targets given.
        index: usize,
    },
    /// A cost region's factor is below 1 or not finite.
    InvalidFactor {
        /// Its index in the cost regions given.
        index: usize,
    },
    /// A target's weight is negative or not finite.
    InvalidWeight {
        /// Its index in the targets given.
        index: usize,
    },
    /// The spacing of points along cost edges is not positive and finite.
    InvalidSpacing,
    /// A cost region's edge properly crosses a region edge, a barrier or
    /// another cost region's edge, or runs along a barrier. Cost regions
    /// may nest, touch and share edges, and run along the region's
    /// boundary; crossings would need rounded vertices, not supported yet.
    CostCrossing {
        /// Index of the cost region, in the cost regions given.
        index: usize,
    },
}

impl From<RouteError> for MapError {
    fn from(error: RouteError) -> Self {
        Self::Route(error)
    }
}

/// Shortest-path distances from every point of a region to the nearest of
/// several targets.
#[derive(Debug, Clone)]
pub struct DistanceMap {
    region: Vec<Polygon>,
    /// Barrier segments: zero-width, with free space on both sides.
    walls: Vec<(Point2, Point2)>,
    obstacles: Vec<(Point2, Point2)>,
    nodes: Vec<Point2>,
    graph: Graph,
    /// Distance from each (vertex, sector) state to its nearest target;
    /// infinite if none.
    distance: Vec<f64>,
    /// The next state towards that target; `usize::MAX` at a target.
    next: Vec<usize>,
    /// Which target, as an index into the targets given.
    target: Vec<usize>,
    /// The targets given.
    sites: Vec<Point2>,
    /// Their weights: the distance each starts at.
    weights: Vec<f64>,
}

/// The nearest target from a point, and the route there.
#[derive(Debug, Clone, PartialEq)]
#[non_exhaustive]
pub struct Reach {
    /// Index of the nearest target in the targets given. Ties go to the
    /// route through the lowest graph vertex, deterministically.
    pub target: usize,
    /// The route, from the query point to the target. Its `length` is the
    /// route's own, without the target's weight.
    pub route: Route,
    /// The distance: the route's length plus the target's weight (see
    /// [`distance_map_weighted`]); the route's length when all weights are
    /// zero.
    pub distance: f64,
}

/// A distance map from `targets` over `region`, avoiding `barriers`.
///
/// # Errors
///
/// [`MapError`] for malformed input, no targets, a target outside the
/// region, or more than [`MAX_VERTICES`] graph vertices.
pub fn distance_map(
    region: &[Polygon],
    barriers: &[Vec<Point2>],
    targets: &[Point2],
) -> Result<DistanceMap, MapError> {
    distance_map_within(region, barriers, targets, MAX_VERTICES)
}

/// [`distance_map`] with a caller-chosen vertex budget.
///
/// # Errors
///
/// As [`distance_map`], with `budget` for [`MAX_VERTICES`].
pub fn distance_map_within(
    region: &[Polygon],
    barriers: &[Vec<Point2>],
    targets: &[Point2],
    budget: usize,
) -> Result<DistanceMap, MapError> {
    let weighted: Vec<(Point2, f64)> = targets.iter().map(|t| (*t, 0.0)).collect();
    distance_map_within_weighted(region, barriers, &weighted, budget)
}

/// A distance map whose targets each start at their own distance: the
/// distance from a point is the least, over targets, of the route's
/// length to the target plus the target's weight (#197). For a way out
/// that carries the rest of a walk beyond it, such as a stair landing.
/// With every weight zero it is [`distance_map`].
///
/// # Errors
///
/// As [`distance_map`], and [`MapError::InvalidWeight`] for a weight
/// that is negative or not finite.
pub fn distance_map_weighted(
    region: &[Polygon],
    barriers: &[Vec<Point2>],
    targets: &[(Point2, f64)],
) -> Result<DistanceMap, MapError> {
    distance_map_within_weighted(region, barriers, targets, MAX_VERTICES)
}

/// [`distance_map_weighted`] with a caller-chosen vertex budget.
///
/// # Errors
///
/// As [`distance_map_weighted`], with `budget` for [`MAX_VERTICES`].
pub fn distance_map_within_weighted(
    region: &[Polygon],
    barriers: &[Vec<Point2>],
    weighted: &[(Point2, f64)],
    budget: usize,
) -> Result<DistanceMap, MapError> {
    validate_region(region, barriers)?;
    if weighted.is_empty() {
        return Err(MapError::NoTargets);
    }
    let targets: Vec<Point2> = weighted.iter().map(|(t, _)| *t).collect();
    let weights: Vec<f64> = weighted.iter().map(|(_, w)| *w).collect();
    if !targets.iter().all(|t| t.is_finite()) {
        return Err(RouteError::NonFinitePoint.into());
    }
    if let Some(index) = weights.iter().position(|w| !(w.is_finite() && *w >= 0.0)) {
        return Err(MapError::InvalidWeight { index });
    }
    for (index, t) in targets.iter().enumerate() {
        if !contains(region, *t)? {
            return Err(MapError::TargetOutside { index });
        }
    }
    let mut nodes = targets.to_vec();
    for polygon in region {
        for ring in core::iter::once(&polygon.outer).chain(polygon.holes.iter()) {
            nodes.extend(ring.points.iter().copied());
        }
    }
    for barrier in barriers {
        nodes.extend(barrier.iter().copied());
    }
    dedup_points(&mut nodes);
    if nodes.len() > budget {
        return Err(RouteError::TooManyVertices {
            supplied: nodes.len(),
            budget,
            lower_bound: 0.0,
        }
        .into());
    }
    let obstacles = obstacle_segments(region, barriers);
    let graph = Graph::build(&nodes, region, barriers, &obstacles)?;
    // Every free sector of every target at its weight, then one Dijkstra;
    // ties go to the lowest state, as in `shortest_path`. Of targets on
    // one point, the lightest (then the first) seeds it.
    let mut sources = Vec::new();
    let mut seed = vec![usize::MAX; graph.adjacency.len()];
    for (i, node) in nodes.iter().enumerate() {
        let lightest = (0..targets.len())
            .filter(|&t| targets[t] == *node)
            .min_by(|&a, &b| weights[a].total_cmp(&weights[b]).then(a.cmp(&b)));
        if let Some(t) = lightest {
            for state in graph.states(i) {
                sources.push((state, weights[t]));
                seed[state] = t;
            }
        }
    }
    let (distance, next, _) = graph::dijkstra_from(&graph.adjacency, &sources, |_| false);
    // Follow each state's chain to its end, the target it is reached
    // from. A heavy target may itself be reached from a lighter one, so
    // being seeded does not end the chain.
    let mut target = seed.clone();
    for (state, t) in target.iter_mut().enumerate() {
        let mut at = state;
        let mut steps = 0;
        while next[at] != usize::MAX && steps <= seed.len() {
            at = next[at];
            steps += 1;
        }
        *t = seed[at];
    }
    Ok(DistanceMap {
        region: region.to_vec(),
        walls: barriers
            .iter()
            .flat_map(|b| b.windows(2).map(|w| (w[0], w[1])))
            .collect(),
        obstacles,
        nodes,
        graph,
        distance,
        next,
        target,
        sites: targets,
        weights,
    })
}

impl DistanceMap {
    /// Vertices of the visibility graph.
    #[must_use]
    pub fn graph_vertices(&self) -> usize {
        self.nodes.len()
    }

    /// How many targets the map was built from.
    #[must_use]
    pub fn targets(&self) -> usize {
        self.sites.len()
    }

    /// The graph's vertices.
    pub(crate) fn nodes(&self) -> &[Point2] {
        &self.nodes
    }

    /// Each graph vertex's distance to the nearest target, over all its
    /// sectors; infinite when no target is reached.
    pub(crate) fn vertex_distances(&self) -> Vec<f64> {
        (0..self.nodes.len())
            .map(|i| {
                self.graph
                    .states(i)
                    .map(|s| self.distance[s])
                    .fold(f64::INFINITY, f64::min)
            })
            .collect()
    }

    /// The free-space region.
    pub(crate) fn region(&self) -> &[Polygon] {
        &self.region
    }

    /// Region boundary and barrier edges.
    pub(crate) fn obstacles(&self) -> &[(Point2, Point2)] {
        &self.obstacles
    }

    /// The targets the map was built from.
    pub(crate) fn sites(&self) -> &[Point2] {
        &self.sites
    }

    /// The distance each target starts at.
    pub(crate) fn weights(&self) -> &[f64] {
        &self.weights
    }

    /// Whether two maps cover the same free space: the same region and the
    /// same barriers.
    pub(crate) fn same_space(&self, other: &Self) -> bool {
        self.region == other.region && self.walls == other.walls
    }

    /// The nearest target from `point` and the route there.
    ///
    /// `Ok(Err(StartOutside))` for a point outside the region,
    /// `Ok(Err(DisconnectedComponents))` for one no target can reach.
    ///
    /// # Errors
    ///
    /// [`RouteError`] for a non-finite point or an undecidable predicate.
    pub fn nearest(&self, point: Point2) -> Result<Result<Reach, Unreachable>, RouteError> {
        if !point.is_finite() {
            return Err(RouteError::NonFinitePoint);
        }
        if !contains(&self.region, point)? {
            return Ok(Err(Unreachable::StartOutside));
        }
        let Some((length, first)) = self.via(point)? else {
            return Ok(Err(Unreachable::DisconnectedComponents));
        };
        let mut polyline = vec![point];
        let mut state = first;
        loop {
            let at = self.nodes[self.graph.node(state)];
            if polyline.last() != Some(&at) {
                polyline.push(at);
            }
            if self.next[state] == usize::MAX {
                break;
            }
            state = self.next[state];
        }
        let target = self.target[first];
        Ok(Ok(Reach {
            target,
            route: Route {
                polyline,
                // Exactly the distance when the weight is zero.
                length: length - self.weights[target],
                graph_vertices: self.nodes.len(),
            },
            distance: length,
        }))
    }

    /// The distance from `point`, known to be inside the region, and the
    /// first state on the way; `None` when no target is reachable.
    fn via(&self, point: Point2) -> Result<Option<(f64, usize)>, RouteError> {
        let mut best: Option<(f64, usize)> = None;
        let offer = |length: f64, state: usize, best: &mut Option<(f64, usize)>| {
            if best.is_none_or(|(b, s)| length < b || (length == b && state < s)) {
                *best = Some((length, state));
            }
        };
        for (i, node) in self.nodes.iter().enumerate() {
            let nearest = self
                .graph
                .states(i)
                .map(|s| self.distance[s])
                .fold(f64::INFINITY, f64::min);
            if nearest.is_infinite() {
                continue;
            }
            if *node == point {
                for s in self.graph.states(i) {
                    offer(self.distance[s], s, &mut best);
                }
                continue;
            }
            let leg = (*node - point).length();
            if best.is_some_and(|(b, _)| leg + nearest > b) {
                continue;
            }
            let ok = graph::sides(
                point,
                *node,
                &self.region,
                &self.obstacles,
                &self.nodes,
                &self.graph.stars,
                &self.graph.rayed,
            )?;
            for (k, on) in [Side::Left, Side::Right].into_iter().enumerate() {
                if !ok[k] {
                    continue;
                }
                if let Some(s) = self.graph.arrival(i, point, on)? {
                    if self.distance[s].is_finite() {
                        offer(leg + self.distance[s], s, &mut best);
                    }
                }
            }
        }
        Ok(best)
    }

    /// Distance to the nearest target, for a point inside the region.
    pub(crate) fn at(&self, point: Point2) -> Result<Option<f64>, RouteError> {
        Ok(self.via(point)?.map(|(length, _)| length))
    }

    /// The largest length of a path in the graph, in edges: bounds how many
    /// roundings a distance carries.
    pub(crate) fn hops(&self) -> usize {
        let mut most = 0;
        for start in 0..self.next.len() {
            let mut state = start;
            let mut hops = 0;
            while self.next[state] != usize::MAX && hops <= self.next.len() {
                state = self.next[state];
                hops += 1;
            }
            most = most.max(hops);
        }
        most
    }
}

/// A closed interval of lengths.
#[derive(Debug, Clone, Copy, PartialEq)]
pub struct LengthInterval {
    /// No greater than the true value.
    pub lower: f64,
    /// No less than the true value.
    pub upper: f64,
}

/// The greatest distance from a subregion to the nearest target.
#[derive(Debug, Clone, Copy, PartialEq)]
#[non_exhaustive]
pub struct Farthest {
    /// Contains the true maximum over the subregion's points in the free
    /// space.
    pub distance: LengthInterval,
    /// A point of the subregion, in the free space, at least
    /// `distance.lower` from every target; `None` only when the budget ran
    /// out before any point of the subregion was sampled.
    pub witness: Option<Point2>,
    /// Whether the interval is no wider than the tolerance asked for. When
    /// the cell budget runs out first it is still sound, only wider.
    pub converged: bool,
    /// Cells examined.
    pub cells: usize,
}

/// Why no bracket was produced.
#[derive(Debug, Clone, Copy, PartialEq)]
#[non_exhaustive]
pub enum FarthestError {
    /// The subregion is malformed, or a predicate was undecidable.
    Route(RouteError),
    /// The tolerance is negative or not finite.
    InvalidTolerance,
    /// Two walls or barriers cross other than at a shared vertex. The free
    /// space is triangulated with them as constraints, and crossings would
    /// need rounded vertices; not supported yet.
    CrossingObstacles,
    /// The free space could not be triangulated, or a triangle of it is
    /// too thin to hold a point strictly inside.
    Triangulation,
    /// Part of the subregion lies in free space that no target reaches, so
    /// its farthest distance is infinite. The triangle lies in the free
    /// space and meets the subregion; no target reaches any of it.
    Unreachable {
        /// The triangle's corners.
        triangle: [Point2; 3],
    },
    /// The subregion does not meet the free space.
    Empty,
    /// Two distance maps cover different free space: their regions or
    /// barriers differ.
    MismatchedMaps,
}

impl From<RouteError> for FarthestError {
    fn from(error: RouteError) -> Self {
        Self::Route(error)
    }
}

/// The greatest distance to the nearest target over the points of
/// `subregion` in the map's free space, bracketed to within `tolerance`.
///
/// # Errors
///
/// [`FarthestError`], among them [`FarthestError::Unreachable`] with a
/// triangle as evidence when part of the subregion reaches no target.
pub fn farthest_point(
    map: &DistanceMap,
    subregion: &Polygon,
    tolerance: f64,
) -> Result<Farthest, FarthestError> {
    farthest_point_within(map, subregion, tolerance, MAX_CELLS)
}

/// A cell: a triangle inside the free-space triangle `root`, with the best
/// anchor known for it and the upper bound that gives.
#[derive(Debug, Clone, Copy)]
struct Cell {
    corners: [Point2; 3],
    root: usize,
    anchor: (Point2, f64),
    upper: f64,
    depth: u32,
    order: usize,
}

impl PartialEq for Cell {
    fn eq(&self, other: &Self) -> bool {
        self.cmp(other) == Ordering::Equal
    }
}

impl Eq for Cell {}

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

impl Ord for Cell {
    /// Greatest upper bound first; the earlier cell on a tie.
    fn cmp(&self, other: &Self) -> Ordering {
        self.upper
            .total_cmp(&other.upper)
            .then(other.order.cmp(&self.order))
    }
}

/// [`farthest_point`] with a caller-chosen cell budget.
///
/// # Errors
///
/// As [`farthest_point`].
pub fn farthest_point_within(
    map: &DistanceMap,
    subregion: &Polygon,
    tolerance: f64,
    max_cells: usize,
) -> Result<Farthest, FarthestError> {
    if !(tolerance.is_finite() && tolerance >= 0.0) {
        return Err(FarthestError::InvalidTolerance);
    }
    validate_region(core::slice::from_ref(subregion), &[])?;
    let roots = free_triangles(map)?;
    let scale = map
        .nodes
        .iter()
        .chain(subregion.outer.points.iter())
        .fold(0.0f64, |m, p| m.max(p.x.abs()).max(p.y.abs()));
    // A distance is a sum of at most `hops + 1` rounded lengths, each a
    // rounded square root of rounded squares of rounded differences.
    let relative = (map.hops() as f64 + 8.0) * 2.0 * f64::EPSILON;
    // Rounded midpoints leave gaps of about an ulp per level between cells,
    // and the radius itself is rounded.
    let slack = |depth: u32, radius: f64| {
        f64::from(depth + 4) * 4.0 * f64::EPSILON * scale + 4.0 * f64::EPSILON * radius
    };
    let mut search = Search {
        map,
        subregion,
        roots: &roots,
        evaluated: HashMap::new(),
        lower: None,
    };
    let mut heap = BinaryHeap::new();
    let mut cells = 0usize;
    for (root, corners) in roots.iter().enumerate() {
        if search.outside(corners)? {
            continue;
        }
        cells += 1;
        // A root's centroid is strictly inside it, off every barrier,
        // unless the triangle is too thin to hold a representable point.
        let Some(cell) = search.cell(*corners, root, None, 0, cells, &slack)? else {
            return Err(FarthestError::Triangulation);
        };
        heap.push(cell);
    }
    if cells == 0 {
        return Err(FarthestError::Empty);
    }
    let best = |search: &Search| search.lower.map_or(f64::NEG_INFINITY, |(d, _)| d);
    while let Some(top) = heap.peek() {
        if top.upper <= best(&search) + tolerance || cells >= max_cells {
            break;
        }
        let cell = heap.pop().expect("peeked");
        let [a, b, c] = cell.corners;
        let mid = |p: Point2, q: Point2| Point2::new(0.5 * p.x + 0.5 * q.x, 0.5 * p.y + 0.5 * q.y);
        let (ab, bc, ca) = (mid(a, b), mid(b, c), mid(c, a));
        for corners in [[a, ab, ca], [ab, b, bc], [ca, bc, c], [ab, bc, ca]] {
            if search.outside(&corners)? {
                continue;
            }
            cells += 1;
            let child = search.cell(
                corners,
                cell.root,
                Some(cell.anchor),
                cell.depth + 1,
                cells,
                &slack,
            )?;
            if let Some(child) = child {
                if child.upper > best(&search) {
                    heap.push(child);
                }
            }
        }
    }
    let Some((lower, witness)) = search.lower else {
        if heap.is_empty() {
            return Err(FarthestError::Empty);
        }
        let upper = heap.peek().map_or(0.0, |c| c.upper);
        return Ok(Farthest {
            distance: LengthInterval {
                lower: 0.0,
                upper: upper * (1.0 + relative),
            },
            witness: None,
            converged: false,
            cells,
        });
    };
    let upper = heap.peek().map_or(lower, |c| c.upper.max(lower));
    Ok(Farthest {
        distance: LengthInterval {
            lower: lower * (1.0 - relative),
            upper: upper * (1.0 + relative),
        },
        witness: Some(witness),
        converged: upper - lower <= tolerance,
        cells,
    })
}

struct Search<'a> {
    map: &'a DistanceMap,
    subregion: &'a Polygon,
    roots: &'a [[Point2; 3]],
    evaluated: HashMap<(u64, u64), Option<f64>>,
    /// The greatest distance at a point of the subregion, and the point.
    lower: Option<(f64, Point2)>,
}

impl Search<'_> {
    fn outside(&self, t: &[Point2; 3]) -> Result<bool, RouteError> {
        outside(self.subregion, t)
    }

    fn admissible(&self, root: usize, a: Point2) -> Result<bool, RouteError> {
        admissible(self.map, &self.roots[root], a)
    }

    fn distance(&mut self, p: Point2) -> Result<Option<f64>, RouteError> {
        let key = (p.x.to_bits(), p.y.to_bits());
        if let Some(d) = self.evaluated.get(&key) {
            return Ok(*d);
        }
        let d = self.map.at(p)?;
        self.evaluated.insert(key, d);
        Ok(d)
    }

    /// The cell's upper bound from its best anchor: its corners and
    /// centroid where proven inside the root triangle, and the parent's.
    /// Raises the lower bound from anchors in the subregion. `None` when
    /// the cell has no anchor (never for a root).
    fn cell(
        &mut self,
        corners: [Point2; 3],
        root: usize,
        inherited: Option<(Point2, f64)>,
        depth: u32,
        order: usize,
        slack: &dyn Fn(u32, f64) -> f64,
    ) -> Result<Option<Cell>, FarthestError> {
        let centroid = Point2::new(
            (corners[0].x + corners[1].x + corners[2].x) / 3.0,
            (corners[0].y + corners[1].y + corners[2].y) / 3.0,
        );
        let radius = |a: Point2| {
            corners
                .iter()
                .map(|c| (*c - a).length())
                .fold(0.0, f64::max)
        };
        let mut best: Option<(f64, (Point2, f64))> =
            inherited.map(|(a, d)| (d + radius(a), (a, d)));
        for a in [corners[0], corners[1], corners[2], centroid] {
            if !self.admissible(root, a)? {
                continue;
            }
            let Some(d) = self.distance(a)? else {
                return Err(FarthestError::Unreachable {
                    triangle: self.roots[root],
                });
            };
            if in_polygon(self.subregion, a)? && self.lower.is_none_or(|(l, _)| d > l) {
                self.lower = Some((d, a));
            }
            let bound = d + radius(a);
            if best.is_none_or(|(b, _)| bound < b) {
                best = Some((bound, (a, d)));
            }
        }
        Ok(best.map(|(bound, anchor)| Cell {
            corners,
            root,
            anchor,
            upper: bound + slack(depth, radius(anchor.0)),
            depth,
            order,
        }))
    }
}

/// Whether a triangle misses `subregion`, decided exactly: no edge of the
/// subregion meets it, and a corner lies outside.
pub(crate) fn outside(subregion: &Polygon, t: &[Point2; 3]) -> Result<bool, RouteError> {
    for ring in core::iter::once(&subregion.outer).chain(subregion.holes.iter()) {
        for (p, q) in ring_edges(ring) {
            if meets_triangle(p, q, t)? {
                return Ok(false);
            }
        }
    }
    Ok(!in_polygon(subregion, t[0])?)
}

/// Whether `a` may anchor a cell of the free-space triangle `root`: inside
/// it, and not on a barrier of `map`.
pub(crate) fn admissible(
    map: &DistanceMap,
    root: &[Point2; 3],
    a: Point2,
) -> Result<bool, RouteError> {
    if !in_triangle(root, a)? {
        return Ok(false);
    }
    for &(p, q) in &map.walls {
        if side(p, q, a)? == Sign::Zero && within(p, q, a) {
            return Ok(false);
        }
    }
    Ok(true)
}

/// The triangles of the free space: a constrained triangulation of every
/// wall and barrier, each segment first cut at the vertices lying on it,
/// keeping the triangles inside the region.
pub(crate) fn free_triangles(map: &DistanceMap) -> Result<Vec<[Point2; 3]>, FarthestError> {
    free_triangles_in(&map.region, &map.obstacles)
}

/// [`free_triangles`] of a region and its obstacle segments.
pub(crate) fn free_triangles_in(
    region: &[Polygon],
    obstacles: &[(Point2, Point2)],
) -> Result<Vec<[Point2; 3]>, FarthestError> {
    let mut points: Vec<Point2> = obstacles.iter().flat_map(|(p, q)| [*p, *q]).collect();
    dedup_points(&mut points);
    let mut pieces: Vec<(Point2, Point2)> = Vec::new();
    for &(p, q) in obstacles {
        let d = q - p;
        let mut cuts = vec![p, q];
        for &v in &points {
            if v != p && v != q && side(p, q, v)? == Sign::Zero && within(p, q, v) {
                cuts.push(v);
            }
        }
        cuts.sort_by(|u, v| (*u - p).dot(d).total_cmp(&(*v - p).dot(d)));
        for pair in cuts.windows(2) {
            if pair[0] != pair[1] {
                pieces.push((pair[0], pair[1]));
            }
        }
    }
    for (i, &(p, q)) in pieces.iter().enumerate() {
        for &(r, s) in &pieces[i + 1..] {
            if crosses(p, q, r, s)? {
                return Err(FarthestError::CrossingObstacles);
            }
        }
    }
    let index = |v: Point2| {
        u32::try_from(points.iter().position(|p| *p == v).expect("an endpoint")).unwrap_or(u32::MAX)
    };
    let mut constraints: Vec<Constraint> = pieces
        .iter()
        .map(|(p, q)| Constraint::new(index(*p), index(*q)))
        .collect();
    constraints.sort_unstable();
    constraints.dedup();
    let triangulation =
        triangulate(&points, &constraints).map_err(|_| FarthestError::Triangulation)?;
    let at = triangulation.points();
    let mut out = Vec::new();
    for t in triangulation.triangles().chunks_exact(3) {
        let corners = [at[t[0] as usize], at[t[1] as usize], at[t[2] as usize]];
        let centroid = Point2::new(
            (corners[0].x + corners[1].x + corners[2].x) / 3.0,
            (corners[0].y + corners[1].y + corners[2].y) / 3.0,
        );
        if contains(region, centroid)? {
            out.push(corners);
        }
    }
    Ok(out)
}

/// Whether `p` lies in the closed counter-clockwise triangle, exactly.
pub(crate) fn in_triangle(t: &[Point2; 3], p: Point2) -> Result<bool, RouteError> {
    for i in 0..3 {
        if side(t[i], t[(i + 1) % 3], p)? == Sign::Negative {
            return Ok(false);
        }
    }
    Ok(true)
}

/// Whether the closed segment `pq` meets the closed triangle, exactly.
pub(crate) fn meets_triangle(p: Point2, q: Point2, t: &[Point2; 3]) -> Result<bool, RouteError> {
    if in_triangle(t, p)? || in_triangle(t, q)? {
        return Ok(true);
    }
    for i in 0..3 {
        if segments_meet(p, q, t[i], t[(i + 1) % 3])? {
            return Ok(true);
        }
    }
    Ok(false)
}

/// Whether two closed segments share a point, exactly.
pub(crate) fn segments_meet(
    p: Point2,
    q: Point2,
    r: Point2,
    s: Point2,
) -> Result<bool, RouteError> {
    let (d1, d2) = (side(p, q, r)?, side(p, q, s)?);
    let (d3, d4) = (side(r, s, p)?, side(r, s, q)?);
    if d1 != d2
        && d3 != d4
        && d1 != Sign::Zero
        && d2 != Sign::Zero
        && d3 != Sign::Zero
        && d4 != Sign::Zero
    {
        return Ok(true);
    }
    Ok((d1 == Sign::Zero && within(p, q, r))
        || (d2 == Sign::Zero && within(p, q, s))
        || (d3 == Sign::Zero && within(r, s, p))
        || (d4 == Sign::Zero && within(r, s, q)))
}

/// Whether `p` lies in the closed polygon (outer ring minus the open
/// holes), exactly.
pub(crate) fn in_polygon(polygon: &Polygon, p: Point2) -> Result<bool, RouteError> {
    if on_ring(&polygon.outer, p)? {
        return Ok(true);
    }
    if winding(&polygon.outer, p)? == 0 {
        return Ok(false);
    }
    for hole in &polygon.holes {
        if !on_ring(hole, p)? && winding(hole, p)? != 0 {
            return Ok(false);
        }
    }
    Ok(true)
}

fn on_ring(ring: &Ring, p: Point2) -> Result<bool, RouteError> {
    for (a, b) in ring_edges(ring) {
        if side(a, b, p)? == Sign::Zero && within(a, b, p) {
            return Ok(true);
        }
    }
    Ok(false)
}

/// Winding number of a ring around a point off it, from exact sides.
fn winding(ring: &Ring, p: Point2) -> Result<i32, RouteError> {
    let mut w = 0;
    for (a, b) in ring_edges(ring) {
        if a.y <= p.y && b.y > p.y && side(a, b, p)? == Sign::Positive {
            w += 1;
        } else if b.y <= p.y && a.y > p.y && side(a, b, p)? == Sign::Negative {
            w -= 1;
        }
    }
    Ok(w)
}

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

    fn p(x: f64, y: f64) -> Point2 {
        Point2::new(x, y)
    }

    fn square() -> Polygon {
        Polygon {
            outer: Ring {
                points: vec![p(0.0, 0.0), p(4.0, 0.0), p(4.0, 4.0), p(0.0, 4.0)],
            },
            holes: Vec::new(),
        }
    }

    #[test]
    fn anchors_lie_in_their_triangle_and_off_every_barrier() {
        let barrier = vec![vec![p(1.0, 1.0), p(3.0, 3.0)]];
        let map = distance_map(&[square()], &barrier, &[p(0.5, 3.5)]).unwrap();
        let roots = [[p(0.0, 0.0), p(4.0, 0.0), p(4.0, 4.0)]];
        let subregion = square();
        let search = Search {
            map: &map,
            subregion: &subregion,
            roots: &roots,
            evaluated: HashMap::new(),
            lower: None,
        };
        assert!(search.admissible(0, p(3.0, 1.0)).unwrap());
        // On the barrier, though inside the triangle.
        assert!(!search.admissible(0, p(2.0, 2.0)).unwrap());
        // Off the barrier, but outside the triangle.
        assert!(!search.admissible(0, p(1.0, 3.0)).unwrap());
    }

    #[test]
    fn a_triangle_is_outside_only_when_no_subregion_edge_meets_it() {
        let map = distance_map(&[square()], &[], &[p(0.5, 0.5)]).unwrap();
        let subregion = Polygon {
            outer: Ring {
                points: vec![p(1.0, 1.0), p(3.0, 1.0), p(3.0, 3.0), p(1.0, 3.0)],
            },
            holes: Vec::new(),
        };
        let search = Search {
            map: &map,
            subregion: &subregion,
            roots: &[],
            evaluated: HashMap::new(),
            lower: None,
        };
        // First corner outside, but the triangle reaches in.
        assert!(!search
            .outside(&[p(0.0, 0.0), p(2.0, 0.0), p(2.0, 2.0)])
            .unwrap());
        assert!(search
            .outside(&[p(0.0, 0.0), p(0.9, 0.0), p(0.0, 0.9)])
            .unwrap());
        assert!(!search
            .outside(&[p(1.5, 1.5), p(2.0, 1.5), p(2.0, 2.0)])
            .unwrap());
    }
}