pdp_lns 0.1.1

use crate::instance::{Instance, arc_bit};
use rand::Rng;
use rand::RngExt;

/// Fast f64 max that compiles to a single `maxsd` instruction.
/// Unlike `f64::max`, does NOT handle NaN (returns NaN if either input is NaN).
/// Safe for our use because distances, arrival times, and time windows are never NaN.
#[inline(always)]
pub fn fmax(a: f64, b: f64) -> f64 {
    if a > b { a } else { b }
}

/// Fast f64 min that compiles to a single `minsd` instruction.
/// Unlike `f64::min`, does NOT handle NaN.
#[inline(always)]
pub fn fmin(a: f64, b: f64) -> f64 {
    if a < b { a } else { b }
}

/// Circle sector for geometric pre-filtering of SWAP* route pairs.
/// Uses u16 wrapping arithmetic (equivalent to HGS-CVRP's positive_mod(x, 65536)).
#[derive(Clone, Copy, Default)]
pub struct CircleSector {
    pub start: u16,
    pub end: u16,
}

impl CircleSector {
    pub fn from_point(angle: u16) -> Self {
        Self {
            start: angle,
            end: angle,
        }
    }

    pub fn is_enclosed(&self, point: u16) -> bool {
        point.wrapping_sub(self.start) <= self.end.wrapping_sub(self.start)
    }

    pub fn extend(&mut self, point: u16) {
        if !self.is_enclosed(point) {
            if point.wrapping_sub(self.end) <= self.start.wrapping_sub(point) {
                self.end = point;
            } else {
                self.start = point;
            }
        }
    }

    pub fn overlap(a: &CircleSector, b: &CircleSector) -> bool {
        b.start.wrapping_sub(a.start) <= a.end.wrapping_sub(a.start)
            || a.start.wrapping_sub(b.start) <= b.end.wrapping_sub(b.start)
    }
}

/// TWData for O(1) time-warp concatenation (Vidal 2022 / Kool 2024).
/// Stores cumulative duration, time warp, and feasible start window
/// for a segment of a route. Two segments can be merged in O(1).
#[derive(Clone, Copy)]
pub struct TWData {
    pub duration: f64,  // cumulative travel + service time
    pub time_warp: f64, // cumulative TW violation
    pub earliest: f64,  // earliest feasible start time
    pub latest: f64,    // latest start without additional TW
}

impl TWData {
    #[inline]
    pub fn from_node(inst: &Instance, v: usize) -> Self {
        TWData {
            duration: inst.svc(v),
            time_warp: 0.0,
            earliest: inst.early(v),
            latest: inst.late(v),
        }
    }
}

/// Merge two consecutive TWData segments A, B separated by travel distance `dist`.
/// Formula source: HGS-CVRP (Vidal 2022).
#[inline]
pub fn tw_merge(a: TWData, b: TWData, dist: f64) -> TWData {
    let delta_tw = fmax(0.0, a.earliest + a.duration + dist - b.latest);
    let delta_wt = fmax(0.0, b.earliest - a.duration - dist - a.latest);
    TWData {
        duration: a.duration + b.duration + dist + delta_wt,
        time_warp: a.time_warp + b.time_warp + delta_tw,
        earliest: fmax(b.earliest - a.duration - delta_wt - dist, a.earliest) + delta_tw,
        latest: fmin(b.latest - a.duration + delta_tw - dist, a.latest) - delta_wt,
    }
}

/// Adaptive penalty coefficients for capacity and TW relaxation (HGS-CVRP idea).
/// When active, local search explores mildly infeasible solutions
/// using soft penalties instead of hard constraint checks.
#[derive(Clone)]
pub struct PenaltyState {
    pub penalty_cap: f64,
    pub penalty_tw: f64,
    cap_feasible_count: u32,
    cap_total_count: u32,
    tw_feasible_count: u32,
    tw_total_count: u32,
}

impl PenaltyState {
    pub fn disabled() -> Self {
        Self {
            penalty_cap: 0.0,
            penalty_tw: 0.0,
            cap_feasible_count: 0,
            cap_total_count: 0,
            tw_feasible_count: 0,
            tw_total_count: 0,
        }
    }

    pub fn new(penalty_cap: f64, penalty_tw: f64) -> Self {
        Self {
            penalty_cap,
            penalty_tw,
            cap_feasible_count: 0,
            cap_total_count: 0,
            tw_feasible_count: 0,
            tw_total_count: 0,
        }
    }

    #[inline]
    pub fn is_active(&self) -> bool {
        self.penalty_cap > 0.0 || self.penalty_tw > 0.0
    }

    pub fn record_cap_feasibility(&mut self, feasible: bool) {
        self.cap_total_count += 1;
        if feasible {
            self.cap_feasible_count += 1;
        }
    }

    pub fn record_tw_feasibility(&mut self, feasible: bool) {
        self.tw_total_count += 1;
        if feasible {
            self.tw_feasible_count += 1;
        }
    }

    /// Adjust capacity penalty every `segment_len` calls. Returns true if adjusted.
    pub fn maybe_adjust_cap(&mut self, segment_len: u32) -> bool {
        if self.cap_total_count < segment_len {
            return false;
        }
        let ratio = f64::from(self.cap_feasible_count) / f64::from(self.cap_total_count);
        if ratio < 0.15 {
            self.penalty_cap *= 1.2; // too many infeasible → increase penalty
        } else if ratio > 0.25 {
            self.penalty_cap *= 0.85; // too many feasible → decrease penalty
        }
        self.penalty_cap = self.penalty_cap.clamp(0.1, 100_000.0);
        self.cap_feasible_count = 0;
        self.cap_total_count = 0;
        true
    }

    /// Adjust TW penalty every `segment_len` calls. Returns true if adjusted.
    pub fn maybe_adjust_tw(&mut self, segment_len: u32) -> bool {
        if self.tw_total_count < segment_len {
            return false;
        }
        let ratio = f64::from(self.tw_feasible_count) / f64::from(self.tw_total_count);
        if ratio < 0.15 {
            self.penalty_tw *= 1.2;
        } else if ratio > 0.25 {
            self.penalty_tw *= 0.85;
        }
        self.penalty_tw = self.penalty_tw.clamp(0.1, 100_000.0);
        self.tw_feasible_count = 0;
        self.tw_total_count = 0;
        true
    }

    /// Compute penalized cost for a route from its RouteInfo.
    #[inline]
    pub fn penalized_cost(&self, info: &RouteInfo) -> f64 {
        info.distance + self.penalty_cap * info.cap_excess + self.penalty_tw * info.tw_excess
    }

    pub fn reset(&mut self) {
        self.cap_feasible_count = 0;
        self.cap_total_count = 0;
        self.tw_feasible_count = 0;
        self.tw_total_count = 0;
    }

    /// Return a copy with doubled TW penalty (Kool 2022 "penalty booster").
    /// Used when no feasible solution has been found yet to push harder
    /// toward TW feasibility.
    pub fn boosted_tw(&self) -> Self {
        let mut copy = self.clone();
        copy.penalty_tw *= 2.0;
        copy
    }
}

/// A PDPTW solution: a set of routes plus any unassigned pickup IDs.
#[derive(Clone)]
pub struct Solution {
    /// Each route: [0, c1, c2, ..., cn, 0] (depot-bookended).
    pub routes: Vec<Vec<usize>>,
    /// Unassigned pickup node IDs (their deliveries are also unassigned).
    pub unassigned: Vec<usize>,
}

/// Big-M for hierarchical objective: vehicles * `BIG_M` + distance.
const BIG_M: f64 = 1e9;

/// Precomputed arrival times and loads for a route, used for fast insertion checks.
pub struct RouteInfo {
    pub arrival: Vec<f64>,
    pub load: Vec<i32>,
    pub distance: f64,
    /// `suffix_max_load[i]` = max(load[i], load[i+1], ..., load[n-2]).
    pub suffix_max_load: Vec<i32>,
    /// `suffix_min_slack[i]` = min(late[route[j]] - arrival[j] for j in i..n-1).
    /// Used to skip O(n) forward propagation when time shift <= slack.
    pub suffix_min_slack: Vec<f64>,
    /// Precomputed edge distances: `edge_dists[i]` = dist(route[i], route[i+1]).
    /// Populated during `compute()` and reused by insertion functions to avoid
    /// redundant distance matrix lookups.
    pub edge_dists: Vec<f64>,
    /// Sum of max(0, load[i] - capacity) for each position. Used by penalized
    /// insertion to compute capacity violation cost.
    pub cap_excess: f64,
    /// Total time-warp violation for this route (from prefix_tw).
    pub tw_excess: f64,
    /// prefix_tw[i] = TWData for route[0..=i].
    pub prefix_tw: Vec<TWData>,
    /// suffix_tw[i] = TWData for route[i..n-1].
    pub suffix_tw: Vec<TWData>,
    /// Circle sector covering this route's customers (for SWAP* geometric filtering).
    pub sector: CircleSector,
}

impl Solution {
    /// Create an empty solution where all pickups are unassigned.
    pub fn new_empty(inst: &Instance) -> Self {
        Solution {
            routes: Vec::new(),
            unassigned: inst.pickups.clone(),
        }
    }

    /// Build an initial solution from explicit routes.
    ///
    /// `routes` may represent a complete or partial assignment.
    /// Missing pickup-delivery pairs are placed into `unassigned`.
    pub fn from_routes(inst: &Instance, routes: Vec<Vec<usize>>) -> Result<Self, String> {
        let mut seen = vec![false; inst.n + 1];

        for (ri, route) in routes.iter().enumerate() {
            if route.len() < 3 {
                return Err(format!(
                    "route {ri} must contain at least one customer (expected [0, ..., 0])"
                ));
            }
            if route.first() != Some(&0) || route.last() != Some(&0) {
                return Err(format!("route {ri} must start and end at depot 0"));
            }

            for (pos, &v) in route.iter().enumerate().skip(1).take(route.len() - 2) {
                if v == 0 {
                    return Err(format!(
                        "route {ri} contains depot in middle at position {pos}"
                    ));
                }
                if v > inst.n {
                    return Err(format!(
                        "route {ri} contains unknown node id {v} (max {})",
                        inst.n
                    ));
                }
                if seen[v] {
                    return Err(format!("node {v} appears more than once"));
                }
                seen[v] = true;
            }

            if !is_route_feasible(inst, route) {
                return Err(format!("route {ri} is infeasible"));
            }
        }

        let mut unassigned = Vec::new();
        for &p in &inst.pickups {
            let d = inst.delivery_of(p);
            let p_seen = seen[p];
            let d_seen = seen[d];

            if p_seen != d_seen {
                return Err(format!(
                    "pair ({p},{d}) is partially assigned; pickup and delivery must both be present or both absent"
                ));
            }
            if !p_seen {
                unassigned.push(p);
            }
        }

        Ok(Solution { routes, unassigned })
    }

