routik-solver 0.1.0

//! Cost matrix abstraction and the Haversine (great-circle) implementation.
//!
//! The solver depends on the [`CostMatrix`] trait **only** — never on a
//! concrete matrix. `HaversineMatrix` is the straight-line fallback;
//! `ProvidedMatrix` carries caller- or OSRM-supplied numbers. OSRM is **not** a
//! matrix type here: it is network I/O (forbidden in this pure lib), so the
//! `routik-api` OSRM client fetches the `N×N` table once and fills a
//! `ProvidedMatrix` the solver then consumes like any other.

use crate::model::{Coord, LocationId, Problem};

/// Mean Earth radius in kilometres.
const EARTH_RADIUS_KM: f64 = 6371.0;

/// Travel cost between any two locations, addressed by [`LocationId`].
///
/// Distance is in kilometres; time is in hours (distance / average speed).
pub trait CostMatrix {
    /// Great-circle / road distance between `a` and `b`, in kilometres.
    fn distance(&self, a: LocationId, b: LocationId) -> f64;

    /// Travel time between `a` and `b`, in hours.
    fn time(&self, a: LocationId, b: LocationId) -> f64;
}

/// Straight-line (great-circle) cost matrix.
///
/// Distance is the Haversine distance between coordinates; time is that
/// distance divided by a configurable average speed (km/h).
#[derive(Debug, Clone)]
pub struct HaversineMatrix {
    coords: Vec<Coord>,
    speed_kmh: f64,
}

impl HaversineMatrix {
    /// Build a matrix over `coords`, indexed by [`LocationId`].
    ///
    /// `speed_kmh` is the average travel speed used to turn distance into time.
    #[must_use]
    pub fn new(coords: Vec<Coord>, speed_kmh: f64) -> Self {
        Self { coords, speed_kmh }
    }

    /// Build a matrix matching a [`Problem`]'s location layout: depot at
    /// [`LocationId::DEPOT`], then stops in order.
    #[must_use]
    pub fn from_problem(problem: &Problem, speed_kmh: f64) -> Self {
        Self::new(problem.coords(), speed_kmh)
    }
}

/// Haversine distance between two coordinates, in kilometres.
fn haversine_km(a: Coord, b: Coord) -> f64 {
    let lat1 = a.lat.to_radians();
    let lat2 = b.lat.to_radians();
    let dlat = (b.lat - a.lat).to_radians();
    let dlon = (b.lon - a.lon).to_radians();

    let h = (dlat / 2.0).sin().powi(2) + lat1.cos() * lat2.cos() * (dlon / 2.0).sin().powi(2);
    // clamp guards against tiny fp overshoot above 1.0 for near-antipodal pairs
    let c = 2.0 * h.sqrt().min(1.0).asin();
    EARTH_RADIUS_KM * c
}

impl CostMatrix for HaversineMatrix {
    fn distance(&self, a: LocationId, b: LocationId) -> f64 {
        haversine_km(self.coords[a.0], self.coords[b.0])
    }

    fn time(&self, a: LocationId, b: LocationId) -> f64 {
        self.distance(a, b) / self.speed_kmh
    }
}

/// Planar Euclidean cost matrix.
///
/// Distance is the straight-line distance between planar `(x, y)` coordinates,
/// stored in [`Coord`] as `lon = x`, `lat = y`. Following the Solomon VRPTW
/// convention, **travel time equals distance** (unit speed), so the same
/// numbers drive both legs and time-window arithmetic. This is the matrix the
/// Solomon benchmark uses — never Haversine.
#[derive(Debug, Clone)]
pub struct EuclideanMatrix {
    coords: Vec<Coord>,
}

impl EuclideanMatrix {
    /// Build a matrix over planar coordinates, indexed by [`LocationId`].
    #[must_use]
    pub fn new(coords: Vec<Coord>) -> Self {
        Self { coords }
    }

    /// Build a matrix matching a [`Problem`]'s location layout: depot at
    /// [`LocationId::DEPOT`], then stops in order.
    #[must_use]
    pub fn from_problem(problem: &Problem) -> Self {
        Self::new(problem.coords())
    }
}

/// Straight-line distance between two planar points (`lon = x`, `lat = y`).
fn euclidean(a: Coord, b: Coord) -> f64 {
    let dx = a.lon - b.lon;
    let dy = a.lat - b.lat;
    dx.hypot(dy)
}

impl CostMatrix for EuclideanMatrix {
    fn distance(&self, a: LocationId, b: LocationId) -> f64 {
        euclidean(self.coords[a.0], self.coords[b.0])
    }

    fn time(&self, a: LocationId, b: LocationId) -> f64 {
        // Solomon convention: unit speed, so time == distance.
        self.distance(a, b)
    }
}

