routik-solver 0.1.0

//! Clarke-Wright savings construction heuristic.
//!
//! Produces an initial **capacity-feasible** solution: each stop starts on its
//! own out-and-back route, then routes are merged in decreasing order of the
//! savings `s(i,j) = d(depot,i) + d(depot,j) - d(i,j)` as long as the combined
//! load fits a vehicle. Time windows are **not** enforced yet.
//!
//! The fleet is treated as homogeneous: the binding capacity is that of the
//! smallest vehicle, so any route built here fits any vehicle.

use crate::feasibility::{build_solution, evaluate_route};
use crate::matrix::CostMatrix;
use crate::model::{LocationId, Problem, Solution, Unassigned, UnassignedCode};

/// A structural error that stops construction before it can begin.
///
/// Over-constrained instances are **not** errors: stops that cannot be placed
/// are returned in [`Solution::unassigned`](crate::Solution::unassigned) with a
/// diagnostic reason, and the routable stops are still solved. The only thing
/// left here is a problem with no fleet at all.
#[derive(Debug, thiserror::Error, PartialEq, Eq)]
pub enum SolveError {
    /// The problem has no vehicles to assign routes to.
    #[error("no vehicles in the problem")]
    NoVehicles,
}

/// Build a solution with the Clarke-Wright savings heuristic, diagnosing any
/// stops it cannot place.
///
/// `matrix` must be indexed to match `problem` (depot at
/// [`LocationId::DEPOT`](crate::model::LocationId::DEPOT), stops at `1..=n`);
/// [`HaversineMatrix::from_problem`](crate::matrix::HaversineMatrix::from_problem)
/// guarantees that layout.
///
/// Over-constrained instances do not fail: a stop that cannot sit on any route
/// (its demand exceeds capacity, its window is unreachable) or that the fleet has
/// no room for is returned in [`Solution::unassigned`](crate::Solution::unassigned)
/// with an indicative reason, while the routable stops are still routed.
///
/// # Errors
/// Returns [`SolveError::NoVehicles`] when the fleet is empty — the one
/// structural case with nothing to solve over.
pub fn clarke_wright(problem: &Problem, matrix: &impl CostMatrix) -> Result<Solution, SolveError> {
    if problem.vehicles.is_empty() {
        return Err(SolveError::NoVehicles);
    }
    let n = problem.stops.len();
    let fleet = problem.vehicles.len();

    // Homogeneous-fleet capacity = the smallest vehicle (safe lower bound).
    let capacity = problem
        .vehicles
        .iter()
        .map(|v| v.capacity)
        .min()
        .unwrap_or(0);

    // Pass 1 — single-stop infeasibility. A stop whose demand exceeds capacity,
    // or that cannot be reached and serviced within its own window, can never
    // sit on any feasible route. These reasons are exact, not heuristic.
    let mut unassigned: Vec<Unassigned> = Vec::new();
    let mut excluded = vec![false; n];
    for (idx, stop) in problem.stops.iter().enumerate() {
        if stop.demand > capacity {
            excluded[idx] = true;
            unassigned.push(Unassigned {
                stop: stop.id,
                code: UnassignedCode::CapacityExceeded,
                detail: format!("demand {} exceeds vehicle capacity {capacity}", stop.demand),
            });
        } else if evaluate_route(problem, matrix, capacity, &[idx + 1]).is_none() {
            excluded[idx] = true;
            unassigned.push(Unassigned {
                stop: stop.id,
                code: UnassignedCode::TimeWindowInfeasible,
                detail: "cannot be reached and serviced within its time window, then \
                         returned before the depot closes — even alone on an empty route"
                    .to_string(),
            });
        }
    }

    // Working state, over the *routable* stops only. Locations are 1..=n; route
    // `r` is an ordered list of those indices. `route_of[loc-1]` is the route
    // currently holding `loc`. Excluded stops start with an empty route and are
    // never merged, so they fall out below.
    let mut routes: Vec<Vec<usize>> = (1..=n)
        .map(|loc| {
            if excluded[loc - 1] {
                Vec::new()
            } else {
                vec![loc]
            }
        })
        .collect();
    let mut route_of: Vec<usize> = (0..n).collect();
    let mut load: Vec<u32> = problem.stops.iter().map(|s| s.demand).collect();

    // Savings for every unordered pair of routable stops.
    let depot = LocationId::DEPOT;
    let mut savings: Vec<(f64, usize, usize)> = Vec::with_capacity(n * n / 2);
    for i in 1..=n {
        if excluded[i - 1] {
            continue;
        }
        let d_di = matrix.distance(depot, LocationId(i));
        for j in (i + 1)..=n {
            if excluded[j - 1] {
                continue;
            }
            let s = d_di + matrix.distance(depot, LocationId(j))
                - matrix.distance(LocationId(i), LocationId(j));
            savings.push((s, i, j));
        }
    }
    // Descending savings; total_cmp keeps it total and clippy-clean on f64.
    savings.sort_by(|a, b| b.0.total_cmp(&a.0));

    for (s, i, j) in savings {
        // No benefit (or a penalty) from here on out.
        if s <= 0.0 {
            break;
        }
        let ri = route_of[i - 1];
        let rj = route_of[j - 1];
        if ri == rj {
            continue;
        }
        // Compare in u64: a merged demand can exceed u32 on adversarial input, and
        // an overflowing add would panic. The summed routes stay u32 once merged
        // (the merge only proceeds when the sum is ≤ capacity ≤ u32::MAX).
        if u64::from(load[ri]) + u64::from(load[rj]) > u64::from(capacity) {
            continue;
        }

        // Merge only if i and j sit at the ends of their routes, so they can
        // become interior-adjacent without reordering anyone else.
        let i_first = routes[ri].first().copied() == Some(i);
        let i_last = routes[ri].last().copied() == Some(i);
        let j_first = routes[rj].first().copied() == Some(j);
        let j_last = routes[rj].last().copied() == Some(j);
        if !(i_first || i_last) || !(j_first || j_last) {
            continue;
        }

        // Build the candidate [..i] ++ [j..] (i and j adjacent) without
        // mutating yet, so we can drop it if it breaks a time window.
        let mut left = routes[ri].clone();
        let mut right = routes[rj].clone();
        if left.first().copied() == Some(i) {
            left.reverse();
        }
        if right.last().copied() == Some(j) {
            right.reverse();
        }
        left.extend_from_slice(&right);

        // Capacity is already checked; reject the merge if it breaks a window.
        if evaluate_route(problem, matrix, capacity, &left).is_none() {
            continue;
        }

        for &loc in &left {
            route_of[loc - 1] = ri;
        }
        load[ri] += load[rj];
        load[rj] = 0;
        routes[ri] = left;
        routes[rj].clear(); // merged away; stays empty
    }

    let mut final_routes: Vec<Vec<usize>> = routes.into_iter().filter(|r| !r.is_empty()).collect();

    // Pass 2 — fleet exhaustion. Construction merged the routable stops into as
    // few routes as the windows and capacity allow; if that is still more than
    // the fleet, some stops cannot be served. Keep the routes that cover the most
    // stops, then try to re-insert each dropped stop into a kept route (the last
    // failed attempt decides its reason). `sort_by` is stable, so equal-length
    // routes keep their construction order and the result stays deterministic.
    if final_routes.len() > fleet {
        final_routes.sort_by_key(|r| std::cmp::Reverse(r.len()));
        let surplus = final_routes.split_off(fleet);
        for route in surplus {
            for loc in route {
                place_or_diagnose(
                    problem,
                    matrix,
                    capacity,
                    &mut final_routes,
                    loc,
                    &mut unassigned,
                );
            }
        }
    }

    let mut solution = build_solution(problem, matrix, capacity, &final_routes);
    solution.feasible = solution.feasible && unassigned.is_empty();
    solution.unassigned = unassigned;
    Ok(solution)
}

