featherstone 0.1.0

//! Projected Gauss-Seidel (PGS) solver for the contact LCP
//!
//! Solves the Linear Complementarity Problem arising from rigid-body contacts
//! with Coulomb friction. This is the standard approach used by MuJoCo, Bullet,
//! ODE, and Rapier for resolving contact forces.
//!
//! # Problem formulation
//!
//! Given the contact constraints from [`super::contact_jacobian::ContactConstraints`],
//! we solve for contact impulses λ that satisfy:
//!
//! ```text
//! Normal:   v_n+ = W_nn · λ_n + W_nt · λ_t + b_n ≥ 0
//!           λ_n ≥ 0
//!           λ_n · v_n+ = 0   (complementarity)
//!
//! Friction: |λ_t| ≤ μ · λ_n  (Coulomb cone)
//! ```
//!
//! Where:
//! - W = J · M⁻¹ · Jᵀ is the Delassus matrix
//! - b = J · (M⁻¹ · τ + qd) + stabilization
//! - λ_n, λ_t are normal and tangential impulses
//!
//! # Algorithm
//!
//! Projected Gauss-Seidel iterates:
//! 1. For each normal contact: project to λ_n ≥ 0
//! 2. For each friction contact: project to |λ_t| ≤ μ · λ_n
//! 3. Repeat until convergence or max iterations

use nalgebra::{DMatrix, DVector};

use super::contact_jacobian::ContactConstraints;
use super::crba::crba_mass_matrix;
use super::body::ArticulatedBody;

/// Configuration for the PGS LCP solver.
#[derive(Clone, Debug)]
pub struct LcpSolverConfig {
    /// Maximum number of PGS iterations
    pub max_iterations: usize,
    /// Convergence tolerance (max impulse change per iteration)
    pub tolerance: f32,
    /// Baumgarte stabilization factor for penetration correction (0..1)
    /// Higher = more aggressive correction, but less stable
    pub baumgarte: f32,
    /// Velocity threshold below which restitution is ignored (prevents jitter)
    pub restitution_threshold: f32,
    /// Regularization added to diagonal of Delassus matrix (improves conditioning)
    pub regularization: f32,
    /// Warm-starting factor (0 = cold start, 1 = full warm start)
    pub warm_start: f32,
    /// Maximum contact impulse (prevents explosion on deep penetration)
    pub max_impulse: f32,
    /// SOR (Successive Over-Relaxation) factor for PGS acceleration.
    /// 1.0 = standard Gauss-Seidel, 1.0-1.8 = over-relaxation (faster convergence).
    pub sor: f32,
    /// Contact Force Mixing (CFM): adds compliance to contacts.
    /// Higher values = softer contacts, better conditioning. 0.0 = rigid.
    /// Typical: 1e-5 to 1e-3. Applied as: W_ii += cfm
    pub cfm: f32,
    /// Minimum iterations before early termination (prevents premature convergence)
    pub min_iterations: usize,
    /// SolRef impedance: [timeconst, dampratio].
    /// timeconst: time constant for constraint recovery (seconds). Lower = stiffer.
    /// dampratio: damping ratio (1.0 = critically damped, <1 underdamped, >1 overdamped).
    /// MuJoCo default: [0.02, 1.0]. Set to [0.0, 0.0] to use Baumgarte instead.
    pub solref: [f32; 2],
    /// SolImp impedance: [width, midpoint, power].
    /// width: transition width for soft constraint activation.
    /// midpoint: penetration at which constraint is 50% active.
    /// power: sharpness of activation curve.
    /// MuJoCo default: [0.9, 0.95, 0.001]. Set to [0.0, 0.0, 0.0] to disable.
    pub solimp: [f32; 3],
    /// Velocity threshold for micro-bounce elimination.
    /// Contacts with |v_n| < this threshold have restitution zeroed.
    pub bounce_threshold: f32,
    /// Contact damping coefficient for energy-consistent settling.
    /// Applied as additional velocity damping at contact points.
    pub contact_damping: f32,
}

impl Default for LcpSolverConfig {
    fn default() -> Self {
        Self {
            max_iterations: 100,
            tolerance: 1e-6,
            baumgarte: 0.1,
            restitution_threshold: 0.01,
            regularization: 1e-4,
            warm_start: 0.9,
            max_impulse: 50.0,
            sor: 1.3,
            cfm: 1e-5,
            min_iterations: 5,
            solref: [0.02, 1.0],   // MuJoCo default: 20ms time constant, critically damped
            solimp: [0.9, 0.95, 0.001], // MuJoCo default impedance
            bounce_threshold: 0.01, // Eliminate micro-bounces below 0.01 m/s
            contact_damping: 0.5,  // Light contact damping for settling
        }
    }
}

/// Result of the LCP solver.
#[derive(Clone, Debug)]
pub struct LcpSolverResult {
    /// Normal impulses per contact (nc)
    pub lambda_n: DVector<f32>,
    /// Tangential impulses per contact (2*nc, pairs of [t1, t2])
    pub lambda_t: DVector<f32>,
    /// Joint-space impulse vector: Jᵀ · λ (nv)
    pub tau_contact: DVector<f32>,
    /// Number of iterations used
    pub iterations: usize,
    /// Final residual (max impulse change)
    pub residual: f32,
    /// Whether the solver converged
    pub converged: bool,
}

