pdp_lns 0.1.1

use crate::instance::Instance;
use crate::solution::{
    RouteInfo, Solution, best_pair_insertion_with_info, route_distance, route_without_pair_buf,
};
use std::time::Instant;

/// Edge in the VLSN improvement graph.
struct VlsnEdge {
    from_pair: usize,
    to_pair: usize,
    delta: f64,
}

/// Execute a cycle of swaps on the solution.
/// `cycle_nodes` contains pair indices in cycle order: node[0]→node[1]→...→node[k-1]→node[0].
/// Each pair moves to the route of the next pair in the cycle.
fn execute_cycle(
    inst: &Instance,
    sol: &mut Solution,
    pairs: &[(usize, usize)],
    cycle_nodes: &[usize],
    adj: &[Vec<(usize, f64)>],
) -> bool {
    let k = cycle_nodes.len();
    if k < 2 {
        return false;
    }

    // Compute actual delta
    let mut delta = 0.0f64;
    for i in 0..k {
        let from = cycle_nodes[i];
        let to = cycle_nodes[(i + 1) % k];
        if let Some(&(_, d)) = adj[from].iter().find(|(t, _)| *t == to) {
            delta += d;
        } else {
            return false;
        }
    }
    if delta >= -1e-9 {
        return false;
    }

    let mut buf = Vec::new();
    let mut info = RouteInfo::new();

    for i in 0..k {
        let from_idx = cycle_nodes[i];
        let to_idx = cycle_nodes[(i + 1) % k];
        let (from_p, _) = pairs[from_idx];
        let (to_p, to_ri) = pairs[to_idx];
        let from_d = inst.delivery_of(from_p);
        let to_d = inst.delivery_of(to_p);

        route_without_pair_buf(&sol.routes[to_ri], to_p, to_d, &mut buf);
        info.compute(inst, &buf);
        if let Some((_dist, ep, ed)) =
            best_pair_insertion_with_info::<false>(inst, &buf, &info, from_p, from_d)
        {
            let mut new_route = Vec::with_capacity(buf.len() + 2);
            new_route.extend_from_slice(&buf[..=ep]);
            new_route.push(from_p);
            if ed == ep {
                new_route.push(from_d);
                new_route.extend_from_slice(&buf[ep + 1..]);
            } else {
                new_route.extend_from_slice(&buf[ep + 1..=ed]);
                new_route.push(from_d);
                new_route.extend_from_slice(&buf[ed + 1..]);
            }
            sol.routes[to_ri] = new_route;
        } else {
            return false;
        }
    }

    sol.routes.retain(|r| r.len() > 2);
    true
}

