navcore_route_find/

gateway.rs

//! Finding a harbour's gateway: the nearest point outside a mole, a
//! breakwater or a marina basin from which [`crate::find_route`]'s
//! open-water search can start.
//!
//! # Why this exists
//!
//! `find_route`'s lattice and visibility search are tuned for open
//! water: a cell size in tenths of a nautical mile, a corridor half a
//! cable wide, margins that escalate from a fraction of the direct
//! distance. A marina basin, a dredged channel or a narrow breakwater
//! gap needs metre-scale precision this crate does not claim, on chart
//! data that this crate's own overview-vs-detail reconciliation
//! (`hazards::fetch_preferring_detail`) is not built to trust blindly
//! either.
//!
//! No chartplotter auto-routes a vessel all the way to its own berth:
//! Garmin's Auto Guidance documentation, Raymarine's LightHouse manual
//! and OpenCPN's routing all stop at "clear of the marina" and hand
//! pilotage back to the mariner. This module automates that same manual
//! search: stand off the harbour a little further, look around, and
//! find the nearest spot the open-water search can use.
//!
//! # The mechanism
//!
//! [`find_gateway`] takes a harbour's nominal position and, if it is
//! not already usable, searches outward in expanding rings. Each
//! candidate is tested with the same corridor-width check
//! [`crate::find_route`]'s lattice search uses for its `from`/`to`
//! endpoints (`Hazards::blocked`), not the zero-length `check_leg` probe
//! that answers "clear" for any position, including one nowhere near
//! water (see `enc-check`'s own `bench` module doc). A candidate is
//! accepted only once one ring further out from it is clear too, so a
//! single-point gap between two hazards cannot pass for a real opening.
//!
//! Ring spacing is constant in arc length, not bearing count: a fixed
//! compass rose can miss a narrow marina entrance far out, because the
//! angular gap between two adjacent rays grows past the gap in the
//! breakwater. [`bearings_for_ring`] grows the bearing count with the
//! ring's radius instead, keeping physical spacing between adjacent
//! rays close to [`GATEWAY_ARC_SPACING_NM`] regardless of distance.
//!
//! # What this does not attempt
//!
//! The gateway found is open water a few tenths of a mile off the
//! coast, not the pier; reaching the berth from there is left to the
//! mariner, the same way every route this crate produces ends at a
//! position, not a cleat. A future version could route the last stretch
//! too, using a chart's own fairway data (`NAVLNE`) where charted.
//!
//! # A second approach: [`find_gateway_route`]
//!
//! [`find_gateway`] answers with a bare point; a caller still has to
//! draw its own line from `harbor` to it, which is unreliable since a
//! `harbor` pin is routinely charted land at the precision `check_leg`
//! insists on. [`find_gateway_route`] answers with the path instead: a
//! real, `check_leg`-approved waypoint list, the same quality
//! [`crate::find_route`] produces for any other passage.
//!
//! The search runs in two phases at two corridors. Phase one,
//! [`search::nearest_safe`], threads `harbor`'s immediate obstruction at
//! [`GATEWAY_ESCAPE_RADIUS_NM`] -- tighter than the caller's own
//! corridor, since real harbour water is routinely narrower than that
//! margin calls safe -- expanding through nodes that pass the tighter
//! test and stopping at the first one that also clears the caller's
//! open-water corridor. Phase two hands off to [`crate::find_route`] at
//! the caller's real corridor. Both halves are straightened and
//! `check_leg`-approved by [`smooth::straighten`] before being joined.
//!
//! # The full passage: [`find_dock_to_dock_route`]
//!
//! [`find_gateway`] answers one harbour's question. A passage between
//! two harbours needs that answered twice, with [`crate::find_route`]'s
//! open-water search filling the middle. [`find_dock_to_dock_route`]
//! does the stitching: `from_harbor`'s gateway, the open-water route
//! between the two gateways, `to_harbor`'s gateway, concatenated into
//! one waypoint list. Built on [`find_gateway`], not
//! [`find_gateway_route`]: the destination harbour only needs a usable
//! point to route to, not its own escape path.