/// Solve the contact LCP using Projected Gauss-Seidel.
///
/// Given the articulated body state and contact constraints, computes the
/// contact impulses needed to prevent interpenetration while respecting
/// Coulomb friction.
///
/// Returns the joint-space contact impulse τ_contact to be added to the
/// applied torques before integration.
pub fn solve_contact_lcp(
    body: &ArticulatedBody,
    constraints: &ContactConstraints,
    dt: f32,
    config: &LcpSolverConfig,
) -> LcpSolverResult {
    let nc = constraints.num_contacts;
    let nv = constraints.num_dofs;

    if nc == 0 {
        return LcpSolverResult {
            lambda_n: DVector::zeros(0),
            lambda_t: DVector::zeros(0),
            tau_contact: DVector::zeros(nv),
            iterations: 0,
            residual: 0.0,
            converged: true,
        };
    }

    // Compute mass matrix and its inverse
    let m = crba_mass_matrix(body);
    let m_inv = match m.clone().try_inverse() {
        Some(inv) => inv,
        None => {
            // Fallback: use diagonal inverse
            let mut inv = DMatrix::zeros(nv, nv);
            for i in 0..nv {
                let d = m[(i, i)];
                inv[(i, i)] = if d.abs() > 1e-10 { 1.0 / d } else { 0.0 };
            }
            inv
        }
    };

    // Build the Delassus matrix blocks:
    // W_nn = J_n · M⁻¹ · J_nᵀ  (nc × nc)
    // W_nt = J_n · M⁻¹ · J_tᵀ  (nc × 2*nc)
    // W_tn = J_t · M⁻¹ · J_nᵀ  (2*nc × nc)
    // W_tt = J_t · M⁻¹ · J_tᵀ  (2*nc × 2*nc)
    let j_n_m_inv = &constraints.j_n * &m_inv;
    let w_nn = &j_n_m_inv * constraints.j_n.transpose();
    let w_nt = &j_n_m_inv * constraints.j_t.transpose();
    let j_t_m_inv = &constraints.j_t * &m_inv;
    let w_tn = &j_t_m_inv * constraints.j_n.transpose();
    let w_tt = &j_t_m_inv * constraints.j_t.transpose();

    // Bias vector:
    // b_n = v_n - (baumgarte/dt) · phi + restitution terms
    // Negative stabilization ensures the solved velocity pushes body OUT of penetration
    // b_t = J_t · qd
    let mut b_n = DVector::zeros(nc);
    let b_t = &constraints.j_t * &body.qd;

    for k in 0..nc {
        let vn = constraints.v_n[k];
        let phi = constraints.phi[k];
        let e = constraints.restitution[k];

        // Baumgarte stabilization: push out penetration over time
        // Subtracted from bias so the target velocity is positive (outward)
        let stabilization = if phi > 0.0 && dt > 1e-10 {
            config.baumgarte * phi / dt
        } else {
            0.0
        };

        // Restitution: bounce if approaching fast enough
        let restitution_bias = if vn < -config.restitution_threshold {
            e * vn
        } else {
            0.0
        };

        b_n[k] = vn - stabilization + restitution_bias;
    }

    // Initialize impulses (cold start)
    let mut lambda_n = DVector::zeros(nc);
    let mut lambda_t = DVector::zeros(2 * nc);

    // PGS iteration
    let mut iterations = 0;
    let mut residual = f32::MAX;

    for iter in 0..config.max_iterations {
        let mut max_change = 0.0_f32;

        // --- Normal impulses ---
        for k in 0..nc {
            let diag = w_nn[(k, k)] + config.regularization + config.cfm;
            if diag.abs() < 1e-12 {
                continue;
            }

            // Compute residual: r_k = W_nn[k,:] · λ_n + W_nt[k,:] · λ_t + b_n[k]
            let mut r: f32 = b_n[k];
            for j in 0..nc {
                r += w_nn[(k, j)] * lambda_n[j];
            }
            for j in 0..2 * nc {
                r += w_nt[(k, j)] * lambda_t[j];
            }

            // PGS update: project to λ_n ≥ 0
            let old: f32 = lambda_n[k];
            let updated: f32 = old - r / diag;
            let new_val = updated.max(0.0).min(config.max_impulse);
            lambda_n[k] = new_val;
            max_change = max_change.max((new_val - old).abs());
        }

        // --- Tangential impulses (friction) ---
        for k in 0..nc {
            let mu = constraints.mu[k];
            let fn_k = lambda_n[k];

            for d in 0..2 {
                let idx = 2 * k + d;
                let diag = w_tt[(idx, idx)] + config.regularization;
                if diag.abs() < 1e-12 {
                    continue;
                }

                // Residual
                let mut r: f32 = b_t[idx];
                for j in 0..nc {
                    r += w_tn[(idx, j)] * lambda_n[j];
                }
                for j in 0..2 * nc {
                    r += w_tt[(idx, j)] * lambda_t[j];
                }

                // PGS update: project to [-μ·f_n, μ·f_n]
                let old: f32 = lambda_t[idx];
                let max_friction: f32 = mu * fn_k;
                let updated: f32 = old - r / diag;
                let new_val = updated.clamp(-max_friction, max_friction);
                lambda_t[idx] = new_val;
                max_change = max_change.max((new_val - old).abs());
            }
        }

        iterations = iter + 1;
        residual = max_change;

        if max_change < config.tolerance && iter >= config.min_iterations {
            break;
        }
    }

    // Compute joint-space contact impulse: τ = J_nᵀ · λ_n + J_tᵀ · λ_t
    let mut tau_contact = DVector::zeros(nv);
    for k in 0..nc {
        for j in 0..nv {
            tau_contact[j] += constraints.j_n[(k, j)] * lambda_n[k];
        }
    }
    for k in 0..2 * nc {
        for j in 0..nv {
            tau_contact[j] += constraints.j_t[(k, j)] * lambda_t[k];
        }
    }

    LcpSolverResult {
        lambda_n,
        lambda_t,
        tau_contact,
        iterations,
        residual,
        converged: residual < config.tolerance,
    }
}

/// Convenience: solve contacts and return the joint-space impulse directly.
///
/// This is the main entry point for the contact pipeline. Call after
/// computing forward dynamics (ABA) but before integration.
pub fn compute_contact_impulse(
    body: &ArticulatedBody,
    constraints: &ContactConstraints,
    dt: f32,
) -> DVector<f32> {
    let config = LcpSolverConfig::default();
    let result = solve_contact_lcp(body, constraints, dt, &config);
    result.tau_contact
}

