pdp_lns 0.1.1

use crate::instance::Instance;
use crate::solution::{
    RouteInfo, Solution, best_pair_insertion_with_info, build_route_with_pair, fmax,
    is_route_capacity_tw_feasible, is_route_feasible, single_pair_feasible_distance,
};
use rand::SeedableRng;
use rand::prelude::SliceRandom;
use rand::rngs::SmallRng;

const EPS: f64 = 1e-10;
const CLP_LEVELS: &[(f64, f64)] = &[(0.05, 0.40), (0.15, 0.60), (0.50, 0.80), (1.0, 1.0)];
// Hosny (2012) PBQ objective weights (scaled to prioritize vehicle count first).
const HOSNY_OBJ_A: f64 = 1_000_000.0;
const HOSNY_OBJ_B: f64 = 1.0;
const HOSNY_OBJ_C: f64 = 0.001;
// HC route scoring penalties (TW should dominate as suggested by paper).
const HOSNY_TW_PENALTY: f64 = 10_000.0;
const HOSNY_CAP_PENALTY: f64 = 5_000.0;
const HOSNY_PREC_PENALTY: f64 = 10_000.0;

#[derive(Clone)]
struct RouteCandidate {
    cost: f64,
    clp: f64,
    route: Vec<usize>,
}

#[derive(Clone)]
struct RouteWY {
    start: Vec<f64>,
    wait: Vec<f64>,
    y: Vec<f64>,
}

impl RouteWY {
    fn compute(inst: &Instance, route: &[usize]) -> Self {
        let n = route.len();
        let mut start = vec![0.0; n];
        let mut wait = vec![0.0; n];
        let mut y = vec![0.0; n];

        start[0] = inst.early(route[0]);
        wait[0] = 0.0;

        for i in 1..n {
            let prev = route[i - 1];
            let curr = route[i];
            let raw_arrival = start[i - 1] + inst.svc(prev) + inst.dist(prev, curr);
            start[i] = fmax(raw_arrival, inst.early(curr));
            wait[i] = fmax(0.0, inst.early(curr) - raw_arrival);
        }

        y[n - 1] = inst.late(route[n - 1]) - start[n - 1];
        for i in (0..n - 1).rev() {
            let slack = inst.late(route[i]) - start[i];
            y[i] = slack.min(y[i + 1] + wait[i + 1]);
        }

        RouteWY { start, wait, y }
    }
}

fn backward_slack_cost(cache: &RouteWY, pos: usize, mut b: f64) -> f64 {
    let mut c1: f64 = 0.0;
    let mut k = pos;

    loop {
        if b >= cache.y[k] - EPS || k == 0 {
            return c1.max(0.0);
        }

        if k == pos {
            c1 = cache.y[k] - b;
        } else if cache.wait[k + 1] > EPS {
            c1 += (cache.y[k] - b).min(cache.wait[k + 1]);
        }

        b += cache.wait[k];
        if k == 0 {
            return c1.max(0.0);
        }
        k -= 1;
    }
}

fn single_location_cost(
    inst: &Instance,
    route: &[usize],
    cache: &RouteWY,
    edge_pos: usize,
    node: usize,
) -> f64 {
    let next_idx = edge_pos + 1;
    let prev = route[edge_pos];
    let next = route[next_idx];

    let a0 = cache.start[edge_pos] + inst.svc(prev) + inst.dist(prev, node);
    let s0 = fmax(a0, inst.early(node));
    let w0 = fmax(0.0, inst.early(node) - a0);

    let c3 = inst.dist(prev, node) + inst.dist(node, next) - inst.dist(prev, next);
    let d = c3 + inst.svc(node) + w0;
    let y0 = (inst.late(node) - s0).min(cache.wait[next_idx] + cache.y[next_idx] - d);

    let c1 = backward_slack_cost(cache, edge_pos, w0 + y0);
    let c2 = inst.late(node) - s0 - y0;

    c1 + c2 + c3
}

fn seg_cross(a: (f64, f64), b: (f64, f64), c: (f64, f64), d: (f64, f64)) -> Option<f64> {
    let r = (b.0 - a.0, b.1 - a.1);
    let s = (d.0 - c.0, d.1 - c.1);
    let denom = r.0 * s.1 - r.1 * s.0;
    if denom.abs() <= EPS {
        return None;
    }

    let qmp = (c.0 - a.0, c.1 - a.1);
    let t = (qmp.0 * s.1 - qmp.1 * s.0) / denom;
    let u = (qmp.0 * r.1 - qmp.1 * r.0) / denom;

    if t > EPS && t < 1.0 - EPS && u > EPS && u < 1.0 - EPS {
        Some(t)
    } else {
        None
    }
}