    /// Number of vehicles (routes) used by this solution.
    pub fn num_vehicles(&self) -> usize {
        self.routes.len()
    }

    /// Sum of travel distances across all routes.
    pub fn total_distance(&self, inst: &Instance) -> f64 {
        self.routes.iter().map(|r| route_distance(inst, r)).sum()
    }

    #[allow(clippy::cast_precision_loss)]
    /// Hierarchical objective value: unassigned penalty + vehicles * BIG_M + distance.
    pub fn cost(&self, inst: &Instance) -> f64 {
        let unassigned_penalty = self.unassigned.len() as f64 * BIG_M * (inst.m as f64 + 1.0);
        unassigned_penalty + self.num_vehicles() as f64 * BIG_M + self.total_distance(inst)
    }

    /// Lexicographic comparison: fewer unassigned > fewer vehicles > shorter distance.
    pub fn is_better_than(&self, other: &Solution, inst: &Instance) -> bool {
        let u1 = self.unassigned.len();
        let u2 = other.unassigned.len();
        if u1 != u2 {
            return u1 < u2;
        }

        let v1 = self.num_vehicles();
        let v2 = other.num_vehicles();
        if v1 != v2 {
            return v1 < v2;
        }
        self.total_distance(inst) < other.total_distance(inst)
    }

    /// Check full feasibility: all pairs assigned, routes satisfy capacity, TW, and precedence.
    pub fn is_feasible(&self, inst: &Instance) -> bool {
        if !self.unassigned.is_empty() {
            return false;
        }
        // Check each route individually (capacity, TW, precedence within route)
        if !self.routes.iter().all(|r| is_route_feasible(inst, r)) {
            return false;
        }
        // Check cross-route customer uniqueness: each customer in exactly one route
        let mut seen = vec![false; inst.n + 1];
        for route in &self.routes {
            for &v in &route[1..route.len() - 1] {
                if seen[v] {
                    return false; // duplicate customer across routes
                }
                seen[v] = true;
            }
        }
        // Check all customers are assigned
        for &p in &inst.pickups {
            if !seen[p] || !seen[inst.delivery_of(p)] {
                return false;
            }
        }
        true
    }
}

/// Total travel distance for a single route (sum of consecutive arc distances).
pub fn route_distance(inst: &Instance, route: &[usize]) -> f64 {
    let mut dist = 0.0;
    for i in 0..route.len() - 1 {
        dist += inst.dist(route[i], route[i + 1]);
    }
    dist
}

impl Default for RouteInfo {
    fn default() -> Self {
        Self::new()
    }
}

impl RouteInfo {
    /// Create a new empty `RouteInfo` for reuse.
    pub fn new() -> Self {
        RouteInfo {
            arrival: Vec::new(),
            load: Vec::new(),
            distance: 0.0,
            suffix_max_load: Vec::new(),
            suffix_min_slack: Vec::new(),
            edge_dists: Vec::new(),
            cap_excess: 0.0,
            tw_excess: 0.0,
            prefix_tw: Vec::new(),
            suffix_tw: Vec::new(),
            sector: CircleSector::default(),
        }
    }

    /// Recompute info for the given route in-place (avoids allocation after first call).
    #[allow(clippy::uninit_vec)]
    pub fn compute(&mut self, inst: &Instance, route: &[usize]) {
        let n = route.len();
        let num_edges = if n > 0 { n - 1 } else { 0 };
        // Ensure capacity without zero-filling (we overwrite all elements below)
        if n > self.arrival.capacity() {
            self.arrival.reserve(n - self.arrival.len());
            self.load.reserve(n - self.load.len());
            self.suffix_max_load.reserve(n - self.suffix_max_load.len());
            self.suffix_min_slack
                .reserve(n - self.suffix_min_slack.len());
        }
        if num_edges > self.edge_dists.capacity() {
            self.edge_dists.reserve(num_edges - self.edge_dists.len());
        }
        // Ensure prefix_tw / suffix_tw capacity
        if n > self.prefix_tw.capacity() {
            self.prefix_tw.reserve(n - self.prefix_tw.len());
            self.suffix_tw.reserve(n - self.suffix_tw.len());
        }
        unsafe {
            self.arrival.set_len(n);
            self.load.set_len(n);
            self.suffix_max_load.set_len(n);
            self.suffix_min_slack.set_len(n);
            self.edge_dists.set_len(num_edges);
            self.prefix_tw.set_len(n);
            self.suffix_tw.set_len(n);
        }

        // SAFETY: all indices below are bounded by n (route.len())
        // and we only access elements we've set via set_len above.
        unsafe {
            *self.arrival.get_unchecked_mut(0) = inst.early(0);
            *self.load.get_unchecked_mut(0) = 0;

            let mut dist = 0.0f64;
            for i in 1..n {
                let prev = *route.get_unchecked(i - 1);
                let curr = *route.get_unchecked(i);
                let d = inst.dist(prev, curr);
                *self.edge_dists.get_unchecked_mut(i - 1) = d;
                dist += d;
                *self.arrival.get_unchecked_mut(i) = fmax(
                    *self.arrival.get_unchecked(i - 1) + inst.svc(prev) + d,
                    inst.early(curr),
                );
                *self.load.get_unchecked_mut(i) = if i < n - 1 {
                    *self.load.get_unchecked(i - 1) + inst.demand[curr]
                } else {
                    0
                };
            }
            self.distance = dist;

            // Compute suffix max over load[0..n-1] and suffix min slack[0..n-1]
            *self.suffix_max_load.get_unchecked_mut(n - 1) = i32::MIN;
            // Include actual depot slack (not f64::MAX) so the suffix_min_slack
            // shortcut in insertion functions correctly accounts for depot TW.
            *self.suffix_min_slack.get_unchecked_mut(n - 1) =
                inst.late(*route.get_unchecked(n - 1)) - *self.arrival.get_unchecked(n - 1);
            for i in (0..n - 1).rev() {
                *self.suffix_max_load.get_unchecked_mut(i) =
                    (*self.load.get_unchecked(i)).max(*self.suffix_max_load.get_unchecked(i + 1));
                let slack = inst.late(*route.get_unchecked(i)) - *self.arrival.get_unchecked(i);
                *self.suffix_min_slack.get_unchecked_mut(i) =
                    fmin(slack, *self.suffix_min_slack.get_unchecked(i + 1));
            }
        }

        self.compute_tw_sector(inst, route, n);
    }

    /// Light recompute: only arrival, load, edge_dists, distance, suffix_max_load,
    /// suffix_min_slack.  Skips prefix_tw, suffix_tw, sector, cap_excess, tw_excess.
    /// Use in hot paths (perturbation) where only insertion-check fields are needed.
    #[allow(clippy::uninit_vec)]
    pub fn compute_light(&mut self, inst: &Instance, route: &[usize]) {
        let n = route.len();
        let num_edges = if n > 0 { n - 1 } else { 0 };
        if n > self.arrival.capacity() {
            self.arrival.reserve(n - self.arrival.len());
            self.load.reserve(n - self.load.len());
            self.suffix_max_load.reserve(n - self.suffix_max_load.len());
            self.suffix_min_slack
                .reserve(n - self.suffix_min_slack.len());
        }
        if num_edges > self.edge_dists.capacity() {
            self.edge_dists.reserve(num_edges - self.edge_dists.len());
        }
        unsafe {
            self.arrival.set_len(n);
            self.load.set_len(n);
            self.suffix_max_load.set_len(n);
            self.suffix_min_slack.set_len(n);
            self.edge_dists.set_len(num_edges);
        }

        if n == 0 {
            self.distance = 0.0;
            return;
        }

        unsafe {
            *self.arrival.get_unchecked_mut(0) = inst.early(0);
            *self.load.get_unchecked_mut(0) = 0;

            let mut dist = 0.0f64;
            for i in 1..n {
                let prev = *route.get_unchecked(i - 1);
                let curr = *route.get_unchecked(i);
                let d = inst.dist(prev, curr);
                *self.edge_dists.get_unchecked_mut(i - 1) = d;
                dist += d;
                *self.arrival.get_unchecked_mut(i) = fmax(
                    *self.arrival.get_unchecked(i - 1) + inst.svc(prev) + d,
                    inst.early(curr),
                );
                *self.load.get_unchecked_mut(i) = if i < n - 1 {
                    *self.load.get_unchecked(i - 1) + inst.demand[curr]
                } else {
                    0
                };
            }
            self.distance = dist;

            *self.suffix_max_load.get_unchecked_mut(n - 1) = i32::MIN;
            *self.suffix_min_slack.get_unchecked_mut(n - 1) =
                inst.late(*route.get_unchecked(n - 1)) - *self.arrival.get_unchecked(n - 1);
            for i in (0..n - 1).rev() {
                *self.suffix_max_load.get_unchecked_mut(i) =
                    (*self.load.get_unchecked(i)).max(*self.suffix_max_load.get_unchecked(i + 1));
                let slack = inst.late(*route.get_unchecked(i)) - *self.arrival.get_unchecked(i);
                *self.suffix_min_slack.get_unchecked_mut(i) =
                    fmin(slack, *self.suffix_min_slack.get_unchecked(i + 1));
            }
        }
    }