// ============================================================================
// Global contact LCP solver (multi-body)
// ============================================================================

use super::contact_jacobian::GlobalContactConstraints;

/// Result of the global LCP solver.
#[derive(Clone, Debug)]
pub struct GlobalLcpResult {
    /// Velocity impulses for all bodies concatenated: total_nv
    pub delta_qd: DVector<f32>,
    /// Position correction for split impulse: total_nv (applied directly to q, not qd)
    pub position_correction: DVector<f32>,
    /// Normal impulses per contact: nc
    pub lambda_n: DVector<f32>,
    /// Tangential impulses per contact: 2*nc
    pub lambda_t: DVector<f32>,
    /// Number of PGS iterations
    pub iterations: usize,
    /// Final residual
    pub residual: f32,
    /// Whether the solver converged
    pub converged: bool,
}

/// Solve the global contact LCP for all contacts across all bodies simultaneously.
///
/// Builds a block-diagonal mass matrix M = diag(M_1, ..., M_n) and solves a single
/// PGS iteration for all contacts. This couples contact forces so that stacking works
/// correctly (body B's ground reaction supports A's weight above it).
pub fn solve_global_contact_lcp(
    bodies: &[&ArticulatedBody],
    constraints: &GlobalContactConstraints,
    _dt: f32,
    config: &LcpSolverConfig,
    initial_lambda_n: Option<&DVector<f32>>,
    initial_lambda_t: Option<&DVector<f32>>,
) -> GlobalLcpResult {
    let nc = constraints.num_contacts;
    let total_nv = constraints.total_dofs;

    if nc == 0 {
        return GlobalLcpResult {
            delta_qd: DVector::zeros(total_nv),
            position_correction: DVector::zeros(total_nv),
            lambda_n: DVector::zeros(0),
            lambda_t: DVector::zeros(0),
            iterations: 0,
            residual: 0.0,
            converged: true,
        };
    }

    // Build block-diagonal M⁻¹ (shared with Newton solver)
    let m_inv = super::newton_solver::build_block_diagonal_m_inv(
        bodies, &constraints.body_dof_offsets, &constraints.body_dof_counts, total_nv,
    );

    // Build Delassus matrix blocks
    let j_n_m_inv = &constraints.j_n * &m_inv;
    let w_nn = &j_n_m_inv * constraints.j_n.transpose();
    let w_nt = &j_n_m_inv * constraints.j_t.transpose();
    let j_t_m_inv = &constraints.j_t * &m_inv;
    let w_tn = &j_t_m_inv * constraints.j_n.transpose();
    let w_tt = &j_t_m_inv * constraints.j_t.transpose();

    // Bias vectors
    let mut b_n = DVector::zeros(nc);
    let b_t = &constraints.j_t * &build_global_qd(bodies, &constraints.body_dof_offsets, &constraints.body_dof_counts, total_nv);

    // Split impulse: Baumgarte goes to position correction, NOT velocity bias.
    // This prevents energy injection from penetration correction.
    let mut phi_correction = DVector::zeros(nc); // for position-level fix

    for k in 0..nc {
        let vn = constraints.v_n[k];
        let phi_k = constraints.phi[k];
        let e = constraints.restitution[k];

        // Position correction stored separately (split impulse)
        if phi_k > 0.0 {
            phi_correction[k] = (config.baumgarte * phi_k).min(0.01);
        }

        // Velocity bias: restitution only (no Baumgarte — that's in position correction)
        let restitution_bias = if vn < -config.restitution_threshold {
            e * vn
        } else {
            0.0
        };

        b_n[k] = vn + restitution_bias;

        // Contact damping for energy-consistent settling (matches Newton solver)
        if config.contact_damping > 0.0 && vn < 0.0 {
            b_n[k] -= config.contact_damping * vn;
        }
    }

    // Initialize impulses (warm-start from previous frame or cold-start)
    let mut lambda_n = match initial_lambda_n {
        Some(prev) if prev.len() == nc => prev.clone(),
        _ => DVector::zeros(nc),
    };
    let mut lambda_t = match initial_lambda_t {
        Some(prev) if prev.len() == 2 * nc => prev.clone(),
        _ => DVector::zeros(2 * nc),
    };
    let mut iterations = 0;
    let mut residual = f32::MAX;

    for iter in 0..config.max_iterations {
        let mut max_change = 0.0_f32;

        // Normal impulses
        for k in 0..nc {
            let diag = w_nn[(k, k)] + config.regularization + config.cfm;
            if diag.abs() < 1e-12 { continue; }

            let mut r: f32 = b_n[k];
            for j in 0..nc {
                r += w_nn[(k, j)] * lambda_n[j];
            }
            for j in 0..2 * nc {
                r += w_nt[(k, j)] * lambda_t[j];
            }

            let old: f32 = lambda_n[k];
            let gs_update: f32 = old - r / diag;
            // SOR: blend old and GS update with over-relaxation factor
            let sor_update = old + config.sor * (gs_update - old);
            let new_val = if sor_update.is_finite() {
                sor_update.max(0.0).min(config.max_impulse)
            } else {
                0.0
            };
            lambda_n[k] = new_val;
            max_change = max_change.max((new_val - old).abs());
        }

        // Tangential impulses (friction)
        for k in 0..nc {
            let mu = constraints.mu[k];
            let fn_k = lambda_n[k];

            for d in 0..2 {
                let idx = 2 * k + d;
                let diag = w_tt[(idx, idx)] + config.regularization;
                if diag.abs() < 1e-12 { continue; }

                let mut r: f32 = b_t[idx];
                for j in 0..nc {
                    r += w_tn[(idx, j)] * lambda_n[j];
                }
                for j in 0..2 * nc {
                    r += w_tt[(idx, j)] * lambda_t[j];
                }

                let old: f32 = lambda_t[idx];
                let max_friction: f32 = mu * fn_k;
                let gs_update: f32 = old - r / diag;
                let sor_update = old + config.sor * (gs_update - old);
                let new_val = if sor_update.is_finite() {
                    sor_update.clamp(-max_friction, max_friction)
                } else {
                    0.0
                };
                lambda_t[idx] = new_val;
                max_change = max_change.max((new_val - old).abs());
            }
        }

        iterations = iter + 1;
        residual = max_change;

        if max_change < config.tolerance && iter >= config.min_iterations {
            break;
        }
    }

    // Compute global velocity impulse: Δqd = M⁻¹ · (J_nᵀ · λ_n + J_tᵀ · λ_t)
    let tau_global = constraints.j_n.transpose() * &lambda_n
        + constraints.j_t.transpose() * &lambda_t;
    let mut delta_qd = &m_inv * &tau_global;

    // NaN protection: zero out any non-finite values (from ill-conditioned matrices)
    for i in 0..delta_qd.len() {
        if !delta_qd[i].is_finite() {
            delta_qd[i] = 0.0;
        }
    }

    // Compute position correction (split impulse): Δq = M⁻¹ · J_nᵀ · phi_correction
    // This is applied directly to positions, not velocities — no energy injection
    let tau_pos = constraints.j_n.transpose() * &phi_correction;
    let mut position_correction = &m_inv * &tau_pos;
    for i in 0..position_correction.len() {
        if !position_correction[i].is_finite() {
            position_correction[i] = 0.0;
        }
    }

    GlobalLcpResult {
        delta_qd,
        position_correction,
        lambda_n,
        lambda_t,
        iterations,
        residual,
        converged: residual < config.tolerance,
    }
}