fn route_clp(inst: &Instance, route: &[usize]) -> f64 {
    if inst.coords.is_none() || route.len() < 5 {
        return 0.0;
    }

    let mut prefix = Vec::with_capacity(route.len() - 1);
    let mut edge_len = Vec::with_capacity(route.len() - 1);
    let mut total = 0.0;
    for i in 0..route.len() - 1 {
        prefix.push(total);
        let len = inst.dist(route[i], route[i + 1]);
        edge_len.push(len);
        total += len;
    }

    if total <= EPS {
        return 0.0;
    }

    let mut sum = 0.0;
    let segs = route.len() - 1;
    for i in 0..segs {
        let Some(a) = inst.coord(route[i]) else {
            return 0.0;
        };
        let Some(b) = inst.coord(route[i + 1]) else {
            return 0.0;
        };
        for j in i + 2..segs {
            if i == 0 && j == segs - 1 {
                continue;
            }

            let Some(c) = inst.coord(route[j]) else {
                return 0.0;
            };
            let Some(d) = inst.coord(route[j + 1]) else {
                return 0.0;
            };

            if let Some(t) = seg_cross(a, b, c, d) {
                let bdist = prefix[i] + t * edge_len[i];
                sum += bdist.min(total - bdist);
            }
        }
    }

    sum / total
}

fn can_two_customers_share_route(inst: &Instance, p1: usize, p2: usize) -> bool {
    let d1 = inst.delivery_of(p1);
    let d2 = inst.delivery_of(p2);

    let orders = [
        [p1, d1, p2, d2],
        [p1, p2, d1, d2],
        [p1, p2, d2, d1],
        [p2, d2, p1, d1],
        [p2, p1, d2, d1],
        [p2, p1, d1, d2],
    ];

    orders.iter().any(|ord| {
        let route = [0, ord[0], ord[1], ord[2], ord[3], 0];
        is_route_feasible(inst, &route)
    })
}

fn greedy_clique_from(start: usize, incompatible: &[Vec<bool>]) -> Vec<usize> {
    let n = incompatible.len();
    let mut clique = vec![start];
    let mut candidates: Vec<usize> = (0..n)
        .filter(|&v| v != start && incompatible[start][v])
        .collect();

    while !candidates.is_empty() {
        let mut best = candidates[0];
        let mut best_deg = 0usize;

        for &v in &candidates {
            let deg = candidates
                .iter()
                .filter(|&&u| u != v && incompatible[v][u])
                .count();
            if deg > best_deg {
                best = v;
                best_deg = deg;
            }
        }

        clique.push(best);
        candidates.retain(|&u| u != best && incompatible[best][u]);
    }

    clique
}

fn largest_exclusive_customer_set(inst: &Instance) -> Vec<usize> {
    let pickups = &inst.pickups;
    let n = pickups.len();
    if n == 0 {
        return Vec::new();
    }

    let mut incompatible = vec![vec![false; n]; n];
    for i in 0..n {
        for j in i + 1..n {
            let conflict = !can_two_customers_share_route(inst, pickups[i], pickups[j]);
            incompatible[i][j] = conflict;
            incompatible[j][i] = conflict;
        }
    }

    let mut best = Vec::new();
    for s in 0..n {
        let clique = greedy_clique_from(s, &incompatible);
        if clique.len() > best.len() {
            best = clique;
        }
    }

    best.into_iter().map(|idx| pickups[idx]).collect()
}

