pdp_lns 0.1.1

use std::fs;
use std::path::Path;

/// Packed per-node time-window data for cache-friendly access.
/// In hot inner loops, accessing early/late/service for the same node
/// hits a single cache line (24 bytes) instead of 3 separate arrays.
#[derive(Clone, Copy)]
#[repr(C)]
pub struct NodeTW {
    pub early: f64,
    pub late: f64,
    pub service: f64,
}

/// PDPTW problem instance in Li & Lim benchmark format.
#[derive(Clone)]
pub struct Instance {
    /// Total number of customer nodes (pickups + deliveries), excluding depot.
    pub n: usize,
    /// Number of PD pairs (= n/2).
    pub m: usize,
    /// Maximum number of vehicles.
    pub num_vehicles: usize,
    /// Vehicle capacity Q.
    pub capacity: i32,
    /// Demand for each location. Positive for pickup, negative for delivery, 0 for depot.
    pub demand: Vec<i32>,
    /// Packed time-window data: early, late, service per node (kept for struct completeness).
    pub tw: Vec<NodeTW>,
    /// Split time-window arrays for cache-friendly access (eliminates ×3 multiply in hot loops).
    pub tw_early: Vec<f64>,
    pub tw_late: Vec<f64>,
    pub tw_svc: Vec<f64>,
    /// For pickup node i, `pair_delivery[i]` = corresponding delivery node.
    /// For delivery/depot nodes, 0.
    pub pair_delivery: Vec<usize>,
    /// For delivery node d, `pair_pickup[d]` = corresponding pickup node.
    /// For pickup/depot nodes, 0.
    pub pair_pickup: Vec<usize>,
    /// List of pickup node IDs.
    pub pickups: Vec<usize>,
    /// Distance/travel time matrix (flat). Access via `inst.dist(i, j)`.
    pub dist_flat: Vec<f64>,
    /// Stride for flat distance matrix (= n + 1).
    pub dist_stride: usize,
    /// Bit-packed feasible arc set A': bit `j` of word `i * arc_stride_words + (j >> 6)` is set
    /// if arc (i,j) is feasible. Packed to ~6KB for 601 nodes (fits L1 cache).
    pub arc_feasible: Vec<u64>,
    /// Transposed feasible arc set: bit `a` of word `b * arc_stride_words + (a >> 6)` is set
    /// if arc (a,b) is feasible. Enables checking "which arcs go TO node b?" with L1 cache hits.
    pub arc_feasible_t: Vec<u64>,
    /// Stride for bit-packed arc matrices in 64-bit words (= (total + 63) / 64).
    pub arc_stride_words: usize,
    /// Number of feasible arcs (|A'|).
    pub num_feasible_arcs: usize,
    /// Bit-packed sparse arc set A'^- (LP reduced cost based).
    pub arc_sparse: Vec<u64>,
    /// Transposed sparse arc set: `arc_sparse_t[b][a]` = `arc_sparse[a][b]`.
    pub arc_sparse_t: Vec<u64>,
    /// Sparse outgoing adjacency list: `sparse_out[v]` = nodes u where arc (v,u) is sparse.
    pub sparse_out: Vec<Vec<usize>>,
    /// Sparse incoming adjacency list: `sparse_in[v]` = nodes u where arc (u,v) is sparse.
    pub sparse_in: Vec<Vec<usize>>,
    /// LP reduced costs per arc: `reduced_costs[i * stride + j]` = reduced cost.
    /// f64::MAX for infeasible arcs.
    pub reduced_costs: Vec<f64>,
    /// Optional Euclidean coordinates (needed by CLP-based initial construction).
    pub coords: Option<Vec<(f64, f64)>>,
    /// Precomputed polar angles (relative to depot) mapped to [0, 65535].
    /// Used for circle sector filtering in SWAP*.
    pub polar_angle: Option<Vec<u16>>,
    /// Bit-packed pairwise request conflict matrix (m × m).
    /// `conflict[i * conflict_stride + (j >> 6)]` bit `(j & 63)` = 1 means
    /// requests i and j cannot share a route (all 6 orderings are TW-infeasible).
    /// Indexed by request index (position in `pickups` vec), not node ID.
    pub conflict: Vec<u64>,
    /// Stride for conflict matrix in 64-bit words (= (m + 63) / 64).
    pub conflict_stride: usize,
    /// Maps pickup node ID → request index in [0..m). Delivery/depot nodes map to usize::MAX.
    pub pickup_to_req: Vec<usize>,
}