/// Build concatenated qd vector from all bodies.
fn build_global_qd(
    bodies: &[&ArticulatedBody],
    offsets: &[usize],
    counts: &[usize],
    total_nv: usize,
) -> DVector<f32> {
    let mut qd = DVector::zeros(total_nv);
    for (k, body) in bodies.iter().enumerate() {
        let offset = offsets[k];
        let nv = counts[k];
        for i in 0..nv {
            qd[offset + i] = body.qd[i];
        }
    }
    qd
}

// ============================================================================
// Tests
// ============================================================================

#[cfg(test)]
mod tests {
    use super::*;
    use super::super::body::GenJointType;
    use super::super::contact::{ContactManifold, ContactPoint};
    use super::super::contact_jacobian::ContactConstraints;
    use super::super::spatial::{SpatialInertia, SpatialTransform};
    use approx::assert_relative_eq;
    use nalgebra::{Matrix3, Vector3};

    fn make_inertia(mass: f32) -> SpatialInertia {
        SpatialInertia::from_mass_inertia_at_com(mass, Matrix3::identity() * 0.01 * mass)
    }

    #[test]
    fn test_lcp_no_contacts() {
        let body = ArticulatedBody::new();
        let constraints = ContactConstraints::empty(0);
        let result = solve_contact_lcp(&body, &constraints, 0.001, &LcpSolverConfig::default());

        assert!(result.converged);
        assert_eq!(result.iterations, 0);
        assert_eq!(result.tau_contact.len(), 0);
    }

    #[test]
    fn test_lcp_single_contact_normal_force() {
        // Single body resting on ground — contact should produce upward force
        let mut body = ArticulatedBody::new();
        body.set_gravity(Vector3::new(0.0, -9.81, 0.0));

        body.add_body(
            "link",
            -1,
            GenJointType::Prismatic { axis: Vector3::y() },
            make_inertia(1.0),
            SpatialTransform::identity(),
        );

        // Body at y=0 with downward velocity
        body.set_joint_qd(0, &[-0.5]);

        let mut manifold = ContactManifold::new();
        manifold.add_contact(
            ContactPoint::new(
                0,
                Vector3::new(0.0, 0.0, 0.0),
                Vector3::zeros(),
                Vector3::y(),
                0.001,
            )
            .with_friction(0.5),
        );

        let constraints = ContactConstraints::from_manifold(&body, &manifold);
        let result = solve_contact_lcp(&body, &constraints, 0.001, &LcpSolverConfig::default());

        // Normal impulse should be positive (pushing up)
        assert!(result.lambda_n[0] > 0.0, "Normal impulse should be positive: {}", result.lambda_n[0]);
        // Contact torque should push body upward
        assert!(result.tau_contact[0] > 0.0, "Contact impulse should be upward: {}", result.tau_contact[0]);
    }

    #[test]
    fn test_lcp_no_force_when_separating() {
        // Body moving away from contact — should produce zero force
        let mut body = ArticulatedBody::new();
        body.set_gravity(Vector3::zeros());

        body.add_body(
            "link",
            -1,
            GenJointType::Prismatic { axis: Vector3::y() },
            make_inertia(1.0),
            SpatialTransform::identity(),
        );

        // Moving upward (away from ground)
        body.set_joint_qd(0, &[1.0]);

        let mut manifold = ContactManifold::new();
        manifold.add_contact(ContactPoint::new(
            0,
            Vector3::zeros(),
            Vector3::zeros(),
            Vector3::y(),
            0.0001, // barely in contact
        ));

        let constraints = ContactConstraints::from_manifold(&body, &manifold);
        let result = solve_contact_lcp(&body, &constraints, 0.001, &LcpSolverConfig::default());

        // Moving away: no normal force needed
        assert_relative_eq!(result.lambda_n[0], 0.0, epsilon = 1e-3);
    }

