open-standards/map/map-rs/crates/map-route/src/cost.rs

//! Multi-modal, traffic-aware routing costs.
//!
//! Path-finding in [`crate`] defaults to distance. Real routing needs a **travel
//! mode** (a car can't use a footpath; a pedestrian can't use a motorway) and a
//! **time** cost that varies with road class and congestion. A [`CostModel`]
//! supplies both: the cost to traverse an edge (or `None` if it's inaccessible to
//! the mode) and an admissible lower bound for the A* heuristic. [`ModeProfile`]
//! is the built-in implementation; [`find_route_with`] is the generalized A* that
//! consumes any model.

use std::cmp::Reverse;
use std::collections::{BTreeMap, BinaryHeap};

use serde::{Deserialize, Serialize};

use crate::{haversine_distance, RoadType, RouteEdge, RouteGraph, RouteResult, RouteSegment};

/// How a journey is made. Determines which road classes are usable and how fast.
#[derive(Debug, Clone, Copy, PartialEq, Eq, Serialize, Deserialize)]
pub enum TravelMode {
    Car,
    Bicycle,
    Foot,
}

/// A cost function over edges for one routing query.
pub trait CostModel {
    /// Cost (seconds) to traverse `edge`, or `None` if it is inaccessible.
    fn edge_cost(&self, edge: &RouteEdge) -> Option<f64>;
    /// The minimum cost per meter any edge can have — used to scale the
    /// straight-line heuristic so it stays an admissible lower bound. For a
    /// time cost this is `1 / max_speed`.
    fn min_cost_per_meter(&self) -> f64;
}

/// The built-in mode profile: free-flow speeds by road class, an access matrix,
/// and a congestion multiplier (≥ 1.0; applies to motor traffic).
#[derive(Debug, Clone, Copy, PartialEq, Serialize, Deserialize)]
pub struct ModeProfile {
    pub mode: TravelMode,
    /// Congestion factor ≥ 1.0 (1.0 = free flow; 2.0 = half speed). Cars only.
    pub traffic: f64,
}

impl ModeProfile {
    pub fn new(mode: TravelMode) -> Self {
        ModeProfile { mode, traffic: 1.0 }
    }
    /// Set a congestion factor (clamped to ≥ 1.0).
    pub fn with_traffic(mut self, traffic: f64) -> Self {
        self.traffic = traffic.max(1.0);
        self
    }

    /// Whether this mode may use a road class.
    pub fn allows(&self, road: RoadType) -> bool {
        use RoadType::*;
        match self.mode {
            TravelMode::Car => matches!(road, Highway | Primary | Secondary | Residential),
            TravelMode::Bicycle => matches!(road, Primary | Secondary | Residential | Cycleway),
            TravelMode::Foot => matches!(road, Primary | Secondary | Residential | Footpath | Cycleway),
        }
    }

    /// Free-flow speed (m/s) on a road class for this mode (before traffic).
    fn base_speed(&self, road: RoadType) -> f64 {
        use RoadType::*;
        match self.mode {
            TravelMode::Car => match road {
                Highway => 33.3,    // ~120 km/h
                Primary => 22.2,    // ~80 km/h
                Secondary => 13.9,  // ~50 km/h
                Residential => 8.3, // ~30 km/h
                _ => 0.0,
            },
            TravelMode::Bicycle => 5.0, // ~18 km/h
            TravelMode::Foot => 1.4,    // ~5 km/h
        }
    }

    /// The fastest this mode can ever go (free flow), for the heuristic bound.
    fn max_speed(&self) -> f64 {
        match self.mode {
            TravelMode::Car => 33.3,
            TravelMode::Bicycle => 5.0,
            TravelMode::Foot => 1.4,
        }
    }
}

impl CostModel for ModeProfile {
    fn edge_cost(&self, edge: &RouteEdge) -> Option<f64> {
        if !self.allows(edge.road_type) {
            return None;
        }
        let mut speed = self.base_speed(edge.road_type);
        if speed <= 0.0 {
            return None;
        }
        if self.mode == TravelMode::Car {
            speed /= self.traffic; // congestion slows motor traffic
        }
        Some(edge.distance_m / speed)
    }
    fn min_cost_per_meter(&self) -> f64 {
        1.0 / self.max_speed()
    }
}