impl Instance {
    /// Load a PDPTW instance from a Li & Lim format text file.
    pub fn from_file(path: &Path) -> Self {
        let content = fs::read_to_string(path).expect("Cannot read instance file");
        let mut lines = content.lines().filter(|l| !l.trim().is_empty());

        // First line: num_vehicles capacity
        let first_line = lines.next().expect("Missing first line");
        let parts: Vec<&str> = first_line.split_whitespace().collect();
        let num_vehicles: usize = parts[0].parse().expect("Bad num_vehicles");
        let capacity: i32 = parts[1].parse().expect("Bad capacity");

        // Remaining lines: customer data
        // id x y demand ready_time due_date service_time pickup delivery
        let mut ids = Vec::new();
        let mut xs = Vec::new();
        let mut ys = Vec::new();
        let mut demands = Vec::new();
        let mut earlies = Vec::new();
        let mut lates = Vec::new();
        let mut services = Vec::new();
        let mut pickups_raw = Vec::new();
        let mut deliveries_raw = Vec::new();

        for line in lines {
            let parts: Vec<&str> = line.split_whitespace().collect();
            if parts.len() < 9 {
                continue;
            }
            let id: usize = parts[0].parse().unwrap();
            ids.push(id);
            xs.push(parts[1].parse::<f64>().unwrap());
            ys.push(parts[2].parse::<f64>().unwrap());
            demands.push(parts[3].parse::<i32>().unwrap());
            earlies.push(parts[4].parse::<f64>().unwrap());
            lates.push(parts[5].parse::<f64>().unwrap());
            services.push(parts[6].parse::<f64>().unwrap());
            pickups_raw.push(parts[7].parse::<usize>().unwrap());
            deliveries_raw.push(parts[8].parse::<usize>().unwrap());
        }

        let n = ids.len() - 1; // exclude depot
        let m = n / 2;

        // Build pair mappings
        let mut pair_delivery = vec![0usize; n + 1];
        let mut pickups = Vec::new();

        for (idx, &id) in ids.iter().enumerate() {
            if id == 0 {
                continue;
            }
            if demands[idx] > 0 {
                // This is a pickup node; deliveries_raw[idx] is its delivery
                pair_delivery[id] = deliveries_raw[idx];
                pickups.push(id);
            }
        }

        // Compute flat distance matrix (Euclidean)
        let total = n + 1; // depot + n customers
        let mut dist_flat = vec![0.0f64; total * total];
        for i in 0..total {
            for j in 0..total {
                let dx = xs[i] - xs[j];
                let dy = ys[i] - ys[j];
                dist_flat[i * total + j] = (dx * dx + dy * dy).sqrt();
            }
        }

        // Build reverse mapping: delivery -> pickup
        let mut pair_pickup = vec![0usize; n + 1];
        for &p in &pickups {
            let d = pair_delivery[p];
            pair_pickup[d] = p;
        }

        let coords: Option<Vec<(f64, f64)>> = Some(xs.into_iter().zip(ys).collect());

        Self::finalize(
            n,
            m,
            num_vehicles,
            capacity,
            demands,
            earlies,
            lates,
            services,
            pair_delivery,
            pair_pickup,
            pickups,
            dist_flat,
            coords,
        )
    }

    /// Build an instance from raw arrays (for programmatic / Python use).
    #[allow(clippy::too_many_arguments)]
    pub fn from_raw(
        num_vehicles: usize,
        capacity: i32,
        demand: Vec<i32>,
        early: Vec<f64>,
        late: Vec<f64>,
        service: Vec<f64>,
        pair_delivery: Vec<usize>,
        dist_matrix: Vec<Vec<f64>>,
        coords: Option<Vec<(f64, f64)>>,
    ) -> Result<Self, String> {
        if demand.is_empty() {
            return Err("demand must not be empty".to_string());
        }

        let total = demand.len();
        if total < 2 {
            return Err("instance must contain depot and at least one customer".to_string());
        }
        if early.len() != total || late.len() != total || service.len() != total {
            return Err("early/late/service length must match demand length".to_string());
        }
        if pair_delivery.len() != total {
            return Err("pair_delivery length must match demand length".to_string());
        }
        if dist_matrix.len() != total {
            return Err("dist_matrix must be square and match demand length".to_string());
        }
        if dist_matrix.iter().any(|row| row.len() != total) {
            return Err("dist_matrix must be square".to_string());
        }
        if let Some(ref c) = coords
            && c.len() != total
        {
            return Err("coords length must match demand length".to_string());
        }
        if demand[0] != 0 {
            return Err("depot demand at index 0 must be 0".to_string());
        }

        let n = total - 1;
        let mut dist_flat = Vec::with_capacity(total * total);
        for row in dist_matrix {
            dist_flat.extend(row);
        }

        let mut pickups: Vec<usize> = Vec::new();
        for id in 1..=n {
            if demand[id] > 0 {
                let d = pair_delivery[id];
                if d == 0 || d > n {
                    return Err(format!("pickup node {id} has invalid delivery index {d}"));
                }
                pickups.push(id);
            } else if demand[id] < 0 && pair_delivery[id] != 0 {
                return Err(format!(
                    "delivery node {id} should have pair_delivery[{id}] = 0"
                ));
            }
        }

        let mut pair_pickup = vec![0usize; total];
        for &p in &pickups {
            let d = pair_delivery[p];
            if demand[d] >= 0 {
                return Err(format!(
                    "pickup node {p} points to node {d}, but node {d} is not a delivery"
                ));
            }
            if pair_pickup[d] != 0 {
                return Err(format!(
                    "delivery node {d} is assigned to multiple pickups ({}, {p})",
                    pair_pickup[d]
                ));
            }
            pair_pickup[d] = p;
        }

        for d in 1..=n {
            if demand[d] < 0 && pair_pickup[d] == 0 {
                return Err(format!("delivery node {d} has no paired pickup"));
            }
        }

        let m = pickups.len();

        Ok(Self::finalize(
            n,
            m,
            num_vehicles,
            capacity,
            demand,
            early,
            late,
            service,
            pair_delivery,
            pair_pickup,
            pickups,
            dist_flat,
            coords,
        ))
    }

