pdp_lns 0.1.1

use crate::instance::Instance;
use crate::solution::{RouteInfo, fmax};

/// Intra-route 2-Opt: reverse segment σ[i+1..j] if improving and feasible.
/// Best improvement per route, with per-route caching.
/// Uses early termination from Pacheco et al.: break inner loop when a complete
/// PD pair enters the reversed segment (all further j are infeasible).
pub fn apply_two_opt(inst: &Instance, routes: &mut [Vec<usize>], infos: &mut [RouteInfo]) -> bool {
    let nr = routes.len();
    let mut any_improved = false;
    // Hoist pos allocation outside route loop: avoids ~25 × 6.7KB alloc+memset
    // per call.  Use sparse set+clear instead of full memset each iteration.
    let mut pos = vec![usize::MAX; inst.n + 1];

    for ri in 0..nr {
        // Ensure RouteInfo is computed for this route (may be empty on first call)
        if infos[ri].arrival.len() != routes[ri].len() {
            infos[ri].compute(inst, &routes[ri]);
        }
        loop {
            let route = &routes[ri];
            let n = route.len();
            if n < 5 {
                break;
            }

            // Sparse write: set position of each node in this route
            for (idx, &v) in route.iter().enumerate() {
                if v != 0 && v <= inst.n {
                    pos[v] = idx;
                }
            }

            let mut best_delta = -1e-10;
            let mut best_i = 0;
            let mut best_j = 0;

            // Iterate over all 2-opt moves: reverse segment route[i+1..=j-1]
            // Edges removed: (route[i], route[i+1]) and (route[j-1], route[j])
            // Edges added:   (route[i], route[j-1]) and (route[i+1], route[j])
            for i in 0..n - 3 {
                for j in (i + 3)..n {
                    // Early termination: check if node entering at position j-1
                    // completes a PD pair inside the reversed segment [i+1..j-1]
                    let entering = route[j - 1];
                    // If entering is a pickup, its delivery is at a later position
                    // and hasn't entered yet, so no issue.
                    // If it's a delivery, check if its pickup is in [i+1..j-2]
                    if entering != 0 && entering <= inst.n && !inst.is_pickup(entering) {
                        let pickup = inst.pickup_of(entering);
                        if pickup != 0 {
                            let pp = pos[pickup];
                            if pp > i && pp <= j - 2 {
                                break; // all further j infeasible
                            }
                        }
                    }

                    // Distance delta
                    let delta = inst.dist(route[i], route[j - 1])
                        + inst.dist(route[i + 1], route[j])
                        - inst.dist(route[i], route[i + 1])
                        - inst.dist(route[j - 1], route[j]);

                    if delta < best_delta {
                        // Check full feasibility (TW + capacity + precedence)
                        if check_two_opt_feasible(inst, route, &infos[ri], i, j) {
                            best_delta = delta;
                            best_i = i;
                            best_j = j;
                        }
                    }
                }
            }

            // Sparse clear: restore pos entries before modifying route
            for &v in &routes[ri] {
                if v != 0 && v <= inst.n {
                    pos[v] = usize::MAX;
                }
            }

            if best_delta < -1e-10 {
                routes[ri][best_i + 1..best_j].reverse();
                infos[ri].compute(inst, &routes[ri]);
                any_improved = true;
            } else {
                break;
            }
        }
    }

    any_improved
}

/// Check if a 2-opt move reversing route[i+1..j-1] is feasible for PDPTW.
/// Uses precomputed RouteInfo to skip the unchanged prefix and leverage
/// suffix_min_slack for early termination (subroute feasibility caching).
fn check_two_opt_feasible(
    inst: &Instance,
    route: &[usize],
    info: &RouteInfo,
    i: usize,
    j: usize,
) -> bool {
    // Skip prefix [0..=i]: use precomputed arrival time and cumulative load.
    let mut time = info.arrival[i];
    let mut load = info.load[i];

    // Through reversed segment: route[j-1], route[j-2], ..., route[i+1]
    let mut prev_node = route[i];
    for k in (i + 1..j).rev() {
        let curr = route[k];
        time = fmax(
            time + inst.svc(prev_node) + inst.dist(prev_node, curr),
            inst.early(curr),
        );
        if time > inst.late(curr) {
            return false;
        }
        load += inst.demand[curr];
        if load > inst.capacity || load < 0 {
            return false;
        }
        prev_node = curr;
    }

    // Suffix [j..n]: capacity is provably identical (same nodes visited,
    // same total demand at segment boundary), so only TW needs checking.
    let n = route.len();
    if j < n {
        let curr = route[j];
        let new_arrival_j = fmax(
            time + inst.svc(prev_node) + inst.dist(prev_node, curr),
            inst.early(curr),
        );
        if new_arrival_j > inst.late(curr) {
            return false;
        }
        // If we arrive no later than the original, all suffix TW hold.
        if new_arrival_j <= info.arrival[j] + 1e-10 {
            return true;
        }
        // Conservative check: if the time increase fits within the minimum
        // slack across the entire suffix, all TW constraints still hold.
        let time_shift = new_arrival_j - info.arrival[j];
        if time_shift <= info.suffix_min_slack[j] + 1e-10 {
            return true;
        }
        // Must simulate the remaining suffix for TW.
        time = new_arrival_j;
        prev_node = curr;
        for &curr in &route[j + 1..] {
            time = fmax(
                time + inst.svc(prev_node) + inst.dist(prev_node, curr),
                inst.early(curr),
            );
            if time > inst.late(curr) {
                return false;
            }
            prev_node = curr;
        }
    }

    true
}