/// Try to insert stop `loc` into the best feasible slot of an already-built
/// `kept` route; on success the route is updated in place. On failure — the
/// fleet has no room for it — record the stop as unassigned, reporting whether
/// capacity or the time windows blocked every attempted insertion (the
/// jsprit-style "reason of the last failed attempt").
fn place_or_diagnose(
    problem: &Problem,
    matrix: &impl CostMatrix,
    capacity: u32,
    kept: &mut [Vec<usize>],
    loc: usize,
    unassigned: &mut Vec<Unassigned>,
) {
    let stop = &problem.stops[loc - 1];
    let mut all_capacity_blocked = true;

    for route in kept.iter_mut() {
        // u64 sum: a route's demand can exceed u32 on adversarial input, so both
        // the accumulation and the comparison run in u64 to avoid an overflow panic.
        let route_load: u64 = route
            .iter()
            .map(|&l| u64::from(problem.stops[l - 1].demand))
            .sum();
        if route_load + u64::from(stop.demand) > u64::from(capacity) {
            continue; // no room on this route; a capacity block
        }
        all_capacity_blocked = false;
        for pos in 0..=route.len() {
            let mut candidate = route.clone();
            candidate.insert(pos, loc);
            if evaluate_route(problem, matrix, capacity, &candidate).is_some() {
                *route = candidate;
                return; // placed
            }
        }
    }

    // The stop is fine on its own (it passed pass 1) but no remaining vehicle can
    // take it: the fleet is exhausted. Name the dominant blocker in the detail.
    let blocker = if all_capacity_blocked {
        "every assigned route is already at capacity"
    } else {
        "no assigned route has a time-feasible slot for it"
    };
    unassigned.push(Unassigned {
        stop: stop.id,
        code: UnassignedCode::NoCompatibleVehicle,
        detail: format!(
            "the fleet of {} vehicle(s) is fully assigned and this stop could not be \
             inserted into any existing route ({blocker})",
            kept.len()
        ),
    });
}