fn best_customer_insertion(
    inst: &Instance,
    route: &[usize],
    pickup: usize,
) -> Option<RouteCandidate> {
    let delivery = inst.delivery_of(pickup);
    let base_cache = RouteWY::compute(inst, route);
    let mut best_cost = f64::MAX;
    let mut best_pickup_pos: usize = 0;
    let mut best_delivery_pos: usize = 0;

    // Reusable buffer avoids heap allocation per position.
    let mut buf = Vec::with_capacity(route.len() + 2);

    for ep in 0..route.len() - 1 {
        let c_pick = single_location_cost(inst, route, &base_cache, ep, pickup);

        // Build intermediate route with pickup inserted at ep+1 (reuse buf).
        buf.clear();
        buf.extend_from_slice(&route[..=ep]);
        buf.push(pickup);
        buf.extend_from_slice(&route[ep + 1..]);

        let pickup_cache = RouteWY::compute(inst, &buf);

        for ed in ep + 1..buf.len() - 1 {
            let c_del = single_location_cost(inst, &buf, &pickup_cache, ed, delivery);
            let cost = c_pick + c_del;

            // Early skip: can't beat current best cost.
            if cost >= best_cost - EPS {
                continue;
            }

            // In-place feasibility check (no heap allocation).
            // Precedence is guaranteed by construction (pickup before delivery).
            buf.insert(ed + 1, delivery);
            let feasible = is_route_capacity_tw_feasible(inst, &buf);
            buf.remove(ed + 1);

            if feasible {
                best_cost = cost;
                best_pickup_pos = ep;
                best_delivery_pos = ed + 1;
            }
        }
    }

    if best_cost >= f64::MAX {
        return None;
    }

    // Reconstruct the winning route once and compute CLP only once.
    buf.clear();
    buf.extend_from_slice(&route[..=best_pickup_pos]);
    buf.push(pickup);
    buf.extend_from_slice(&route[best_pickup_pos + 1..]);
    buf.insert(best_delivery_pos, delivery);

    Some(RouteCandidate {
        cost: best_cost,
        clp: route_clp(inst, &buf),
        route: buf,
    })
}

fn remove_pickup(unassigned: &mut Vec<usize>, pickup: usize) {
    if let Some(pos) = unassigned.iter().position(|&p| p == pickup) {
        unassigned.remove(pos);
    }
}

fn level_for_assigned_ratio(ratio: f64) -> usize {
    CLP_LEVELS
        .iter()
        .position(|&(_, q)| ratio < q - EPS)
        .unwrap_or(CLP_LEVELS.len() - 1)
}

#[inline]
fn seed_round_trip(inst: &Instance, pickup: usize) -> f64 {
    let delivery = inst.delivery_of(pickup);
    inst.dist(0, pickup) + inst.dist(pickup, delivery) + inst.dist(delivery, 0)
}

fn select_sequential_seed(inst: &Instance, remaining: &[usize]) -> Option<usize> {
    let mut best_idx: Option<usize> = None;
    let mut best_delivery_late = f64::MAX;
    let mut best_pickup_late = f64::MAX;
    let mut best_round_trip = -1.0;
    let mut best_pickup = usize::MAX;

    for (idx, &pickup) in remaining.iter().enumerate() {
        let delivery = inst.delivery_of(pickup);
        if single_pair_feasible_distance(inst, pickup, delivery).is_none() {
            continue;
        }

        let delivery_late = inst.late(delivery);
        let pickup_late = inst.late(pickup);
        let round_trip = seed_round_trip(inst, pickup);

        let better = delivery_late < best_delivery_late - EPS
            || ((delivery_late - best_delivery_late).abs() <= EPS
                && (pickup_late < best_pickup_late - EPS
                    || ((pickup_late - best_pickup_late).abs() <= EPS
                        && (round_trip > best_round_trip + EPS
                            || ((round_trip - best_round_trip).abs() <= EPS
                                && pickup < best_pickup)))));

        if better {
            best_idx = Some(idx);
            best_delivery_late = delivery_late;
            best_pickup_late = pickup_late;
            best_round_trip = round_trip;
            best_pickup = pickup;
        }
    }

    best_idx
}

