pdp_lns 0.1.1

use crate::instance::Instance;
use crate::solution::RouteInfo;
use crate::two_k_opt::is_route_feasible_with_reuse;

/// Intra-route 4-Opt via 2-Alternating Cycle decomposition (Pacheco et al. 2022).
/// Removes 4 edges and reconnects 5 segments in optimal configuration.
/// Evaluated in O(n²) per route via DP over alternating cycles.
///
/// Returns true if any route was improved.
pub fn apply_four_opt(inst: &Instance, routes: &mut [Vec<usize>], infos: &mut [RouteInfo]) -> bool {
    let nr = routes.len();
    let mut any_improved = false;

    // Allocate reusable buffers
    let max_route = routes.iter().map(|r| r.len()).max().unwrap_or(0);
    if max_route < 6 {
        return false;
    }
    let stride = max_route;

    let mut pos = vec![0usize; inst.n + 1];
    let mut sub_reversible = vec![false; stride * stride];
    let mut sub_before_later = vec![-1i32; stride * stride];
    let mut cost_c = vec![0.0f64; stride * stride];
    let mut cost_d = vec![0.0f64; stride * stride];
    let mut best_reach_c = vec![f64::MAX; stride];
    let mut best_reach_d = vec![f64::MAX; stride];
    let mut pred_reach_c = vec![0usize; stride];
    let mut pred_reach_d = vec![0usize; stride];
    let mut best_cross_c = vec![f64::MAX; stride];
    let mut best_cross_d = vec![f64::MAX; stride];
    let mut pred_cross_c_i = vec![0usize; stride];
    let mut pred_cross_c_j = vec![0usize; stride];
    let mut pred_cross_d_i = vec![0usize; stride];
    let mut pred_cross_d_j = vec![0usize; stride];
    let mut new_route: Vec<usize> = Vec::with_capacity(max_route);
    let mut prec_state: Vec<u8> = vec![0; inst.n + 1];
    let mut touched: Vec<usize> = Vec::new();

    for ri in 0..nr {
        loop {
            let route = &routes[ri];
            let n = route.len();
            if n < 6 {
                break; // need at least 4 internal edges to cut
            }

            // n_edges = n - 1 (edges in route 0..n-1)
            // The C++ uses n = route.size() - 1 as edge count
            let ne = n - 1; // number of edges

            // Build position map
            for (idx, &v) in route.iter().enumerate() {
                if v != 0 && v <= inst.n {
                    pos[v] = idx;
                }
            }

            // Precompute subroute info
            let s = n; // stride for this route
            precompute_subroute_info(
                inst,
                route,
                &pos,
                s,
                &mut sub_reversible,
                &mut sub_before_later,
            );

            // Compute 2AC cost matrix
            compute_2ac_costs(inst, route, ne, s, &mut cost_c, &mut cost_d);

            // DP: find best 4-opt move
            let result = dp_find_best(
                inst,
                route,
                ne,
                s,
                &cost_c,
                &cost_d,
                &sub_reversible,
                &sub_before_later,
                &mut best_reach_c,
                &mut best_reach_d,
                &mut pred_reach_c,
                &mut pred_reach_d,
                &mut best_cross_c,
                &mut best_cross_d,
                &mut pred_cross_c_i,
                &mut pred_cross_c_j,
                &mut pred_cross_d_i,
                &mut pred_cross_d_j,
                &mut new_route,
                &mut prec_state,
                &mut touched,
            );

            // Clear position map
            for &v in routes[ri].iter() {
                if v != 0 && v <= inst.n {
                    pos[v] = 0;
                }
            }

            if let Some(new_r) = result {
                routes[ri] = new_r;
                infos[ri].compute(inst, &routes[ri]);
                any_improved = true;
            } else {
                break;
            }
        }
    }

    any_improved
}