    /// Shared post-parsing logic: TW tightening, arc feasibility, bit-packing,
    /// sparse arcs, and conflict computation.
    #[allow(clippy::too_many_arguments)]
    fn finalize(
        n: usize,
        m: usize,
        num_vehicles: usize,
        capacity: i32,
        demand: Vec<i32>,
        mut earlies: Vec<f64>,
        mut lates: Vec<f64>,
        services: Vec<f64>,
        pair_delivery: Vec<usize>,
        pair_pickup: Vec<usize>,
        pickups: Vec<usize>,
        dist_flat: Vec<f64>,
        coords: Option<Vec<(f64, f64)>>,
    ) -> Self {
        let total = n + 1;

        // Time window tightening (Goeke 2019, Section 3.1, step i)
        // Propagate constraints from depot and PD pair relationships.
        let mut changed = true;
        while changed {
            changed = false;
            for &p in &pickups {
                let d = pair_delivery[p];
                let t_pd = dist_flat[p * total + d];
                let t_0p = dist_flat[p];
                let t_d0 = dist_flat[d * total];

                // Pickup: earliest = max(e_p, e_0 + t_{0,p})
                let new_ep = f64::max(earlies[p], earlies[0] + services[0] + t_0p);
                if new_ep > earlies[p] + 1e-10 {
                    earlies[p] = new_ep;
                    changed = true;
                }

                // Pickup: latest = min(l_p, l_d - s_p - t_{p,d})
                let new_lp = f64::min(lates[p], lates[d] - services[p] - t_pd);
                if new_lp < lates[p] - 1e-10 {
                    lates[p] = new_lp;
                    changed = true;
                }

                // Delivery: earliest = max(e_d, e_p + s_p + t_{p,d})
                let new_ed = f64::max(earlies[d], earlies[p] + services[p] + t_pd);
                if new_ed > earlies[d] + 1e-10 {
                    earlies[d] = new_ed;
                    changed = true;
                }

                // Delivery: latest = min(l_d, l_0 - s_d - t_{d,0})
                let new_ld = f64::min(lates[d], lates[0] - services[d] - t_d0);
                if new_ld < lates[d] - 1e-10 {
                    lates[d] = new_ld;
                    changed = true;
                }
            }
        }

        // Compute feasible arc set A' (Goeke 2019, Section 3.1, step ii)
        // Pack directly into bit-packed u64 words (+ transposed) for L1-cache-resident access.
        let arc_stride_words = total.div_ceil(64);
        let mut arc_feasible = vec![0u64; total * arc_stride_words];
        let mut arc_feasible_t = vec![0u64; total * arc_stride_words];
        let mut num_feasible_arcs: usize = 0;

        for i in 0..total {
            for j in 0..total {
                if i == j {
                    continue;
                }

                let mut feasible = true;

                // (24) Time window infeasibility: e_i + s_i + t_{ij} > l_j
                if earlies[i] + services[i] + dist_flat[i * total + j] > lates[j] + 1e-10 {
                    feasible = false;
                }

                // Structural PDPTW constraints:
                if feasible && j != 0 {
                    // (22) Depot → delivery is infeasible (pickup must come first in route)
                    if i == 0 && demand[j] < 0 {
                        feasible = false;
                    }
                }

                if feasible && i != 0 {
                    // Pickup → depot is infeasible (delivery must come after pickup)
                    if j == 0 && demand[i] > 0 {
                        feasible = false;
                    }
                }

                // (23) Delivery → its own pickup is infeasible
                if feasible && i != 0 && j != 0 && demand[i] < 0 && pair_pickup[i] == j {
                    feasible = false;
                }

                // Extended: for pickup p going to node j, check if there's time to
                // reach p's delivery d and return to depot: e_j + s_j + ... must be feasible
                // Check: can we go i → j and still complete j's pair obligations?
                if feasible && j != 0 && demand[j] > 0 {
                    // j is a pickup; must visit j's delivery d_j and return to depot
                    let d_j = pair_delivery[j];
                    if d_j != 0 {
                        let arr_j = f64::max(
                            earlies[i] + services[i] + dist_flat[i * total + j],
                            earlies[j],
                        );
                        let arr_dj = arr_j + services[j] + dist_flat[j * total + d_j];
                        if arr_dj > lates[d_j] + 1e-10 {
                            feasible = false;
                        }
                    }
                }

                if feasible && j != 0 && demand[j] < 0 {
                    // j is a delivery; must return to depot after j
                    let arr_j = f64::max(
                        earlies[i] + services[i] + dist_flat[i * total + j],
                        earlies[j],
                    );
                    if arr_j + services[j] + dist_flat[j * total] > lates[0] + 1e-10 {
                        feasible = false;
                    }
                }

                // (25) Cordeau (2006): if i is a pickup, check detour i → j → delivery_of(i).
                // If the earliest arrival at j via i, plus travel j → delivery(i), exceeds
                // delivery(i)'s latest time, then arc (i,j) is infeasible — any route using
                // this arc cannot satisfy i's delivery time window.
                if feasible && i != 0 && j != 0 && demand[i] > 0 {
                    let d_i = pair_delivery[i];
                    if d_i != 0 {
                        let arr_j = f64::max(
                            earlies[j],
                            earlies[i] + services[i] + dist_flat[i * total + j],
                        );
                        if arr_j + services[j] + dist_flat[j * total + d_i] > lates[d_i] + 1e-10 {
                            feasible = false;
                        }
                    }
                }

                // (26) Cordeau (2006): if j is a delivery, check detour pickup_of(j) → i → j.
                // If the earliest arrival at i via pickup(j), plus travel i → j, exceeds
                // j's latest time, then arc (i,j) is infeasible — any route using this arc
                // cannot satisfy j's pickup-before-delivery constraint.
                if feasible && i != 0 && j != 0 && demand[j] < 0 {
                    let p_j = pair_pickup[j];
                    if p_j != 0 {
                        let arr_i = f64::max(
                            earlies[i],
                            earlies[p_j] + services[p_j] + dist_flat[p_j * total + i],
                        );
                        if arr_i + services[i] + dist_flat[i * total + j] > lates[j] + 1e-10 {
                            feasible = false;
                        }
                    }
                }

                if feasible {
                    arc_feasible[i * arc_stride_words + (j >> 6)] |= 1u64 << (j & 63);
                    arc_feasible_t[j * arc_stride_words + (i >> 6)] |= 1u64 << (i & 63);
                    num_feasible_arcs += 1;
                }
            }
        }

        let total_arcs = total * (total - 1);
        let removed = total_arcs - num_feasible_arcs;
        eprintln!(
            "  Arc preprocessing: {num_feasible_arcs}/{total_arcs} feasible ({removed} removed, {:.1}%)",
            removed as f64 / total_arcs as f64 * 100.0
        );
        eprintln!(
            "  Arc matrices: {}×{} = {} bytes bit-packed (vs {} bytes bool)",
            total,
            arc_stride_words,
            arc_feasible.len() * 8,
            total * total,
        );

        let tw: Vec<NodeTW> = (0..total)
            .map(|i| NodeTW {
                early: earlies[i],
                late: lates[i],
                service: services[i],
            })
            .collect();

        let mut inst = Instance {
            n,
            m,
            num_vehicles,
            capacity,
            demand,
            tw,
            tw_early: earlies,
            tw_late: lates,
            tw_svc: services,
            pair_delivery,
            pair_pickup,
            pickups,
            dist_flat,
            dist_stride: total,
            arc_feasible,
            arc_feasible_t,
            arc_stride_words,
            num_feasible_arcs,
            arc_sparse: Vec::new(),   // computed below
            arc_sparse_t: Vec::new(), // computed below
            sparse_out: Vec::new(),
            sparse_in: Vec::new(),
            reduced_costs: Vec::new(),
            coords,
            polar_angle: None, // computed below
            conflict: Vec::new(),
            conflict_stride: 0,
            pickup_to_req: Vec::new(),
        };
        inst.polar_angle = Self::compute_polar_angles(inst.coords.as_deref());

        // Compute sparse arc set using LP reduced costs
        let arc_result = crate::arc_scoring::compute_sparse_arcs(&inst);
        let arc_sparse_bool = arc_result.sparse;
        inst.reduced_costs = arc_result.reduced_costs;
        inst.arc_sparse = Self::pack_bool_matrix(&arc_sparse_bool, total, arc_stride_words);

        // Transposed sparse matrix: arc_sparse_t[b][a] = arc_sparse[a][b]
        let mut arc_sparse_t_bool = vec![false; total * total];
        for i in 0..total {
            for j in 0..total {
                arc_sparse_t_bool[j * total + i] = arc_sparse_bool[i * total + j];
            }
        }
        inst.arc_sparse_t = Self::pack_bool_matrix(&arc_sparse_t_bool, total, arc_stride_words);

        // Build sparse adjacency lists from arc_sparse bool matrix
        let mut sparse_out = vec![Vec::new(); total];
        let mut sparse_in = vec![Vec::new(); total];
        for i in 0..total {
            for j in 0..total {
                if arc_sparse_bool[i * total + j] {
                    sparse_out[i].push(j);
                    sparse_in[j].push(i);
                }
            }
        }
        inst.sparse_out = sparse_out;
        inst.sparse_in = sparse_in;

        // Compute pairwise request conflict matrix
        inst.compute_conflicts();

        inst
    }