/// Caller-supplied cost matrix: explicit distance and time between every pair
/// of locations, addressed by [`LocationId`] (`0` = depot, `1..=n` = stops).
///
/// This is the matrix used when the caller brings their own routing numbers
/// (e.g. from a maps engine) instead of relying on the straight-line
/// [`HaversineMatrix`]. Both inner matrices are square and share the same `n`.
#[derive(Debug, Clone)]
pub struct ProvidedMatrix {
    n: usize,
    distances: Vec<f64>, // row-major n×n
    times: Vec<f64>,     // row-major n×n
}

/// Why a [`ProvidedMatrix`] could not be built from caller input.
#[derive(Debug, thiserror::Error, PartialEq, Eq)]
pub enum MatrixError {
    /// A matrix was empty or not square (a row's length ≠ the number of rows).
    #[error("matrix must be a non-empty square: {rows} rows but a row of {row_len}")]
    NotSquare { rows: usize, row_len: usize },
    /// The distance and time matrices have different dimensions.
    #[error("distance matrix is {distance}×{distance} but time matrix is {time}×{time}")]
    SizeMismatch { distance: usize, time: usize },
}

impl ProvidedMatrix {
    /// Build from row-major `distances` and `times`, each an `n×n` matrix whose
    /// rows and columns are indexed by [`LocationId`] (depot first, then stops).
    ///
    /// # Errors
    /// [`MatrixError::NotSquare`] if either matrix is empty or any row's length
    /// differs from the row count; [`MatrixError::SizeMismatch`] if the two
    /// matrices disagree on `n`.
    pub fn new(distances: Vec<Vec<f64>>, times: Vec<Vec<f64>>) -> Result<Self, MatrixError> {
        let (n_d, distances) = Self::flatten(distances)?;
        let (n_t, times) = Self::flatten(times)?;
        if n_d != n_t {
            return Err(MatrixError::SizeMismatch {
                distance: n_d,
                time: n_t,
            });
        }
        Ok(Self {
            n: n_d,
            distances,
            times,
        })
    }

    /// Materialise any [`CostMatrix`] into a dense `n×n` store, indexed by
    /// [`LocationId`] `0..n`.
    ///
    /// The local search queries `distance`/`time` for the same pairs millions of
    /// times; precomputing turns each query into an array read instead of (for
    /// [`HaversineMatrix`]) recomputing great-circle trigonometry. The `n²`
    /// upfront cost and the dense storage only pay off below a size bound, so the
    /// caller decides when to densify.
    #[must_use]
    pub fn densify<M: CostMatrix>(source: &M, n: usize) -> Self {
        let mut distances = Vec::with_capacity(n * n);
        let mut times = Vec::with_capacity(n * n);
        for a in 0..n {
            for b in 0..n {
                distances.push(source.distance(LocationId(a), LocationId(b)));
                times.push(source.time(LocationId(a), LocationId(b)));
            }
        }
        Self {
            n,
            distances,
            times,
        }
    }

    /// Number of locations the matrix covers (`stops + 1`).
    #[must_use]
    pub fn len(&self) -> usize {
        self.n
    }

    /// Whether the matrix is empty (never true for a value built by [`Self::new`]).
    #[must_use]
    pub fn is_empty(&self) -> bool {
        self.n == 0
    }

    /// Validate squareness and flatten to row-major, returning `(n, flat)`.
    fn flatten(rows: Vec<Vec<f64>>) -> Result<(usize, Vec<f64>), MatrixError> {
        let n = rows.len();
        if n == 0 {
            return Err(MatrixError::NotSquare {
                rows: 0,
                row_len: 0,
            });
        }
        let mut flat = Vec::with_capacity(n * n);
        for row in rows {
            if row.len() != n {
                return Err(MatrixError::NotSquare {
                    rows: n,
                    row_len: row.len(),
                });
            }
            flat.extend(row);
        }
        Ok((n, flat))
    }
}

impl CostMatrix for ProvidedMatrix {
    fn distance(&self, a: LocationId, b: LocationId) -> f64 {
        self.distances[a.0 * self.n + b.0]
    }