#[cfg(test)]
mod tests {
    use super::*;
    use crate::matrix::HaversineMatrix;
    use crate::model::{Coord, Stop, StopId, TimeWindow, Vehicle, VehicleId};
    use std::collections::HashSet;

    fn stop(id: u32, lat: f64, lon: f64, demand: u32) -> Stop {
        Stop {
            id: StopId(id),
            coord: Coord::new(lat, lon),
            demand,
            time_window: None,
            service_time: 0.1,
        }
    }

    fn sample_problem() -> Problem {
        Problem {
            depot: Coord::new(48.8566, 2.3522),
            stops: vec![
                stop(1, 48.8606, 2.3376, 10),
                stop(2, 48.8530, 2.3499, 15),
                stop(3, 48.8738, 2.2950, 20),
                stop(4, 48.8462, 2.3372, 12),
                stop(5, 48.8867, 2.3431, 8),
                stop(6, 48.8330, 2.3708, 25),
            ],
            vehicles: vec![
                Vehicle {
                    id: VehicleId(1),
                    capacity: 50,
                },
                Vehicle {
                    id: VehicleId(2),
                    capacity: 50,
                },
                Vehicle {
                    id: VehicleId(3),
                    capacity: 50,
                },
            ],
            depot_window: None,
        }
    }

    #[test]
    fn each_route_respects_capacity() {
        let problem = sample_problem();
        let matrix = HaversineMatrix::from_problem(&problem, 50.0);
        let solution = clarke_wright(&problem, &matrix).expect("feasible");

        assert!(solution.feasible);
        for route in &solution.routes {
            assert!(route.load <= 50, "route load {} > 50", route.load);
        }
    }

    #[test]
    fn total_load_equals_total_demand() {
        let problem = sample_problem();
        let matrix = HaversineMatrix::from_problem(&problem, 50.0);
        let solution = clarke_wright(&problem, &matrix).expect("feasible");

        let demand: u32 = problem.stops.iter().map(|s| s.demand).sum();
        let load: u32 = solution.routes.iter().map(|r| r.load).sum();
        assert_eq!(load, demand);
    }

    #[test]
    fn every_stop_served_exactly_once() {
        let problem = sample_problem();
        let matrix = HaversineMatrix::from_problem(&problem, 50.0);
        let solution = clarke_wright(&problem, &matrix).expect("feasible");

        let mut seen: Vec<StopId> = solution
            .routes
            .iter()
            .flat_map(|r| r.stop_ids.iter().copied())
            .collect();
        let unique: HashSet<StopId> = seen.iter().copied().collect();

        assert_eq!(
            seen.len(),
            problem.stops.len(),
            "wrong number of stops served"
        );
        assert_eq!(unique.len(), problem.stops.len(), "a stop was duplicated");

        seen.sort();
        let mut expected: Vec<StopId> = problem.stops.iter().map(|s| s.id).collect();
        expected.sort();
        assert_eq!(seen, expected);
    }