    /// Pack a flat bool matrix (total × total) into bit-packed u64 words.
    fn pack_bool_matrix(bools: &[bool], total: usize, stride_words: usize) -> Vec<u64> {
        let mut packed = vec![0u64; total * stride_words];
        for i in 0..total {
            for j in 0..total {
                if bools[i * total + j] {
                    packed[i * stride_words + (j >> 6)] |= 1u64 << (j & 63);
                }
            }
        }
        packed
    }

    /// Compute polar angles relative to the depot, mapped to [0, 65535].
    fn compute_polar_angles(coords: Option<&[(f64, f64)]>) -> Option<Vec<u16>> {
        coords.map(|c| {
            let (dep_x, dep_y) = c[0];
            c.iter()
                .map(|&(x, y)| {
                    let angle = (y - dep_y).atan2(x - dep_x); // [-pi, pi]
                    ((angle + std::f64::consts::PI) / (2.0 * std::f64::consts::PI) * 65536.0) as u16
                })
                .collect()
        })
    }

    /// Compute pairwise request conflict matrix.
    /// For each pair of requests (r_i, r_j), tests all 6 orderings of their
    /// pickup/delivery nodes in a minimal route (depot → ... → depot).
    /// If all 6 orderings are TW-infeasible, the pair is conflicting.
    fn compute_conflicts(&mut self) {
        let m = self.m;
        // Pad stride to next multiple of 8 u64s (512 bits) for AVX-512 vectorization
        // of has_conflict_with_mask. Extra words are zero-initialized and don't affect
        // correctness (AND with zero mask bits = 0).
        let stride = (m.div_ceil(64) + 7) & !7;
        let mut conflict = vec![0u64; m * stride];
        let mut pickup_to_req = vec![usize::MAX; self.n + 1];
        for (idx, &p) in self.pickups.iter().enumerate() {
            pickup_to_req[p] = idx;
        }

        let mut num_conflicts = 0usize;
        for i in 0..m {
            let p1 = self.pickups[i];
            let d1 = self.pair_delivery[p1];
            for j in (i + 1)..m {
                let p2 = self.pickups[j];
                let d2 = self.pair_delivery[p2];

                // Test all 6 orderings of (p1,d1) and (p2,d2) respecting PD precedence
                let orderings: [[usize; 4]; 6] = [
                    [p1, d1, p2, d2],
                    [p1, p2, d1, d2],
                    [p1, p2, d2, d1],
                    [p2, d2, p1, d1],
                    [p2, p1, d2, d1],
                    [p2, p1, d1, d2],
                ];

                let mut any_feasible = false;
                for seq in &orderings {
                    if self.is_ordering_tw_feasible(seq) {
                        any_feasible = true;
                        break;
                    }
                }

                if !any_feasible {
                    conflict[i * stride + (j >> 6)] |= 1u64 << (j & 63);
                    conflict[j * stride + (i >> 6)] |= 1u64 << (i & 63);
                    num_conflicts += 1;
                }
            }
        }

        eprintln!(
            "  Request conflicts: {num_conflicts}/{} pairs ({:.1}%)",
            m * (m - 1) / 2,
            if m > 1 {
                num_conflicts as f64 / (m * (m - 1) / 2) as f64 * 100.0
            } else {
                0.0
            }
        );

        self.conflict = conflict;
        self.conflict_stride = stride;
        self.pickup_to_req = pickup_to_req;
    }