    fn time(&self, a: LocationId, b: LocationId) -> f64 {
        self.times[a.0 * self.n + b.0]
    }
}

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

    fn matrix() -> HaversineMatrix {
        // 0: equator origin, 1: 1° east, 2: 1° north
        HaversineMatrix::new(
            vec![
                Coord::new(0.0, 0.0),
                Coord::new(0.0, 1.0),
                Coord::new(1.0, 0.0),
            ],
            60.0,
        )
    }

    #[test]
    fn distance_to_self_is_zero() {
        let m = matrix();
        assert_eq!(m.distance(LocationId(0), LocationId(0)), 0.0);
    }

    #[test]
    fn distance_is_symmetric() {
        let m = matrix();
        for a in 0..3 {
            for b in 0..3 {
                let ab = m.distance(LocationId(a), LocationId(b));
                let ba = m.distance(LocationId(b), LocationId(a));
                assert!((ab - ba).abs() < 1e-9, "asymmetry {a}->{b}: {ab} vs {ba}");
            }
        }
    }

    #[test]
    fn one_degree_is_about_111_km() {
        let m = matrix();
        // One degree of longitude at the equator ≈ 111.19 km.
        let d = m.distance(LocationId(0), LocationId(1));
        assert!((d - 111.19).abs() < 0.5, "expected ~111.19 km, got {d}");
        // One degree of latitude is also ≈ 111.19 km.
        let d_lat = m.distance(LocationId(0), LocationId(2));
        assert!(
            (d_lat - 111.19).abs() < 0.5,
            "expected ~111.19 km, got {d_lat}"
        );
    }

    #[test]
    fn time_is_distance_over_speed() {
        let m = matrix();
        let d = m.distance(LocationId(0), LocationId(1));
        let t = m.time(LocationId(0), LocationId(1));
        assert!((t - d / 60.0).abs() < 1e-9);
    }

    #[test]
    fn euclidean_is_pythagorean_and_time_equals_distance() {
        // (0,0), (3,4) -> distance 5. Coord stores lon=x, lat=y.
        let m = EuclideanMatrix::new(vec![Coord::new(0.0, 0.0), Coord::new(4.0, 3.0)]);
        let d = m.distance(LocationId(0), LocationId(1));
        assert!((d - 5.0).abs() < 1e-9, "expected 5.0, got {d}");
        // Solomon convention: time == distance.
        assert_eq!(m.time(LocationId(0), LocationId(1)), d);
        assert_eq!(m.distance(LocationId(0), LocationId(0)), 0.0);
    }

    #[test]
    fn provided_matrix_indexes_row_major() {
        let m = ProvidedMatrix::new(
            vec![
                vec![0.0, 1.0, 2.0],
                vec![3.0, 0.0, 4.0],
                vec![5.0, 6.0, 0.0],
            ],
            vec![
                vec![0.0, 10.0, 20.0],
                vec![30.0, 0.0, 40.0],
                vec![50.0, 60.0, 0.0],
            ],
        )
        .expect("square");
        assert_eq!(m.len(), 3);
        assert_eq!(m.distance(LocationId(1), LocationId(2)), 4.0);
        assert_eq!(m.distance(LocationId(2), LocationId(0)), 5.0);
        assert_eq!(m.time(LocationId(0), LocationId(2)), 20.0);
        // Asymmetry is preserved (a real road matrix need not be symmetric).
        assert_ne!(
            m.distance(LocationId(0), LocationId(1)),
            m.distance(LocationId(1), LocationId(0))
        );
    }

    #[test]
    fn provided_matrix_rejects_non_square() {
        let err = ProvidedMatrix::new(
            vec![vec![0.0, 1.0], vec![1.0]],
            vec![vec![0.0, 1.0], vec![1.0, 0.0]],
        )
        .unwrap_err();
        assert_eq!(
            err,
            MatrixError::NotSquare {
                rows: 2,
                row_len: 1
            }
        );
        assert_eq!(
            ProvidedMatrix::new(vec![], vec![]).unwrap_err(),
            MatrixError::NotSquare {
                rows: 0,
                row_len: 0
            }
        );
    }

    #[test]
    fn densify_matches_source_exactly() {
        // Densifying must be value-identical to the lazy source, or it would
        // change solver results. Bit-exact, not approximate.
        let h = HaversineMatrix::new(
            vec![
                Coord::new(48.8566, 2.3522),
                Coord::new(48.8606, 2.3376),
                Coord::new(48.8530, 2.3499),
                Coord::new(48.8738, 2.2950),
            ],
            50.0,
        );
        let dense = ProvidedMatrix::densify(&h, 4);
        assert_eq!(dense.len(), 4);
        for a in 0..4 {
            for b in 0..4 {
                let (la, lb) = (LocationId(a), LocationId(b));
                assert_eq!(dense.distance(la, lb), h.distance(la, lb));
                assert_eq!(dense.time(la, lb), h.time(la, lb));
            }
        }
    }

    #[test]
    fn provided_matrix_rejects_size_mismatch() {
        let err =
            ProvidedMatrix::new(vec![vec![0.0, 1.0], vec![1.0, 0.0]], vec![vec![0.0]]).unwrap_err();
        assert_eq!(
            err,
            MatrixError::SizeMismatch {
                distance: 2,
                time: 1
            }
        );
    }
}