pdp_lns 0.1.1

use crate::instance::Instance;
use crate::solution::{Solution, fmax};
use rand::prelude::*;
use rand::rngs::SmallRng;
use std::time::Instant;

/// Random vertices sampled per iteration.
const SAMPLE_VERTICES: usize = 16;
/// Min tabu tenure.
const TENURE_MIN: usize = 4;
/// Max tabu tenure.
const TENURE_MAX: usize = 12;
/// Penalty adjustment interval (every ρ_it iterations).
const PENALTY_ADJUST_INTERVAL: usize = 2;
/// Penalty update factor ρ_upd.
const PENALTY_UPDATE_FACTOR: f64 = 1.2;
/// Max σ_tw.
const MAX_SIGMA_TW: f64 = 500.0;
/// Max general penalty.
const MAX_SIGMA: f64 = 5000.0;
/// Min penalty.
const MIN_SIGMA: f64 = 0.1;

/// Tabu list: (vertex, old_route) pair is tabu after moving vertex away.
struct TabuList {
    entries: Vec<(usize, usize, usize)>, // (vertex, old_route, expire_iter)
}

impl TabuList {
    fn new() -> Self {
        Self {
            entries: Vec::with_capacity(64),
        }
    }

    fn prune(&mut self, iter: usize) {
        self.entries.retain(|&(_, _, exp)| exp > iter);
    }

    fn is_tabu(&self, vertex: usize, route: usize, iter: usize) -> bool {
        self.entries
            .iter()
            .any(|&(v, r, exp)| v == vertex && r == route && exp > iter)
    }

    fn add(&mut self, vertex: usize, old_route: usize, iter: usize, tenure: usize) {
        self.entries.push((vertex, old_route, iter + tenure));
    }
}

/// Compact per-route cached state for fast evaluation.
struct RouteCache {
    /// TW violation of this route.
    tw_viol: f64,
    /// Capacity violation of this route.
    cap_viol: f64,
}

/// Compute route cache from scratch via forward pass.
fn compute_cache(inst: &Instance, route: &[usize]) -> RouteCache {
    let mut tw_viol = 0.0;
    let mut cap_viol = 0.0;
    let mut time = inst.early(0);
    let mut load: i32 = 0;
    for i in 1..route.len() {
        let prev = route[i - 1];
        let curr = route[i];
        let d = inst.dist(prev, curr);
        time = fmax(time + inst.svc(prev) + d, inst.early(curr));
        if time > inst.late(curr) {
            tw_viol += time - inst.late(curr);
        }
        if curr != 0 {
            load += inst.demand[curr];
            if load > inst.capacity {
                cap_viol += f64::from(load - inst.capacity);
            }
            if load < 0 {
                cap_viol += f64::from(-load);
            }
        }
    }
    RouteCache { tw_viol, cap_viol }
}