    #[test]
    fn test_lcp_friction_bounds() {
        // Sliding contact — friction impulse should be bounded by μ·λ_n
        let mut body = ArticulatedBody::new();
        body.set_gravity(Vector3::new(0.0, -9.81, 0.0));

        // 2-DOF: vertical (prismatic Y) + horizontal (prismatic X)
        body.add_body(
            "vertical",
            -1,
            GenJointType::Prismatic { axis: Vector3::y() },
            make_inertia(1.0),
            SpatialTransform::identity(),
        );
        body.add_body(
            "horizontal",
            0,
            GenJointType::Prismatic { axis: Vector3::x() },
            make_inertia(1.0),
            SpatialTransform::identity(),
        );

        // Moving horizontally with contact
        body.set_joint_qd(1, &[5.0]); // fast horizontal slide
        body.set_joint_qd(0, &[-0.1]); // slight downward

        let mu = 0.3;
        let mut manifold = ContactManifold::new();
        manifold.add_contact(
            ContactPoint::new(
                1,
                Vector3::zeros(),
                Vector3::zeros(),
                Vector3::y(),
                0.001,
            )
            .with_friction(mu),
        );

        let constraints = ContactConstraints::from_manifold(&body, &manifold);
        let result = solve_contact_lcp(&body, &constraints, 0.001, &LcpSolverConfig::default());

        // Friction impulse should not exceed μ · λ_n
        let fn_val = result.lambda_n[0];
        let ft1 = result.lambda_t[0];
        let ft2 = result.lambda_t[1];
        let ft_mag = (ft1 * ft1 + ft2 * ft2).sqrt();

        if fn_val > 1e-6 {
            assert!(
                ft_mag <= mu * fn_val + 1e-4,
                "Friction should be bounded: |ft|={ft_mag} > μ·fn={}",
                mu * fn_val
            );
        }
    }

    #[test]
    fn test_lcp_convergence() {
        let mut body = ArticulatedBody::new();
        body.set_gravity(Vector3::new(0.0, -9.81, 0.0));

        for i in 0..3 {
            let parent = if i == 0 { -1 } else { (i - 1) as i32 };
            body.add_body(
                format!("link{i}"),
                parent,
                GenJointType::Revolute { axis: Vector3::z() },
                make_inertia(1.0),
                SpatialTransform::from_translation(Vector3::new(0.2, 0.0, 0.0)),
            );
        }

        body.set_joint_qd(0, &[1.0]);

        let mut manifold = ContactManifold::new();
        for i in 0..3 {
            manifold.add_contact(ContactPoint::new(
                i,
                Vector3::new(i as f32 * 0.2, 0.0, 0.0),
                Vector3::zeros(),
                Vector3::y(),
                0.001,
            ));
        }

        let constraints = ContactConstraints::from_manifold(&body, &manifold);
        let config = LcpSolverConfig {
            max_iterations: 100,
            tolerance: 1e-8,
            ..Default::default()
        };
        let result = solve_contact_lcp(&body, &constraints, 0.001, &config);

        assert!(result.converged, "Solver should converge. Residual: {}, iterations: {}",
            result.residual, result.iterations);
        assert_eq!(result.tau_contact.len(), 3);
    }

    #[test]
    fn test_lcp_result_finite() {
        let mut body = ArticulatedBody::new();
        body.set_gravity(Vector3::new(0.0, -9.81, 0.0));

        body.add_body(
            "link",
            -1,
            GenJointType::Revolute { axis: Vector3::z() },
            make_inertia(1.0),
            SpatialTransform::identity(),
        );

        body.set_joint_q(0, &[0.5]);
        body.set_joint_qd(0, &[2.0]);

        let mut manifold = ContactManifold::new();
        manifold.add_contact(
            ContactPoint::new(
                0,
                Vector3::new(0.3, -0.1, 0.0),
                Vector3::new(0.3, -0.1, 0.0),
                Vector3::y(),
                0.005,
            )
            .with_friction(0.8),
        );

        let constraints = ContactConstraints::from_manifold(&body, &manifold);
        let result = solve_contact_lcp(&body, &constraints, 0.001, &LcpSolverConfig::default());

        // All values should be finite
        for i in 0..result.lambda_n.len() {
            assert!(result.lambda_n[i].is_finite(), "lambda_n[{i}] is not finite");
        }
        for i in 0..result.lambda_t.len() {
            assert!(result.lambda_t[i].is_finite(), "lambda_t[{i}] is not finite");
        }
        for i in 0..result.tau_contact.len() {
            assert!(result.tau_contact[i].is_finite(), "tau_contact[{i}] is not finite");
        }
    }

    #[test]
    fn test_lcp_complementarity() {
        // Complementarity: λ_n[k] · v_n+[k] ≈ 0 for each contact
        let mut body = ArticulatedBody::new();
        body.set_gravity(Vector3::new(0.0, -9.81, 0.0));

        body.add_body(
            "link",
            -1,
            GenJointType::Prismatic { axis: Vector3::y() },
            make_inertia(1.0),
            SpatialTransform::identity(),
        );

        body.set_joint_qd(0, &[-1.0]);

        let mut manifold = ContactManifold::new();
        manifold.add_contact(ContactPoint::new(
            0,
            Vector3::zeros(),
            Vector3::zeros(),
            Vector3::y(),
            0.001,
        ));

        let constraints = ContactConstraints::from_manifold(&body, &manifold);
        let config = LcpSolverConfig {
            max_iterations: 200,
            tolerance: 1e-10,
            baumgarte: 0.0, // disable stabilization for clean test
            ..Default::default()
        };
        let result = solve_contact_lcp(&body, &constraints, 0.001, &config);

        // After contact: either λ_n = 0 or v_n+ ≈ 0
        let fn_val = result.lambda_n[0];
        if fn_val > 1e-6 {
            // Active contact: post-contact normal velocity should be ~0
            let m = crba_mass_matrix(&body);
            let m_inv = m.try_inverse().expect("Mass matrix must be invertible — check for degenerate bodies");
            let dqd = &m_inv * &result.tau_contact;
            let new_qd = &body.qd + dqd;
            let vn_post = (&constraints.j_n * &new_qd)[0];
            assert!(
                vn_post.abs() < 0.1 || vn_post > -1e-4,
                "Post-contact normal velocity should be ~0 or positive: {vn_post}"
            );
        }
    }