    /// Compute only the tw/sector fields (call after compute_light to upgrade to full).
    #[allow(clippy::uninit_vec)]
    pub fn compute_tw_sector(&mut self, inst: &Instance, route: &[usize], n: usize) {
        // Ensure prefix_tw / suffix_tw capacity
        if n > self.prefix_tw.capacity() {
            self.prefix_tw.reserve(n - self.prefix_tw.len());
            self.suffix_tw.reserve(n - self.suffix_tw.len());
        }
        unsafe {
            self.prefix_tw.set_len(n);
            self.suffix_tw.set_len(n);
        }

        unsafe {
            // Fast cap_excess
            let cap = inst.capacity;
            if n >= 2 && *self.suffix_max_load.get_unchecked(0) > cap {
                let mut cap_excess = 0.0;
                for i in 0..n - 1 {
                    let excess = *self.load.get_unchecked(i) - cap;
                    if excess > 0 {
                        cap_excess += f64::from(excess);
                    }
                }
                self.cap_excess = cap_excess;
            } else {
                self.cap_excess = 0.0;
            }
        }

        // Compute circle sector from customer polar angles
        if let Some(ref angles) = inst.polar_angle {
            if n >= 3 {
                // At least depot + 1 customer + depot
                self.sector = CircleSector::from_point(angles[route[1]]);
                for i in 2..n - 1 {
                    self.sector.extend(angles[route[i]]);
                }
            } else {
                self.sector = CircleSector::default();
            }
        }

        // Prefix TWData (forward)
        if n > 0 {
            self.prefix_tw[0] = TWData::from_node(inst, route[0]);
            for (i, &node) in route.iter().enumerate().skip(1) {
                self.prefix_tw[i] = tw_merge(
                    self.prefix_tw[i - 1],
                    TWData::from_node(inst, node),
                    self.edge_dists[i - 1],
                );
            }
            self.tw_excess = self.prefix_tw[n - 1].time_warp;

            // Suffix TWData (backward)
            self.suffix_tw[n - 1] = TWData::from_node(inst, route[n - 1]);
            for i in (0..n - 1).rev() {
                self.suffix_tw[i] = tw_merge(
                    TWData::from_node(inst, route[i]),
                    self.suffix_tw[i + 1],
                    self.edge_dists[i],
                );
            }
        } else {
            self.tw_excess = 0.0;
        }
    }
}

/// Ensure `infos` has at least `n` elements, compute route info for each route.
pub fn compute_all_infos(inst: &Instance, routes: &[Vec<usize>], infos: &mut Vec<RouteInfo>) {
    while infos.len() < routes.len() {
        infos.push(RouteInfo::new());
    }
    for (i, route) in routes.iter().enumerate() {
        infos[i].compute(inst, route);
    }
}

/// Like `compute_all_infos` but only computes light info (forward+backward passes).
/// Skips prefix_tw, suffix_tw, sector, cap_excess, tw_excess.
/// Sufficient for `first_feasible_insertion_with_info` and `best_pair_insertion_with_info`.
pub fn compute_all_infos_light(inst: &Instance, routes: &[Vec<usize>], infos: &mut Vec<RouteInfo>) {
    while infos.len() < routes.len() {
        infos.push(RouteInfo::new());
    }
    for (i, route) in routes.iter().enumerate() {
        infos[i].compute_light(inst, route);
    }
}

/// Check if a single route is feasible (capacity, time windows, precedence).
/// Check capacity and time-window feasibility only (no precedence/uniqueness).
/// Use when precedence is guaranteed by construction (e.g. insertion heuristic).
pub fn is_route_capacity_tw_feasible(inst: &Instance, route: &[usize]) -> bool {
    if route.len() < 2 {
        return false;
    }
    let mut load: i32 = 0;
    for &node in &route[1..route.len() - 1] {
        load += inst.demand[node];
        if load > inst.capacity || load < 0 {
            return false;
        }
    }
    let mut time = inst.early(0);
    for i in 1..route.len() {
        let prev = route[i - 1];
        let curr = route[i];
        time = fmax(
            time + inst.svc(prev) + inst.dist(prev, curr),
            inst.early(curr),
        );
        if time > inst.late(curr) {
            return false;
        }
    }
    true
}

/// Check only capacity feasibility for a route (no TW or precedence).
#[allow(dead_code)]
pub fn is_route_capacity_feasible(inst: &Instance, route: &[usize]) -> bool {
    if route.len() < 2 {
        return false;
    }
    let mut load: i32 = 0;
    for &node in &route[1..route.len() - 1] {
        load += inst.demand[node];
        if load > inst.capacity || load < 0 {
            return false;
        }
    }
    true
}

/// Check only time-window feasibility for a route (no capacity or precedence).
#[allow(dead_code)]
pub fn is_route_tw_feasible(inst: &Instance, route: &[usize]) -> bool {
    if route.len() < 2 {
        return false;
    }
    let mut time = inst.early(0);
    for i in 1..route.len() {
        let prev = route[i - 1];
        let curr = route[i];
        time = fmax(
            time + inst.svc(prev) + inst.dist(prev, curr),
            inst.early(curr),
        );
        if time > inst.late(curr) {
            return false;
        }
    }
    true
}

pub fn is_route_feasible(inst: &Instance, route: &[usize]) -> bool {
    if route.len() < 2 {
        return false;
    }
    let mut load: i32 = 0;
    for &node in &route[1..route.len() - 1] {
        load += inst.demand[node];
        if load > inst.capacity || load < 0 {
            return false;
        }
    }
    let mut time = inst.early(0);
    for i in 1..route.len() {
        let prev = route[i - 1];
        let curr = route[i];
        time = fmax(
            time + inst.svc(prev) + inst.dist(prev, curr),
            inst.early(curr),
        );
        if time > inst.late(curr) {
            return false;
        }
    }
    // Precedence + uniqueness in one pass:
    // state[p] = 0 unseen, 1 pickup visited (open), 2 delivery visited (closed).
    let mut state = vec![0u8; inst.n + 1];
    for &v in &route[1..route.len() - 1] {
        if inst.is_pickup(v) {
            if state[v] != 0 {
                return false;
            }
            state[v] = 1;
        } else {
            let p = inst.pickup_of(v);
            if p == 0 || state[p] != 1 {
                return false;
            }
            state[p] = 2;
        }
    }
    if state.contains(&1) {
        return false;
    }
    true
}