use nav_math::{Position, rhumb};

use enc_store::{ChartStore, Leg, StoreError, Vessel};

use crate::grid::Grid;
use crate::hazards::Hazards;
use crate::search;
use crate::smooth;
use crate::{FindError, FindOptions};

/// How far apart, in nautical miles, [`find_gateway`] tries to keep
/// adjacent test rays at any given ring radius -- see [`bearings_for_ring`].
/// The same scale [`crate::find_route`]'s own default corridor half-width
/// already uses elsewhere in this crate, chosen for the same reason: it
/// is roughly the narrowest real gap (a breakwater entrance, a channel
/// between two shoals) worth resolving at all, not merely a round number.
const GATEWAY_ARC_SPACING_NM: f64 = 0.05;

/// How far outward [`find_gateway`] widens its search rings before
/// giving up, in nautical miles. Three miles is generous against a
/// working gateway, typically under half a mile from a hand-picked pier
/// position, while still bounded: a harbour this module cannot help
/// with (a marina reachable only through a dredged channel narrower
/// than any real chart resolves) would otherwise search forever rather
/// than answering `None`.
const GATEWAY_MAX_RADIUS_NM: f64 = 3.0;

/// The ring-to-ring step, in nautical miles: how far each expanding
/// ring moves out, and, doubled, the "one ring further" confirmation
/// distance a candidate must stay clear past. A tenth of a mile matches
/// the granularity a real harbour approach needs.
const GATEWAY_RING_STEP_NM: f64 = 0.1;

/// The fewest bearings any ring is tested at, however small its radius.
/// Without this floor, [`bearings_for_ring`]'s arc-length scaling would
/// let the tightest ring (a tenth of a mile out) get by with as few as
/// seven or eight bearings, coarse enough to plausibly miss a gap even
/// that close in.
const GATEWAY_MIN_BEARINGS: usize = 16;

/// The most bearings any one ring is tested at, however large its
/// radius. A defensive ceiling, not a working limit: it sits well past
/// the roughly 380 rays [`GATEWAY_ARC_SPACING_NM`]'s target spacing
/// needs even at [`GATEWAY_MAX_RADIUS_NM`], the widest ring this module
/// searches, so ordinary use never reaches it. It bounds the per-ring
/// cost if either constant above changes.
const GATEWAY_MAX_BEARINGS: usize = 512;

/// Lattice spacing [`find_gateway_route`]'s escape-phase grid uses, in
/// nautical miles: roughly 18 m, fine enough to resolve a marina
/// channel or breakwater gap, near-shore precision
/// [`crate::find_route`]'s open-water lattice (auto-scaled from the
/// direct distance, routinely far coarser) is not built for.
const GATEWAY_ESCAPE_CELL_NM: f64 = 0.01;

/// The escape phase's corridor half-width, in nautical miles: much
/// tighter than a caller's real `port_xtd_nm`/`starboard_xtd_nm` (the
/// open-water passage margin), since a marina channel or dredged gap is
/// routinely narrower than that margin calls safe. Roughly 5.5 m either
/// side (an 11 m minimum channel width), an engineering default for
/// slow-speed harbour manoeuvring rather than a specific vessel's beam
/// -- this crate has no beam measurement to draw on (see
/// [`FindOptions`]). A wider figure (18 m) would call a genuinely open
/// berth exit blocked, sweeping sideways into the land either side of a
/// gap this narrow. A future version could take a vessel's beam as an
/// explicit setting and use it here directly.
const GATEWAY_ESCAPE_RADIUS_NM: f64 = 0.003;