    /// Check if a 4-node ordering [a, b, c, d] is TW-feasible as a route 0→a→b→c→d→0.
    fn is_ordering_tw_feasible(&self, seq: &[usize; 4]) -> bool {
        let dep_svc = self.svc(0);
        let mut arr = f64::max(
            self.early(seq[0]),
            self.early(0) + dep_svc + self.dist(0, seq[0]),
        );
        if arr > self.late(seq[0]) {
            return false;
        }
        for w in 0..3 {
            arr = f64::max(
                self.early(seq[w + 1]),
                arr + self.svc(seq[w]) + self.dist(seq[w], seq[w + 1]),
            );
            if arr > self.late(seq[w + 1]) {
                return false;
            }
        }
        arr + self.svc(seq[3]) + self.dist(seq[3], 0) <= self.late(0)
    }

    /// Earliest service start time for node `i`.
    #[inline(always)]
    pub fn early(&self, i: usize) -> f64 {
        unsafe { *self.tw_early.get_unchecked(i) }
    }

    /// Latest service start time for node `i`.
    #[inline(always)]
    pub fn late(&self, i: usize) -> f64 {
        unsafe { *self.tw_late.get_unchecked(i) }
    }

    /// Service duration for node `i`.
    #[inline(always)]
    pub fn svc(&self, i: usize) -> f64 {
        unsafe { *self.tw_svc.get_unchecked(i) }
    }