/// Find the best feasible insertion using a precomputed `RouteInfo`.
///
/// # Safety
/// Uses `get_unchecked` for performance in inner loops. All indices are
/// bounded by the route length which equals `info.arrival.len()`.
/// If `SPARSE` is true, only consider positions where at least one new arc
/// is in the sparse set A'^- (Goeke 2019, Section 3.4).
#[allow(clippy::similar_names)]
pub fn best_pair_insertion_with_info<const SPARSE: bool>(
    inst: &Instance,
    route: &[usize],
    info: &RouteInfo,
    p: usize,
    d: usize,
) -> Option<(f64, usize, usize)> {
    let n = route.len();
    let num_edges = n - 1;
    let q_p = inst.demand[p];
    let cap = inst.capacity;
    let early_p = inst.early(p);
    let late_p = inst.late(p);
    let svc_p = inst.svc(p);
    let early_d = inst.early(d);
    let late_d = inst.late(d);
    let svc_d = inst.svc(d);
    let mut best: Option<(f64, usize, usize)> = None;
    let mut best_dist = f64::MAX;

    // SAFETY: all ep, ed indices are < num_edges = n-1, so ep+1, ed+1 < n.
    // route, info.arrival, info.load, info.suffix_* all have length n.
    // dist_p, dist_d point to rows p and d of the distance matrix (valid for indices < total).
    // arc_feas_* point to precomputed rows of arc feasibility/sparse matrices for constant p, d.
    unsafe {
        // Use precomputed edge distances from RouteInfo (populated in compute()).
        let edge_dists = info.edge_dists.as_slice();
        let dist_p = inst.dist_flat.as_ptr().add(p * inst.dist_stride);
        let dist_d = inst.dist_flat.as_ptr().add(d * inst.dist_stride);
        let dist_pd = *dist_p.add(d);
        let arc_feas_t_p = inst.arc_feas_t_row(p);
        let arc_feas_d = inst.arc_feas_row(d);
        let arc_feas_t_d = inst.arc_feas_t_row(d);
        let arc_sparse_p = inst.arc_sparse_row(p);
        let arc_sparse_d = inst.arc_sparse_row(d);
        let arc_sparse_t_p = inst.arc_sparse_t_row(p);
        let arc_sparse_t_d = inst.arc_sparse_t_row(d);

        for ep in 0..num_edges {
            let load_at_p = *info.load.get_unchecked(ep) + q_p;
            if load_at_p > cap {
                continue;
            }

            let r_ep = *route.get_unchecked(ep);

            // Arc feasibility pre-check: if arc (r_ep, p) is infeasible, then
            // arr_p > late_p is guaranteed (since arrival[ep] >= early[r_ep]).
            // This O(1) boolean check avoids distance computation for ~62% of arcs.
            if !arc_bit(arc_feas_t_p, r_ep) {
                continue;
            }

            let r_ep1 = *route.get_unchecked(ep + 1);

            // Sparse arc filter: precompute whether pickup at ep contributes a sparse arc
            let p_has_sparse =
                !SPARSE || arc_bit(arc_sparse_t_p, r_ep) || arc_bit(arc_sparse_p, r_ep1);

            let a_ep = *info.arrival.get_unchecked(ep);

            // Precompute distances for this edge (reused in ed==ep and ed>ep)
            let dist_ep_p = *dist_p.add(r_ep);
            let arr_p = fmax(a_ep + inst.svc(r_ep) + dist_ep_p, early_p);
            if arr_p > late_p {
                continue;
            }

            let dist_ep_removed = *edge_dists.get_unchecked(ep);
            let dist_p_ep1 = *dist_p.add(r_ep1);

            // Lower bound: delta_p is the minimum cost of inserting pickup at ep.
            // Any delivery position adds >= 0 cost (triangle inequality).
            // If delta_p alone exceeds budget, skip this ep entirely.
            let delta_p = dist_ep_p + dist_p_ep1 - dist_ep_removed;
            // Precompute base cost for pickup insertion — avoids keeping dist_ep_p,
            // dist_p_ep1, and dist_ep_removed alive in the inner delivery loop.
            let base_cost = info.distance + delta_p;
            if base_cost >= best_dist {
                continue;
            }

            let suffix_start = ep + 1;
            let all_cap_ok = suffix_start >= n - 1
                || *info.suffix_max_load.get_unchecked(suffix_start) + q_p <= cap;

            // ed == ep: new arcs are (r_ep, p), (p, d), (d, r_ep1)
            if arc_bit(arc_feas_d, r_ep1) {
                // Sparse check for ed==ep: need any of (r_ep,p), (p,d), (d,r_ep1) sparse
                let ed_ep_sparse =
                    p_has_sparse || arc_bit(arc_sparse_p, d) || arc_bit(arc_sparse_d, r_ep1);
                if !SPARSE || ed_ep_sparse {
                    let arr_d = fmax(arr_p + svc_p + dist_pd, early_d);
                    if arr_d <= late_d {
                        let dist_d_ep1 = *dist_d.add(r_ep1);
                        let mut arr = fmax(arr_d + svc_d + dist_d_ep1, inst.early(r_ep1));
                        let mut feasible = arr <= inst.late(r_ep1);
                        if feasible {
                            // O(1) slack check: if shift at ep+1 fits within min slack of ep+2..n-1,
                            // all subsequent positions are feasible (shift is non-increasing).
                            let shift = arr - *info.arrival.get_unchecked(ep + 1);
                            if shift > 1e-10 && ep + 2 < n {
                                if shift <= *info.suffix_min_slack.get_unchecked(ep + 2) {
                                    // Guaranteed feasible, skip forward propagation
                                } else {
                                    for i in (ep + 2)..n {
                                        let prev = *route.get_unchecked(i - 1);
                                        let curr = *route.get_unchecked(i);
                                        arr = fmax(
                                            arr + inst.svc(prev) + *edge_dists.get_unchecked(i - 1),
                                            inst.early(curr),
                                        );
                                        if arr > inst.late(curr) {
                                            feasible = false;
                                            break;
                                        }
                                        if arr <= *info.arrival.get_unchecked(i) + 1e-10 {
                                            break;
                                        }
                                    }
                                }
                            }
                        }
                        if feasible {
                            let new_dist = base_cost - dist_p_ep1 + dist_pd + dist_d_ep1;
                            if new_dist < best_dist {
                                best_dist = new_dist;
                                best = Some((new_dist, ep, ep));
                            }
                        }
                    }
                } // sparse filter for ed==ep
            } // is_arc_feasible(d, r_ep1)

            // ed > ep
            let mut arr_at_curr = fmax(arr_p + svc_p + dist_p_ep1, inst.early(r_ep1));
            if arr_at_curr > inst.late(r_ep1) {
                continue;
            }

            // Convergence detection: once the pickup-insertion shift is absorbed
            // by time windows, arr_at_curr matches original arrival.  Skip
            // recomputing next_arr and the late check for all subsequent ed.
            let mut converged = arr_at_curr <= *info.arrival.get_unchecked(ep + 1) + 1e-10;
            if converged {
                arr_at_curr = *info.arrival.get_unchecked(ep + 1);
            }

            let mut running_max = *info.load.get_unchecked(ep + 1);

            if ep + 1 < n - 1 && !all_cap_ok && running_max + q_p > cap {
                continue;
            }

            for ed in (ep + 1)..num_edges {
                // Early termination: arrival already exceeds delivery window.
                // Since arr_at_curr is non-decreasing, no future ed can yield
                // arr_d <= late_d (because arr_d >= arr_at_curr).
                if arr_at_curr > late_d {
                    break;
                }

                if ed > ep + 1 && ed < n - 1 && !all_cap_ok {
                    running_max = running_max.max(*info.load.get_unchecked(ed));
                    if running_max + q_p > cap {
                        break;
                    }
                }

                let r_ed = *route.get_unchecked(ed);
                let r_ed1 = *route.get_unchecked(ed + 1);

                // Advance arrival along route.  When converged (pickup shift
                // absorbed), use precomputed info value — skips fmax + late check.
                let svc_r_ed = inst.svc(r_ed);
                let next_arr = if converged {
                    *info.arrival.get_unchecked(ed + 1)
                } else {
                    let na = fmax(
                        arr_at_curr + svc_r_ed + *edge_dists.get_unchecked(ed),
                        inst.early(r_ed1),
                    );
                    if na > inst.late(r_ed1) {
                        break;
                    }
                    if na <= *info.arrival.get_unchecked(ed + 1) + 1e-10 {
                        converged = true;
                        *info.arrival.get_unchecked(ed + 1)
                    } else {
                        na
                    }
                };

                // Arc feasibility pre-check for delivery: both (r_ed→d) and (d→r_ed1)
                if !arc_bit(arc_feas_t_d, r_ed) || !arc_bit(arc_feas_d, r_ed1) {
                    arr_at_curr = next_arr;
                    continue;
                }

                // Sparse filter for ed > ep: at least one new arc must be sparse
                if SPARSE
                    && !p_has_sparse
                    && !arc_bit(arc_sparse_t_d, r_ed)
                    && !arc_bit(arc_sparse_d, r_ed1)
                {
                    arr_at_curr = next_arr;
                    continue;
                }

                let dist_d_red = *dist_d.add(r_ed);
                let arr_d = fmax(arr_at_curr + svc_r_ed + dist_d_red, early_d);
                if arr_d <= late_d {
                    let dist_d_red1 = *dist_d.add(r_ed1);
                    let mut arr_fwd = fmax(arr_d + svc_d + dist_d_red1, inst.early(r_ed1));
                    let mut feasible = arr_fwd <= inst.late(r_ed1);
                    if feasible {
                        let shift = arr_fwd - *info.arrival.get_unchecked(ed + 1);
                        if shift > 1e-10 && ed + 2 < n {
                            if shift <= *info.suffix_min_slack.get_unchecked(ed + 2) {
                                // Guaranteed feasible, skip forward propagation
                            } else {
                                for i in (ed + 2)..n {
                                    let prev = *route.get_unchecked(i - 1);
                                    let curr = *route.get_unchecked(i);
                                    arr_fwd = fmax(
                                        arr_fwd + inst.svc(prev) + *edge_dists.get_unchecked(i - 1),
                                        inst.early(curr),
                                    );
                                    if arr_fwd > inst.late(curr) {
                                        feasible = false;
                                        break;
                                    }
                                    if arr_fwd <= *info.arrival.get_unchecked(i) + 1e-10 {
                                        break;
                                    }
                                }
                            }
                        }
                    }
                    if feasible {
                        let new_dist =
                            base_cost - *edge_dists.get_unchecked(ed) + dist_d_red + dist_d_red1;
                        if new_dist < best_dist {
                            best_dist = new_dist;
                            best = Some((new_dist, ep, ed));
                        }
                    }
                }

                arr_at_curr = next_arr;
            }
        }
    } // unsafe

    best
}