/// Precompute subroute info for all [i..j] segments.
/// - reversible: no PD pair has both pickup and delivery inside [i..j]
/// - before_later: max position of a pickup that is before i,
///   whose delivery is inside [i..j]
fn precompute_subroute_info(
    inst: &Instance,
    route: &[usize],
    pos: &[usize],
    stride: usize,
    sub_reversible: &mut [bool],
    sub_before_later: &mut [i32],
) {
    let sz = route.len();
    for i in 0..sz {
        let mut reversible = true;
        let mut before_later: i32 = -1;

        for j in i..sz {
            let v = route[j];
            if v != 0 && v <= inst.n {
                let pair = if inst.is_pickup(v) {
                    inst.pair_delivery[v]
                } else {
                    inst.pickup_of(v)
                };
                if pair != 0 {
                    let pair_pos = pos[pair];
                    if pair_pos >= i && pair_pos <= j {
                        // Both in segment — not reversible
                        reversible = false;
                    } else if pair_pos < i {
                        // Pair is before segment
                        // In C++: only track for deliveries (node whose pair/pickup is before)
                        if !inst.is_pickup(v) {
                            // v is delivery, pair_pos is position of its pickup
                            if pair_pos as i32 > before_later {
                                before_later = pair_pos as i32;
                            }
                        }
                    }
                }
            }
            sub_reversible[i * stride + j] = reversible;
            sub_before_later[i * stride + j] = before_later;
        }
    }
}

/// Compute the 2-opt alternating cycle cost matrix.
/// cost_c[i*s+j] = delta for forward 2-opt at (i,j)
/// cost_d[i*s+j] = delta for reversed 2-opt at (i,j) = cost_c[j*s+i] conceptually
fn compute_2ac_costs(
    inst: &Instance,
    route: &[usize],
    ne: usize, // number of edges = route.len() - 1
    s: usize,  // stride
    cost_c: &mut [f64],
    cost_d: &mut [f64],
) {
    // C++ indexing: for i in 0..ne-2, j in i+2..ne-1
    // cost[i][j] = dist(route[i], route[j]) + dist(route[i+1], route[j+1])
    //            - dist(route[i], route[i+1]) - dist(route[j], route[j+1])
    // cost[j][i] = dist(route[i], route[j+1]) + dist(route[i+1], route[j])
    //            - dist(route[i], route[i+1]) - dist(route[j], route[j+1])
    for i in 0..ne.saturating_sub(2) {
        let prev_i = route[i];
        let next_i = route[i + 1];
        let dist_pi_ni = inst.dist(prev_i, next_i);

        for j in (i + 2)..ne {
            let prev_j = route[j];
            let next_j = route[j + 1];
            let dist_pj_nj = inst.dist(prev_j, next_j);
            let delta_r = dist_pi_ni + dist_pj_nj;

            // cost_c[i][j] = "forward" 2-opt cost
            cost_c[i * s + j] = inst.dist(prev_i, prev_j) + inst.dist(next_i, next_j) - delta_r;
            // cost_d[i][j] = "reversed" 2-opt cost = C++ cost[j][i]
            cost_d[i * s + j] = inst.dist(prev_i, next_j) + inst.dist(next_i, prev_j) - delta_r;
        }
    }
}

/// Move types for 4-opt
#[derive(Clone, Copy, PartialEq)]
#[allow(dead_code)]
enum MoveType {
    CC, // Simple 2-opt (degenerate)
    DD, // A → D → C → B → E
    DC, // A → D → B_rev → C_rev → E
    CD, // A → C_rev → D_rev → B → E
}