    /// Distance from node `a` to node `b`.
    #[inline(always)]
    pub fn dist(&self, a: usize, b: usize) -> f64 {
        unsafe { *self.dist_flat.get_unchecked(a * self.dist_stride + b) }
    }

    /// Returns the delivery node for a given pickup node.
    #[inline(always)]
    pub fn delivery_of(&self, pickup: usize) -> usize {
        self.pair_delivery[pickup]
    }

    /// Check if a node is a pickup.
    #[inline(always)]
    pub fn is_pickup(&self, node: usize) -> bool {
        node > 0 && self.demand[node] > 0
    }

    /// Returns the pickup node for a given delivery node.
    #[inline(always)]
    pub fn pickup_of(&self, delivery: usize) -> usize {
        self.pair_pickup[delivery]
    }

    /// Check if arc (a, b) is in the feasible arc set A' (bit-packed).
    #[inline(always)]
    pub fn is_arc_feasible(&self, a: usize, b: usize) -> bool {
        unsafe {
            let word = *self
                .arc_feasible
                .get_unchecked(a * self.arc_stride_words + (b >> 6));
            (word >> (b & 63)) & 1 != 0
        }
    }

    /// Check if arc (a, b) is feasible, using transposed matrix (indexed by b first).
    /// Semantically identical to `is_arc_feasible(a, b)` but accesses row b instead of row a.
    /// Use when b is loop-invariant to keep row b in L1 cache.
    #[inline(always)]
    pub fn is_arc_feasible_rev(&self, a: usize, b: usize) -> bool {
        unsafe {
            let word = *self
                .arc_feasible_t
                .get_unchecked(b * self.arc_stride_words + (a >> 6));
            (word >> (a & 63)) & 1 != 0
        }
    }

    /// LP reduced cost of arc (a, b). Returns f64::MAX for infeasible arcs.
    #[inline(always)]
    pub fn rc(&self, a: usize, b: usize) -> f64 {
        self.reduced_costs[a * self.dist_stride + b]
    }

    /// Check if arc (a, b) is in the sparse arc set A'^- (bit-packed).
    #[inline(always)]
    pub fn is_arc_sparse(&self, a: usize, b: usize) -> bool {
        unsafe {
            let word = *self
                .arc_sparse
                .get_unchecked(a * self.arc_stride_words + (b >> 6));
            (word >> (b & 63)) & 1 != 0
        }
    }

    /// Row pointer into arc_feasible for node `a`.
    #[inline(always)]
    pub fn arc_feas_row(&self, a: usize) -> *const u64 {
        unsafe { self.arc_feasible.as_ptr().add(a * self.arc_stride_words) }
    }

    /// Row pointer into arc_feasible_t for node `b`.
    #[inline(always)]
    pub fn arc_feas_t_row(&self, b: usize) -> *const u64 {
        unsafe { self.arc_feasible_t.as_ptr().add(b * self.arc_stride_words) }
    }

    /// Row pointer into arc_sparse for node `a`.
    #[inline(always)]
    pub fn arc_sparse_row(&self, a: usize) -> *const u64 {
        unsafe { self.arc_sparse.as_ptr().add(a * self.arc_stride_words) }
    }

    /// Row pointer into arc_sparse_t for node `b` (transposed: tests which arcs go TO `b`).
    #[inline(always)]
    pub fn arc_sparse_t_row(&self, b: usize) -> *const u64 {
        unsafe { self.arc_sparse_t.as_ptr().add(b * self.arc_stride_words) }
    }

    /// Node coordinates if available (from file-based instances).
    #[inline(always)]
    pub fn coord(&self, node: usize) -> Option<(f64, f64)> {
        self.coords
            .as_ref()
            .and_then(|coords| coords.get(node))
            .copied()
    }

    /// Check if two requests conflict (cannot share a route).
    /// `p1` and `p2` are pickup node IDs.
    #[inline(always)]
    pub fn requests_conflict(&self, p1: usize, p2: usize) -> bool {
        let i = self.pickup_to_req[p1];
        let j = self.pickup_to_req[p2];
        if i == usize::MAX || j == usize::MAX {
            return false;
        }
        unsafe {
            let word = *self
                .conflict
                .get_unchecked(i * self.conflict_stride + (j >> 6));
            (word >> (j & 63)) & 1 != 0
        }
    }