/// How far out [`find_gateway_route`]'s escape phase fetches hazards
/// and lays its grid, in nautical miles: much smaller than
/// [`GATEWAY_MAX_RADIUS_NM`], since this phase only needs to reach
/// clear, open-water-corridor water close to `harbor`, not the far
/// gateway itself.
const GATEWAY_ESCAPE_SEARCH_RADIUS_NM: f64 = 1.5;

/// How many lattice nodes [`search::nearest_safe`] explores in
/// [`find_gateway_route`]'s escape phase before giving up on `harbor`
/// ever reaching water clear at the wide, open-water corridor.
///
/// [`search::nearest_safe`] has no heuristic to steer by -- a safe node
/// could be in any direction from `harbor` -- so it explores by
/// accumulated distance in a growing disc. At [`GATEWAY_ESCAPE_CELL_NM`],
/// forty thousand nodes cover a disc a little over a nautical mile in
/// radius (`sqrt(n / pi) * cell_nm`), past the widest escape this
/// module handles in practice, so this is a defensive ceiling against a
/// `harbor` with no nearby safe water at all.
const GATEWAY_ESCAPE_MAX_NODES: usize = 40_000;

/// A `check_leg`-approved route between two harbours: `from_harbor`'s
/// gateway, [`crate::find_route`]'s open-water search between the two
/// gateways, then `to_harbor`'s gateway, combining two [`find_gateway`]
/// calls with the open-water search between them so a caller does not
/// stitch them by hand.
///
/// `from_harbor` and `to_harbor` are included as the first and last
/// positions, unless a harbour was already its own gateway, but neither
/// is validated by a `check_leg` call the way every position between
/// them is: a harbour pin is routinely charted land at the precision
/// `check_leg` insists on (see [`find_gateway_route`]'s own doc). Both
/// bookend the route as a position, not a cleat, left for the mariner
/// to close.
///
/// `Ok(None)` when either harbour has no gateway within
/// `GATEWAY_MAX_RADIUS_NM`, or when [`crate::find_route`] itself finds
/// no safe passage between the two gateways once found.
///
/// # Errors
///
/// If the chart cannot be read.
pub fn find_dock_to_dock_route(
    chart: &ChartStore,
    from_harbor: Position,
    to_harbor: Position,
    options: &FindOptions,
) -> Result<Option<Vec<Position>>, FindError> {
    let Some(from_gateway) = find_gateway(chart, from_harbor, options)? else {
        return Ok(None);
    };
    let Some(to_gateway) = find_gateway(chart, to_harbor, options)? else {
        return Ok(None);
    };

    let Some(open_water) = crate::find_route(chart, from_gateway, to_gateway, *options)? else {
        return Ok(None);
    };

    let mut route = Vec::with_capacity(open_water.len() + 2);
    if from_harbor != from_gateway {
        route.push(from_harbor);
    }
    route.extend(open_water.into_iter().map(|waypoint| waypoint.position));
    if to_harbor != to_gateway {
        route.push(to_harbor);
    }
    Ok(Some(route))
}

/// How many rays to test around a ring of radius `radius_nm`, keeping
/// the physical distance between adjacent rays close to
/// [`GATEWAY_ARC_SPACING_NM`] regardless of the ring's radius.
#[must_use]
fn bearings_for_ring(radius_nm: f64) -> usize {
    let circumference_nm = 2.0 * std::f64::consts::PI * radius_nm;
    let scaled = (circumference_nm / GATEWAY_ARC_SPACING_NM).ceil() as usize;
    scaled.clamp(GATEWAY_MIN_BEARINGS, GATEWAY_MAX_BEARINGS)
}