/// Sequential insertion heuristic from Ropke & Pisinger (2006, Section 3.7):
/// build one route at a time until all requests are planned.
pub fn construct_initial_solution_ropke(inst: &Instance) -> Solution {
    let mut remaining = inst.pickups.clone();
    let mut routes: Vec<Vec<usize>> = Vec::new();
    let mut unassigned: Vec<usize> = Vec::new();
    let mut info = RouteInfo::new();

    while !remaining.is_empty() {
        let Some(seed_idx) = select_sequential_seed(inst, &remaining) else {
            unassigned.append(&mut remaining);
            break;
        };

        let seed_pickup = remaining.swap_remove(seed_idx);
        let seed_delivery = inst.delivery_of(seed_pickup);
        let mut route = vec![0, seed_pickup, seed_delivery, 0];
        info.compute(inst, &route);

        loop {
            let mut best_idx: Option<usize> = None;
            let mut best_delta = f64::MAX;
            let mut best_pickup = usize::MAX;
            let mut best_ep = 0usize;
            let mut best_ed = 0usize;

            for (idx, &pickup) in remaining.iter().enumerate() {
                let delivery = inst.delivery_of(pickup);
                if inst.has_route_conflict(&route, pickup) {
                    continue;
                }
                let Some((new_dist, ep, ed)) =
                    best_pair_insertion_with_info::<false>(inst, &route, &info, pickup, delivery)
                else {
                    continue;
                };

                let delta = new_dist - info.distance;
                let better = if best_idx.is_none() || delta < best_delta - EPS {
                    true
                } else if (delta - best_delta).abs() <= EPS {
                    let delivery_late = inst.late(delivery);
                    let best_delivery_late = if best_pickup == usize::MAX {
                        f64::MAX
                    } else {
                        inst.late(inst.delivery_of(best_pickup))
                    };
                    if delivery_late < best_delivery_late - EPS {
                        true
                    } else if (delivery_late - best_delivery_late).abs() <= EPS {
                        pickup < best_pickup
                    } else {
                        false
                    }
                } else {
                    false
                };

                if better {
                    best_idx = Some(idx);
                    best_delta = delta;
                    best_pickup = pickup;
                    best_ep = ep;
                    best_ed = ed;
                }
            }

            let Some(idx) = best_idx else {
                break;
            };

            let pickup = remaining.swap_remove(idx);
            let delivery = inst.delivery_of(pickup);
            route = build_route_with_pair(&route, pickup, best_ep, delivery, best_ed);
            info.compute(inst, &route);
        }

        routes.push(route);
    }

    Solution { routes, unassigned }
}

/// Cluster-paper embedded insertion constructor (Pankratz GGA, Section 3.4):
/// process requests in random order; for each request choose the feasible insertion
/// with minimum additional cost across all existing routes and one tentative new route.
pub fn construct_initial_solution_cluster(inst: &Instance, seed: u64) -> Solution {
    let mut rng = SmallRng::seed_from_u64(seed);
    let mut requests = inst.pickups.clone();
    requests.shuffle(&mut rng);

    // Start with one empty vehicle route as described in the paper.
    let mut routes: Vec<Vec<usize>> = vec![vec![0, 0]];
    let mut infos: Vec<RouteInfo> = vec![RouteInfo::new()];
    infos[0].compute(inst, &routes[0]);

    let mut unassigned: Vec<usize> = Vec::new();

    for pickup in requests {
        let delivery = inst.delivery_of(pickup);

        // Best insertion into existing routes.
        let mut best_existing: Option<(usize, usize, usize, f64)> = None; // (ri, ep, ed, delta)
        for ri in 0..routes.len() {
            if inst.has_route_conflict(&routes[ri], pickup) {
                continue;
            }
            let Some((new_dist, ep, ed)) = best_pair_insertion_with_info::<false>(
                inst,
                &routes[ri],
                &infos[ri],
                pickup,
                delivery,
            ) else {
                continue;
            };

            let delta = new_dist - infos[ri].distance;
            let better = if let Some((best_ri, _, _, best_delta)) = best_existing {
                delta < best_delta - EPS || ((delta - best_delta).abs() <= EPS && ri < best_ri)
            } else {
                true
            };

            if better {
                best_existing = Some((ri, ep, ed, delta));
            }
        }

        // Tentative insertion by allocating a new vehicle.
        let new_vehicle_delta = single_pair_feasible_distance(inst, pickup, delivery);

        match (best_existing, new_vehicle_delta) {
            (Some((ri, ep, ed, ex_delta)), Some(new_delta)) => {
                if ex_delta <= new_delta + EPS {
                    routes[ri] = build_route_with_pair(&routes[ri], pickup, ep, delivery, ed);
                    infos[ri].compute(inst, &routes[ri]);
                } else {
                    let route = vec![0, pickup, delivery, 0];
                    let mut info = RouteInfo::new();
                    info.compute(inst, &route);
                    routes.push(route);
                    infos.push(info);
                }
            }
            (Some((ri, ep, ed, _)), None) => {
                routes[ri] = build_route_with_pair(&routes[ri], pickup, ep, delivery, ed);
                infos[ri].compute(inst, &routes[ri]);
            }
            (None, Some(_)) => {
                let route = vec![0, pickup, delivery, 0];
                let mut info = RouteInfo::new();
                info.compute(inst, &route);
                routes.push(route);
                infos.push(info);
            }
            (None, None) => {
                unassigned.push(pickup);
            }
        }
    }

    routes.retain(|r| r.len() > 2);
    Solution { routes, unassigned }
}

