featherstone 0.1.0

//! Newton contact solver — quadratic convergence for MuJoCo-grade stacking
//!
//! Implements a projected Newton method for solving the contact LCP.
//! Unlike PGS (which converges linearly), Newton's method converges
//! quadratically — typically 3-10 iterations vs 100+ for PGS on stacking.
//!
//! # Algorithm
//!
//! 1. Evaluate constraint violations C(q) = phi (penetration depths)
//! 2. Compute constraint Jacobian nabla-C = J_n (contact normal Jacobian)
//! 3. Form KKT system: [H J^T; J 0] [dq; lambda] = [-g; -c]
//! 4. Solve via Schur complement: (J H^{-1} J^T) lambda = J H^{-1} g - c
//! 5. Project lambda >= 0 (complementarity)
//! 6. Line search to ensure sufficient decrease
//! 7. Repeat until convergence
//!
//! The Schur complement (J H^{-1} J^T) is exactly the Delassus matrix W
//! from the PGS solver — so Newton reuses the same matrix structure.

use nalgebra::{DMatrix, DVector};

use super::body::ArticulatedBody;
use super::contact::ContactManifold;
use super::contact_jacobian::{GlobalContactConstraints, InterBodyContact};
use super::crba::crba_mass_matrix;
// use super::kinematics::forward_kinematics; // available for future use
use super::lcp_solver::LcpSolverConfig;

/// Constraint evaluation result: C(q) and nabla-C.
#[derive(Clone, Debug)]
pub struct ConstraintEval {
    /// Constraint violations: penetration depths (nc)
    /// Positive = penetrating, negative = separated
    pub c: DVector<f32>,
    /// Constraint Jacobian: dc/dq (nc × total_nv)
    pub jacobian: DMatrix<f32>,
    /// Friction Jacobian: J_t * qd (2*nc × total_nv)
    pub friction_jacobian: DMatrix<f32>,
    /// Normal velocities at contacts (nc)
    pub v_n: DVector<f32>,
    /// Per-contact friction coefficients (nc)
    pub mu: DVector<f32>,
    /// Per-contact restitution coefficients (nc)
    pub restitution: DVector<f32>,
    /// Number of contacts
    pub num_contacts: usize,
    /// Total DOFs
    pub total_dofs: usize,
}

/// Constraint function for Newton's method.
///
/// Wraps `GlobalContactConstraints` to provide a differentiable
/// constraint function C(q) with gradient nabla-C = dC/dq.
pub struct ConstraintFunction;

impl ConstraintFunction {
    /// Evaluate constraint violations C(q) = penetration depths.
    pub fn evaluate(
        bodies: &[&ArticulatedBody],
        dof_offsets: &[usize],
        dof_counts: &[usize],
        ground_contacts: &[(usize, &ContactManifold)],
        inter_contacts: &[InterBodyContact],
    ) -> DVector<f32> {
        let gc = GlobalContactConstraints::build(
            bodies, dof_offsets, dof_counts, ground_contacts, inter_contacts,
        );
        gc.phi.clone()
    }

    /// Evaluate constraint Jacobian nabla-C = J_n.
    pub fn gradient(
        bodies: &[&ArticulatedBody],
        dof_offsets: &[usize],
        dof_counts: &[usize],
        ground_contacts: &[(usize, &ContactManifold)],
        inter_contacts: &[InterBodyContact],
    ) -> DMatrix<f32> {
        let gc = GlobalContactConstraints::build(
            bodies, dof_offsets, dof_counts, ground_contacts, inter_contacts,
        );
        gc.j_n.clone()
    }

    /// Evaluate both C(q) and nabla-C in a single pass (avoids double FK computation).
    pub fn evaluate_with_gradient(
        bodies: &[&ArticulatedBody],
        dof_offsets: &[usize],
        dof_counts: &[usize],
        ground_contacts: &[(usize, &ContactManifold)],
        inter_contacts: &[InterBodyContact],
    ) -> ConstraintEval {
        let gc = GlobalContactConstraints::build(
            bodies, dof_offsets, dof_counts, ground_contacts, inter_contacts,
        );
        ConstraintEval {
            c: gc.phi.clone(),
            jacobian: gc.j_n.clone(),
            friction_jacobian: gc.j_t.clone(),
            v_n: gc.v_n.clone(),
            mu: gc.mu.clone(),
            restitution: gc.restitution.clone(),
            num_contacts: gc.num_contacts,
            total_dofs: gc.total_dofs,
        }
    }
}