/// Route feasibility with reusable precedence state buffer.
/// Equivalent to `is_route_feasible`, but avoids allocating precedence arrays
/// on every call from hot neighborhoods like Or-Opt.
pub(crate) fn is_route_feasible_with_reuse(
    inst: &Instance,
    route: &[usize],
    state: &mut [u8],
    touched: &mut Vec<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;
        }
    }

    touched.clear();
    let mut ok = true;
    for &v in &route[1..route.len() - 1] {
        if inst.is_pickup(v) {
            if state[v] != 0 {
                ok = false;
                break;
            }
            state[v] = 1;
            touched.push(v);
        } else {
            let p = inst.pickup_of(v);
            if p == 0 || state[p] != 1 {
                ok = false;
                break;
            }
            state[p] = 2;
        }
    }
    if ok {
        for &p in touched.iter() {
            if state[p] == 1 {
                ok = false;
                break;
            }
        }
    }
    for &p in touched.iter() {
        state[p] = 0;
    }

    ok
}

/// Intra-route Or-Opt: relocate a segment of 1..K_OR consecutive visits to another
/// position in the same route (forward orientation only).
/// For PDPTW: checks precedence, time windows, and capacity after each move.
pub fn apply_or_opt(inst: &Instance, routes: &mut [Vec<usize>], infos: &mut [RouteInfo]) -> bool {
    const K_OR: usize = 4; // max segment length

    let nr = routes.len();
    let mut any_improved = false;
    let mut new_route_buf: Vec<usize> = Vec::new();
    let mut precedence_state: Vec<u8> = vec![0; inst.n + 1];
    let mut touched_pickups: Vec<usize> = Vec::new();

    for ri in 0..nr {
        loop {
            let route = &routes[ri];
            let n = route.len();
            if n < 5 {
                break;
            }

            let mut best_delta = -1e-10;
            let mut best_seg_start = 0;
            let mut best_seg_len = 0;
            let mut best_insert_pos = 0;

            // Try all segments of length 1..K_OR starting at positions 1..n-2
            for seg_len in 1..=K_OR {
                for seg_start in 1..n - 1 {
                    let seg_end = seg_start + seg_len; // exclusive
                    if seg_end >= n {
                        break;
                    }

                    // Cost of removing the segment
                    let before_seg = route[seg_start - 1];
                    let after_seg = route[seg_end];
                    let first_seg = route[seg_start];
                    let last_seg = route[seg_end - 1];

                    let removal_cost = inst.dist(before_seg, after_seg)
                        - inst.dist(before_seg, first_seg)
                        - inst.dist(last_seg, after_seg);

                    // Try inserting the segment at all other positions
                    for insert_after in 0..n - 1 {
                        // Skip if inserting back at the same position
                        if insert_after >= seg_start - 1 && insert_after < seg_end {
                            continue;
                        }

                        let ins_before = route[insert_after];
                        let ins_after_node = if insert_after >= seg_start && insert_after < seg_end
                        {
                            // This position will shift after removal; skip
                            continue;
                        } else if insert_after < seg_start {
                            route[insert_after + 1]
                        } else {
                            // insert_after >= seg_end
                            route[insert_after + 1]
                        };

                        let insertion_cost = inst.dist(ins_before, first_seg)
                            + inst.dist(last_seg, ins_after_node)
                            - inst.dist(ins_before, ins_after_node);

                        let delta = removal_cost + insertion_cost;
                        if delta < best_delta {
                            // Build new route and check feasibility
                            build_or_opt_route(
                                route,
                                seg_start,
                                seg_end,
                                insert_after,
                                &mut new_route_buf,
                            );
                            if is_route_feasible_with_reuse(
                                inst,
                                &new_route_buf,
                                &mut precedence_state,
                                &mut touched_pickups,
                            ) {
                                best_delta = delta;
                                best_seg_start = seg_start;
                                best_seg_len = seg_len;
                                best_insert_pos = insert_after;
                            }
                        }
                    }
                }
            }

            if best_delta < -1e-10 {
                build_or_opt_route(
                    &routes[ri],
                    best_seg_start,
                    best_seg_start + best_seg_len,
                    best_insert_pos,
                    &mut new_route_buf,
                );
                routes[ri].clone_from(&new_route_buf);
                infos[ri].compute(inst, &routes[ri]);
                any_improved = true;
            } else {
                break;
            }
        }
    }

    any_improved
}

/// Build the route after an Or-Opt move: remove segment [seg_start..seg_end)
/// and insert it after position insert_after (in the route AFTER removal).
fn build_or_opt_route(
    route: &[usize],
    seg_start: usize,
    seg_end: usize,
    insert_after: usize,
    buf: &mut Vec<usize>,
) {
    buf.clear();

    if insert_after < seg_start {
        // Insert before the original segment position
        // [0..=insert_after] + segment + [insert_after+1..seg_start) + [seg_end..n)
        buf.extend_from_slice(&route[..=insert_after]);
        buf.extend_from_slice(&route[seg_start..seg_end]);
        buf.extend_from_slice(&route[insert_after + 1..seg_start]);
        buf.extend_from_slice(&route[seg_end..]);
    } else {
        // insert_after >= seg_end: insert after the original segment position
        // [0..seg_start) + [seg_end..=insert_after] + segment + [insert_after+1..n)
        buf.extend_from_slice(&route[..seg_start]);
        buf.extend_from_slice(&route[seg_end..=insert_after]);
        buf.extend_from_slice(&route[seg_start..seg_end]);
        buf.extend_from_slice(&route[insert_after + 1..]);
    }
}