#[inline]
fn hosny_route_distance(inst: &Instance, route: &[usize]) -> f64 {
    route.windows(2).map(|w| inst.dist(w[0], w[1])).sum()
}

#[inline]
fn hosny_route_nodes(route: &[usize]) -> i64 {
    route.len().saturating_sub(2) as i64
}

fn hosny_solution_dcost_replace(
    inst: &Instance,
    routes: &[Vec<usize>],
    route_idx: usize,
    new_route: &[usize],
) -> f64 {
    let old_route = &routes[route_idx];
    let old_active = f64::from(old_route.len() > 2);
    let new_active = f64::from(new_route.len() > 2);
    let delta_vehicles = new_active - old_active;

    let delta_dist = hosny_route_distance(inst, new_route) - hosny_route_distance(inst, old_route);

    let old_nodes = hosny_route_nodes(old_route) as f64;
    let new_nodes = hosny_route_nodes(new_route) as f64;
    let delta_nodes_sq = new_nodes * new_nodes - old_nodes * old_nodes;

    HOSNY_OBJ_A * delta_vehicles + HOSNY_OBJ_B * delta_dist - HOSNY_OBJ_C * delta_nodes_sq
}

fn hosny_route_metrics(
    inst: &Instance,
    route: &[usize],
    state: &mut [u8],
) -> (f64, usize, usize, usize) {
    let mut tw_viol = 0usize;
    let mut cap_viol = 0usize;
    let mut prec_viol = 0usize;

    let mut time = inst.early(0);
    let mut load = 0i32;

    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) + EPS {
            tw_viol += 1;
        }

        if curr != 0 {
            load += inst.demand[curr];
            if load > inst.capacity || load < 0 {
                cap_viol += 1;
            }

            if inst.is_pickup(curr) {
                if state[curr] != 0 {
                    prec_viol += 1;
                } else {
                    state[curr] = 1;
                }
            } else {
                let p = inst.pickup_of(curr);
                if p == 0 || state[p] != 1 {
                    prec_viol += 1;
                } else {
                    state[p] = 2;
                }
            }
        }
    }

    for &p in &inst.pickups {
        if state[p] == 1 {
            prec_viol += 1;
        }
    }

    // Sparse clear: reset only nodes touched by this route
    for &node in &route[1..route.len() - 1] {
        if node != 0 {
            if inst.is_pickup(node) {
                state[node] = 0;
            } else {
                state[inst.pickup_of(node)] = 0;
            }
        }
    }

    let duration = (time - inst.early(0)).max(0.0);
    (duration, tw_viol, cap_viol, prec_viol)
}

fn hosny_route_cost(inst: &Instance, route: &[usize], state: &mut [u8]) -> f64 {
    let (duration, twv, cv, pv) = hosny_route_metrics(inst, route, state);
    duration
        + HOSNY_TW_PENALTY * twv as f64
        + HOSNY_CAP_PENALTY * cv as f64
        + HOSNY_PREC_PENALTY * pv as f64
}

/// Like `hosny_route_cost` but returns early (with a value >= `cost_limit`)
/// when the running cost lower bound already exceeds `cost_limit`.
/// This avoids evaluating the rest of the route for clearly uncompetitive swaps.
fn hosny_route_cost_bounded(
    inst: &Instance,
    route: &[usize],
    state: &mut [u8],
    cost_limit: f64,
) -> f64 {
    let mut tw_viol = 0usize;
    let mut cap_viol = 0usize;
    let mut prec_viol = 0usize;

    let early0 = inst.early(0);
    let mut time = early0;
    let mut load = 0i32;
    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) + EPS {
            tw_viol += 1;
        }

        if curr != 0 {
            load += inst.demand[curr];
            if load > inst.capacity || load < 0 {
                cap_viol += 1;
            }

            if inst.is_pickup(curr) {
                if state[curr] != 0 {
                    prec_viol += 1;
                } else {
                    state[curr] = 1;
                }
            } else {
                let p = inst.pickup_of(curr);
                if p == 0 || state[p] != 1 {
                    prec_viol += 1;
                } else {
                    state[p] = 2;
                }
            }
        }

        // Early termination: running lower bound on cost exceeds limit.
        // Duration is non-decreasing (time - early0), violations only accumulate.
        // Use exact penalties (not min_penalty) for a tighter bound.
        let running_lb = (time - early0)
            + HOSNY_TW_PENALTY * tw_viol as f64
            + HOSNY_CAP_PENALTY * cap_viol as f64
            + HOSNY_PREC_PENALTY * prec_viol as f64;
        if running_lb >= cost_limit {
            // Clean up state before returning
            for &node in &route[1..route.len() - 1] {
                if node != 0 {
                    if inst.is_pickup(node) {
                        state[node] = 0;
                    } else {
                        state[inst.pickup_of(node)] = 0;
                    }
                }
            }
            return running_lb;
        }
    }

    for &p in &inst.pickups {
        if state[p] == 1 {
            prec_viol += 1;
        }
    }

    // Sparse clear: reset only nodes touched by this route
    for &node in &route[1..route.len() - 1] {
        if node != 0 {
            if inst.is_pickup(node) {
                state[node] = 0;
            } else {
                state[inst.pickup_of(node)] = 0;
            }
        }
    }

    let duration = (time - early0).max(0.0);
    duration
        + HOSNY_TW_PENALTY * tw_viol as f64
        + HOSNY_CAP_PENALTY * cap_viol as f64
        + HOSNY_PREC_PENALTY * prec_viol as f64
}