/// Build block-diagonal inverse mass matrix for all bodies.
pub fn build_block_diagonal_m_inv(
    bodies: &[&ArticulatedBody],
    dof_offsets: &[usize],
    dof_counts: &[usize],
    total_nv: usize,
) -> DMatrix<f32> {
    let mut m_inv = DMatrix::zeros(total_nv, total_nv);
    for (k, body) in bodies.iter().enumerate() {
        let offset = dof_offsets[k];
        let nv_k = dof_counts[k];
        if nv_k == 0 { continue; }

        let m_k = crba_mass_matrix(body);
        let inv_k = match m_k.clone().try_inverse() {
            Some(inv) => inv,
            None => {
                let mut inv = DMatrix::zeros(nv_k, nv_k);
                for i in 0..nv_k {
                    let d = m_k[(i, i)];
                    inv[(i, i)] = if d.abs() > 1e-10 { 1.0 / d } else { 0.0 };
                }
                inv
            }
        };
        m_inv.view_mut((offset, offset), (nv_k, nv_k)).copy_from(&inv_k);
    }
    m_inv
}

/// Newton solver result.
#[derive(Clone, Debug)]
pub struct NewtonResult {
    /// Velocity impulses for all bodies: total_nv
    pub delta_qd: DVector<f32>,
    /// Position correction: total_nv
    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 Newton iterations
    pub iterations: usize,
    /// Final constraint violation norm
    pub residual: f32,
    /// Whether the solver converged
    pub converged: bool,
}