/// The nearest point to `harbor` that [`crate::find_route`]'s open-water
/// search can use as a `from` or `to`: `harbor` itself if that is
/// already clear, otherwise the first point found searching outward in
/// expanding rings. See this module's own doc for the search itself and
/// what it does not attempt.
///
/// `Ok(None)` when nothing was found within `GATEWAY_MAX_RADIUS_NM`:
/// either `harbor` is nowhere near navigable water, or its gateway lies
/// down a channel narrower than this search's rays cross paths with.
///
/// # Errors
///
/// If the chart cannot be read.
pub fn find_gateway(chart: &ChartStore, harbor: Position, options: &FindOptions) -> Result<Option<Position>, FindError> {
    let vessel = Vessel { safety_contour_m: options.safety_contour_m };
    let radius_nm = options.port_xtd_nm.max(options.starboard_xtd_nm);

    let area = crate::search_area(harbor, harbor, GATEWAY_MAX_RADIUS_NM + GATEWAY_RING_STEP_NM);
    let hazards = Hazards::fetch(chart, area, options.avoid_restricted_areas)?;

    if !hazards.blocked(harbor, radius_nm, vessel) {
        return Ok(Some(harbor));
    }

    let mut ring_nm = GATEWAY_RING_STEP_NM;
    while ring_nm <= GATEWAY_MAX_RADIUS_NM {
        let bearings = bearings_for_ring(ring_nm);
        for step in 0..bearings {
            let bearing_deg = 360.0 * step as f64 / bearings as f64;
            let candidate = rhumb::destination(harbor, bearing_deg, ring_nm);
            if hazards.blocked(candidate, radius_nm, vessel) {
                continue;
            }

            // One ring further out, same bearing: a candidate only
            // narrowly clear -- a rounding artefact of where this ring
            // happened to fall, not a real opening -- fails this and is
            // left for a later, wider ring to find honestly instead.
            let confirmed = rhumb::destination(harbor, bearing_deg, ring_nm + GATEWAY_RING_STEP_NM);
            if !hazards.blocked(confirmed, radius_nm, vessel) {
                return Ok(Some(candidate));
            }
        }
        ring_nm += GATEWAY_RING_STEP_NM;
    }

    Ok(None)
}

