maps_engine_rust/

navigation.rs

//! Turn-by-turn navigation math. Port of `navigation.ts` + `routing.ts`.

use crate::geo::{bearing, destination, haversine, polyline_length, LonLat};

/// A single navigation maneuver.
#[derive(Debug, Clone)]
pub struct Maneuver {
    /// Index of the route point where the maneuver happens.
    pub at_index: usize,
    pub location: LonLat,
    /// Turn angle in degrees: negative = left, positive = right.
    pub turn_deg: f64,
    pub instruction_vi: String,
    pub instruction_en: String,
}

/// Navigation progress along a route.
#[derive(Debug, Clone)]
pub struct NavProgress {
    /// Distance already covered in meters.
    pub distance_done_m: f64,
    /// Distance remaining in meters.
    pub distance_left_m: f64,
    /// Fraction completed in `[0, 1]`.
    pub fraction: f64,
    /// Index of the next route point ahead.
    pub next_index: usize,
}

/// Classify a turn angle into a human instruction.
pub fn turn_instruction(turn_deg: f64) -> (&'static str, &'static str) {
    let a = turn_deg.abs();
    if a < 20.0 {
        ("Đi thẳng", "Go straight")
    } else if a < 70.0 {
        if turn_deg < 0.0 {
            ("Rẽ trái", "Turn left")
        } else {
            ("Rẽ phải", "Turn right")
        }
    } else if a < 130.0 {
        if turn_deg < 0.0 {
            ("Rẽ trái gắt", "Sharp left")
        } else {
            ("Rẽ phải gắt", "Sharp right")
        }
    } else {
        ("Quay đầu", "Make a U-turn")
    }
}

/// Detect maneuvers along a route polyline.
pub fn detect_maneuvers(route: &[LonLat]) -> Vec<Maneuver> {
    let mut out = Vec::new();
    if route.len() < 3 {
        return out;
    }
    for i in 1..route.len() - 1 {
        let b1 = bearing(route[i - 1], route[i]);
        let b2 = bearing(route[i], route[i + 1]);
        let mut turn = b2 - b1;
        while turn > 180.0 {
            turn -= 360.0;
        }
        while turn < -180.0 {
            turn += 360.0;
        }
        if turn.abs() >= 20.0 {
            let (vi, en) = turn_instruction(turn);
            out.push(Maneuver {
                at_index: i,
                location: route[i],
                turn_deg: turn,
                instruction_vi: vi.to_string(),
                instruction_en: en.to_string(),
            });
        }
    }
    out
}

/// Compute navigation progress given the user's current position.
///
/// Finds the nearest route point ahead and reports distance done/left.
pub fn nav_progress(route: &[LonLat], user: LonLat) -> Option<NavProgress> {
    if route.len() < 2 {
        return None;
    }
    let total = polyline_length(route);
    // Nearest point index (simple; fine for demo-scale routes).
    let mut best = 0;
    let mut best_d = f64::INFINITY;
    for (i, p) in route.iter().enumerate() {
        let d = haversine(user, *p);
        if d < best_d {
            best_d = d;
            best = i;
        }
    }
    let done: f64 = route
        .windows(2)
        .take(best)
        .map(|w| haversine(w[0], w[1]))
        .sum();
    let left = (total - done).max(0.0);
    Some(NavProgress {
        distance_done_m: done,
        distance_left_m: left,
        fraction: if total > 0.0 {
            (done / total).min(1.0)
        } else {
            0.0
        },
        next_index: (best + 1).min(route.len() - 1),
    })
}

/// Simulate moving along the route: position after travelling `dist_m` from start.
pub fn position_at_distance(route: &[LonLat], dist_m: f64) -> Option<LonLat> {
    if route.is_empty() {
        return None;
    }
    let mut remaining = dist_m;
    for w in route.windows(2) {
        let seg = haversine(w[0], w[1]);
        if remaining <= seg {
            let brg = bearing(w[0], w[1]);
            return Some(destination(w[0], remaining, brg));
        }
        remaining -= seg;
    }
    Some(*route.last().unwrap())
}

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

    fn l_shape() -> Vec<LonLat> {
        vec![
            LonLat::new(105.85, 21.02),
            LonLat::new(105.86, 21.02), // east
            LonLat::new(105.86, 21.03), // north -> left turn
        ]
    }

    fn gentle_left() -> Vec<LonLat> {
        vec![
            LonLat::new(105.85, 21.02),
            LonLat::new(105.86, 21.02), // east
            LonLat::new(105.87, 21.03), // north-east -> ~45° left turn
        ]
    }

    #[test]
    fn detects_left_turn() {
        let m = detect_maneuvers(&gentle_left());
        assert_eq!(m.len(), 1);
        assert!(m[0].turn_deg < 0.0);
        assert_eq!(m[0].instruction_vi, "Rẽ trái");
    }

    #[test]
    fn detects_sharp_left_turn() {
        let m = detect_maneuvers(&l_shape());
        assert_eq!(m.len(), 1);
        assert_eq!(m[0].instruction_vi, "Rẽ trái gắt");
    }

    #[test]
    fn straight_route_has_no_maneuvers() {
        let r = vec![
            LonLat::new(105.85, 21.02),
            LonLat::new(105.86, 21.02),
            LonLat::new(105.87, 21.02),
        ];
        assert!(detect_maneuvers(&r).is_empty());
    }

    #[test]
    fn progress_at_start_and_end() {
        let r = l_shape();
        let p0 = nav_progress(&r, r[0]).unwrap();
        assert!(p0.distance_done_m < 1.0);
        assert!(p0.fraction < 0.01);
        let p1 = nav_progress(&r, *r.last().unwrap()).unwrap();
        assert!(p1.distance_left_m < 1.0);
        assert!((p1.fraction - 1.0).abs() < 0.01);
    }

    #[test]
    fn position_at_distance_mid() {
        let r = vec![LonLat::new(105.85, 21.02), LonLat::new(105.86, 21.02)];
        let total = polyline_length(&r);
        let mid = position_at_distance(&r, total / 2.0).unwrap();
        assert!((mid.lon - 105.855).abs() < 0.001);
    }
}