fn hosny_hc_improve_route(inst: &Instance, route: &mut [usize], state: &mut [u8]) {
    if route.len() <= 3 {
        return;
    }

    let mut improved = true;
    let mut current_cost = hosny_route_cost(inst, route, state);

    while improved {
        improved = false;

        for i in 1..route.len() - 1 {
            for j in i + 1..route.len() - 1 {
                // Only consider swaps where the latter location has tighter deadline.
                if inst.late(route[j]) + EPS >= inst.late(route[i]) {
                    continue;
                }

                route.swap(i, j);
                let new_cost = hosny_route_cost_bounded(inst, route, state, current_cost - EPS);
                if new_cost < current_cost - EPS {
                    current_cost = new_cost;
                    improved = true;
                } else {
                    route.swap(i, j);
                }
            }
        }
    }
}

/// Hosny (2012): Parallel construction - best request (PBQ),
/// reported as best-quality constructor in travel distance.
pub fn construct_initial_solution_hosny2012(inst: &Instance) -> Solution {
    if inst.pickups.is_empty() {
        return Solution {
            routes: Vec::new(),
            unassigned: Vec::new(),
        };
    }

    let mut remaining = inst.pickups.clone();
    // Farthest first by depot-to-delivery distance.
    remaining.sort_by(|&a, &b| {
        let da = inst.dist(0, inst.delivery_of(a));
        let db = inst.dist(0, inst.delivery_of(b));
        db.partial_cmp(&da).unwrap().then_with(|| a.cmp(&b))
    });

    let total_pickup_demand: i64 = inst
        .pickups
        .iter()
        .map(|&p| i64::from(inst.demand[p].max(0)))
        .sum();
    let cap = i64::from(inst.capacity.max(1));
    let mut m_est = ((total_pickup_demand + cap - 1) / cap) as usize;
    m_est = m_est.max(1).min(remaining.len().max(1));

    let mut routes: Vec<Vec<usize>> = Vec::new();
    let mut unassigned: Vec<usize> = Vec::new();
    let mut state = vec![0u8; inst.n + 1];

    // Initialize with seed requests.
    for _ in 0..m_est {
        if remaining.is_empty() {
            break;
        }

        let pickup = remaining.remove(0);
        let delivery = inst.delivery_of(pickup);
        if single_pair_feasible_distance(inst, pickup, delivery).is_some() {
            routes.push(vec![0, pickup, delivery, 0]);
        } else {
            unassigned.push(pickup);
        }
    }
    if routes.is_empty() {
        routes.push(vec![0, 0]);
    }

    let mut trial = Vec::with_capacity(64);
    while !remaining.is_empty() {
        let mut global_min = f64::MAX;
        let mut global_best_pickup = usize::MAX;
        let mut global_best_route_idx = 0usize;
        let mut global_best_route: Option<Vec<usize>> = None;

        // Requests that cannot be inserted in any existing route are placed
        // immediately in new routes (as in Algorithm 5).
        let mut idx = 0usize;
        while idx < remaining.len() {
            let pickup = remaining[idx];
            let delivery = inst.delivery_of(pickup);

            let mut local_min = f64::MAX;
            let mut local_best_route_idx = 0usize;
            let mut local_best_route: Option<Vec<usize>> = None;

            for route_idx in 0..routes.len() {
                trial.clear();
                trial.extend_from_slice(&routes[route_idx]);
                let insert_pos = trial.len() - 1;
                trial.insert(insert_pos, pickup);
                trial.insert(insert_pos + 1, delivery);
                hosny_hc_improve_route(inst, &mut trial, &mut state);

                if !is_route_feasible(inst, &trial) {
                    continue;
                }

                let dcost = hosny_solution_dcost_replace(inst, &routes, route_idx, &trial);
                if dcost < local_min - EPS {
                    local_min = dcost;
                    local_best_route_idx = route_idx;
                    local_best_route = Some(trial.clone());
                }
            }

            if let Some(route) = local_best_route {
                if local_min < global_min - EPS {
                    global_min = local_min;
                    global_best_pickup = pickup;
                    global_best_route_idx = local_best_route_idx;
                    global_best_route = Some(route);
                }
                idx += 1;
            } else {
                // No feasible insertion found in existing routes: allocate new route.
                if single_pair_feasible_distance(inst, pickup, delivery).is_some() {
                    routes.push(vec![0, pickup, delivery, 0]);
                } else {
                    unassigned.push(pickup);
                }
                remaining.remove(idx);
            }
        }

        if let Some(best_route) = global_best_route {
            if let Some(pos) = remaining.iter().position(|&p| p == global_best_pickup) {
                routes[global_best_route_idx] = best_route;
                remaining.remove(pos);
            }
        } else if !remaining.is_empty() {
            // Safety fallback to avoid stalling.
            let pickup = remaining.remove(0);
            let delivery = inst.delivery_of(pickup);
            if single_pair_feasible_distance(inst, pickup, delivery).is_some() {
                routes.push(vec![0, pickup, delivery, 0]);
            } else {
                unassigned.push(pickup);
            }
        }
    }

    routes.retain(|r| r.len() > 2);
    // Keep deterministic ordering by descending route load (fills before spread).
    let sort_costs: Vec<f64> = routes
        .iter()
        .map(|r| hosny_route_cost(inst, r, &mut state))
        .collect();
    let mut sort_indices: Vec<usize> = (0..routes.len()).collect();
    sort_indices.sort_by(|&a, &b| sort_costs[a].partial_cmp(&sort_costs[b]).unwrap());
    let sorted_routes: Vec<Vec<usize>> = sort_indices
        .into_iter()
        .map(|i| std::mem::take(&mut routes[i]))
        .collect();
    routes = sorted_routes;

    Solution { routes, unassigned }
}