/// Run one round of VLSN: build improvement graph, find negative cycle via Bellman-Ford.
/// Returns true if an improving composite move was found and applied.
pub fn vlsn_round(inst: &Instance, sol: &mut Solution) -> bool {
    let nr = sol.routes.len();
    if nr < 2 {
        return false;
    }

    // Collect all routed pickup nodes and their route indices
    let mut pairs: Vec<(usize, usize)> = Vec::new();
    for (ri, route) in sol.routes.iter().enumerate() {
        for &v in route {
            if v != 0 && inst.is_pickup(v) {
                pairs.push((v, ri));
            }
        }
    }
    let np = pairs.len();
    if np < 2 {
        return false;
    }

    // Precompute RouteInfo for each route
    let mut infos: Vec<RouteInfo> = Vec::with_capacity(nr);
    for route in &sol.routes {
        let mut info = RouteInfo::new();
        info.compute(inst, route);
        infos.push(info);
    }

    // Precompute removal delta for each pair
    let mut removal_delta: Vec<f64> = Vec::with_capacity(np);
    let mut route_without_buf = Vec::new();
    for &(p, ri) in &pairs {
        let d = inst.delivery_of(p);
        route_without_pair_buf(&sol.routes[ri], p, d, &mut route_without_buf);
        let dist_without = route_distance(inst, &route_without_buf);
        removal_delta.push(dist_without - infos[ri].distance);
    }

    // Build VLSN graph
    let graph_start = Instant::now();
    let mut adj: Vec<Vec<(usize, f64)>> = vec![Vec::new(); np];
    let mut edges: Vec<VlsnEdge> = Vec::new();
    let mut shortened_info = RouteInfo::new();

    for to_idx in 0..np {
        let (to_p, to_ri) = pairs[to_idx];
        let to_d = inst.delivery_of(to_p);

        route_without_pair_buf(&sol.routes[to_ri], to_p, to_d, &mut route_without_buf);
        shortened_info.compute(inst, &route_without_buf);

        for from_idx in 0..np {
            let (from_p, from_ri) = pairs[from_idx];
            if from_ri == to_ri {
                continue;
            }

            let from_d = inst.delivery_of(from_p);

            if let Some((new_dist, _, _)) = best_pair_insertion_with_info::<false>(
                inst,
                &route_without_buf,
                &shortened_info,
                from_p,
                from_d,
            ) {
                let insertion_cost = new_dist - shortened_info.distance;
                let delta = insertion_cost + removal_delta[from_idx];
                adj[from_idx].push((to_idx, delta));
                edges.push(VlsnEdge {
                    from_pair: from_idx,
                    to_pair: to_idx,
                    delta,
                });
            }
        }
    }

    let graph_ms = graph_start.elapsed().as_millis();

    if edges.is_empty() {
        return false;
    }

    // Bellman-Ford: find negative-weight cycle
    let mut dist = vec![0.0f64; np];
    let mut pred = vec![usize::MAX; np];

    let mut last_relaxed = usize::MAX;
    for iteration in 0..np {
        let mut any_relaxed = false;
        for (ei, e) in edges.iter().enumerate() {
            let new_dist = dist[e.from_pair] + e.delta;
            if new_dist < dist[e.to_pair] - 1e-9 {
                dist[e.to_pair] = new_dist;
                pred[e.to_pair] = ei;
                any_relaxed = true;
                if iteration == np - 1 {
                    last_relaxed = e.to_pair;
                }
            }
        }
        if !any_relaxed {
            return false;
        }
    }

    if last_relaxed == usize::MAX {
        return false;
    }

    // Trace back to find the cycle
    let mut node = last_relaxed;
    for _ in 0..np {
        if pred[node] == usize::MAX {
            return false;
        }
        node = edges[pred[node]].from_pair;
    }

    let cycle_start = node;
    let mut cycle_nodes: Vec<usize> = Vec::new();
    let mut cur = cycle_start;
    loop {
        let ei = pred[cur];
        if ei == usize::MAX {
            return false;
        }
        cycle_nodes.push(cur);
        cur = edges[ei].from_pair;
        if cur == cycle_start {
            break;
        }
        if cycle_nodes.len() > np {
            return false;
        }
    }
    cycle_nodes.reverse();

    // Validate: each route appears at most once
    let mut route_seen = vec![false; nr];
    for &n in &cycle_nodes {
        let (_, ri) = pairs[n];
        if route_seen[ri] {
            return false;
        }
        route_seen[ri] = true;
    }

    // Compute cycle delta for logging
    let mut cycle_delta = 0.0f64;
    for i in 0..cycle_nodes.len() {
        let from = cycle_nodes[i];
        let to = cycle_nodes[(i + 1) % cycle_nodes.len()];
        if let Some(&(_, d)) = adj[from].iter().find(|(t, _)| *t == to) {
            cycle_delta += d;
        }
    }
    eprintln!(
        "    VLSN cycle: len={}, delta={:.2} (build={}ms)",
        cycle_nodes.len(),
        cycle_delta,
        graph_ms,
    );

    execute_cycle(inst, sol, &pairs, &cycle_nodes, &adj)
}

/// Run VLSN until no more improving cycles are found.
/// Returns true if any improvement was made.
pub fn vlsn(inst: &Instance, sol: &mut Solution) -> bool {
    let mut improved = false;
    let mut rounds = 0;
    for _ in 0..20 {
        if !vlsn_round(inst, sol) {
            break;
        }
        improved = true;
        rounds += 1;
    }
    if rounds > 0 {
        eprintln!("    VLSN total: {rounds} rounds applied");
    }
    improved
}