/// Find the best insertion with **soft capacity and TW** constraints.
///
/// Like `best_pair_insertion_with_info::<true>` (always sparse), except:
/// - Capacity violations are penalized rather than rejected.
/// - TW violations are penalized using O(1) TWData concatenation when
///   `penalty_tw > 0`. When `penalty_tw == 0`, TW stays hard.
/// - Returns `(penalized_cost, ep, ed)` so the caller can compute
///   route-level penalized costs.
///
/// # Safety
/// Same invariants as `best_pair_insertion_with_info`.
#[allow(clippy::similar_names, clippy::too_many_lines)]
pub fn best_pair_insertion_penalized(
    inst: &Instance,
    route: &[usize],
    info: &RouteInfo,
    p: usize,
    d: usize,
    penalty_cap: f64,
    penalty_tw: f64,
) -> Option<(f64, usize, usize)> {
    let n = route.len();
    let num_edges = n - 1;
    let q_p = inst.demand[p];
    let cap = inst.capacity;
    let early_p = inst.early(p);
    let late_p = inst.late(p);
    let svc_p = inst.svc(p);
    let _early_d = inst.early(d);
    let _late_d = inst.late(d);
    let _svc_d = inst.svc(d);
    let tw_soft = penalty_tw > 0.0;

    // Warmstart: run the fast non-penalized (hard-constraint) search first.
    // If it finds a feasible insertion, use its distance as a tight initial bound.
    // The penalized search then only considers infeasible positions that beat it.
    let mut best: Option<(f64, usize, usize)> =
        best_pair_insertion_with_info::<true>(inst, route, info, p, d);
    let mut best_penalized = best.map_or(f64::MAX, |(dist, _, _)| dist);

    // Pre-check: if route has 0 time-warp and 0 cap-excess, and we found a
    // feasible insertion, no infeasible position can beat it (its penalty terms
    // are 0, so penalized_cost = distance ≤ any infeasible position's distance
    // + positive penalty terms). Skip the expensive penalized search entirely.
    if best.is_some() && info.tw_excess < 1e-10 && info.cap_excess < 1e-10 {
        return best;
    }

    let twd_p = TWData::from_node(inst, p);
    let twd_d = TWData::from_node(inst, d);

    unsafe {
        let edge_dists = info.edge_dists.as_slice();
        let dist_p = inst.dist_flat.as_ptr().add(p * inst.dist_stride);
        let dist_d = inst.dist_flat.as_ptr().add(d * inst.dist_stride);
        let dist_pd = *dist_p.add(d);
        let arc_feas_t_p = inst.arc_feas_t_row(p);
        let arc_feas_d = inst.arc_feas_row(d);
        let arc_feas_t_d = inst.arc_feas_t_row(d);
        let arc_sparse_p = inst.arc_sparse_row(p);
        let arc_sparse_d = inst.arc_sparse_row(d);
        let arc_sparse_t_p = inst.arc_sparse_t_row(p);
        let arc_sparse_t_d = inst.arc_sparse_t_row(d);

        for ep in 0..num_edges {
            // Soft capacity: compute excess at pickup position instead of skipping
            let load_at_p = *info.load.get_unchecked(ep) + q_p;

            let r_ep = *route.get_unchecked(ep);

            // TW arc feasibility pre-check: kept even when TW is soft because
            // positions failing this have huge TW violations and are never competitive.
            if !arc_bit(arc_feas_t_p, r_ep) {
                continue;
            }

            let r_ep1 = *route.get_unchecked(ep + 1);

            // Sparse arc filter
            let p_has_sparse = arc_bit(arc_sparse_t_p, r_ep) || arc_bit(arc_sparse_p, r_ep1);

            let a_ep = *info.arrival.get_unchecked(ep);
            let dist_ep_p = *dist_p.add(r_ep);

            // When TW is hard, check arrival at p against late window
            if !tw_soft {
                let arr_p = fmax(a_ep + inst.svc(r_ep) + dist_ep_p, early_p);
                if arr_p > late_p {
                    continue;
                }
            }

            let dist_ep_removed = *edge_dists.get_unchecked(ep);
            let dist_p_ep1 = *dist_p.add(r_ep1);
            let delta_p = dist_ep_p + dist_p_ep1 - dist_ep_removed;
            let base_dist = info.distance + delta_p;

            // Fast path: check if all positions after ep are capacity-feasible
            let suffix_start = ep + 1;
            let all_cap_ok = suffix_start >= n - 1
                || *info.suffix_max_load.get_unchecked(suffix_start) + q_p <= cap;

            // Compute cap excess contribution from position ep (the pickup insertion point)
            let excess_at_ep = if load_at_p > cap {
                f64::from(load_at_p - cap)
            } else {
                0.0
            };

            // Precompute TWData: prefix up to ep, then with p inserted
            let prefix_with_p = tw_merge(*info.prefix_tw.get_unchecked(ep), twd_p, dist_ep_p);

            // --- ed == ep ---
            if arc_bit(arc_feas_d, r_ep1) {
                let ed_ep_sparse =
                    p_has_sparse || arc_bit(arc_sparse_p, d) || arc_bit(arc_sparse_d, r_ep1);
                if ed_ep_sparse {
                    // O(1) TWData evaluation: prefix_with_p + d + suffix_tw[ep+1]
                    let with_pd = tw_merge(prefix_with_p, twd_d, dist_pd);
                    let dist_d_ep1 = *dist_d.add(r_ep1);
                    let total_tw =
                        tw_merge(with_pd, *info.suffix_tw.get_unchecked(ep + 1), dist_d_ep1);
                    let new_tw_excess = total_tw.time_warp;

                    // If TW is hard and there's time-warp, skip
                    if !tw_soft && new_tw_excess > 1e-10 {
                        // skip
                    } else {
                        let new_dist = base_dist - dist_p_ep1 + dist_pd + dist_d_ep1;
                        let new_cap_excess = info.cap_excess + excess_at_ep;
                        let penalized =
                            new_dist + penalty_cap * new_cap_excess + penalty_tw * new_tw_excess;
                        if penalized < best_penalized {
                            best_penalized = penalized;
                            best = Some((penalized, ep, ep));
                        }
                    }
                }
            }

            // --- ed > ep ---
            // Build running TWData: prefix_tw[ep] + p + route[ep+1]
            let dist_p_rep1 = dist_p_ep1; // alias for clarity
            let mut running_tw =
                tw_merge(prefix_with_p, TWData::from_node(inst, r_ep1), dist_p_rep1);

            // When TW is hard, check arrival feasibility at ep+1
            if !tw_soft {
                let arr_at_curr = fmax(
                    a_ep + inst.svc(r_ep) + dist_ep_p + svc_p + dist_p_rep1,
                    fmax(early_p + svc_p + dist_p_rep1, inst.early(r_ep1)),
                );
                if arr_at_curr > inst.late(r_ep1) && running_tw.time_warp > 1e-10 {
                    continue;
                }
            }

            let mut running_max = *info.load.get_unchecked(ep + 1);
            let mut running_excess = excess_at_ep;
            if !all_cap_ok && ep + 1 < n - 1 {
                let e = running_max + q_p - cap;
                if e > 0 {
                    running_excess += f64::from(e);
                }
            }

            for ed in (ep + 1)..num_edges {
                if !all_cap_ok && ed > ep + 1 && ed < n - 1 {
                    running_max = running_max.max(*info.load.get_unchecked(ed));
                    let e = *info.load.get_unchecked(ed) + q_p - cap;
                    if e > 0 {
                        running_excess += f64::from(e);
                    }
                }

                let r_ed = *route.get_unchecked(ed);
                let r_ed1 = *route.get_unchecked(ed + 1);

                // Advance running TWData along route
                if ed > ep + 1 {
                    running_tw = tw_merge(
                        running_tw,
                        TWData::from_node(inst, r_ed),
                        *edge_dists.get_unchecked(ed - 1),
                    );
                }

                // TW arc feasibility: kept even when TW is soft (same reasoning as pickup).
                if !arc_bit(arc_feas_t_d, r_ed) || !arc_bit(arc_feas_d, r_ed1) {
                    continue;
                }

                // Sparse filter
                if !p_has_sparse && !arc_bit(arc_sparse_t_d, r_ed) && !arc_bit(arc_sparse_d, r_ed1)
                {
                    continue;
                }

                // O(1) TWData evaluation: running_tw + d + suffix_tw[ed+1]
                let dist_d_red = *dist_d.add(r_ed);
                let with_d = tw_merge(running_tw, twd_d, dist_d_red);
                let dist_d_red1 = *dist_d.add(r_ed1);
                let total_tw = tw_merge(with_d, *info.suffix_tw.get_unchecked(ed + 1), dist_d_red1);
                let new_tw_excess = total_tw.time_warp;

                // If TW is hard and there's time-warp, skip
                if !tw_soft && new_tw_excess > 1e-10 {
                    continue;
                }

                let new_dist = base_dist - *edge_dists.get_unchecked(ed) + dist_d_red + dist_d_red1;
                let new_cap_excess = info.cap_excess + running_excess;
                let penalized =
                    new_dist + penalty_cap * new_cap_excess + penalty_tw * new_tw_excess;
                if penalized < best_penalized {
                    best_penalized = penalized;
                    best = Some((penalized, ep, ed));
                }
            }
        }
    } // unsafe

    best
}

/// Random feasible insertion using a precomputed `RouteInfo`.
///
/// # Safety
/// Uses `get_unchecked` for performance in inner loops. All indices are
/// bounded by the route length which equals `info.arrival.len()`.
#[allow(clippy::similar_names)]
pub fn random_feasible_insertion_with_info(
    inst: &Instance,
    route: &[usize],
    info: &RouteInfo,
    p: usize,
    d: usize,
    rng: &mut impl Rng,
) -> Option<(f64, usize, usize)> {
    let n = route.len();
    let num_edges = n - 1;
    let q_p = inst.demand[p];
    let cap = inst.capacity;
    let early_p = inst.early(p);
    let late_p = inst.late(p);
    let svc_p = inst.svc(p);
    let early_d = inst.early(d);
    let late_d = inst.late(d);
    let svc_d = inst.svc(d);
    let mut count: u64 = 0;
    let mut selected: Option<(f64, usize, usize)> = None;

    // SAFETY: all ep, ed indices are < num_edges = n-1, so ep+1, ed+1 < n.
    // route, info.arrival, info.load, info.suffix_* all have length n.
    unsafe {
        let dist_p = inst.dist_flat.as_ptr().add(p * inst.dist_stride);
        let dist_d = inst.dist_flat.as_ptr().add(d * inst.dist_stride);
        let dist_pd = *dist_p.add(d);
        let arc_feas_t_p = inst.arc_feas_t_row(p);
        let arc_feas_d = inst.arc_feas_row(d);
        let arc_feas_t_d = inst.arc_feas_t_row(d);
        for ep in 0..num_edges {
            let load_at_p = *info.load.get_unchecked(ep) + q_p;
            if load_at_p > cap {
                continue;
            }

            let r_ep = *route.get_unchecked(ep);

            // Arc feasibility pre-check
            if !arc_bit(arc_feas_t_p, r_ep) {
                continue;
            }

            let r_ep1 = *route.get_unchecked(ep + 1);
            let a_ep = *info.arrival.get_unchecked(ep);
            let dist_p_rep = *dist_p.add(r_ep);
            let arr_p = fmax(a_ep + inst.svc(r_ep) + dist_p_rep, early_p);
            if arr_p > late_p {
                continue;
            }

            let suffix_start = ep + 1;
            let all_cap_ok = suffix_start >= n - 1
                || *info.suffix_max_load.get_unchecked(suffix_start) + q_p <= cap;

            // ed == ep
            if arc_bit(arc_feas_d, r_ep1) {
                let arr_d = fmax(arr_p + svc_p + dist_pd, early_d);
                if arr_d <= late_d {
                    let dist_d_ep1 = *dist_d.add(r_ep1);
                    let mut arr = fmax(arr_d + svc_d + dist_d_ep1, inst.early(r_ep1));
                    let mut feasible = arr <= inst.late(r_ep1);
                    if feasible {
                        let shift = arr - *info.arrival.get_unchecked(ep + 1);
                        if shift > 1e-10 && ep + 2 < n {
                            if shift <= *info.suffix_min_slack.get_unchecked(ep + 2) {
                                // Guaranteed feasible
                            } else {
                                for i in (ep + 2)..n {
                                    let prev = *route.get_unchecked(i - 1);
                                    let curr = *route.get_unchecked(i);
                                    arr = fmax(
                                        arr + inst.svc(prev)
                                            + *info.edge_dists.get_unchecked(i - 1),
                                        inst.early(curr),
                                    );
                                    if arr > inst.late(curr) {
                                        feasible = false;
                                        break;
                                    }
                                    if arr <= *info.arrival.get_unchecked(i) + 1e-10 {
                                        break;
                                    }
                                }
                            }
                        }
                    }
                    if feasible {
                        let dist_delta =
                            -*info.edge_dists.get_unchecked(ep) + dist_p_rep + dist_pd + dist_d_ep1;
                        count += 1;
                        if rng.random_range(0..count) == 0 {
                            selected = Some((info.distance + dist_delta, ep, ep));
                        }
                    }
                }
            }

            // ed > ep
            if ep + 1 >= num_edges {
                continue;
            }
            let dist_p_ep1 = *dist_p.add(r_ep1);
            let mut arr_at_curr = fmax(arr_p + svc_p + dist_p_ep1, inst.early(r_ep1));
            if arr_at_curr > inst.late(r_ep1) {
                continue;
            }

            let mut converged = arr_at_curr <= *info.arrival.get_unchecked(ep + 1) + 1e-10;
            if converged {
                arr_at_curr = *info.arrival.get_unchecked(ep + 1);
            }

            let mut running_max = *info.load.get_unchecked(ep + 1);

            if ep + 1 < n - 1 && !all_cap_ok && running_max + q_p > cap {
                continue;
            }

            let base_cost =
                info.distance + dist_p_rep + dist_p_ep1 - *info.edge_dists.get_unchecked(ep);

            for ed in (ep + 1)..num_edges {
                if ed > ep + 1 && ed < n - 1 && !all_cap_ok {
                    running_max = running_max.max(*info.load.get_unchecked(ed));
                    if running_max + q_p > cap {
                        break;
                    }
                }

                let r_ed = *route.get_unchecked(ed);
                let r_ed1 = *route.get_unchecked(ed + 1);

                let svc_r_ed = inst.svc(r_ed);
                let next_arr = if converged {
                    *info.arrival.get_unchecked(ed + 1)
                } else {
                    let na = fmax(
                        arr_at_curr + svc_r_ed + *info.edge_dists.get_unchecked(ed),
                        inst.early(r_ed1),
                    );
                    if na > inst.late(r_ed1) {
                        break;
                    }
                    if na <= *info.arrival.get_unchecked(ed + 1) + 1e-10 {
                        converged = true;
                        *info.arrival.get_unchecked(ed + 1)
                    } else {
                        na
                    }
                };

                // Arc feasibility pre-check for delivery: both (r_ed→d) and (d→r_ed1)
                if !arc_bit(arc_feas_t_d, r_ed) || !arc_bit(arc_feas_d, r_ed1) {
                    arr_at_curr = next_arr;
                    continue;
                }

                let dist_d_red = *dist_d.add(r_ed);
                let arr_d = fmax(arr_at_curr + svc_r_ed + dist_d_red, early_d);
                if arr_d <= late_d {
                    let dist_d_red1 = *dist_d.add(r_ed1);
                    let mut arr_fwd = fmax(arr_d + svc_d + dist_d_red1, inst.early(r_ed1));
                    let mut feasible = arr_fwd <= inst.late(r_ed1);
                    if feasible {
                        let shift = arr_fwd - *info.arrival.get_unchecked(ed + 1);
                        if shift > 1e-10 && ed + 2 < n {
                            if shift <= *info.suffix_min_slack.get_unchecked(ed + 2) {
                                // Guaranteed feasible
                            } else {
                                for i in (ed + 2)..n {
                                    let prev = *route.get_unchecked(i - 1);
                                    let curr = *route.get_unchecked(i);
                                    arr_fwd = fmax(
                                        arr_fwd
                                            + inst.svc(prev)
                                            + *info.edge_dists.get_unchecked(i - 1),
                                        inst.early(curr),
                                    );
                                    if arr_fwd > inst.late(curr) {
                                        feasible = false;
                                        break;
                                    }
                                    if arr_fwd <= *info.arrival.get_unchecked(i) + 1e-10 {
                                        break;
                                    }
                                }
                            }
                        }
                    }
                    if feasible {
                        let new_dist = base_cost - *info.edge_dists.get_unchecked(ed)
                            + dist_d_red
                            + dist_d_red1;
                        count += 1;
                        if rng.random_range(0..count) == 0 {
                            selected = Some((new_dist, ep, ed));
                        }
                    }
                }

                arr_at_curr = next_arr;
            }
        }
    } // unsafe

    selected
}