/// Generalized A* over a [`CostModel`]: finds the least-cost accessible route,
/// skipping edges the model rejects. Segment durations in the result are the
/// model's edge costs (seconds); distances are geometric.
pub fn find_route_with(
    graph: &RouteGraph,
    start: u64,
    end: u64,
    model: &dyn CostModel,
) -> Option<RouteResult> {
    if !graph.nodes.contains_key(&start) || !graph.nodes.contains_key(&end) {
        return None;
    }
    if start == end {
        let coord = graph.nodes[&start].coord;
        return Some(RouteResult {
            waypoints: vec![coord],
            total_distance_m: 0.0,
            total_duration_secs: 0.0,
            segments: Vec::new(),
        });
    }

    let goal = &graph.nodes[&end].coord;
    let per_m = model.min_cost_per_meter();

    let mut g: BTreeMap<u64, f64> = BTreeMap::new();
    let mut prev: BTreeMap<u64, (u64, usize)> = BTreeMap::new();
    g.insert(start, 0.0);
    let h0 = haversine_distance(&graph.nodes[&start].coord, goal) * per_m;
    let mut heap: BinaryHeap<Reverse<(u64, u64)>> = BinaryHeap::new();
    heap.push(Reverse(((h0 * 1000.0) as u64, start)));

    while let Some(Reverse((_f, u))) = heap.pop() {
        if u == end {
            break;
        }
        let g_u = g.get(&u).copied().unwrap_or(f64::INFINITY);
        if let Some(adj) = graph.adjacency.get(&u) {
            for &edge_idx in adj {
                let edge = &graph.edges[edge_idx];
                let Some(w) = model.edge_cost(edge) else {
                    continue; // inaccessible to this mode
                };
                let tentative = g_u + w;
                if tentative < g.get(&edge.to).copied().unwrap_or(f64::INFINITY) {
                    g.insert(edge.to, tentative);
                    prev.insert(edge.to, (u, edge_idx));
                    let h = haversine_distance(&graph.nodes[&edge.to].coord, goal) * per_m;
                    heap.push(Reverse((((tentative + h) * 1000.0) as u64, edge.to)));
                }
            }
        }
    }

    if !prev.contains_key(&end) {
        return None;
    }

    let mut path: Vec<usize> = Vec::new();
    let mut cur = end;
    while let Some(&(p, idx)) = prev.get(&cur) {
        path.push(idx);
        cur = p;
    }
    path.reverse();

    let mut waypoints = Vec::new();
    let mut segments = Vec::new();
    let mut total_distance_m = 0.0;
    let mut total_duration_secs = 0.0;
    for (i, &idx) in path.iter().enumerate() {
        let edge = &graph.edges[idx];
        let from = graph.nodes[&edge.from].coord;
        let to = graph.nodes[&edge.to].coord;
        let dur = model.edge_cost(edge).unwrap_or(edge.duration_secs);
        if i == 0 {
            waypoints.push(from);
        }
        waypoints.push(to);
        total_distance_m += edge.distance_m;
        total_duration_secs += dur;
        segments.push(RouteSegment {
            from,
            to,
            distance_m: edge.distance_m,
            duration_secs: dur,
            instruction: format!("Continue on {:?} for {:.0} m", edge.road_type, edge.distance_m),
        });
    }

    Some(RouteResult { waypoints, total_distance_m, total_duration_secs, segments })
}

#[cfg(test)]
mod tests {
    use super::*;
    use crate::{Coordinate, RoadType, RouteEdge, RouteGraph, RouteNode};

    /// Two ways from A(0) to B(2): a fast highway via node 3, or a slow
    /// residential street via node 1 — plus a foot/cycle path 0→4→2.
    fn city() -> RouteGraph {
        let mut g = RouteGraph::new();
        let coords = [
            (0.0, 0.0),    // 0 start
            (0.0, 0.5),    // 1 residential mid
            (0.0, 1.0),    // 2 end
            (0.01, 0.5),   // 3 highway mid
            (-0.01, 0.5),  // 4 footpath mid
        ];
        for (id, c) in coords.iter().enumerate() {
            g.add_node(RouteNode { id: id as u64, coord: Coordinate::new(c.0, c.1) });
        }
        let mut edge = |from, to, road| {
            let d = haversine_distance(&g.nodes[&from].coord, &g.nodes[&to].coord);
            g.add_edge(RouteEdge { from, to, distance_m: d, duration_secs: 0.0, road_type: road });
        };
        // Residential A→1→B
        edge(0, 1, RoadType::Residential);
        edge(1, 2, RoadType::Residential);
        // Highway A→3→B
        edge(0, 3, RoadType::Highway);
        edge(3, 2, RoadType::Highway);
        // Footpath A→4→B
        edge(0, 4, RoadType::Footpath);
        edge(4, 2, RoadType::Footpath);
        g
    }

    #[test]
    fn car_prefers_highway_foot_cannot_use_it() {
        let g = city();
        let car = find_route_with(&g, 0, 2, &ModeProfile::new(TravelMode::Car)).unwrap();
        // Car takes the highway (node 3), the fastest by time.
        assert!(car.waypoints.iter().any(|w| (w.lat - 0.01).abs() < 1e-9));

        let foot = find_route_with(&g, 0, 2, &ModeProfile::new(TravelMode::Foot)).unwrap();
        // Foot cannot use highway → uses the footpath (node 4) or residential.
        assert!(!foot.waypoints.iter().any(|w| (w.lat - 0.01).abs() < 1e-9));
    }

    #[test]
    fn traffic_slows_the_car() {
        let g = city();
        let free = find_route_with(&g, 0, 2, &ModeProfile::new(TravelMode::Car)).unwrap();
        let jam = find_route_with(&g, 0, 2, &ModeProfile::new(TravelMode::Car).with_traffic(3.0)).unwrap();
        assert!(jam.total_duration_secs > free.total_duration_secs * 2.5);
        // Same distance, just slower.
        assert!((jam.total_distance_m - free.total_distance_m).abs() < 1e-6);
    }

    #[test]
    fn bicycle_uses_cycleway_but_not_footpath_or_highway() {
        let p = ModeProfile::new(TravelMode::Bicycle);
        assert!(p.allows(RoadType::Cycleway));
        assert!(p.allows(RoadType::Residential));
        assert!(!p.allows(RoadType::Footpath));
        assert!(!p.allows(RoadType::Highway));
    }

    #[test]
    fn no_accessible_path_is_none() {
        // Only a highway edge 0→1; foot cannot traverse it.
        let mut g = RouteGraph::new();
        g.add_node(RouteNode { id: 0, coord: Coordinate::new(0.0, 0.0) });
        g.add_node(RouteNode { id: 1, coord: Coordinate::new(0.0, 0.1) });
        let d = haversine_distance(&g.nodes[&0].coord, &g.nodes[&1].coord);
        g.add_edge(RouteEdge { from: 0, to: 1, distance_m: d, duration_secs: 0.0, road_type: RoadType::Highway });
        assert!(find_route_with(&g, 0, 1, &ModeProfile::new(TravelMode::Foot)).is_none());
        assert!(find_route_with(&g, 0, 1, &ModeProfile::new(TravelMode::Car)).is_some());
    }
}