/// Solve contacts using projected Newton's method.
///
/// Forms the Schur complement system W * lambda = rhs where W = J * M^{-1} * J^T,
/// then solves with projected Newton iteration (ensuring lambda >= 0).
///
/// Returns converged=false if Newton does not converge within max iterations.
pub fn solve_newton_contact(
    bodies: &[&ArticulatedBody],
    constraint_eval: &ConstraintEval,
    m_inv: &DMatrix<f32>,
    config: &LcpSolverConfig,
) -> NewtonResult {
    let nc = constraint_eval.num_contacts;
    let total_nv = constraint_eval.total_dofs;

    if nc == 0 {
        return NewtonResult {
            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,
        };
    }

    let j_n = &constraint_eval.jacobian;
    let j_t = &constraint_eval.friction_jacobian;

    // Delassus matrix: W = J_n * M^{-1} * J_n^T (compute in f64 for precision)
    let j_n_m_inv = j_n * m_inv;
    let w = &j_n_m_inv * j_n.transpose();

    // Cast to f64 for Cholesky factorization precision
    let mut w_reg_f64 = nalgebra::DMatrix::<f64>::zeros(nc, nc);
    for r in 0..nc {
        for c in 0..nc {
            w_reg_f64[(r, c)] = w[(r, c)] as f64;
        }
    }
    for k in 0..nc {
        w_reg_f64[(k, k)] += (config.regularization + config.cfm) as f64;
    }

    // Keep f32 copy for residual computation
    let mut w_reg = w.clone();
    for k in 0..nc {
        w_reg[(k, k)] += config.regularization + config.cfm;
    }

    // Right-hand side using solref/solimp impedance model
    let mut b = constraint_eval.v_n.clone();
    let [timeconst, dampratio] = config.solref;
    let [imp_width, imp_midpoint, _imp_power] = config.solimp;
    // Validate parameters (clamp to safe ranges)
    let timeconst = timeconst.max(1e-6);
    let dampratio = dampratio.max(0.0);
    let imp_width = imp_width.max(1e-6);

    for k in 0..nc {
        let phi = constraint_eval.c[k];
        let vn = constraint_eval.v_n[k];

        // SolRef impedance stabilization (replaces Baumgarte)
        if phi > 0.0 && timeconst > 1e-8 {
            // MuJoCo-style: spring-damper in constraint space
            // k_spring = 1 / (timeconst² * (1 + dampratio²))
            // d_damp = 2 * dampratio / (timeconst * (1 + dampratio²))
            let tc2 = timeconst * timeconst;
            let dr2 = dampratio * dampratio;
            let denom = tc2 * (1.0 + dr2);
            let k_spring = if denom > 1e-10 { 1.0 / denom } else { 100.0 };
            let d_damp = if denom > 1e-10 { 2.0 * dampratio / (timeconst * (1.0 + dr2)) } else { 10.0 };

            // SolImp: smooth activation based on penetration depth
            let activation = if imp_width > 1e-10 {
                let x = (phi / imp_width).min(1.0);
                imp_midpoint + (1.0 - imp_midpoint) * x
            } else {
                1.0
            };

            let stab = (k_spring * phi + d_damp * (-vn).max(0.0)) * activation;
            b[k] -= stab.min(10.0);
        } else if phi > 0.0 {
            // Fallback to Baumgarte if solref is disabled
            let stab = (config.baumgarte * phi).min(10.0);
            b[k] -= stab;
        }

        // Restitution with micro-bounce elimination
        let e = constraint_eval.restitution[k];
        if vn < -config.bounce_threshold {
            b[k] += e * vn;
        }

        // Contact damping
        if config.contact_damping > 0.0 && vn < 0.0 {
            b[k] -= config.contact_damping * vn;
        }
    }

    // Newton iteration on the Schur complement system
    // Solve: W * lambda + b = 0 subject to lambda >= 0
    let mut lambda_n = DVector::zeros(nc);
    let mut iterations = 0;
    let mut residual = f32::MAX;

    // Try Cholesky factorization for direct solve
    // f64 Cholesky for maximum precision
    let cholesky_f64 = w_reg_f64.clone().cholesky();

    for iter in 0..config.max_iterations {
        // Newton residual: r = W * lambda + b
        let r = &w_reg * &lambda_n + &b;

        // Check convergence
        let r_norm = r.norm();
        if r_norm < config.tolerance && iter >= config.min_iterations {
            residual = r_norm;
            iterations = iter + 1;
            break;
        }

        // Newton step in f64: delta_lambda = -W^{-1} * r
        let delta_lambda = if let Some(ref chol) = cholesky_f64 {
            let r_f64 = nalgebra::DVector::<f64>::from_iterator(nc, r.iter().map(|&v| v as f64));
            let dl_f64 = chol.solve(&(-&r_f64));
            DVector::from_iterator(nc, dl_f64.iter().map(|&v| v as f32))
        } else {
            // Fallback: diagonal approximation
            let mut dl = DVector::zeros(nc);
            for k in 0..nc {
                let diag = w_reg[(k, k)];
                if diag.abs() > 1e-12 {
                    dl[k] = -r[k] / diag;
                }
            }
            dl
        };

        // Line search: find step size that keeps lambda >= 0
        let mut alpha = 1.0_f32;
        for _ in 0..10 {
            let trial = &lambda_n + &delta_lambda * alpha;
            let all_positive = trial.iter().all(|&v| v >= -1e-8);
            if all_positive {
                break;
            }
            alpha *= 0.5;
        }

        // Apply step with projection
        lambda_n += &delta_lambda * alpha;
        for k in 0..nc {
            lambda_n[k] = lambda_n[k].max(0.0).min(config.max_impulse);
            if !lambda_n[k].is_finite() {
                lambda_n[k] = 0.0;
            }
        }

        iterations = iter + 1;
        residual = r_norm;
    }

    // Friction solve: iterated Gauss-Seidel on friction cone (5 iterations max)
    let mut lambda_t = DVector::<f32>::zeros(2 * nc);
    if nc > 0 {
        let j_t_m_inv = j_t * m_inv;
        let w_tt = &j_t_m_inv * j_t.transpose();
        let b_t_qd = build_global_qd(bodies, constraint_eval);

        for _fric_iter in 0..5 {
            let mut max_change = 0.0_f32;

            for k in 0..nc {
                let mu = constraint_eval.mu[k];
                let fn_k = lambda_n[k];
                let max_friction = mu * fn_k;

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

                    // Full residual with cross-coupling
                    let mut r = b_t_qd[idx];
                    for j in 0..2 * nc {
                        r += w_tt[(idx, j)] * lambda_t[j];
                    }

                    let old = lambda_t[idx];
                    let gs_update = old - r / diag;
                    let new_val = if gs_update.is_finite() {
                        gs_update.clamp(-max_friction, max_friction)
                    } else {
                        0.0
                    };
                    lambda_t[idx] = new_val;
                    max_change = max_change.max((new_val - old).abs());
                }
            }

            if max_change < config.tolerance { break; }
        }
    }

    // Compute velocity impulse: delta_qd = M^{-1} * (J_n^T * lambda_n + J_t^T * lambda_t)
    let tau = j_n.transpose() * &lambda_n + j_t.transpose() * &lambda_t;
    let mut delta_qd = m_inv * &tau;

    // Position correction using solref impedance (split impulse — no energy injection)
    let mut phi_correction = DVector::zeros(nc);
    {
        let [tc, dr] = config.solref;
        let tc = tc.max(1e-6);
        let dr2 = dr.max(0.0).powi(2);
        let imp_width = config.solimp[0].max(1e-6);
        let imp_midpoint = config.solimp[1];
        let denom = tc * tc * (1.0 + dr2);
        let k_spring = if denom > 1e-10 { 1.0 / denom } else { 100.0 };

        for k in 0..nc {
            let phi = constraint_eval.c[k];
            if phi > 0.0 {
                // SolImp activation: smooth ramp based on penetration depth
                let activation = if imp_width > 1e-10 {
                    imp_midpoint + (1.0 - imp_midpoint) * (phi / imp_width).min(1.0)
                } else {
                    1.0
                };
                // Spring-based position correction scaled by baumgarte and capped
                phi_correction[k] = (k_spring * phi * activation * config.baumgarte).min(0.01);
            }
        }
    }
    let tau_pos = j_n.transpose() * &phi_correction;
    let mut position_correction = m_inv * &tau_pos;

    // NaN protection
    for i in 0..delta_qd.len() {
        if !delta_qd[i].is_finite() { delta_qd[i] = 0.0; }
        if !position_correction[i].is_finite() { position_correction[i] = 0.0; }
    }

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