/// First feasible insertion: returns immediately on the first feasible position found.
/// Much faster than random_feasible when we just need to know IF insertion is possible.
/// Used in GES ejection where we don't need random/best position selection.
#[allow(clippy::similar_names)]
pub fn first_feasible_insertion_with_info(
    inst: &Instance,
    route: &[usize],
    info: &RouteInfo,
    p: usize,
    d: usize,
) -> Option<(f64, usize, usize)> {
    let n = route.len();
    let num_edges = n - 1;
    let q_p = inst.demand[p];
    let cap = inst.capacity;
    let early_p = inst.early(p);
    let late_p = inst.late(p);
    let svc_p = inst.svc(p);
    let early_d = inst.early(d);
    let late_d = inst.late(d);
    let svc_d = inst.svc(d);

    // Use precomputed edge distances from RouteInfo.
    let edge_dists = info.edge_dists.as_slice();

    unsafe {
        let dist_p = inst.dist_flat.as_ptr().add(p * inst.dist_stride);
        let dist_d = inst.dist_flat.as_ptr().add(d * inst.dist_stride);
        let dist_pd = *dist_p.add(d);
        let arc_feas_t_p = inst.arc_feas_t_row(p);
        let arc_feas_d = inst.arc_feas_row(d);
        let arc_feas_t_d = inst.arc_feas_t_row(d);
        for ep in 0..num_edges {
            let load_at_p = *info.load.get_unchecked(ep) + q_p;
            if load_at_p > cap {
                continue;
            }

            let r_ep = *route.get_unchecked(ep);

            // Arc feasibility pre-check (same as best_pair_insertion_with_info)
            if !arc_bit(arc_feas_t_p, r_ep) {
                continue;
            }

            let r_ep1 = *route.get_unchecked(ep + 1);
            let a_ep = *info.arrival.get_unchecked(ep);
            let dist_p_rep = *dist_p.add(r_ep);
            let arr_p = fmax(a_ep + inst.svc(r_ep) + dist_p_rep, early_p);
            if arr_p > late_p {
                continue;
            }

            let dist_ep_removed = *edge_dists.get_unchecked(ep);

            let suffix_start = ep + 1;
            let all_cap_ok = suffix_start >= n - 1
                || *info.suffix_max_load.get_unchecked(suffix_start) + q_p <= cap;

            // ed == ep
            if arc_bit(arc_feas_d, r_ep1) {
                let arr_d = fmax(arr_p + svc_p + dist_pd, early_d);
                if arr_d <= late_d {
                    let dist_d_ep1 = *dist_d.add(r_ep1);
                    let mut arr = fmax(arr_d + svc_d + dist_d_ep1, inst.early(r_ep1));
                    let mut feasible = arr <= inst.late(r_ep1);
                    if feasible {
                        let shift = arr - *info.arrival.get_unchecked(ep + 1);
                        if shift > 1e-10 && ep + 2 < n {
                            if shift <= *info.suffix_min_slack.get_unchecked(ep + 2) {
                                // Guaranteed feasible
                            } else {
                                for i in (ep + 2)..n {
                                    let prev = *route.get_unchecked(i - 1);
                                    let curr = *route.get_unchecked(i);
                                    arr = fmax(
                                        arr + inst.svc(prev) + *edge_dists.get_unchecked(i - 1),
                                        inst.early(curr),
                                    );
                                    if arr > inst.late(curr) {
                                        feasible = false;
                                        break;
                                    }
                                    if arr <= *info.arrival.get_unchecked(i) + 1e-10 {
                                        break;
                                    }
                                }
                            }
                        }
                    }
                    if feasible {
                        let dist_delta = -dist_ep_removed + dist_p_rep + dist_pd + dist_d_ep1;
                        return Some((info.distance + dist_delta, ep, ep));
                    }
                }
            }

            // ed > ep
            if ep + 1 >= num_edges {
                continue;
            }
            let dist_p_ep1 = *dist_p.add(r_ep1);
            let mut arr_at_curr = fmax(arr_p + svc_p + dist_p_ep1, inst.early(r_ep1));
            if arr_at_curr > inst.late(r_ep1) {
                continue;
            }

            let mut converged = arr_at_curr <= *info.arrival.get_unchecked(ep + 1) + 1e-10;
            if converged {
                arr_at_curr = *info.arrival.get_unchecked(ep + 1);
            }

            let mut running_max = *info.load.get_unchecked(ep + 1);

            if ep + 1 < n - 1 && !all_cap_ok && running_max + q_p > cap {
                continue;
            }

            let base_cost =
                info.distance + dist_p_rep + dist_p_ep1 - *info.edge_dists.get_unchecked(ep);

            // Fast path: when arrival has converged (pickup insertion shift absorbed)
            // and capacity is not a concern, build a bitmask of arc-feasible delivery
            // positions and scan only those using trailing_zeros. Skips ~95% of positions
            // that fail arc checks, replacing O(n) sequential iteration with O(k) scan
            // where k = number of arc-feasible positions (typically 0-2).
            if converged && all_cap_ok && num_edges - ep - 1 <= 63 {
                let scan_base = ep + 1;
                let mut arc_mask: u64 = 0;
                for f in scan_base..num_edges {
                    if *info.arrival.get_unchecked(f) > late_d {
                        break;
                    }
                    if arc_bit(arc_feas_t_d, *route.get_unchecked(f))
                        && arc_bit(arc_feas_d, *route.get_unchecked(f + 1))
                    {
                        arc_mask |= 1u64 << (f - scan_base);
                    }
                }
                while arc_mask != 0 {
                    let bit = arc_mask.trailing_zeros() as usize;
                    let ed = scan_base + bit;
                    arc_mask &= arc_mask - 1;

                    let r_ed_m = *route.get_unchecked(ed);
                    let r_ed1_m = *route.get_unchecked(ed + 1);
                    let svc_m = inst.svc(r_ed_m);
                    let arr_m = *info.arrival.get_unchecked(ed);
                    let dd_r = *dist_d.add(r_ed_m);
                    let arr_d_m = fmax(arr_m + svc_m + dd_r, early_d);
                    if arr_d_m <= late_d {
                        let dd_r1 = *dist_d.add(r_ed1_m);
                        let mut arr_fwd = fmax(arr_d_m + svc_d + dd_r1, inst.early(r_ed1_m));
                        let mut feasible = arr_fwd <= inst.late(r_ed1_m);
                        if feasible {
                            let shift = arr_fwd - *info.arrival.get_unchecked(ed + 1);
                            if shift > 1e-10 && ed + 2 < n {
                                if shift <= *info.suffix_min_slack.get_unchecked(ed + 2) {
                                    // Guaranteed feasible
                                } else {
                                    for i in (ed + 2)..n {
                                        let prev = *route.get_unchecked(i - 1);
                                        let curr = *route.get_unchecked(i);
                                        arr_fwd = fmax(
                                            arr_fwd
                                                + inst.svc(prev)
                                                + *edge_dists.get_unchecked(i - 1),
                                            inst.early(curr),
                                        );
                                        if arr_fwd > inst.late(curr) {
                                            feasible = false;
                                            break;
                                        }
                                        if arr_fwd <= *info.arrival.get_unchecked(i) + 1e-10 {
                                            break;
                                        }
                                    }
                                }
                            }
                        }
                        if feasible {
                            let new_dist = base_cost - *edge_dists.get_unchecked(ed) + dd_r + dd_r1;
                            return Some((new_dist, ep, ed));
                        }
                    }
                }
                continue; // next ep
            }

            for ed in (ep + 1)..num_edges {
                // Early termination: arrival already exceeds delivery window.
                // Since arr_at_curr is non-decreasing, no future ed can yield
                // arr_d <= late_d (because arr_d >= arr_at_curr).
                if arr_at_curr > late_d {
                    break;
                }

                if ed > ep + 1 && ed < n - 1 && !all_cap_ok {
                    running_max = running_max.max(*info.load.get_unchecked(ed));
                    if running_max + q_p > cap {
                        break;
                    }
                }

                let r_ed = *route.get_unchecked(ed);
                let r_ed1 = *route.get_unchecked(ed + 1);

                let svc_r_ed = inst.svc(r_ed);
                let next_arr = if converged {
                    *info.arrival.get_unchecked(ed + 1)
                } else {
                    let na = fmax(
                        arr_at_curr + svc_r_ed + *info.edge_dists.get_unchecked(ed),
                        inst.early(r_ed1),
                    );
                    if na > inst.late(r_ed1) {
                        break;
                    }
                    if na <= *info.arrival.get_unchecked(ed + 1) + 1e-10 {
                        converged = true;
                        *info.arrival.get_unchecked(ed + 1)
                    } else {
                        na
                    }
                };

                // Arc feasibility pre-check for delivery: both (r_ed→d) and (d→r_ed1)
                if !arc_bit(arc_feas_t_d, r_ed) || !arc_bit(arc_feas_d, r_ed1) {
                    arr_at_curr = next_arr;
                    continue;
                }

                let dist_d_red = *dist_d.add(r_ed);
                let arr_d = fmax(arr_at_curr + svc_r_ed + dist_d_red, early_d);
                if arr_d <= late_d {
                    let dist_d_red1 = *dist_d.add(r_ed1);
                    let mut arr_fwd = fmax(arr_d + svc_d + dist_d_red1, inst.early(r_ed1));
                    let mut feasible = arr_fwd <= inst.late(r_ed1);
                    if feasible {
                        let shift = arr_fwd - *info.arrival.get_unchecked(ed + 1);
                        if shift > 1e-10 && ed + 2 < n {
                            if shift <= *info.suffix_min_slack.get_unchecked(ed + 2) {
                                // Guaranteed feasible
                            } else {
                                for i in (ed + 2)..n {
                                    let prev = *route.get_unchecked(i - 1);
                                    let curr = *route.get_unchecked(i);
                                    arr_fwd = fmax(
                                        arr_fwd + inst.svc(prev) + *edge_dists.get_unchecked(i - 1),
                                        inst.early(curr),
                                    );
                                    if arr_fwd > inst.late(curr) {
                                        feasible = false;
                                        break;
                                    }
                                    if arr_fwd <= *info.arrival.get_unchecked(i) + 1e-10 {
                                        break;
                                    }
                                }
                            }
                        }
                    }
                    if feasible {
                        let new_dist =
                            base_cost - *edge_dists.get_unchecked(ed) + dist_d_red + dist_d_red1;
                        return Some((new_dist, ep, ed));
                    }
                }

                arr_at_curr = next_arr;
            }
        }
    } // unsafe

    None
}