/// Lu & Dessouky (2006) insertion-based construction heuristic.
pub fn construct_initial_solution(inst: &Instance) -> Solution {
    let exclusive = largest_exclusive_customer_set(inst);
    let mut seeded = vec![false; inst.n + 1];
    let mut routes: Vec<Vec<usize>> = Vec::new();

    for p in exclusive {
        let d = inst.delivery_of(p);
        if single_pair_feasible_distance(inst, p, d).is_some() {
            routes.push(vec![0, p, d, 0]);
            seeded[p] = true;
        }
    }

    let mut unassigned: Vec<usize> = inst
        .pickups
        .iter()
        .copied()
        .filter(|&p| !seeded[p])
        .collect();

    // Pre-compute all candidates once; then update incrementally.
    let mut candidates: Vec<(usize, usize, RouteCandidate)> = Vec::new();
    for &pickup in &unassigned {
        for (ri, route) in routes.iter().enumerate() {
            if let Some(cand) = best_customer_insertion(inst, route, pickup) {
                candidates.push((pickup, ri, cand));
            }
        }
    }

    while !unassigned.is_empty() {
        let assigned_ratio = if inst.m == 0 {
            1.0
        } else {
            1.0 - unassigned.len() as f64 / inst.m as f64
        };

        let mut level = level_for_assigned_ratio(assigned_ratio);
        let mut chosen: Option<(usize, usize, RouteCandidate)> = None;
        while level < CLP_LEVELS.len() {
            let clp_limit = CLP_LEVELS[level].0 + EPS;
            // Find index of min-cost candidate passing CLP threshold.
            let mut best_idx: Option<usize> = None;
            let mut best_cost = f64::MAX;
            for (idx, (_, _, cand)) in candidates.iter().enumerate() {
                if cand.clp <= clp_limit && cand.cost < best_cost - EPS {
                    best_cost = cand.cost;
                    best_idx = Some(idx);
                }
            }
            if let Some(idx) = best_idx {
                let (pickup, ri, cand) = candidates.swap_remove(idx);
                chosen = Some((pickup, ri, cand));
                break;
            }
            level += 1;
        }

        if let Some((pickup, ri, cand)) = chosen {
            routes[ri] = cand.route;
            remove_pickup(&mut unassigned, pickup);

            // Invalidate stale candidates: remove entries for the assigned
            // pickup or the modified route.
            candidates.retain(|(p, r, _)| *p != pickup && *r != ri);

            // Recompute candidates for remaining pickups into the modified route.
            for &p in &unassigned {
                if let Some(c) = best_customer_insertion(inst, &routes[ri], p) {
                    candidates.push((p, ri, c));
                }
            }
            continue;
        }

        let mut best_single: Option<(usize, f64)> = None;
        for &pickup in &unassigned {
            let d = inst.delivery_of(pickup);
            if let Some(dist) = single_pair_feasible_distance(inst, pickup, d)
                && best_single.is_none_or(|(_, bd)| dist < bd - EPS)
            {
                best_single = Some((pickup, dist));
            }
        }

        if let Some((pickup, _)) = best_single {
            let d = inst.delivery_of(pickup);
            let new_ri = routes.len();
            routes.push(vec![0, pickup, d, 0]);
            remove_pickup(&mut unassigned, pickup);

            // Remove stale candidates for the assigned pickup.
            candidates.retain(|(p, _, _)| *p != pickup);

            // Compute candidates for remaining pickups into the new route.
            for &p in &unassigned {
                if let Some(c) = best_customer_insertion(inst, &routes[new_ri], p) {
                    candidates.push((p, new_ri, c));
                }
            }
        } else {
            break;
        }
    }

    Solution { routes, unassigned }
}