    /// Check if inserting pickup `p` into `route` would conflict with any existing request.
    /// Returns true if any request already in the route conflicts with `p`.
    #[inline]
    pub fn has_route_conflict(&self, route: &[usize], p: usize) -> bool {
        let ri = self.pickup_to_req[p];
        if ri == usize::MAX {
            return false;
        }
        let conflict_row = &self.conflict[ri * self.conflict_stride..];
        for &v in route {
            if v == 0 {
                continue;
            }
            if self.demand[v] <= 0 {
                continue;
            } // skip deliveries and depot
            let rj = unsafe { *self.pickup_to_req.get_unchecked(v) };
            if rj == usize::MAX {
                continue;
            }
            let word = unsafe { *conflict_row.get_unchecked(rj >> 6) };
            if (word >> (rj & 63)) & 1 != 0 {
                return true;
            }
        }
        false
    }

    /// Build a bit-packed request mask for a route: bit `r` is set if request `r` is present.
    /// The mask has `conflict_stride` u64 words.
    #[inline]
    pub fn build_route_req_mask(&self, route: &[usize], buf: &mut Vec<u64>) {
        let stride = self.conflict_stride;
        buf.clear();
        buf.resize(stride, 0u64);
        for &v in route {
            if v == 0 {
                continue;
            }
            let r = unsafe { *self.pickup_to_req.get_unchecked(v) };
            if r != usize::MAX {
                buf[r >> 6] |= 1u64 << (r & 63);
            }
        }
    }

    /// Check conflict using a pre-computed route request mask (bitwise AND).
    /// Returns true if pickup `p` conflicts with any request in the mask.
    #[inline(always)]
    pub fn has_conflict_with_mask(&self, mask: &[u64], p: usize) -> bool {
        let ri = self.pickup_to_req[p];
        if ri == usize::MAX {
            return false;
        }
        let stride = self.conflict_stride;
        let row_start = ri * stride;
        unsafe {
            let cp = self.conflict.as_ptr().add(row_start);
            let mp = mask.as_ptr();
            if stride == 8 {
                // Manually unrolled for stride=8 (padded to 512-bit alignment).
                // 8 independent ANDs + OR-tree: compiler auto-vectorizes to
                // AVX-512 vpandq + vpor or AVX2 vpand (256-bit) pairs.
                ((*cp.add(0) & *mp.add(0))
                    | (*cp.add(1) & *mp.add(1))
                    | (*cp.add(2) & *mp.add(2))
                    | (*cp.add(3) & *mp.add(3))
                    | (*cp.add(4) & *mp.add(4))
                    | (*cp.add(5) & *mp.add(5))
                    | (*cp.add(6) & *mp.add(6))
                    | (*cp.add(7) & *mp.add(7)))
                    != 0
            } else {
                for w in 0..stride {
                    if *cp.add(w) & *mp.add(w) != 0 {
                        return true;
                    }
                }
                false
            }
        }
    }

    /// Check conflict using a pre-computed route mask, but excluding one request.
    /// 2-level approach: fast full-mask check first (O(stride)), then derived
    /// check excluding `exclude_pickup`'s request only if needed (~6.5% of cases).
    #[inline(always)]
    pub fn has_conflict_with_mask_excluding(
        &self,
        mask: &[u64],
        p: usize,
        exclude_pickup: usize,
    ) -> bool {
        // Fast path: no conflict with full mask → definitely no conflict with subset
        if !self.has_conflict_with_mask(mask, p) {
            return false;
        }
        // Conflict with full mask. Check if excluding one request resolves it.
        let exclude_req = self.pickup_to_req[exclude_pickup];
        if exclude_req == usize::MAX {
            return true; // nothing to exclude, conflict is real
        }
        let ri = self.pickup_to_req[p];
        // ri can't be usize::MAX here because has_conflict_with_mask returned true
        let stride = self.conflict_stride;
        let row = ri * stride;
        let exc_w = exclude_req >> 6;
        let exc_bit = 1u64 << (exclude_req & 63);
        unsafe {
            let cp = self.conflict.as_ptr().add(row);
            let mp = mask.as_ptr();
            for w in 0..stride {
                let mask_val = *mp.add(w);
                let mask_mod = if w == exc_w {
                    mask_val & !exc_bit
                } else {
                    mask_val
                };
                if *cp.add(w) & mask_mod != 0 {
                    return true;
                }
            }
            false
        }
    }