pub fn build_route_with_pair(
    route: &[usize],
    p: usize,
    ep: usize,
    d: usize,
    ed: usize,
) -> Vec<usize> {
    let mut new_route = Vec::with_capacity(route.len() + 2);
    new_route.extend_from_slice(&route[..=ep]);
    new_route.push(p);
    if ed == ep {
        new_route.push(d);
        new_route.extend_from_slice(&route[ep + 1..]);
    } else {
        new_route.extend_from_slice(&route[ep + 1..=ed]);
        new_route.push(d);
        new_route.extend_from_slice(&route[ed + 1..]);
    }
    new_route
}

/// Like `build_route_with_pair`, but writes into a reusable buffer to avoid allocation.
pub fn build_route_with_pair_buf(
    route: &[usize],
    p: usize,
    ep: usize,
    d: usize,
    ed: usize,
    buf: &mut Vec<usize>,
) {
    buf.clear();
    buf.extend_from_slice(&route[..=ep]);
    buf.push(p);
    if ed == ep {
        buf.push(d);
        buf.extend_from_slice(&route[ep + 1..]);
    } else {
        buf.extend_from_slice(&route[ep + 1..=ed]);
        buf.push(d);
        buf.extend_from_slice(&route[ed + 1..]);
    }
}

/// Build route WITH pair (p inserted after ep, d inserted after ed) AND compute its
/// RouteInfo in one fused pass. Equivalent to `build_route_with_pair_buf` + `compute_light`,
/// but saves one O(n) traversal by combining route construction with the forward info pass.
pub fn compute_info_with_pair(
    inst: &Instance,
    base_route: &[usize],
    p: usize,
    ep: usize,
    d: usize,
    ed: usize,
    dest_route: &mut Vec<usize>,
    info: &mut RouteInfo,
) {
    let base_n = base_route.len();
    let n = base_n + 2;
    let num_edges = n - 1;

    // Ensure capacity
    if n > info.arrival.capacity() {
        info.arrival.reserve(n - info.arrival.len());
        info.load.reserve(n - info.load.len());
        info.suffix_max_load.reserve(n - info.suffix_max_load.len());
        info.suffix_min_slack
            .reserve(n - info.suffix_min_slack.len());
    }
    if num_edges > info.edge_dists.capacity() {
        info.edge_dists.reserve(num_edges - info.edge_dists.len());
    }
    dest_route.clear();
    dest_route.reserve(n);
    #[allow(clippy::uninit_vec)]
    // All elements are written in the fused forward/backward passes below via raw pointers.
    unsafe {
        dest_route.set_len(n);
        info.arrival.set_len(n);
        info.load.set_len(n);
        info.suffix_max_load.set_len(n);
        info.suffix_min_slack.set_len(n);
        info.edge_dists.set_len(num_edges);
    }

    // Fused forward pass: build route + compute arrival/load/edge_dists.
    let dr = dest_route.as_mut_ptr();
    unsafe {
        *dr = base_route[0]; // depot
        *info.arrival.get_unchecked_mut(0) = inst.early(0);
        *info.load.get_unchecked_mut(0) = 0;
    }
    let mut di = 0usize;
    let mut dist = 0.0f64;

    // Macro: emit next node into dest_route and compute its info
    macro_rules! emit {
        ($node:expr) => {{
            let prev_node = unsafe { *dr.add(di) };
            di += 1;
            let node = $node;
            unsafe {
                *dr.add(di) = node;
                let dd = inst.dist(prev_node, node);
                *info.edge_dists.get_unchecked_mut(di - 1) = dd;
                dist += dd;
                *info.arrival.get_unchecked_mut(di) = fmax(
                    *info.arrival.get_unchecked(di - 1) + inst.svc(prev_node) + dd,
                    inst.early(node),
                );
                *info.load.get_unchecked_mut(di) = if di < n - 1 {
                    *info.load.get_unchecked(di - 1) + inst.demand[node]
                } else {
                    0
                };
            }
        }};
    }

    // base_route[1..=ep]
    for bi in 1..=ep {
        emit!(unsafe { *base_route.get_unchecked(bi) });
    }
    // insert pickup
    emit!(p);
    if ed == ep {
        // insert delivery right after pickup
        emit!(d);
        // base_route[ep+1..)
        for bi in (ep + 1)..base_n {
            emit!(unsafe { *base_route.get_unchecked(bi) });
        }
    } else {
        // base_route[ep+1..=ed]
        for bi in (ep + 1)..=ed {
            emit!(unsafe { *base_route.get_unchecked(bi) });
        }
        // insert delivery
        emit!(d);
        // base_route[ed+1..)
        for bi in (ed + 1)..base_n {
            emit!(unsafe { *base_route.get_unchecked(bi) });
        }
    }
    info.distance = dist;

    // Backward pass: suffix arrays
    unsafe {
        *info.suffix_max_load.get_unchecked_mut(n - 1) = i32::MIN;
        *info.suffix_min_slack.get_unchecked_mut(n - 1) =
            inst.late(*dest_route.get_unchecked(n - 1)) - *info.arrival.get_unchecked(n - 1);
        for i in (0..n - 1).rev() {
            *info.suffix_max_load.get_unchecked_mut(i) =
                (*info.load.get_unchecked(i)).max(*info.suffix_max_load.get_unchecked(i + 1));
            let slack = inst.late(*dest_route.get_unchecked(i)) - *info.arrival.get_unchecked(i);
            *info.suffix_min_slack.get_unchecked_mut(i) =
                fmin(slack, *info.suffix_min_slack.get_unchecked(i + 1));
        }
    }
}

pub fn single_pair_feasible_distance(inst: &Instance, p: usize, d: usize) -> Option<f64> {
    if p == 0 || d == 0 || !inst.is_pickup(p) || inst.pickup_of(d) != p {
        return None;
    }

    let mut load = inst.demand[p];
    if load > inst.capacity || load < 0 {
        return None;
    }

    load += inst.demand[d];
    if load > inst.capacity || load < 0 {
        return None;
    }

    let mut time = inst.early(0);

    let dist_0p = inst.dist(0, p);
    let dist_pd = inst.dist(p, d);
    let dist_d0 = inst.dist(d, 0);

    time = f64::max(time + inst.svc(0) + dist_0p, inst.early(p));
    if time > inst.late(p) {
        return None;
    }

    time = f64::max(time + inst.svc(p) + dist_pd, inst.early(d));
    if time > inst.late(d) {
        return None;
    }

    time = f64::max(time + inst.svc(d) + dist_d0, inst.early(0));
    if time > inst.late(0) {
        return None;
    }

    Some(dist_0p + dist_pd + dist_d0)
}

pub fn is_single_pair_feasible(inst: &Instance, p: usize, d: usize) -> bool {
    single_pair_feasible_distance(inst, p, d).is_some()
}