/// Build global qd vector and compute tangential velocity bias: J_t * qd_global.
fn build_global_qd(bodies: &[&ArticulatedBody], eval: &ConstraintEval) -> DVector<f32> {
    // Assemble global qd from all bodies' generalized velocities
    let total_nv = eval.total_dofs;
    let mut qd = DVector::zeros(total_nv);

    // Fill in each body's qd at its DOF offset
    // Note: we need the DOF offsets, but they're not stored in ConstraintEval.
    // Reconstruct from body DOF counts.
    let mut offset = 0;
    for body in bodies {
        let nv = body.dof_count();
        for i in 0..nv {
            if offset + i < total_nv {
                qd[offset + i] = body.qd[i];
            }
        }
        offset += nv;
    }

    &eval.friction_jacobian * &qd
}

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

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

    fn make_floating_body(y: f32) -> ArticulatedBody {
        let mut body = ArticulatedBody::new();
        body.set_gravity(Vector3::new(0.0, -9.81, 0.0));
        let inertia = SpatialInertia::from_mass_inertia_at_com(
            1.0,
            Matrix3::from_diagonal(&Vector3::new(0.167, 0.167, 0.167)),
        );
        body.add_body("link", -1, GenJointType::Floating, inertia, SpatialTransform::identity());
        body.set_joint_q(0, &[0.0, y, 0.0, 1.0, 0.0, 0.0, 0.0]);
        body
    }

    #[test]
    fn test_constraint_function_empty() {
        let body = make_floating_body(1.0);
        let bodies = vec![&body];
        let offsets = vec![0];
        let counts = vec![6];

        let c = ConstraintFunction::evaluate(&bodies, &offsets, &counts, &[], &[]);
        assert_eq!(c.len(), 0);
    }

    #[test]
    fn test_constraint_function_single_penetrating() {
        let body = make_floating_body(0.5);
        let bodies = vec![&body];
        let offsets = vec![0];
        let counts = vec![6];

        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 contacts = vec![(0usize, &manifold)];
        let c = ConstraintFunction::evaluate(&bodies, &offsets, &counts, &contacts, &[]);
        assert_eq!(c.len(), 1);
        assert!((c[0] - 0.01).abs() < 1e-5, "phi={}", c[0]);
    }

    #[test]
    fn test_constraint_gradient_matches_jacobian() {
        let body = make_floating_body(0.5);
        let bodies = vec![&body];
        let offsets = vec![0];
        let counts = vec![6];

        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 contacts = vec![(0usize, &manifold)];
        let eval = ConstraintFunction::evaluate_with_gradient(&bodies, &offsets, &counts, &contacts, &[]);

        assert_eq!(eval.c.len(), 1);
        assert_eq!(eval.jacobian.nrows(), 1);
        assert_eq!(eval.jacobian.ncols(), 6);
        assert_eq!(eval.num_contacts, 1);
    }

    #[test]
    fn test_newton_solver_single_contact() {
        let body = make_floating_body(0.5);
        let bodies = vec![&body];
        let offsets = vec![0];
        let counts = vec![6];

        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 contacts = vec![(0usize, &manifold)];
        let eval = ConstraintFunction::evaluate_with_gradient(&bodies, &offsets, &counts, &contacts, &[]);
        let m_inv = build_block_diagonal_m_inv(&bodies, &offsets, &counts, 6);
        let config = LcpSolverConfig::default();

        let result = solve_newton_contact(&bodies, &eval, &m_inv, &config);
        assert!(result.converged || result.iterations <= 20);
        assert!(result.lambda_n[0] >= 0.0, "lambda must be non-negative");
        assert!(result.delta_qd.len() == 6);
    }

    #[test]
    fn test_newton_solver_empty() {
        let body = make_floating_body(5.0);
        let bodies = vec![&body];
        let offsets = vec![0];
        let counts = vec![6];

        let eval = ConstraintFunction::evaluate_with_gradient(&bodies, &offsets, &counts, &[], &[]);
        let m_inv = build_block_diagonal_m_inv(&bodies, &offsets, &counts, 6);
        let config = LcpSolverConfig::default();

        let result = solve_newton_contact(&bodies, &eval, &m_inv, &config);
        assert!(result.converged);
        assert_eq!(result.iterations, 0);
    }

    #[test]
    fn test_build_block_diagonal_m_inv() {
        let body = make_floating_body(1.0);
        let bodies = vec![&body];
        let offsets = vec![0];
        let counts = vec![6];

        let m_inv = build_block_diagonal_m_inv(&bodies, &offsets, &counts, 6);
        assert_eq!(m_inv.nrows(), 6);
        assert_eq!(m_inv.ncols(), 6);
        // Translation DOFs should have inv_mass = 1.0
        assert!((m_inv[(0, 0)] - 1.0).abs() < 0.1, "m_inv[0,0]={}", m_inv[(0, 0)]);
        assert!((m_inv[(1, 1)] - 1.0).abs() < 0.1, "m_inv[1,1]={}", m_inv[(1, 1)]);
    }

    #[test]
    fn test_two_overlapping_boxes_contact_chain() {
        use super::super::contact::{inter_body_contacts, ShapePair, ContactManifold, ContactPoint};
        use super::super::collider::ColliderShape as StoredShape;
        use super::super::contact_jacobian::InterBodyContact;
        use nalgebra::Matrix3;

        // Body A: sitting on ground at y=0.5 (box half_extent=0.5, bottom at y=0)
        let mut body_a = ArticulatedBody::new();
        body_a.set_gravity(Vector3::new(0.0, -9.81, 0.0));
        let inertia = SpatialInertia::from_mass_inertia_at_com(
            1.0, Matrix3::from_diagonal(&Vector3::new(0.167, 0.167, 0.167)),
        );
        body_a.add_body("a", -1, GenJointType::Floating, inertia.clone(), SpatialTransform::identity());
        body_a.set_joint_q(0, &[0.0, 0.5, 0.0, 1.0, 0.0, 0.0, 0.0]);
        body_a.set_joint_qd(0, &[0.0, 0.0, 0.0, 0.0, 0.0, 0.0]); // stationary

        // Body B: at y=1.4, falling at -2 m/s (top of A is at y=1.0, so 0.4m gap)
        // After 1 step with gravity: v_y = -2 - 9.81*0.001 = -2.0098
        // Position after step: y = 1.4 - 2*0.001 = 1.398
        // But for this test, place B overlapping A: y=0.95 (bottom at 0.45, overlaps A top at 1.0 by... no)
        // Better: place B at y=1.05 so bottom of B (0.55) overlaps top of A (1.0) by -0.45... no.
        //
        // Place A at y=0.5 (occupies 0.0 to 1.0), B at y=1.3 (occupies 0.8 to 1.8)
        // Overlap: A top (1.0) > B bottom (0.8) = 0.2m penetration
        let mut body_b = ArticulatedBody::new();
        body_b.set_gravity(Vector3::new(0.0, -9.81, 0.0));
        body_b.add_body("b", -1, GenJointType::Floating, inertia, SpatialTransform::identity());
        body_b.set_joint_q(0, &[0.0, 1.3, 0.0, 1.0, 0.0, 0.0, 0.0]);
        body_b.set_joint_qd(0, &[0.0, -1.0, 0.0, 0.0, 0.0, 0.0]); // falling

        let bodies = vec![&body_a, &body_b];
        let offsets = vec![0, 6];
        let counts = vec![6, 6];
        let he = Vector3::new(0.5, 0.5, 0.5);

        // 1. Test inter-body contact detection
        let shape = StoredShape::Box { half_extents: he };
        let pos_a = Vector3::new(0.0, 0.5, 0.0);
        let pos_b = Vector3::new(0.0, 1.3, 0.0);
        let rot = Matrix3::identity();

        let pair = ShapePair {
            pos_a, rot_a: rot, pos_b, rot_b: rot,
            body_id_a: 0, body_id_b: 0, friction: 0.5, restitution: 0.0,
        };
        let m = inter_body_contacts(&shape, &shape, &pair);
        eprintln!("DIAG: inter-body contacts detected: {}", m.contacts.len());
        for (i, c) in m.contacts.iter().enumerate() {
            eprintln!("  contact {i}: pen={:.4}, normal=({:.2},{:.2},{:.2}), point=({:.2},{:.2},{:.2})",
                c.penetration, c.normal.x, c.normal.y, c.normal.z,
                c.point_world.x, c.point_world.y, c.point_world.z);
        }
        assert!(!m.is_empty(), "Should detect contacts between overlapping boxes");

        // 2. Build InterBodyContact list
        let inter_contacts: Vec<InterBodyContact> = m.contacts.iter().map(|c| {
            InterBodyContact {
                body_a: 0, link_a: 0, body_b: 1, link_b: 0,
                point_world: c.point_world, normal: c.normal,
                penetration: c.penetration, friction: 0.5, restitution: 0.0,
            }
        }).collect();

        // 3. Build ground contacts for body A (on ground at y=0)
        let mut ground_manifold = ContactManifold::new();
        // 4 bottom corners of box A
        for &(dx, dz) in &[(-0.5_f32, -0.5_f32), (0.5, -0.5), (-0.5, 0.5), (0.5, 0.5)] {
            ground_manifold.add_contact(
                ContactPoint::new(0, Vector3::new(dx, 0.0, dz), Vector3::new(dx, -0.5, dz), Vector3::y(), 0.0)
                    .with_friction(0.5),
            );
        }
        let ground_contacts = vec![(0usize, &ground_manifold)];

        // 4. Evaluate constraints
        let eval = ConstraintFunction::evaluate_with_gradient(
            &bodies, &offsets, &counts, &ground_contacts, &inter_contacts,
        );
        eprintln!("DIAG: total contacts: {} (ground: {}, inter: {})",
            eval.num_contacts, ground_manifold.active_count(), inter_contacts.len());
        eprintln!("DIAG: phi = {:?}", eval.c);
        eprintln!("DIAG: v_n = {:?}", eval.v_n);
        eprintln!("DIAG: J_n shape = {}x{}", eval.jacobian.nrows(), eval.jacobian.ncols());

        // 5. Solve
        let m_inv = build_block_diagonal_m_inv(&bodies, &offsets, &counts, 12);
        let config = super::super::lcp_solver::LcpSolverConfig::default();
        let result = solve_newton_contact(&bodies, &eval, &m_inv, &config);

        eprintln!("DIAG: Newton iterations={}, residual={:.6}, converged={}",
            result.iterations, result.residual, result.converged);
        eprintln!("DIAG: lambda_n = {:?}", result.lambda_n);
        eprintln!("DIAG: delta_qd (body_a) = [{:.4}, {:.4}, {:.4}, {:.4}, {:.4}, {:.4}]",
            result.delta_qd[0], result.delta_qd[1], result.delta_qd[2],
            result.delta_qd[3], result.delta_qd[4], result.delta_qd[5]);
        eprintln!("DIAG: delta_qd (body_b) = [{:.4}, {:.4}, {:.4}, {:.4}, {:.4}, {:.4}]",
            result.delta_qd[6], result.delta_qd[7], result.delta_qd[8],
            result.delta_qd[9], result.delta_qd[10], result.delta_qd[11]);

        // 6. Verify physics correctness
        // Body B is falling at -1 m/s. Inter-body contact should push B up (positive delta_qd_y).
        // Body A should be pushed down slightly by Newton's 3rd law.
        let dqd_a_y = result.delta_qd[1]; // body A y velocity correction
        let dqd_b_y = result.delta_qd[7]; // body B y velocity correction

        eprintln!("DIAG: body_a delta_qd_y = {dqd_a_y:.6}");
        eprintln!("DIAG: body_b delta_qd_y = {dqd_b_y:.6}");

        // Inter-body lambda should be non-zero
        let has_inter_lambda = result.lambda_n.iter().any(|&l| l > 1e-6);
        eprintln!("DIAG: has non-zero inter-body lambda: {has_inter_lambda}");

        assert!(has_inter_lambda, "Inter-body contact must produce non-zero lambda");
        // B should be pushed up (or at least not pushed further down)
        assert!(dqd_b_y > -0.01, "Body B should be pushed up, got dqd_b_y={dqd_b_y}");
    }

    #[test]
    fn determinism_constraint_evaluate_same_input_same_output() {
        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.03)
            .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 eval1 = ConstraintFunction::evaluate(
            &bodies, &dof_offsets, &dof_counts, &ground_contacts, &inter_contacts);
        let eval2 = ConstraintFunction::evaluate(
            &bodies, &dof_offsets, &dof_counts, &ground_contacts, &inter_contacts);

        assert_eq!(eval1.len(), eval2.len(), "evaluate must return same-length vectors");
        for i in 0..eval1.len() {
            assert_eq!(eval1[i], eval2[i],
                "ConstraintFunction must be deterministic: element[{}] differs", i);
        }
    }

    #[test]
    fn constraint_gradient_dimensions_match_contacts_and_dofs() {
        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.03)
            .with_friction(0.5));
        manifold.add_contact(ContactPoint::new(0,
            Vector3::new(0.1, -0.1, 0.0), Vector3::zeros(), Vector3::y(), 0.02)
            .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 grad = ConstraintFunction::gradient(
            &bodies, &dof_offsets, &dof_counts, &ground_contacts, &inter_contacts);

        // Gradient (Jacobian) should be nc × total_nv
        let nc = manifold.active_count();
        let nv = body.dof_count();
        assert_eq!(grad.nrows(), nc, "gradient rows should equal contact count");
        assert_eq!(grad.ncols(), nv, "gradient cols should equal total DOFs");
    }

    #[test]
    fn evaluate_with_gradient_consistent_with_separate_calls() {
        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.03)
            .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 eval = ConstraintFunction::evaluate(
            &bodies, &dof_offsets, &dof_counts, &ground_contacts, &inter_contacts);
        let grad = ConstraintFunction::gradient(
            &bodies, &dof_offsets, &dof_counts, &ground_contacts, &inter_contacts);
        let combined = ConstraintFunction::evaluate_with_gradient(
            &bodies, &dof_offsets, &dof_counts, &ground_contacts, &inter_contacts);

        // Combined should match separate calls
        assert_eq!(eval.len(), combined.c.len(), "constraint values length mismatch");
        assert_eq!(grad.nrows(), combined.jacobian.nrows(), "Jacobian rows mismatch");
        assert_eq!(grad.ncols(), combined.jacobian.ncols(), "Jacobian cols mismatch");

        for i in 0..eval.len() {
            assert!((eval[i] - combined.c[i]).abs() < 1e-6,
                "constraint[{i}]: separate={} combined={}", eval[i], combined.c[i]);
        }
    }

    #[test]
    fn build_block_diagonal_m_inv_dimensions() {
        let mut body = ArticulatedBody::new();
        body.set_gravity(Vector3::new(0.0, -9.81, 0.0));
        body.add_body("l1", -1, GenJointType::Revolute { axis: Vector3::z() },
            SpatialInertia::sphere(1.0, 0.1), SpatialTransform::identity());
        body.add_body("l2", 0, GenJointType::Revolute { axis: Vector3::z() },
            SpatialInertia::sphere(0.5, 0.1), SpatialTransform::identity());

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

        let total_dof = body.dof_count();
        let m_inv = build_block_diagonal_m_inv(&bodies, &dof_offsets, &dof_counts, total_dof);
        assert_eq!(m_inv.nrows(), total_dof, "M_inv rows should equal total DOF");
        assert_eq!(m_inv.ncols(), total_dof, "M_inv cols should equal total DOF");
    }

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

    #[test]
    fn intent_newton_all_impulses_non_negative() {
        // Intent: Contact impulses must never pull (only push) — fundamental physics requirement
        let body = make_floating_body(0.5);
        let bodies = vec![&body];
        let offsets = vec![0];
        let counts = vec![6];

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

        let contacts = vec![(0usize, &manifold)];
        let eval = ConstraintFunction::evaluate_with_gradient(&bodies, &offsets, &counts, &contacts, &[]);
        let m_inv = build_block_diagonal_m_inv(&bodies, &offsets, &counts, 6);
        let config = LcpSolverConfig::default();

        let result = solve_newton_contact(&bodies, &eval, &m_inv, &config);
        for (i, &l) in result.lambda_n.iter().enumerate() {
            assert!(l >= -1e-8, "Newton impulse lambda_n[{i}]={l} must be non-negative");
        }
    }

    #[test]
    fn intent_newton_friction_cone_satisfied() {
        // Intent: Tangential impulse bounded by Coulomb cone: |lambda_t| <= mu * lambda_n
        let body = make_floating_body(0.5);
        let bodies = vec![&body];
        let offsets = vec![0];
        let counts = vec![6];

        let mu = 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.02)
                .with_friction(mu),
        );

        let contacts = vec![(0usize, &manifold)];
        let eval = ConstraintFunction::evaluate_with_gradient(&bodies, &offsets, &counts, &contacts, &[]);
        let m_inv = build_block_diagonal_m_inv(&bodies, &offsets, &counts, 6);
        let config = LcpSolverConfig::default();

        let result = solve_newton_contact(&bodies, &eval, &m_inv, &config);
        for i in 0..result.lambda_n.len() {
            let fn_val = result.lambda_n[i];
            let ft1 = result.lambda_t[2 * i];
            let ft2 = result.lambda_t[2 * i + 1];
            let ft_mag = (ft1 * ft1 + ft2 * ft2).sqrt();
            assert!(
                ft_mag <= mu * fn_val + 1e-4,
                "Friction cone violated at contact {i}: |ft|={ft_mag} > mu*fn={}", mu * fn_val
            );
        }
    }

    #[test]
    fn stress_newton_four_contacts_all_finite() {
        // Stress: 4 simultaneous contacts on a floating body — solver must remain stable
        let body = make_floating_body(0.5);
        let bodies = vec![&body];
        let offsets = vec![0];
        let counts = vec![6];

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

        let contacts = vec![(0usize, &manifold)];
        let eval = ConstraintFunction::evaluate_with_gradient(&bodies, &offsets, &counts, &contacts, &[]);
        let m_inv = build_block_diagonal_m_inv(&bodies, &offsets, &counts, 6);
        let config = LcpSolverConfig::default();

        let result = solve_newton_contact(&bodies, &eval, &m_inv, &config);
        assert!(result.delta_qd.iter().all(|v| v.is_finite()), "all delta_qd must be finite");
        assert!(result.lambda_n.iter().all(|v| v.is_finite()), "all lambda_n must be finite");
        assert!(result.lambda_t.iter().all(|v| v.is_finite()), "all lambda_t must be finite");
        assert!(result.residual.is_finite(), "residual must be finite");
    }

    #[test]
    fn intent_newton_delta_qd_opposes_penetration() {
        // Intent: Velocity correction should push the body away from the ground
        // A body penetrating the ground should receive an upward velocity correction
        let mut body = make_floating_body(0.4); // below ground plane
        body.set_joint_qd(0, &[0.0, -2.0, 0.0, 0.0, 0.0, 0.0]); // falling
        let bodies = vec![&body];
        let offsets = vec![0];
        let counts = vec![6];

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

        let contacts = vec![(0usize, &manifold)];
        let eval = ConstraintFunction::evaluate_with_gradient(&bodies, &offsets, &counts, &contacts, &[]);
        let m_inv = build_block_diagonal_m_inv(&bodies, &offsets, &counts, 6);
        let config = LcpSolverConfig::default();

        let result = solve_newton_contact(&bodies, &eval, &m_inv, &config);
        // Y component of delta_qd (index 1 for floating base translation) should be positive (upward)
        let delta_y = result.delta_qd[1];
        assert!(
            delta_y > 0.0,
            "Velocity correction should push body up (opposing penetration), got delta_qd_y={delta_y}"
        );
    }

    #[test]
    fn property_newton_result_all_finite_for_well_posed() {
        // Property: For a well-posed problem (positive mass, valid contacts), all outputs are finite
        let body = make_floating_body(1.0);
        let bodies = vec![&body];
        let offsets = vec![0];
        let counts = vec![6];

        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).with_restitution(0.3),
        );

        let contacts = vec![(0usize, &manifold)];
        let eval = ConstraintFunction::evaluate_with_gradient(&bodies, &offsets, &counts, &contacts, &[]);
        let m_inv = build_block_diagonal_m_inv(&bodies, &offsets, &counts, 6);
        let config = LcpSolverConfig::default();

        let result = solve_newton_contact(&bodies, &eval, &m_inv, &config);
        assert!(result.residual.is_finite());
        assert!(result.position_correction.iter().all(|v| v.is_finite()));
    }

    // ── SLAM Cycle 19: Newton solver proptest ─────────────────────────

    use proptest::prelude::*;

    proptest! {
        #[test]
        fn prop_newton_non_negative_impulses(
            pen in 0.001f32..0.05,
            mu in 0.1f32..1.0,
        ) {
            let body = make_floating_body(0.5);
            let bodies = vec![&body];
            let offsets = vec![0];
            let counts = vec![6];

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

            let contacts = vec![(0usize, &manifold)];
            let eval = ConstraintFunction::evaluate_with_gradient(&bodies, &offsets, &counts, &contacts, &[]);
            let m_inv = build_block_diagonal_m_inv(&bodies, &offsets, &counts, 6);
            let config = LcpSolverConfig::default();

            let result = solve_newton_contact(&bodies, &eval, &m_inv, &config);
            for (i, &l) in result.lambda_n.iter().enumerate() {
                prop_assert!(l >= -1e-6, "Newton lambda_n[{i}]={l} must be non-negative");
            }
        }
    }
}