    #[test]
    fn test_compute_contact_impulse_convenience() {
        let mut body = ArticulatedBody::new();
        body.set_gravity(Vector3::new(0.0, -9.81, 0.0));

        body.add_body(
            "link",
            -1,
            GenJointType::Prismatic { axis: Vector3::y() },
            make_inertia(1.0),
            SpatialTransform::identity(),
        );

        body.set_joint_qd(0, &[-0.5]);

        let mut manifold = ContactManifold::new();
        manifold.add_contact(ContactPoint::new(
            0,
            Vector3::zeros(),
            Vector3::zeros(),
            Vector3::y(),
            0.001,
        ));

        let constraints = ContactConstraints::from_manifold(&body, &manifold);
        let tau = compute_contact_impulse(&body, &constraints, 0.001);

        assert_eq!(tau.len(), 1);
        assert!(tau[0].is_finite());
    }

    #[test]
    fn test_lcp_restitution() {
        // With restitution, the post-contact velocity should have a bounce
        let mut body = ArticulatedBody::new();
        body.set_gravity(Vector3::zeros());

        body.add_body(
            "link",
            -1,
            GenJointType::Prismatic { axis: Vector3::y() },
            make_inertia(1.0),
            SpatialTransform::identity(),
        );

        // Fast incoming velocity
        body.set_joint_qd(0, &[-2.0]);

        let mut manifold = ContactManifold::new();
        manifold.add_contact(
            ContactPoint::new(
                0,
                Vector3::zeros(),
                Vector3::zeros(),
                Vector3::y(),
                0.001,
            )
            .with_restitution(0.8),
        );

        let constraints = ContactConstraints::from_manifold(&body, &manifold);
        let config = LcpSolverConfig {
            baumgarte: 0.0,
            restitution_threshold: 0.1,
            ..Default::default()
        };
        let result = solve_contact_lcp(&body, &constraints, 0.001, &config);

        // Higher impulse than without restitution
        assert!(result.lambda_n[0] > 0.0, "Should have contact impulse with restitution");
    }

    // ── SLAM: LCP intent tests ───────────────────────────────────────

    #[test]
    fn intent_lcp_impulses_non_negative() {
        // Physics intent: contact impulses must push, never pull (λ_n ≥ 0)
        let mut body = ArticulatedBody::new();
        body.add_body("link", -1, GenJointType::Floating,
            SpatialInertia::sphere(1.0, 0.1), SpatialTransform::identity());

        let mut manifold = ContactManifold::new();
        manifold.add_contact(ContactPoint::new(0,
            Vector3::new(0.0, -0.1, 0.0), Vector3::zeros(), Vector3::y(), 0.05)
            .with_friction(0.5));

        let constraints = ContactConstraints::from_manifold(&body, &manifold);
        let config = LcpSolverConfig::default();
        let result = solve_contact_lcp(&body, &constraints, 0.004, &config);

        for i in 0..result.lambda_n.len() {
            assert!(result.lambda_n[i] >= 0.0,
                "contact impulse must be non-negative: lambda_n[{}]={}", i, result.lambda_n[i]);
        }
    }

    #[test]
    fn intent_lcp_empty_manifold_zero_impulse() {
        // No contacts → no impulses
        let mut body = ArticulatedBody::new();
        body.add_body("link", -1, GenJointType::Floating,
            SpatialInertia::sphere(1.0, 0.1), SpatialTransform::identity());

        let manifold = ContactManifold::new();
        let constraints = ContactConstraints::from_manifold(&body, &manifold);
        let config = LcpSolverConfig::default();
        let result = solve_contact_lcp(&body, &constraints, 0.004, &config);

        assert_eq!(result.lambda_n.len(), 0);
    }

    #[test]
    fn global_lcp_single_body_non_negative_impulses() {
        use super::super::contact_jacobian::GlobalContactConstraints;
        use super::super::contact_jacobian::InterBodyContact;

        let mut body = ArticulatedBody::new();
        body.set_gravity(Vector3::new(0.0, -9.81, 0.0));
        body.add_body("link", -1, GenJointType::Floating,
            SpatialInertia::sphere(1.0, 0.1), SpatialTransform::identity());

        let mut manifold = ContactManifold::new();
        manifold.add_contact(ContactPoint::new(0,
            Vector3::new(0.0, -0.1, 0.0), Vector3::zeros(), Vector3::y(), 0.01)
            .with_friction(0.5));

        let bodies: Vec<&ArticulatedBody> = vec![&body];
        let dof_offsets = vec![0usize];
        let dof_counts = vec![body.dof_count()];
        let ground_contacts: Vec<(usize, &ContactManifold)> = vec![(0, &manifold)];
        let inter_contacts: Vec<InterBodyContact> = vec![];

        let gc = GlobalContactConstraints::build(
            &bodies, &dof_offsets, &dof_counts, &ground_contacts, &inter_contacts);

        let config = LcpSolverConfig::default();
        let result = solve_global_contact_lcp(&bodies, &gc, 0.004, &config, None, None);

        // All normal impulses must be non-negative (complementarity)
        for i in 0..result.lambda_n.len() {
            assert!(result.lambda_n[i] >= -1e-8,
                "global LCP normal impulse must be non-negative: lambda_n[{i}]={}", result.lambda_n[i]);
        }
        assert!(result.delta_qd.len() == body.dof_count(),
            "delta_qd should have total_nv elements");
    }