/// Infeasible-space tabu search (Goeke 2019 style).
///
/// High-throughput relocate-only search with O(1) distance delta evaluation
/// and O(route_len) TW/cap recomputation only for the best candidate per iteration.
pub fn infeasible_tabu_search(
    sol: &Solution,
    inst: &Instance,
    rng: &mut SmallRng,
    budget_ms: u64,
) -> Solution {
    let start = Instant::now();
    let deadline = start + std::time::Duration::from_millis(budget_ms);

    let n = inst.n;
    let nr_initial = sol.routes.len();
    // Add a few empty routes so we can relocate into them.
    let extra = 2.min(inst.num_vehicles.saturating_sub(nr_initial));
    let nr = nr_initial + extra;

    // Build mutable route storage and indices.
    let mut routes: Vec<Vec<usize>> = Vec::with_capacity(nr);
    for r in &sol.routes {
        routes.push(r.clone());
    }
    for _ in 0..extra {
        routes.push(vec![0, 0]);
    }

    // node_route[v], node_pos[v]: which route and position for vertex v.
    let mut node_route = vec![usize::MAX; n + 1];
    let mut node_pos = vec![0usize; n + 1];
    for (ri, route) in routes.iter().enumerate() {
        for (pos, &v) in route.iter().enumerate() {
            if v != 0 {
                node_route[v] = ri;
                node_pos[v] = pos;
            }
        }
    }

    // Per-route caches.
    let mut caches: Vec<RouteCache> = routes.iter().map(|r| compute_cache(inst, r)).collect();

    // Precompute sums for fast delta evaluation.
    let mut sum_tw: f64 = caches.iter().map(|c| c.tw_viol).sum();
    let mut sum_cap: f64 = caches.iter().map(|c| c.cap_viol).sum();

    // Pair and precedence violations.
    let mut pair_viol: f64 = count_pair_violations(inst, &node_route);
    let mut prec_viol: f64 = count_prec_violations(inst, &node_route, &node_pos);

    // Penalties.
    let mut sigma_tw: f64 = 1.0;
    let mut sigma_cap: f64 = 1.0;
    let mut sigma_pair: f64 = 10.0;
    let mut sigma_prec: f64 = 10.0;

    // Best feasible tracking.
    let input_cost = sol.cost(inst);
    let mut best_feasible_cost = input_cost;
    let mut best_feasible: Option<Solution> = None;

    let is_feasible =
        |tw: f64, cap: f64, pv: f64, pr: f64| tw < 1e-6 && cap < 1e-6 && pv < 0.5 && pr < 0.5;

    if is_feasible(sum_tw, sum_cap, pair_viol, prec_viol) {
        best_feasible = Some(sol.clone());
    }

    // Collect assigned vertices for sampling.
    let assigned_vertices: Vec<usize> = (1..=n).filter(|&v| node_route[v] != usize::MAX).collect();

    let mut tabu = TabuList::new();

    let mut iter = 0usize;
    while Instant::now() < deadline {
        iter += 1;

        if iter.is_multiple_of(64) {
            tabu.prune(iter);
        }

        // Best move this iteration.
        let mut best_delta = f64::MAX;
        let mut best_vertex = 0usize;
        let mut best_from = 0usize;
        let mut best_to = 0usize;
        let mut best_to_pos = 0usize;

        let nv = assigned_vertices.len();
        if nv == 0 {
            break;
        }

        let samples = SAMPLE_VERTICES.min(nv);
        for _ in 0..samples {
            let vi = rng.random_range(0..nv);
            let vertex = assigned_vertices[vi];
            let from_ri = node_route[vertex];
            if from_ri == usize::MAX {
                continue;
            }
            let from_pos = node_pos[vertex];
            let from_route = &routes[from_ri];

            // O(1) distance delta for removal.
            let prev = from_route[from_pos - 1];
            let next = from_route[from_pos + 1];
            let remove_dist_delta =
                inst.dist(prev, next) - inst.dist(prev, vertex) - inst.dist(vertex, next);

            // Pair/prec delta for this vertex (only its pair is affected).
            let is_pickup = inst.is_pickup(vertex);
            let partner = if is_pickup {
                inst.delivery_of(vertex)
            } else {
                inst.pickup_of(vertex)
            };
            let partner_route = node_route[partner];

            // Try relocating to each other route.
            for to_ri in 0..nr {
                if to_ri == from_ri {
                    continue; // suppress intra-route (Goeke 2019)
                }

                let to_route = &routes[to_ri];
                let to_len = to_route.len();

                // Try each insertion position.
                for ins_pos in 1..to_len {
                    let a = to_route[ins_pos - 1];
                    let b = to_route[ins_pos];

                    // O(1) distance delta for insertion.
                    let insert_dist_delta =
                        inst.dist(a, vertex) + inst.dist(vertex, b) - inst.dist(a, b);
                    let dist_delta = remove_dist_delta + insert_dist_delta;

                    // Pair violation delta.
                    let old_pair_ok = i32::from(partner_route == from_ri);
                    let new_pair_ok = i32::from(partner_route == to_ri);
                    let pair_delta = f64::from(old_pair_ok - new_pair_ok);

                    // Precedence violation delta (quick estimate).
                    let prec_delta = estimate_prec_delta(
                        inst,
                        vertex,
                        is_pickup,
                        partner,
                        from_ri,
                        to_ri,
                        ins_pos,
                        &node_route,
                        &node_pos,
                        &routes,
                    );

                    // Use distance + pair + prec for fast screening.
                    // TW/cap are expensive — we'll compute them only for the best candidate.
                    let approx_delta =
                        dist_delta + sigma_pair * pair_delta + sigma_prec * prec_delta;

                    if approx_delta >= best_delta {
                        continue;
                    }

                    // Tabu check.
                    let is_tabu = tabu.is_tabu(vertex, to_ri, iter);
                    if is_tabu {
                        // Aspiration: only allow if we can estimate global improvement.
                        // Use distance improvement as proxy.
                        if dist_delta >= 0.0 {
                            continue;
                        }
                    }

                    best_delta = approx_delta;
                    best_vertex = vertex;
                    best_from = from_ri;
                    best_to = to_ri;
                    best_to_pos = ins_pos;
                }
            }
        }

        if best_delta == f64::MAX {
            break;
        }

        // Apply the best relocate move.
        let vertex = best_vertex;
        let from_ri = best_from;
        let to_ri = best_to;
        let to_pos = best_to_pos;
        let from_pos = node_pos[vertex];

        // Remove from source route.
        routes[from_ri].remove(from_pos);
        // Insert into dest route.
        routes[to_ri].insert(to_pos, vertex);

        // Recompute caches for affected routes.
        let old_from = &caches[from_ri];
        sum_tw -= old_from.tw_viol;
        sum_cap -= old_from.cap_viol;
        caches[from_ri] = compute_cache(inst, &routes[from_ri]);
        sum_tw += caches[from_ri].tw_viol;
        sum_cap += caches[from_ri].cap_viol;

        let old_to = &caches[to_ri];
        sum_tw -= old_to.tw_viol;
        sum_cap -= old_to.cap_viol;
        caches[to_ri] = compute_cache(inst, &routes[to_ri]);
        sum_tw += caches[to_ri].tw_viol;
        sum_cap += caches[to_ri].cap_viol;

        // Update node maps for affected routes.
        update_node_maps(&routes[from_ri], from_ri, &mut node_route, &mut node_pos);
        update_node_maps(&routes[to_ri], to_ri, &mut node_route, &mut node_pos);

        // Recompute pair/prec violations (fast — only iterates over m pairs).
        pair_viol = count_pair_violations(inst, &node_route);
        prec_viol = count_prec_violations(inst, &node_route, &node_pos);

        // Add tabu entry.
        let tenure = rng.random_range(TENURE_MIN..=TENURE_MAX);
        tabu.add(vertex, from_ri, iter, tenure);

        // Check feasibility and track best.
        if is_feasible(sum_tw, sum_cap, pair_viol, prec_viol) {
            let fsol = extract_solution(&routes);
            let fcost = fsol.cost(inst);
            if fcost < best_feasible_cost {
                best_feasible_cost = fcost;
                best_feasible = Some(fsol);
            }
        }

        // Adjust penalties periodically.
        if iter.is_multiple_of(PENALTY_ADJUST_INTERVAL) {
            if sum_tw > 1e-6 {
                sigma_tw *= PENALTY_UPDATE_FACTOR;
            } else {
                sigma_tw /= PENALTY_UPDATE_FACTOR;
            }
            sigma_tw = sigma_tw.clamp(MIN_SIGMA, MAX_SIGMA_TW);

            if sum_cap > 1e-6 {
                sigma_cap *= PENALTY_UPDATE_FACTOR;
            } else {
                sigma_cap /= PENALTY_UPDATE_FACTOR;
            }
            sigma_cap = sigma_cap.clamp(MIN_SIGMA, MAX_SIGMA);

            if pair_viol > 0.5 {
                sigma_pair *= PENALTY_UPDATE_FACTOR;
            } else {
                sigma_pair /= PENALTY_UPDATE_FACTOR;
            }
            sigma_pair = sigma_pair.clamp(MIN_SIGMA, MAX_SIGMA);

            if prec_viol > 0.5 {
                sigma_prec *= PENALTY_UPDATE_FACTOR;
            } else {
                sigma_prec /= PENALTY_UPDATE_FACTOR;
            }
            sigma_prec = sigma_prec.clamp(MIN_SIGMA, MAX_SIGMA);
        }
    }

    match best_feasible {
        Some(fsol) if fsol.cost(inst) < input_cost - 1e-10 => fsol,
        _ => sol.clone(),
    }
}