/// DP to find best improving 4-opt move. Returns the new route if improving.
#[allow(clippy::too_many_arguments)]
fn dp_find_best(
    inst: &Instance,
    route: &[usize],
    ne: usize,
    s: usize,
    cost_c: &[f64],
    cost_d: &[f64],
    sub_rev: &[bool],
    sub_bl: &[i32],
    best_reach_c: &mut [f64],
    best_reach_d: &mut [f64],
    pred_reach_c: &mut [usize],
    pred_reach_d: &mut [usize],
    best_cross_c: &mut [f64],
    best_cross_d: &mut [f64],
    pred_cross_c_i: &mut [usize],
    pred_cross_c_j: &mut [usize],
    pred_cross_d_i: &mut [usize],
    pred_cross_d_j: &mut [usize],
    new_route: &mut Vec<usize>,
    prec_state: &mut [u8],
    touched: &mut Vec<usize>,
) -> Option<Vec<usize>> {
    // Track best feasible move
    let mut best_t = -1e-10_f64;
    let mut best_move: Option<(usize, usize, usize, usize, MoveType)> = None;

    // Validation helpers (inline closures)
    let validate_cc = |i1: usize, j1: usize| -> bool { sub_rev[(i1 + 1) * s + j1] };

    let validate_dd = |i1: usize, j1: usize, i2: usize, j2: usize| -> bool {
        sub_bl[(i2 + 1) * s + j2] <= i1 as i32 && sub_bl[(j1 + 1) * s + j2] <= i1 as i32
    };

    let validate_cd = |i1: usize, j1: usize, i2: usize, j2: usize| -> bool {
        sub_bl[(i2 + 1) * s + j2] <= i1 as i32
            && sub_rev[(i2 + 1) * s + j1]
            && sub_rev[(j1 + 1) * s + j2]
    };

    let validate_dc = |i1: usize, j1: usize, i2: usize, j2: usize| -> bool {
        sub_bl[(j1 + 1) * s + j2] <= i1 as i32
            && sub_rev[(i1 + 1) * s + i2]
            && sub_rev[(i2 + 1) * s + j1]
    };

    // Try a candidate move: check TW feasibility, update best
    let mut try_candidate =
        |delta: f64,
         i1: usize,
         j1: usize,
         i2: usize,
         j2: usize,
         mt: MoveType,
         best_t: &mut f64,
         best_move: &mut Option<(usize, usize, usize, usize, MoveType)>| {
            if delta < *best_t {
                build_4opt_route(route, i1, j1, i2, j2, mt, new_route);
                if is_route_feasible_with_reuse(inst, new_route, prec_state, touched) {
                    *best_t = delta;
                    *best_move = Some((i1, j1, i2, j2, mt));
                }
            }
        };

    // Step 1: Initialization (i=0)
    for j in 2..ne {
        best_reach_c[j] = cost_c[j]; // cost_c(0, j)
        best_reach_d[j] = cost_d[j]; // cost_d(0, j)
        pred_reach_c[j] = 0;
        pred_reach_d[j] = 0;

        // SimpleTerminalNodeUpdate: CC check at (i=0, j)
        // CC is a degenerate 2-opt — skip it, 2-opt handles this already
        // But for completeness with the DP, include it:
        if cost_c[j] < best_t && validate_cc(0, j) {
            // CC move: just a 2-opt from i+1..j reversed
            // We skip CC since regular 2-opt already covers it
        }
    }

    // Step 2: Main DP loop
    for i in 1..ne.saturating_sub(1) {
        // Initialize best_cross at i+1 from best_reach
        best_cross_c[i + 1] = best_reach_c[i + 1];
        pred_cross_c_i[i + 1] = pred_reach_c[i + 1];
        pred_cross_c_j[i + 1] = i + 1;

        best_cross_d[i + 1] = best_reach_d[i + 1];
        pred_cross_d_i[i + 1] = pred_reach_d[i + 1];
        pred_cross_d_j[i + 1] = i + 1;

        for j in (i + 2)..ne {
            // BestTerminalNodeUpdate: check DD, CD, DC at (i, j)
            // DD: y=d, x=d => cost_d(i,j) + best_cross_d[j-1]
            {
                let delta = cost_d[i * s + j] + best_cross_d[j - 1];
                let pi = pred_cross_d_i[j - 1];
                let pj = pred_cross_d_j[j - 1];
                if delta < best_t && validate_dd(pi, pj, i, j) {
                    try_candidate(
                        delta,
                        pi,
                        pj,
                        i,
                        j,
                        MoveType::DD,
                        &mut best_t,
                        &mut best_move,
                    );
                }
            }

            // CD: y=c, x=d => cost_d(i,j) + best_cross_c[j-1]
            {
                let delta = cost_d[i * s + j] + best_cross_c[j - 1];
                let pi = pred_cross_c_i[j - 1];
                let pj = pred_cross_c_j[j - 1];
                if delta < best_t && validate_cd(pi, pj, i, j) {
                    try_candidate(
                        delta,
                        pi,
                        pj,
                        i,
                        j,
                        MoveType::CD,
                        &mut best_t,
                        &mut best_move,
                    );
                }
            }

            // DC: y=d, x=c => cost_c(i,j) + best_cross_d[j-1]
            {
                let delta = cost_c[i * s + j] + best_cross_d[j - 1];
                let pi = pred_cross_d_i[j - 1];
                let pj = pred_cross_d_j[j - 1];
                if delta < best_t && validate_dc(pi, pj, i, j) {
                    try_candidate(
                        delta,
                        pi,
                        pj,
                        i,
                        j,
                        MoveType::DC,
                        &mut best_t,
                        &mut best_move,
                    );
                }
            }

            // SimpleTerminalNodeUpdate: CC at (i, j) — skip (covered by 2-opt)

            // BestCrossUpdate & BestReachUpdate (only if j < ne - 1)
            if j < ne - 1 {
                // BestCrossUpdate
                if best_reach_c[j] < best_cross_c[j - 1] {
                    best_cross_c[j] = best_reach_c[j];
                    pred_cross_c_i[j] = pred_reach_c[j];
                    pred_cross_c_j[j] = j;
                } else {
                    best_cross_c[j] = best_cross_c[j - 1];
                    pred_cross_c_i[j] = pred_cross_c_i[j - 1];
                    pred_cross_c_j[j] = pred_cross_c_j[j - 1];
                }

                if best_reach_d[j] < best_cross_d[j - 1] {
                    best_cross_d[j] = best_reach_d[j];
                    pred_cross_d_i[j] = pred_reach_d[j];
                    pred_cross_d_j[j] = j;
                } else {
                    best_cross_d[j] = best_cross_d[j - 1];
                    pred_cross_d_i[j] = pred_cross_d_i[j - 1];
                    pred_cross_d_j[j] = pred_cross_d_j[j - 1];
                }

                // BestReachUpdate
                if cost_c[i * s + j] < best_reach_c[j] {
                    best_reach_c[j] = cost_c[i * s + j];
                    pred_reach_c[j] = i;
                }
                if cost_d[i * s + j] < best_reach_d[j] {
                    best_reach_d[j] = cost_d[i * s + j];
                    pred_reach_d[j] = i;
                }
            }
        }
    }

    // Apply best move if found
    if let Some((i1, j1, i2, j2, mt)) = best_move {
        build_4opt_route(route, i1, j1, i2, j2, mt, new_route);
        debug_assert!(is_route_feasible_with_reuse(
            inst, new_route, prec_state, touched
        ));
        Some(new_route.clone())
    } else {
        None
    }
}