#[cfg(test)]
mod tests {
    use super::{
        construct_initial_solution, construct_initial_solution_cluster,
        construct_initial_solution_hosny2012, construct_initial_solution_ropke,
    };
    use crate::instance::tests::make_test_instance;

    #[test]
    fn construction_returns_complete_solution_on_simple_instance() {
        let inst = make_test_instance(
            1,
            10,
            &[0.0, 1.0, 2.0, 1.0, 0.0, 1.0, 2.0, 1.0, 0.0],
            &[0, 4, -4],
            &[0.0, 0.0, 0.0],
            &[1000.0, 1000.0, 1000.0],
            &[0.0, 0.0, 0.0],
        );

        let sol = construct_initial_solution(&inst);
        assert!(sol.unassigned.is_empty());
        assert!(sol.is_feasible(&inst));
    }

    #[test]
    fn ropke_construction_returns_complete_solution_on_simple_instance() {
        let inst = make_test_instance(
            1,
            10,
            &[0.0, 1.0, 2.0, 1.0, 0.0, 1.0, 2.0, 1.0, 0.0],
            &[0, 4, -4],
            &[0.0, 0.0, 0.0],
            &[1000.0, 1000.0, 1000.0],
            &[0.0, 0.0, 0.0],
        );

        let sol = construct_initial_solution_ropke(&inst);
        assert!(sol.unassigned.is_empty());
        assert!(sol.is_feasible(&inst));
    }

    #[test]
    fn cluster_construction_returns_complete_solution_on_simple_instance() {
        let inst = make_test_instance(
            1,
            10,
            &[0.0, 1.0, 2.0, 1.0, 0.0, 1.0, 2.0, 1.0, 0.0],
            &[0, 4, -4],
            &[0.0, 0.0, 0.0],
            &[1000.0, 1000.0, 1000.0],
            &[0.0, 0.0, 0.0],
        );

        let sol = construct_initial_solution_cluster(&inst, 42);
        assert!(sol.unassigned.is_empty());
        assert!(sol.is_feasible(&inst));
    }

    #[test]
    fn hosny2012_construction_returns_complete_solution_on_simple_instance() {
        let inst = make_test_instance(
            1,
            10,
            &[0.0, 1.0, 2.0, 1.0, 0.0, 1.0, 2.0, 1.0, 0.0],
            &[0, 4, -4],
            &[0.0, 0.0, 0.0],
            &[1000.0, 1000.0, 1000.0],
            &[0.0, 0.0, 0.0],
        );

        let sol = construct_initial_solution_hosny2012(&inst);
        assert!(sol.unassigned.is_empty());
        assert!(sol.is_feasible(&inst));
    }
}