/// A `check_leg`-approved route from `harbor` out to [`find_gateway`]'s
/// confirmed destination: the actual path there, not just the point,
/// validated end to end from `harbor`'s exact position onward. See this
/// module's own doc, "A second approach", for how this compares to
/// [`find_gateway`].
///
/// Two phases, at two corridors, joined at whichever node the first
/// phase reaches: `search::nearest_safe` threads `harbor`'s immediate
/// obstruction -- a marina basin, a breakwater gap -- at
/// `GATEWAY_ESCAPE_RADIUS_NM`, tighter than a caller's open-water
/// `port_xtd_nm`/`starboard_xtd_nm`, since harbour water is routinely
/// narrower than that margin calls safe; then [`crate::find_route`]
/// takes over at the caller's real corridor for the rest of the
/// passage. Both halves are straightened and `check_leg`-approved by
/// `smooth::straighten` before being joined.
///
/// `Ok(None)` when [`find_gateway`] finds no destination, when `harbor`
/// has no `check_leg`-safe way out of its immediate obstruction within
/// `GATEWAY_ESCAPE_MAX_NODES` lattice steps, or when
/// [`crate::find_route`] finds no safe passage from there to the
/// gateway.
///
/// Depth is not tested during the escape phase -- only land, a no-entry
/// restricted area, or the tighter corridor width can block it -- on
/// the mariner's own instruction: a marina berth's charted depth is
/// routinely less trustworthy than the mariner's own knowledge of it
/// (confirmed against a position the chart placed to only 6 m accuracy
/// at its compilation scale). [`find_gateway`]'s far target and the
/// open-water passage [`crate::find_route`] finds beyond `launch` are
/// unaffected: depth still applies exactly as `options` asks everywhere
/// past the harbour's immediate obstruction.
///
/// # Errors
///
/// If the chart cannot be read.
pub fn find_gateway_route(chart: &ChartStore, harbor: Position, options: &FindOptions) -> Result<Option<Vec<Position>>, FindError> {
    let vessel = Vessel { safety_contour_m: options.safety_contour_m };
    let radius_nm = options.port_xtd_nm.max(options.starboard_xtd_nm);

    // The confirmed-far target: the same point [`find_gateway`]'s own
    // ring search already finds and validates. Reused rather than
    // re-derived, so the two functions can never quietly disagree about
    // what "clear of the harbour" means.
    let Some(gateway) = find_gateway(chart, harbor, options)? else {
        return Ok(None);
    };
    if gateway == harbor {
        // Nothing to escape from: harbor was already clear.
        return Ok(Some(vec![harbor]));
    }

    // Phase one: a real, corridor-respecting path out of whatever tight
    // space `harbor` sits in, at the escape phase's own tighter
    // corridor -- not the caller's open-water one, which is exactly what
    // `harbor` just failed above. Depth is deliberately not tested here,
    // on the mariner's own explicit instruction, scoped to this phase
    // only -- `Hazards::ignoring_depth` drops every depth entry from the
    // index outright, which is the only way to actually ignore one: a
    // vessel with an impossibly permissive contour still cannot bypass
    // `Kind::Depth { drval1_m: None }`'s unconditional "unsurveyed means
    // drying" rule, confirmed live against a real marina position where
    // that rule fired even at a 1.0 m safety contour. Land and no-entry
    // restricted areas are untouched by `ignoring_depth` and still
    // block, exactly as `crosses_no_entry`/`intersects_band` always did.
    //
    // `passable` judges the real step, not `Hazards::blocked`'s
    // symmetric point-square -- confirmed live that a real marina berth
    // is routinely land on most compass directions and open on one:
    // land showed up on five of eight bearings within 40 m of a real
    // position this was tested against, open water on the other three.
    // A point-square test would have called every node in that berth
    // blocked regardless of which side it was ever actually approached
    // from; a directional leg band judges the one side a step actually
    // crosses.
    // `fetch_without_land_cache`, not `Hazards::fetch`: the baked land
    // cache is a raster built for the open-water lattice and visibility
    // searches, gridded far too coarse (roughly 200-300 m at this
    // crate's working latitudes) for this phase's own metre-scale
    // precision -- confirmed live, a genuine 18 m gap read as blocked
    // through a freshly rebaked cache and correctly as open water once
    // bypassed. `safe` below stays on `hazards`, the normal, possibly
    // cached fetch: it works at the caller's own wide, open-water
    // corridor, the scale that raster was built for.
    let area = crate::search_area(harbor, harbor, GATEWAY_ESCAPE_SEARCH_RADIUS_NM);
    let hazards = Hazards::fetch(chart, area, options.avoid_restricted_areas)?;
    let live = Hazards::fetch_without_land_cache(chart, area, options.avoid_restricted_areas)?;
    let depth_free = live.ignoring_depth();
    let grid = Grid::covering(area, GATEWAY_ESCAPE_CELL_NM);
    let start = grid.nearest(harbor);
    let passable = |from: (usize, usize), to: (usize, usize)| {
        let leg = Leg::symmetric(
            grid.position(from.0, from.1),
            grid.position(to.0, to.1),
            GATEWAY_ESCAPE_RADIUS_NM,
        );
        !depth_free.crosses_no_entry(leg)
            && !leg.band().is_some_and(|band| depth_free.intersects_band(&band, vessel))
    };
    let safe = |node: (usize, usize)| !hazards.blocked(grid.position(node.0, node.1), radius_nm, vessel);

    let Some(raw_escape) = search::nearest_safe(&grid, start, GATEWAY_ESCAPE_MAX_NODES, passable, safe)
    else {
        return Ok(None);
    };

    let mut escape_positions: Vec<Position> =
        raw_escape.iter().map(|&(row, col)| grid.position(row, col)).collect();
    // The promised route starts at `harbor` itself, not the lattice node
    // nearest it -- `CoarseOnly` below re-tests every leg it touches
    // anyway, so this costs nothing and the first leg it checks is the
    // one that will actually be sailed.
    if let Some(first) = escape_positions.first_mut() {
        *first = harbor;
    }

    // Not `smooth::straighten`/`smooth::Safety`: those call the real
    // `check_leg` oracle directly against `chart`, bypassing this
    // function's own `Hazards` entirely -- which means `ignoring_depth`
    // above could not stop it from finding, and rejecting a leg for,
    // the exact same "no charted depth, treated as drying" verdict this
    // phase exists to set aside. `CoarseOnly` re-tests each candidate
    // leg only against `depth_free`'s own coarse band test instead --
    // still real land, still a real no-entry area, never a real
    // `check_leg` depth finding.
    let escape_safety = CoarseOnly { hazards: &depth_free };
    let Some(escape_route) = smooth::straighten(
        &escape_positions,
        GATEWAY_ESCAPE_RADIUS_NM,
        GATEWAY_ESCAPE_RADIUS_NM,
        vessel,
        &escape_safety,
    )?
    else {
        return Ok(None);
    };
    let Some(&launch) = escape_route.last() else {
        return Ok(None);
    };

    // Phase two: `crate::find_route`'s own job from here, at the
    // caller's real, wider corridor.
    let Some(onward) = crate::find_route(chart, launch, gateway, *options)? else {
        return Ok(None);
    };

    let mut route = escape_route;
    route.extend(onward.into_iter().map(|waypoint| waypoint.position).skip(1));
    Ok(Some(route))
}