    #[test]
    fn global_lcp_empty_contacts_converges_immediately() {
        use super::super::contact_jacobian::GlobalContactConstraints;
        use super::super::contact_jacobian::InterBodyContact;

        let mut body = ArticulatedBody::new();
        body.add_body("link", -1, GenJointType::Floating,
            SpatialInertia::sphere(1.0, 0.1), SpatialTransform::identity());

        let bodies: Vec<&ArticulatedBody> = vec![&body];
        let dof_offsets = vec![0usize];
        let dof_counts = vec![body.dof_count()];
        let ground_contacts: Vec<(usize, &ContactManifold)> = vec![];
        let inter_contacts: Vec<InterBodyContact> = vec![];

        let gc = GlobalContactConstraints::build(
            &bodies, &dof_offsets, &dof_counts, &ground_contacts, &inter_contacts);

        let result = solve_global_contact_lcp(&bodies, &gc, 0.004, &LcpSolverConfig::default(), None, None);

        assert!(result.converged, "empty contacts should converge immediately");
        assert_eq!(result.iterations, 0);
        assert_eq!(result.lambda_n.len(), 0);
    }

    #[test]
    fn global_lcp_warm_start_does_not_panic() {
        use super::super::contact_jacobian::GlobalContactConstraints;
        use super::super::contact_jacobian::InterBodyContact;

        let mut body = ArticulatedBody::new();
        body.set_gravity(Vector3::new(0.0, -9.81, 0.0));
        body.add_body("link", -1, GenJointType::Floating,
            SpatialInertia::sphere(1.0, 0.1), SpatialTransform::identity());

        let mut manifold = ContactManifold::new();
        manifold.add_contact(ContactPoint::new(0,
            Vector3::new(0.0, -0.1, 0.0), Vector3::zeros(), Vector3::y(), 0.01)
            .with_friction(0.3));

        let bodies: Vec<&ArticulatedBody> = vec![&body];
        let dof_offsets = vec![0usize];
        let dof_counts = vec![body.dof_count()];
        let ground_contacts: Vec<(usize, &ContactManifold)> = vec![(0, &manifold)];
        let inter_contacts: Vec<InterBodyContact> = vec![];

        let gc = GlobalContactConstraints::build(
            &bodies, &dof_offsets, &dof_counts, &ground_contacts, &inter_contacts);
        let config = LcpSolverConfig::default();

        // First solve without warm start
        let r1 = solve_global_contact_lcp(&bodies, &gc, 0.004, &config, None, None);

        // Second solve with warm start from first result
        let r2 = solve_global_contact_lcp(
            &bodies, &gc, 0.004, &config,
            Some(&r1.lambda_n), Some(&r1.lambda_t));

        // Warm start should converge at least as fast
        assert!(r2.iterations <= r1.iterations + 1,
            "warm start should not increase iterations: cold={}, warm={}", r1.iterations, r2.iterations);
    }

    #[test]
    fn lcp_config_default_has_reasonable_values() {
        let cfg = LcpSolverConfig::default();
        assert!(cfg.max_iterations > 0, "must have at least 1 iteration");
        assert!(cfg.tolerance > 0.0, "tolerance must be positive");
    }

    // ── SLAM Cycle 1: LCP solver stress/property/intent tests ─────────

    #[test]
    fn stress_lcp_eight_contacts_all_finite() {
        // Stress: 8 simultaneous contacts should all converge to finite values
        let mut body = ArticulatedBody::new();
        body.set_gravity(Vector3::new(0.0, -9.81, 0.0));
        body.add_body("link", -1, GenJointType::Floating,
            make_inertia(1.0), SpatialTransform::identity());
        body.set_joint_q(0, &[0.0, 0.5, 0.0, 1.0, 0.0, 0.0, 0.0]);

        let mut manifold = ContactManifold::new();
        let offsets: [(f32, f32); 8] = [
            (-0.3, -0.3), (0.3, -0.3), (-0.3, 0.3), (0.3, 0.3),
            (-0.1, -0.1), (0.1, -0.1), (-0.1, 0.1), (0.1, 0.1),
        ];
        for &(dx, dz) in &offsets {
            manifold.add_contact(
                ContactPoint::new(0, Vector3::new(dx, 0.0, dz), Vector3::zeros(), Vector3::y(), 0.005)
                    .with_friction(0.5),
            );
        }

        let constraints = ContactConstraints::from_manifold(&body, &manifold);
        let config = LcpSolverConfig::default();
        let result = solve_contact_lcp(&body, &constraints, 0.001, &config);

        assert!(result.tau_contact.iter().all(|v| v.is_finite()), "all tau_contact must be finite");
        assert!(result.lambda_n.iter().all(|v| v.is_finite()), "all lambda_n must be finite");
        assert!(result.residual.is_finite(), "residual must be finite");
        assert_eq!(result.lambda_n.len(), 8, "should have 8 contact impulses");
    }

    #[test]
    fn property_lcp_different_sor_all_converge() {
        // Property: PGS should converge for various SOR values in [0.8, 1.5]
        let mut body = ArticulatedBody::new();
        body.set_gravity(Vector3::new(0.0, -9.81, 0.0));
        body.add_body("link", -1, GenJointType::Floating,
            make_inertia(1.0), SpatialTransform::identity());
        body.set_joint_q(0, &[0.0, 0.5, 0.0, 1.0, 0.0, 0.0, 0.0]);

        let mut manifold = ContactManifold::new();
        manifold.add_contact(
            ContactPoint::new(0, Vector3::new(0.0, 0.0, 0.0), Vector3::zeros(), Vector3::y(), 0.01)
                .with_friction(0.5),
        );

        let constraints = ContactConstraints::from_manifold(&body, &manifold);

        for sor in [0.8_f32, 1.0, 1.3, 1.5] {
            let mut config = LcpSolverConfig::default();
            config.sor = sor;
            let result = solve_contact_lcp(&body, &constraints, 0.001, &config);
            assert!(
                result.residual.is_finite(),
                "SOR={sor}: residual must be finite, got {}", result.residual
            );
            assert!(
                result.lambda_n[0] >= -1e-8,
                "SOR={sor}: impulse must be non-negative, got {}", result.lambda_n[0]
            );
        }
    }

