graphways 0.3.0

use petgraph::algo::astar;
use petgraph::graph::NodeIndex;

use crate::error::OsmGraphError;
use crate::graph::{SpatialGraph, XmlWay};
use crate::overpass::NetworkType;
use crate::utils::calculate_distance;

#[derive(Debug, Clone)]
pub struct Route {
    /// Ordered list of (lat, lon) coordinates along the route
    pub coordinates: Vec<(f64, f64)>,
    /// Cumulative travel time in seconds at each coordinate (parallel to `coordinates`)
    pub cumulative_times_s: Vec<f64>,
    /// Total route distance in meters
    pub distance_m: f64,
    /// Total travel time in seconds for the given network type
    pub duration_s: f64,
}

pub fn route(
    sg: &SpatialGraph,
    origin_lat: f64,
    origin_lon: f64,
    dest_lat: f64,
    dest_lon: f64,
    network_type: NetworkType,
) -> Result<Route, OsmGraphError> {
    let origin = sg
        .nearest_node(origin_lat, origin_lon)
        .ok_or(OsmGraphError::OriginNodeNotFound)?;
    let dest = sg
        .nearest_node(dest_lat, dest_lon)
        .ok_or(OsmGraphError::DestinationNodeNotFound)?;

    let edge_cost =
        |e: petgraph::graph::EdgeReference<XmlWay>| -> f64 { e.weight().travel_time(network_type) };

    // Heuristic: straight-line travel time from node to destination
    let heuristic = |node: NodeIndex| -> f64 {
        let n = &sg.graph[node];
        let d = &sg.graph[dest];
        let dist = calculate_distance(n.lat, n.lon, d.lat, d.lon);
        // Use a generous speed so the heuristic is admissible (never overestimates)
        let max_speed_m_per_s = 200.0 / 3.6; // 200 kph
        dist / max_speed_m_per_s
    };

    let result = astar(&*sg.graph, origin, |n| n == dest, edge_cost, heuristic)
        .ok_or(OsmGraphError::PathNotFound)?;

    let (_, path) = result;

    let coordinates: Vec<(f64, f64)> = path
        .iter()
        .map(|&idx| {
            let n = &sg.graph[idx];
            (n.lat, n.lon)
        })
        .collect();

    // Aggregate distance, duration, and cumulative times along the path
    let mut distance_m = 0.0;
    let mut duration_s = 0.0;
    let mut cumulative_times_s = vec![0.0_f64]; // origin starts at t=0
    for window in path.windows(2) {
        if let [u, v] = window {
            if let Some(edge) = sg.graph.find_edge(*u, *v) {
                let way = sg.graph.edge_weight(edge).unwrap();
                distance_m += way.length;
                duration_s += way.travel_time(network_type);
                cumulative_times_s.push(duration_s);
            }
        }
    }

    Ok(Route {
        coordinates,
        cumulative_times_s,
        distance_m,
        duration_s,
    })
}

impl SpatialGraph {
    /// Find the shortest route between two lat/lon points.
    ///
    /// Snaps both points to the nearest graph nodes, then runs A* to find the
    /// optimal path. Returns [`OsmGraphError::OriginNodeNotFound`] or
    /// [`OsmGraphError::DestinationNodeNotFound`] if snapping fails, and
    /// [`OsmGraphError::PathNotFound`] if the snapped nodes are disconnected.
    pub fn route(
        &self,
        origin_lat: f64,
        origin_lon: f64,
        dest_lat: f64,
        dest_lon: f64,
        network_type: NetworkType,
    ) -> Result<Route, OsmGraphError> {
        route(
            self,
            origin_lat,
            origin_lon,
            dest_lat,
            dest_lon,
            network_type,
        )
    }
}

#[cfg(test)]
mod tests {
    use super::*;
    use crate::graph::{SpatialGraph, XmlNode, XmlTag, XmlWay};
    use crate::overpass::NetworkType;
    use petgraph::graph::DiGraph;

    fn make_node(id: i64, lat: f64, lon: f64) -> XmlNode {
        XmlNode {
            id,
            lat,
            lon,
            tags: vec![],
        }
    }

    fn make_way(drive_travel_time: f64, length: f64) -> XmlWay {
        XmlWay {
            id: 1,
            nodes: vec![],
            tags: vec![XmlTag {
                key: "highway".into(),
                value: "residential".into(),
            }],
            length,
            speed_kph: 50.0,
            walk_travel_time: length / (5.0 / 3.6),
            bike_travel_time: length / (15.0 / 3.6),
            drive_travel_time,
        }
    }

    fn linear_graph() -> SpatialGraph {
        // A → B → C along a straight line
        let mut g = DiGraph::new();
        let a = g.add_node(make_node(1, 0.0, 0.0));
        let b = g.add_node(make_node(2, 0.001, 0.0));
        let c = g.add_node(make_node(3, 0.002, 0.0));
        g.add_edge(a, b, make_way(10.0, 111.0));
        g.add_edge(b, c, make_way(10.0, 111.0));
        SpatialGraph::new(g)
    }

    #[test]
    fn test_cumulative_times_starts_at_zero() {
        let sg = linear_graph();
        let r = route(&sg, 0.0, 0.0, 0.002, 0.0, NetworkType::Drive).unwrap();
        assert_eq!(r.cumulative_times_s[0], 0.0);
    }

    #[test]
    fn test_cumulative_times_parallel_to_coordinates() {
        let sg = linear_graph();
        let r = route(&sg, 0.0, 0.0, 0.002, 0.0, NetworkType::Drive).unwrap();
        assert_eq!(r.cumulative_times_s.len(), r.coordinates.len());
    }

    #[test]
    fn test_cumulative_times_monotonic() {
        let sg = linear_graph();
        let r = route(&sg, 0.0, 0.0, 0.002, 0.0, NetworkType::Drive).unwrap();
        for w in r.cumulative_times_s.windows(2) {
            assert!(w[1] >= w[0], "times decreased: {:?}", r.cumulative_times_s);
        }
    }

    #[test]
    fn test_cumulative_times_last_equals_duration() {
        let sg = linear_graph();
        let r = route(&sg, 0.0, 0.0, 0.002, 0.0, NetworkType::Drive).unwrap();
        let last = *r.cumulative_times_s.last().unwrap();
        assert!(
            (last - r.duration_s).abs() < 1e-6,
            "last cumulative time {last:.6} != duration {:.6}",
            r.duration_s
        );
    }
}