/// [`smooth::EdgeClear`] for [`find_gateway_route`]'s escape phase:
/// `hazards`'s coarse band test, land and no-entry areas included, and
/// nothing else. Not the `check_leg` oracle [`smooth::Safety`] calls,
/// which reads `chart` directly and so cannot be steered clear of a
/// depth finding by anything done to `hazards` alone -- see
/// [`find_gateway_route`]'s own doc for why that distinction matters
/// here.
struct CoarseOnly<'a> {
    hazards: &'a Hazards,
}

impl smooth::EdgeClear for CoarseOnly<'_> {
    fn is_clear(&self, leg: Leg, vessel: Vessel) -> Result<bool, StoreError> {
        if self.hazards.crosses_no_entry(leg) {
            return Ok(false);
        }
        Ok(!leg.band().is_some_and(|band| self.hazards.intersects_band(&band, vessel)))
    }
}

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

    #[test]
    fn a_ring_close_in_still_gets_the_minimum_bearing_count() {
        assert_eq!(bearings_for_ring(0.01), GATEWAY_MIN_BEARINGS);
    }

    #[test]
    fn bearing_count_grows_with_ring_radius() {
        assert!(bearings_for_ring(1.0) > bearings_for_ring(0.1));
    }

    #[test]
    fn bearing_count_is_capped_for_a_wide_ring() {
        assert_eq!(bearings_for_ring(100.0), GATEWAY_MAX_BEARINGS);
    }

    #[test]
    fn ray_spacing_stays_close_to_target_across_every_ring_this_module_searches() {
        // The whole reason bearings_for_ring exists: a real gap the
        // width of GATEWAY_ARC_SPACING_NM should never fall between two
        // adjacent rays, at any radius find_gateway's own
        // GATEWAY_MAX_RADIUS_NM ever actually reaches -- GATEWAY_MAX_BEARINGS
        // is chosen wide enough that it never binds within that range
        // (see the next test for the far-out regime where it does).
        for radius_nm in [0.1, 0.5, 1.0, 2.0, GATEWAY_MAX_RADIUS_NM] {
            let bearings = bearings_for_ring(radius_nm);
            let circumference_nm = 2.0 * std::f64::consts::PI * radius_nm;
            let spacing_nm = circumference_nm / bearings as f64;
            assert!(
                spacing_nm <= GATEWAY_ARC_SPACING_NM * 1.01,
                "ring {radius_nm} NM: spacing {spacing_nm} NM exceeds target"
            );
        }
    }

}