pub fn route_without_pair(route: &[usize], p: usize, d: usize) -> Vec<usize> {
    let mut result = Vec::with_capacity(route.len() - 2);
    for &v in route {
        if v != p && v != d {
            result.push(v);
        }
    }
    result
}

/// Like `route_without_pair`, but writes into a reusable buffer to avoid allocation.
pub fn route_without_pair_buf(route: &[usize], p: usize, d: usize, buf: &mut Vec<usize>) {
    buf.clear();
    for &v in route {
        if v != p && v != d {
            buf.push(v);
        }
    }
}

/// Build route without pair (skip_p, skip_d) AND compute its RouteInfo in one pass.
/// Equivalent to `route_without_pair_buf` + `info.compute`, but ~30% faster
/// by iterating the route only twice (fused forward + backward) instead of thrice.
pub fn compute_info_without_pair(
    inst: &Instance,
    route: &[usize],
    skip_p: usize,
    skip_d: usize,
    virt_route: &mut Vec<usize>,
    info: &mut RouteInfo,
) {
    virt_route.clear();

    // We always remove exactly 2 nodes (one pickup + one delivery) from the route.
    let vn = route.len() - 2;
    virt_route.reserve(vn);

    let num_edges = if vn > 0 { vn - 1 } else { 0 };
    // Ensure info capacity (same pattern as RouteInfo::compute)
    if vn > info.arrival.capacity() {
        info.arrival.reserve(vn - info.arrival.len());
        info.load.reserve(vn - info.load.len());
        info.suffix_max_load
            .reserve(vn - info.suffix_max_load.len());
        info.suffix_min_slack
            .reserve(vn - info.suffix_min_slack.len());
    }
    if num_edges > info.edge_dists.capacity() {
        info.edge_dists.reserve(num_edges - info.edge_dists.len());
    }
    unsafe {
        info.arrival.set_len(vn);
        info.load.set_len(vn);
        info.suffix_max_load.set_len(vn);
        info.suffix_min_slack.set_len(vn);
        info.edge_dists.set_len(num_edges);
    }

    if vn == 0 {
        info.distance = 0.0;
        info.cap_excess = 0.0;
        return;
    }

    // Fused forward pass: build virtual route + compute arrival/load/edge_dists.
    // Use set_len + direct indexing instead of push to avoid capacity checks.
    unsafe {
        virt_route.set_len(vn);
    }
    let vr = virt_route.as_mut_ptr();

    unsafe {
        *vr = route[0]; // depot at position 0
        *info.arrival.get_unchecked_mut(0) = inst.early(0);
        *info.load.get_unchecked_mut(0) = 0;
    }
    let mut vi = 1usize;
    let mut prev_node = route[0];
    let mut dist_total = 0.0f64;

    for i in 1..route.len() {
        let node = *unsafe { route.get_unchecked(i) };
        if node == skip_p || node == skip_d {
            continue;
        }
        unsafe {
            *vr.add(vi) = node;
            let d = inst.dist(prev_node, node);
            *info.edge_dists.get_unchecked_mut(vi - 1) = d;
            dist_total += d;
            *info.arrival.get_unchecked_mut(vi) = fmax(
                *info.arrival.get_unchecked(vi - 1) + inst.svc(prev_node) + d,
                inst.early(node),
            );
            *info.load.get_unchecked_mut(vi) = if vi < vn - 1 {
                *info.load.get_unchecked(vi - 1) + inst.demand[node]
            } else {
                0
            };
        }
        prev_node = node;
        vi += 1;
    }
    info.distance = dist_total;

    // Backward pass: suffix arrays (same as RouteInfo::compute)
    unsafe {
        *info.suffix_max_load.get_unchecked_mut(vn - 1) = i32::MIN;
        *info.suffix_min_slack.get_unchecked_mut(vn - 1) =
            inst.late(*virt_route.get_unchecked(vn - 1)) - *info.arrival.get_unchecked(vn - 1);
        for i in (0..vn - 1).rev() {
            *info.suffix_max_load.get_unchecked_mut(i) =
                (*info.load.get_unchecked(i)).max(*info.suffix_max_load.get_unchecked(i + 1));
            let slack = inst.late(*virt_route.get_unchecked(i)) - *info.arrival.get_unchecked(i);
            *info.suffix_min_slack.get_unchecked_mut(i) =
                fmin(slack, *info.suffix_min_slack.get_unchecked(i + 1));
        }

        // Fast cap_excess
        let cap = inst.capacity;
        if vn >= 2 && *info.suffix_max_load.get_unchecked(0) > cap {
            let mut cap_excess = 0.0;
            for i in 0..vn - 1 {
                let excess = *info.load.get_unchecked(i) - cap;
                if excess > 0 {
                    cap_excess += f64::from(excess);
                }
            }
            info.cap_excess = cap_excess;
        } else {
            info.cap_excess = 0.0;
        }

        // Compute tw_excess from arrival times
        let mut tw_ex = 0.0f64;
        for i in 0..vn {
            let excess = *info.arrival.get_unchecked(i) - inst.late(*vr.add(i));
            if excess > 1e-10 {
                tw_ex += excess;
            }
        }
        info.tw_excess = tw_ex;
    }
}

#[cfg(test)]
mod tests {
    use super::{Solution, is_route_feasible, is_single_pair_feasible};
    use crate::instance::Instance;

    fn tiny_instance() -> Instance {
        crate::instance::tests::make_test_instance(
            1,  // m = 1 pair
            10, // capacity
            &[
                // dist: 0->1=1, 0->2=2, 1->2=1, symmetric
                0.0, 1.0, 2.0, 1.0, 0.0, 1.0, 2.0, 1.0, 0.0,
            ],
            &[0, 1, -1],            // demand
            &[0.0, 0.0, 0.0],       // early
            &[100.0, 100.0, 100.0], // late
            &[0.0, 0.0, 0.0],       // service
        )
    }

    #[test]
    fn incomplete_solution_is_not_feasible() {
        let inst = tiny_instance();
        let sol = Solution {
            routes: Vec::new(),
            unassigned: vec![1],
        };
        assert!(!sol.is_feasible(&inst));
    }

    #[test]
    fn complete_solution_beats_incomplete_solution() {
        let inst = tiny_instance();
        let complete = Solution {
            routes: vec![vec![0, 1, 2, 0]],
            unassigned: Vec::new(),
        };
        let incomplete = Solution {
            routes: Vec::new(),
            unassigned: vec![1],
        };

        assert!(is_route_feasible(&inst, &complete.routes[0]));
        assert!(complete.is_better_than(&incomplete, &inst));
        assert!(!incomplete.is_better_than(&complete, &inst));
    }

    #[test]
    fn route_with_open_pickup_is_infeasible() {
        let inst = tiny_instance();
        assert!(!is_route_feasible(&inst, &[0, 1, 0]));
    }

    #[test]
    fn single_pair_fast_check_matches_full_route_check() {
        let inst = tiny_instance();
        for p in 0..=inst.n {
            for d in 0..=inst.n {
                assert_eq!(
                    is_single_pair_feasible(&inst, p, d),
                    is_route_feasible(&inst, &[0, p, d, 0]),
                    "p={p}, d={d}"
                );
            }
        }
    }

    /// Instance with 2 PD pairs for cross-route tests.
    fn two_pair_instance() -> Instance {
        crate::instance::tests::make_test_instance(
            2, // m = 2 pairs: (1,3) and (2,4)
            10,
            &[
                // 5x5 distance matrix (depot + 4 customers), all distances = 1
                0.0, 1.0, 1.0, 1.0, 1.0, 1.0, 0.0, 1.0, 1.0, 1.0, 1.0, 1.0, 0.0, 1.0, 1.0, 1.0, 1.0,
                1.0, 0.0, 1.0, 1.0, 1.0, 1.0, 1.0, 0.0,
            ],
            &[0, 1, 2, -1, -2],
            &[0.0, 0.0, 0.0, 0.0, 0.0],
            &[100.0, 100.0, 100.0, 100.0, 100.0],
            &[0.0, 0.0, 0.0, 0.0, 0.0],
        )
    }

    #[test]
    fn duplicate_customer_across_routes_is_infeasible() {
        let inst = two_pair_instance();
        // Pair 1 appears in both routes → infeasible
        let sol = Solution {
            routes: vec![
                vec![0, 1, 3, 0],       // pair (1,3)
                vec![0, 1, 2, 3, 4, 0], // pair (1,3) AND pair (2,4) — duplicate!
            ],
            unassigned: Vec::new(),
        };
        assert!(!sol.is_feasible(&inst));
    }

    #[test]
    fn missing_pair_in_routes_is_infeasible() {
        let inst = two_pair_instance();
        // Only pair 1 assigned, pair 2 missing from both routes and unassigned
        let sol = Solution {
            routes: vec![vec![0, 1, 3, 0]],
            unassigned: Vec::new(),
        };
        assert!(!sol.is_feasible(&inst));
    }

    #[test]
    fn valid_two_route_solution_is_feasible() {
        let inst = two_pair_instance();
        let sol = Solution {
            routes: vec![
                vec![0, 1, 3, 0], // pair (1,3)
                vec![0, 2, 4, 0], // pair (2,4)
            ],
            unassigned: Vec::new(),
        };
        assert!(sol.is_feasible(&inst));
    }

    #[test]
    fn from_routes_builds_complete_feasible_solution() {
        let inst = two_pair_instance();
        let sol = Solution::from_routes(&inst, vec![vec![0, 1, 3, 0], vec![0, 2, 4, 0]])
            .expect("valid initial routes");
        assert!(sol.unassigned.is_empty());
        assert!(sol.is_feasible(&inst));
    }

    #[test]
    fn from_routes_marks_missing_pairs_as_unassigned() {
        let inst = two_pair_instance();
        let sol =
            Solution::from_routes(&inst, vec![vec![0, 1, 3, 0]]).expect("valid partial routes");

        assert_eq!(sol.unassigned, vec![2]);
        assert!(!sol.is_feasible(&inst));
    }

    #[test]
    fn from_routes_rejects_invalid_route() {
        let inst = two_pair_instance();
        let err = Solution::from_routes(&inst, vec![vec![0, 3, 1, 0]])
            .err()
            .expect("delivery before pickup must fail");
        assert!(err.contains("route 0"));
    }
}