    #[test]
    fn oversized_demand_is_unassigned() {
        // A stop bigger than any truck can never be placed; the rest still are.
        let mut problem = sample_problem();
        problem.stops[0].demand = 999;
        let matrix = HaversineMatrix::from_problem(&problem, 50.0);
        let solution = clarke_wright(&problem, &matrix).expect("routable stops still solved");

        assert!(
            !solution.feasible,
            "an unassigned stop must flip feasible to false"
        );
        assert_eq!(solution.unassigned.len(), 1);
        let u = &solution.unassigned[0];
        assert_eq!(u.stop, StopId(1));
        assert_eq!(u.code, UnassignedCode::CapacityExceeded);
        assert!(u.detail.contains("999"), "detail should quote the demand");

        // The other five stops are routed, and stop 1 appears in no route.
        let served: usize = solution.routes.iter().map(|r| r.stop_ids.len()).sum();
        assert_eq!(served, problem.stops.len() - 1);
        assert!(
            solution
                .routes
                .iter()
                .flat_map(|r| &r.stop_ids)
                .all(|s| *s != StopId(1))
        );
    }

    #[test]
    fn unreachable_window_is_unassigned() {
        // Stop 3 sits kilometres from the depot, but its window slams shut at
        // t=0.001 h — unreachable even alone on an empty route.
        let mut problem = sample_problem();
        problem.stops[2].time_window = Some(TimeWindow {
            start: 0.0,
            end: 0.001,
        });
        let matrix = HaversineMatrix::from_problem(&problem, 50.0);
        let solution = clarke_wright(&problem, &matrix).expect("other stops solved");

        let u = solution
            .unassigned
            .iter()
            .find(|u| u.stop == StopId(3))
            .expect("stop 3 should be unassigned");
        assert_eq!(u.code, UnassignedCode::TimeWindowInfeasible);
        assert!(!solution.feasible);
    }

    #[test]
    fn fleet_exhaustion_unassigns_with_no_compatible_vehicle() {
        // Four full-truck stops (demand == capacity) need four routes, but only
        // two vehicles exist: two stops cannot be served by any remaining truck.
        let problem = Problem {
            depot: Coord::new(48.8566, 2.3522),
            stops: vec![
                stop(1, 48.8606, 2.3376, 30),
                stop(2, 48.8530, 2.3499, 30),
                stop(3, 48.8738, 2.2950, 30),
                stop(4, 48.8462, 2.3372, 30),
            ],
            vehicles: vec![
                Vehicle {
                    id: VehicleId(1),
                    capacity: 30,
                },
                Vehicle {
                    id: VehicleId(2),
                    capacity: 30,
                },
            ],
            depot_window: None,
        };
        let matrix = HaversineMatrix::from_problem(&problem, 50.0);
        let solution = clarke_wright(&problem, &matrix).expect("two stops solved");

        assert!(!solution.feasible);
        assert_eq!(solution.routes.len(), 2);
        assert_eq!(solution.unassigned.len(), 2);
        assert!(
            solution
                .unassigned
                .iter()
                .all(|u| u.code == UnassignedCode::NoCompatibleVehicle)
        );

        // Nothing vanishes: every stop is either routed or explicitly unassigned.
        let routed: usize = solution.routes.iter().map(|r| r.stop_ids.len()).sum();
        assert_eq!(routed + solution.unassigned.len(), problem.stops.len());
    }

    #[test]
    fn rejects_empty_fleet() {
        let mut problem = sample_problem();
        problem.vehicles.clear();
        let matrix = HaversineMatrix::from_problem(&problem, 50.0);
        assert_eq!(
            clarke_wright(&problem, &matrix),
            Err(SolveError::NoVehicles)
        );
    }

    #[test]
    fn merges_into_fewer_routes_than_stops() {
        // With slack capacity, savings merges should beat one-route-per-stop.
        let problem = sample_problem();
        let matrix = HaversineMatrix::from_problem(&problem, 50.0);
        let solution = clarke_wright(&problem, &matrix).expect("feasible");
        assert!(solution.routes.len() < problem.stops.len());
    }
}