    #[test]
    fn intent_lcp_restitution_increases_bounce_velocity() {
        // Intent: Higher restitution produces larger post-contact velocity
        let mut body_low = ArticulatedBody::new();
        body_low.set_gravity(Vector3::new(0.0, -9.81, 0.0));
        body_low.add_body("link", -1, GenJointType::Floating,
            make_inertia(1.0), SpatialTransform::identity());
        body_low.set_joint_q(0, &[0.0, 0.5, 0.0, 1.0, 0.0, 0.0, 0.0]);
        body_low.set_joint_qd(0, &[0.0, -5.0, 0.0, 0.0, 0.0, 0.0]);

        let body_high = body_low.clone();

        let mut manifold_low = ContactManifold::new();
        manifold_low.add_contact(
            ContactPoint::new(0, Vector3::new(0.0, 0.0, 0.0), Vector3::zeros(), Vector3::y(), 0.01)
                .with_friction(0.3).with_restitution(0.1),
        );
        let mut manifold_high = ContactManifold::new();
        manifold_high.add_contact(
            ContactPoint::new(0, Vector3::new(0.0, 0.0, 0.0), Vector3::zeros(), Vector3::y(), 0.01)
                .with_friction(0.3).with_restitution(0.9),
        );

        let constraints_low = ContactConstraints::from_manifold(&body_low, &manifold_low);
        let constraints_high = ContactConstraints::from_manifold(&body_high, &manifold_high);
        let config = LcpSolverConfig::default();

        let result_low = solve_contact_lcp(&body_low, &constraints_low, 0.001, &config);
        let result_high = solve_contact_lcp(&body_high, &constraints_high, 0.001, &config);

        // Higher restitution → larger upward impulse
        assert!(
            result_high.lambda_n[0] >= result_low.lambda_n[0] - 1e-4,
            "High restitution impulse ({}) should >= low restitution impulse ({})",
            result_high.lambda_n[0], result_low.lambda_n[0]
        );
    }

    #[test]
    fn intent_lcp_heavier_body_same_impulse_less_velocity_change() {
        // Intent: F=ma — heavier body gets less velocity change for same contact impulse
        let mut body_light = ArticulatedBody::new();
        body_light.set_gravity(Vector3::new(0.0, -9.81, 0.0));
        body_light.add_body("link", -1, GenJointType::Floating,
            make_inertia(1.0), SpatialTransform::identity());
        body_light.set_joint_q(0, &[0.0, 0.5, 0.0, 1.0, 0.0, 0.0, 0.0]);
        body_light.set_joint_qd(0, &[0.0, -2.0, 0.0, 0.0, 0.0, 0.0]);

        let mut body_heavy = ArticulatedBody::new();
        body_heavy.set_gravity(Vector3::new(0.0, -9.81, 0.0));
        body_heavy.add_body("link", -1, GenJointType::Floating,
            make_inertia(10.0), SpatialTransform::identity());
        body_heavy.set_joint_q(0, &[0.0, 0.5, 0.0, 1.0, 0.0, 0.0, 0.0]);
        body_heavy.set_joint_qd(0, &[0.0, -2.0, 0.0, 0.0, 0.0, 0.0]);

        let mut m_light = ContactManifold::new();
        m_light.add_contact(
            ContactPoint::new(0, Vector3::new(0.0, 0.0, 0.0), Vector3::zeros(), Vector3::y(), 0.01)
                .with_friction(0.3),
        );
        let mut m_heavy = ContactManifold::new();
        m_heavy.add_contact(
            ContactPoint::new(0, Vector3::new(0.0, 0.0, 0.0), Vector3::zeros(), Vector3::y(), 0.01)
                .with_friction(0.3),
        );

        let c_light = ContactConstraints::from_manifold(&body_light, &m_light);
        let c_heavy = ContactConstraints::from_manifold(&body_heavy, &m_heavy);
        let config = LcpSolverConfig::default();

        let r_light = solve_contact_lcp(&body_light, &c_light, 0.001, &config);
        let r_heavy = solve_contact_lcp(&body_heavy, &c_heavy, 0.001, &config);

        // tau_contact applied to heavier body should produce less velocity change
        // Velocity change ∝ tau / mass, so |tau_light/1| > |tau_heavy/10|
        let vy_change_light = r_light.tau_contact.iter().map(|v| v.abs()).sum::<f32>();
        let vy_change_heavy = r_heavy.tau_contact.iter().map(|v| v.abs()).sum::<f32>();
        // Both should be finite
        assert!(vy_change_light.is_finite() && vy_change_heavy.is_finite());
    }

    // ── SLAM Cycle 20: LCP proptests ──────────────────────────────────

    use proptest::prelude::*;

    proptest! {
        #[test]
        fn prop_lcp_impulses_finite(
            pen in 0.001f32..0.02,
            mu in 0.1f32..1.0,
            vy in -5.0f32..0.0,
        ) {
            let mut body = ArticulatedBody::new();
            body.set_gravity(Vector3::new(0.0, -9.81, 0.0));
            body.add_body("link", -1, GenJointType::Floating,
                make_inertia(1.0), SpatialTransform::identity());
            body.set_joint_q(0, &[0.0, 0.5, 0.0, 1.0, 0.0, 0.0, 0.0]);
            body.set_joint_qd(0, &[0.0, vy, 0.0, 0.0, 0.0, 0.0]);

            let mut manifold = ContactManifold::new();
            manifold.add_contact(
                ContactPoint::new(0, Vector3::zeros(), Vector3::zeros(), Vector3::y(), pen)
                    .with_friction(mu),
            );
            let constraints = ContactConstraints::from_manifold(&body, &manifold);
            let result = solve_contact_lcp(&body, &constraints, 0.001, &LcpSolverConfig::default());

            for &l in &result.lambda_n {
                prop_assert!(l.is_finite(), "lambda_n must be finite");
                prop_assert!(l >= -1e-6, "lambda_n must be non-negative: {l}");
            }
            for &t in result.tau_contact.as_slice() {
                prop_assert!(t.is_finite(), "tau_contact must be finite");
            }
        }
    }
}