/// Count pair violations: pickup and delivery in different routes.
#[inline]
fn count_pair_violations(inst: &Instance, node_route: &[usize]) -> f64 {
    let mut count = 0.0;
    for &p in &inst.pickups {
        let d = inst.delivery_of(p);
        let rp = node_route[p];
        let rd = node_route[d];
        if rp == usize::MAX || rd == usize::MAX || rp != rd {
            count += 1.0;
        }
    }
    count
}

/// Count precedence violations: delivery before pickup in same route.
#[inline]
fn count_prec_violations(inst: &Instance, node_route: &[usize], node_pos: &[usize]) -> f64 {
    let mut count = 0.0;
    for &p in &inst.pickups {
        let d = inst.delivery_of(p);
        if node_route[p] != usize::MAX
            && node_route[p] == node_route[d]
            && node_pos[d] < node_pos[p]
        {
            count += 1.0;
        }
    }
    count
}

/// Estimate precedence violation delta for relocating vertex.
#[inline]
fn estimate_prec_delta(
    inst: &Instance,
    vertex: usize,
    is_pickup: bool,
    partner: usize,
    from_ri: usize,
    to_ri: usize,
    ins_pos: usize,
    node_route: &[usize],
    node_pos: &[usize],
    _routes: &[Vec<usize>],
) -> f64 {
    let _ = inst; // used for type context only
    let partner_route = node_route[partner];
    if partner_route == usize::MAX {
        return 0.0;
    }
    let partner_pos = node_pos[partner];

    // Current state: is there a precedence violation for this pair?
    let old_viol = if partner_route == from_ri {
        // Both in same route currently.
        if is_pickup {
            // vertex is pickup, partner is delivery.
            f64::from(u8::from(partner_pos < node_pos[vertex]))
        } else {
            // vertex is delivery, partner is pickup.
            f64::from(u8::from(node_pos[vertex] < partner_pos))
        }
    } else {
        0.0 // different routes = no prec violation
    };

    // New state: will there be a precedence violation?
    let new_viol = if partner_route == to_ri {
        // After move, both will be in to_ri.
        // partner_pos is in to_ri; ins_pos is where vertex goes.
        // But partner_pos might shift if partner is in to_ri and ins_pos <= partner_pos.
        let adjusted_partner_pos = if ins_pos <= partner_pos {
            partner_pos + 1
        } else {
            partner_pos
        };
        if is_pickup {
            // vertex (pickup) at ins_pos, partner (delivery) at adjusted_partner_pos.
            f64::from(u8::from(adjusted_partner_pos < ins_pos))
        } else {
            // vertex (delivery) at ins_pos, partner (pickup) at adjusted_partner_pos.
            f64::from(u8::from(ins_pos < adjusted_partner_pos))
        }
    } else if partner_route == from_ri {
        // Partner was in from_ri with vertex; after move, they're in different routes.
        0.0 // no prec violation across routes
    } else {
        0.0 // partner in third route, no change
    };

    new_viol - old_viol
}

/// Update node_route and node_pos for all nodes in a route.
#[inline]
fn update_node_maps(route: &[usize], ri: usize, node_route: &mut [usize], node_pos: &mut [usize]) {
    for (pos, &v) in route.iter().enumerate() {
        if v != 0 {
            node_route[v] = ri;
            node_pos[v] = pos;
        }
    }
}

/// Extract a Solution from the routes (filtering empty ones).
fn extract_solution(routes: &[Vec<usize>]) -> Solution {
    Solution {
        routes: routes.iter().filter(|r| r.len() > 2).cloned().collect(),
        unassigned: Vec::new(),
    }
}