/// Build the rearranged route for a 4-opt move.
/// Cut points i1 < j1 and i2 < j2 where i1 < i2, creating 5 segments:
///   A = route[0..=i1]
///   B = route[i1+1..=i2]
///   C = route[i2+1..=j1]
///   D = route[j1+1..=j2]
///   E = route[j2+1..]
fn build_4opt_route(
    route: &[usize],
    i1: usize,
    j1: usize,
    i2: usize,
    j2: usize,
    mt: MoveType,
    buf: &mut Vec<usize>,
) {
    buf.clear();
    match mt {
        MoveType::CC => {
            // Degenerate: A + reversed(B+C) + D + E
            // = route[0..=i2] reversed to route[0..=i1] + reversed(route[i1+1..=j1]) + route[j1+1..]
            // Actually CC in C++: blkAcc = [0, i2], blkcc = [j2, i2+1], blkE = [j2+1, n]
            // i.e. route[0..=i2] + reversed(route[i2+1..=j2]) + route[j2+1..]
            // This is just a 2-opt on (i2, j2+1) — skip
            buf.extend_from_slice(&route[..=i1]);
            for k in (i1 + 1..=j1).rev() {
                buf.push(route[k]);
            }
            buf.extend_from_slice(&route[j1 + 1..]);
        }
        MoveType::DD => {
            // A → D → C → B → E
            buf.extend_from_slice(&route[..=i1]);
            buf.extend_from_slice(&route[j1 + 1..=j2]);
            buf.extend_from_slice(&route[i2 + 1..=j1]);
            buf.extend_from_slice(&route[i1 + 1..=i2]);
            buf.extend_from_slice(&route[j2 + 1..]);
        }
        MoveType::DC => {
            // A → D → B_rev → C_rev → E
            buf.extend_from_slice(&route[..=i1]);
            buf.extend_from_slice(&route[j1 + 1..=j2]);
            for k in (i1 + 1..=i2).rev() {
                buf.push(route[k]);
            }
            for k in (i2 + 1..=j1).rev() {
                buf.push(route[k]);
            }
            buf.extend_from_slice(&route[j2 + 1..]);
        }
        MoveType::CD => {
            // A → C_rev → D_rev → B → E
            buf.extend_from_slice(&route[..=i1]);
            for k in (i2 + 1..=j1).rev() {
                buf.push(route[k]);
            }
            for k in (j1 + 1..=j2).rev() {
                buf.push(route[k]);
            }
            buf.extend_from_slice(&route[i1 + 1..=i2]);
            buf.extend_from_slice(&route[j2 + 1..]);
        }
    }
}