    /// Count conflicts between pickup `p` and requests in a pre-computed route mask.
    /// Returns the count, early-exiting at `limit` (caller typically passes 3).
    #[inline(always)]
    pub fn count_conflicts_with_mask(&self, mask: &[u64], p: usize, limit: usize) -> usize {
        let ri = self.pickup_to_req[p];
        if ri == usize::MAX {
            return 0;
        }
        let stride = self.conflict_stride;
        let row_start = ri * stride;
        unsafe {
            let cp = self.conflict.as_ptr().add(row_start);
            let mp = mask.as_ptr();
            if stride == 8 {
                // Branchless unrolled for stride=8: enables AVX-512 vpopcntq
                // vectorization (all 8 AND + popcnt operations independent).
                let count = (*cp.add(0) & *mp.add(0)).count_ones()
                    + (*cp.add(1) & *mp.add(1)).count_ones()
                    + (*cp.add(2) & *mp.add(2)).count_ones()
                    + (*cp.add(3) & *mp.add(3)).count_ones()
                    + (*cp.add(4) & *mp.add(4)).count_ones()
                    + (*cp.add(5) & *mp.add(5)).count_ones()
                    + (*cp.add(6) & *mp.add(6)).count_ones()
                    + (*cp.add(7) & *mp.add(7)).count_ones();
                count as usize
            } else {
                let mut count = 0usize;
                for w in 0..stride {
                    let bits = *cp.add(w) & *mp.add(w);
                    count += bits.count_ones() as usize;
                    if count >= limit {
                        return count;
                    }
                }
                count
            }
        }
    }
}

/// Test bit `idx` in a precomputed arc row pointer.
#[allow(clippy::not_unsafe_ptr_arg_deref)]
#[inline(always)]
pub fn arc_bit(row: *const u64, idx: usize) -> bool {
    unsafe {
        let word = *row.add(idx >> 6);
        (word >> (idx & 63)) & 1 != 0
    }
}

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

    /// Create a test instance with `m` PD pairs.
    /// Nodes: 0 = depot, 1..m = pickups, m+1..2m = deliveries.
    /// Pair i: pickup = i, delivery = m + i.
    pub fn make_test_instance(
        m: usize,
        capacity: i32,
        dist_flat: &[f64],
        demand: &[i32],
        early: &[f64],
        late: &[f64],
        service: &[f64],
    ) -> Instance {
        let n = 2 * m;
        let total = n + 1;
        assert_eq!(dist_flat.len(), total * total);
        assert_eq!(demand.len(), total);
        assert_eq!(early.len(), total);
        assert_eq!(late.len(), total);
        assert_eq!(service.len(), total);

        let mut pair_delivery = vec![0usize; total];
        let mut pair_pickup = vec![0usize; total];
        let mut pickups = Vec::new();
        for i in 1..=m {
            pair_delivery[i] = m + i;
            pair_pickup[m + i] = i;
            pickups.push(i);
        }

        // Build all-feasible arc matrix (except self-loops)
        let arc_stride_words = total.div_ceil(64);
        let mut arc_bools = vec![false; total * total];
        for i in 0..total {
            for j in 0..total {
                if i != j {
                    arc_bools[i * total + j] = true;
                }
            }
        }
        let arc_feasible = Instance::pack_bool_matrix(&arc_bools, total, arc_stride_words);
        let arc_feasible_t = arc_feasible.clone();
        let arc_sparse = arc_feasible.clone();
        let arc_sparse_t = arc_sparse.clone();

        let mut sparse_out = vec![Vec::new(); total];
        let mut sparse_in = vec![Vec::new(); total];
        for i in 0..total {
            for j in 0..total {
                if arc_bools[i * total + j] {
                    sparse_out[i].push(j);
                    sparse_in[j].push(i);
                }
            }
        }

        // No conflict checking in test instances (all-feasible arcs)
        let conflict_stride = (m.div_ceil(64) + 7) & !7;
        let mut pickup_to_req = vec![usize::MAX; total];
        for (idx, &p) in pickups.iter().enumerate() {
            pickup_to_req[p] = idx;
        }

        Instance {
            n,
            m,
            num_vehicles: m,
            capacity,
            demand: demand.to_vec(),
            tw: (0..total)
                .map(|i| NodeTW {
                    early: early[i],
                    late: late[i],
                    service: service[i],
                })
                .collect(),
            tw_early: early.to_vec(),
            tw_late: late.to_vec(),
            tw_svc: service.to_vec(),
            pair_delivery,
            pair_pickup,
            pickups,
            dist_flat: dist_flat.to_vec(),
            dist_stride: total,
            arc_feasible,
            arc_feasible_t,
            arc_stride_words,
            num_feasible_arcs: total * (total - 1),
            arc_sparse,
            arc_sparse_t,
            sparse_out,
            sparse_in,
            reduced_costs: vec![0.0; total * total],
            coords: None,
            polar_angle: None,
            conflict: vec![0u64; m * conflict_stride],
            conflict_stride,
            pickup_to_req,
        }
    }
}