featherstone 0.1.0

//! Smooth differentiable contact model (MuJoCo-style)
//!
//! Unlike the LCP solver which enforces exact complementarity constraints,
//! smooth contacts use a differentiable penalty function that approximates
//! rigid contact. This makes the dynamics differentiable — essential for
//! gradient-based optimization, trajectory optimization, and differentiable
//! simulation in RL.
//!
//! The model follows MuJoCo's approach:
//! - **Normal force**: spring-damper with one-sided activation
//! - **Friction**: smooth Coulomb approximation using a softmax-like function
//! - **Penetration**: controlled by stiffness and damping parameters
//!
//! Trade-off vs LCP:
//! - Smooth: differentiable, faster (no iterative solve), slight penetration
//! - LCP: exact non-penetration, not differentiable, iterative convergence

use nalgebra::{DVector, Vector3};

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

/// Configuration for smooth contact dynamics.
#[derive(Clone, Debug)]
pub struct SmoothContactConfig {
    /// Contact stiffness (N/m) — how hard the contact pushes back
    pub stiffness: f32,
    /// Contact damping (Ns/m) — dissipates energy at contact
    pub damping: f32,
    /// Friction smoothing parameter — width of the smooth transition
    /// near zero sliding velocity. Smaller = sharper (closer to Coulomb).
    pub friction_smoothing: f32,
    /// Maximum penetration depth before force saturates (prevents blow-up)
    pub max_penetration: f32,
    /// Time constant for penetration recovery (seconds)
    pub time_constant: f32,
}

impl Default for SmoothContactConfig {
    fn default() -> Self {
        Self {
            stiffness: 1e5,
            damping: 1e3,
            friction_smoothing: 0.01,
            max_penetration: 0.01,
            time_constant: 0.01,
        }
    }
}

impl SmoothContactConfig {
    /// High-stiffness configuration (closer to rigid contacts).
    pub fn stiff() -> Self {
        Self {
            stiffness: 1e6,
            damping: 1e4,
            friction_smoothing: 0.005,
            max_penetration: 0.005,
            time_constant: 0.005,
        }
    }

    /// Soft contact configuration (more penetration, smoother gradients).
    pub fn soft() -> Self {
        Self {
            stiffness: 1e4,
            damping: 1e2,
            friction_smoothing: 0.05,
            max_penetration: 0.02,
            time_constant: 0.02,
        }
    }
}

/// Result of smooth contact force computation.
#[derive(Clone, Debug)]
pub struct SmoothContactResult {
    /// Joint-space contact forces: τ_contact (nv)
    pub tau_contact: DVector<f32>,
    /// Per-contact normal force magnitude (nc)
    pub normal_forces: Vec<f32>,
    /// Per-contact friction force magnitude (nc)
    pub friction_forces: Vec<f32>,
    /// Total contact potential energy (for energy monitoring)
    pub potential_energy: f32,
    /// Total dissipation power (for energy monitoring)
    pub dissipation_power: f32,
}

/// Compute smooth contact forces for all active contacts.
///
/// Returns joint-space forces to be added to applied torques before
/// forward dynamics (ABA). Unlike the LCP solver, this does not require
/// matrix inversion or iteration — it's a direct force computation.
pub fn smooth_contact_forces(
    _body: &ArticulatedBody,
    constraints: &ContactConstraints,
    config: &SmoothContactConfig,
) -> SmoothContactResult {
    let nv = constraints.num_dofs;
    let nc = constraints.num_contacts;

    if nc == 0 {
        return SmoothContactResult {
            tau_contact: DVector::zeros(nv),
            normal_forces: Vec::new(),
            friction_forces: Vec::new(),
            potential_energy: 0.0,
            dissipation_power: 0.0,
        };
    }

    let mut tau_contact = DVector::zeros(nv);
    let mut normal_forces = Vec::with_capacity(nc);
    let mut friction_forces = Vec::with_capacity(nc);
    let mut total_pe = 0.0_f32;
    let mut total_dissipation = 0.0_f32;

    for k in 0..nc {
        let cd = &constraints.per_contact[k];
        let phi = cd.penetration;
        let vn = cd.v_normal;

        // ================================================================
        // Normal force: one-sided spring-damper
        // f_n = max(0, stiffness * phi + damping * (-v_n))
        //
        // Only active when penetrating (phi > 0), dissipates when
        // approaching (v_n < 0).
        // ================================================================
        let phi_clamped = phi.min(config.max_penetration);
        let spring_force = config.stiffness * phi_clamped.max(0.0);
        let damping_force = if phi > 0.0 {
            config.damping * (-vn).max(0.0)
        } else {
            0.0
        };
        let f_n = (spring_force + damping_force).max(0.0);

        // Potential energy: 0.5 * k * phi^2
        if phi > 0.0 {
            total_pe += 0.5 * config.stiffness * phi_clamped * phi_clamped;
            total_dissipation += damping_force * (-vn).max(0.0);
        }

        // ================================================================
        // Friction force: smooth Coulomb approximation
        //
        // f_t = -μ * f_n * v_t / (|v_t| + ε)
        //
        // This gives zero friction at zero velocity and approaches
        // μ * f_n * sign(v_t) at high velocity. The smoothing parameter
        // ε controls the transition width.
        // ================================================================
        let vt1 = cd.v_tangent[0];
        let vt2 = cd.v_tangent[1];
        let vt_mag = (vt1 * vt1 + vt2 * vt2).sqrt();
        let smooth_denom = vt_mag + config.friction_smoothing;

        let ft1 = -cd.friction * f_n * vt1 / smooth_denom;
        let ft2 = -cd.friction * f_n * vt2 / smooth_denom;
        let ft_mag = (ft1 * ft1 + ft2 * ft2).sqrt();

        normal_forces.push(f_n);
        friction_forces.push(ft_mag);

        // ================================================================
        // Project forces to joint space: τ += J_nᵀ * f_n + J_tᵀ * f_t
        // ================================================================
        for j in 0..nv {
            tau_contact[j] += cd.j_normal[j] * f_n;
            tau_contact[j] += cd.j_tangent[(0, j)] * ft1;
            tau_contact[j] += cd.j_tangent[(1, j)] * ft2;
        }
    }

    SmoothContactResult {
        tau_contact,
        normal_forces,
        friction_forces,
        potential_energy: total_pe,
        dissipation_power: total_dissipation,
    }
}

/// Compute the contact force Jacobian ∂f/∂q for smooth contacts.
///
/// This is the key advantage of smooth contacts: the force is differentiable
/// with respect to the state, enabling gradient-based optimization.
///
/// Returns an nv × nv matrix where each column j is ∂τ_contact/∂q_j,
/// approximated via finite differences.
pub fn smooth_contact_force_jacobian(
    body: &ArticulatedBody,
    constraints: &ContactConstraints,
    config: &SmoothContactConfig,
    epsilon: f32,
) -> nalgebra::DMatrix<f32> {
    let nv = body.dof_count();
    let nq = body.nq();
    let mut jacobian = nalgebra::DMatrix::zeros(nv, nq);

    let tau0 = smooth_contact_forces(body, constraints, config).tau_contact;

    // Finite difference for each q component
    for j in 0..nq {
        let mut body_pert = body.clone();
        body_pert.q[j] += epsilon;

        // Re-compute FK and constraints for perturbed state
        let manifold_pert = recompute_manifold_from_constraints(
            &body_pert,
            constraints,
        );
        let constraints_pert = super::contact_jacobian::ContactConstraints::from_manifold(
            &body_pert,
            &manifold_pert,
        );
        let tau_pert = smooth_contact_forces(&body_pert, &constraints_pert, config).tau_contact;

        for i in 0..nv {
            jacobian[(i, j)] = (tau_pert[i] - tau0[i]) / epsilon;
        }
    }

    jacobian
}

/// Reconstruct a contact manifold from existing constraint data at a new state.
///
/// Uses the same body IDs and approximate contact geometry from the original
/// constraints, recomputed at the new body state.
fn recompute_manifold_from_constraints(
    body: &ArticulatedBody,
    constraints: &ContactConstraints,
) -> super::contact::ContactManifold {
    let _fk = super::kinematics::forward_kinematics(body);
    let mut manifold = super::contact::ContactManifold::new();

    for cd in &constraints.per_contact {
        // Reuse original geometry with correct body ID (accurate for small perturbations)
        manifold.add_contact(
            super::contact::ContactPoint::new(
                cd.body_id,
                Vector3::zeros(),
                Vector3::zeros(),
                cd.normal,
                cd.penetration,
            )
            .with_friction(cd.friction)
            .with_restitution(cd.restitution),
        );
    }

    manifold
}

// ============================================================================
// 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;

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

    #[test]
    fn test_smooth_no_contacts() {
        let body = ArticulatedBody::new();
        let constraints = ContactConstraints::empty(0);
        let result = smooth_contact_forces(&body, &constraints, &SmoothContactConfig::default());

        assert_eq!(result.tau_contact.len(), 0);
        assert!(result.normal_forces.is_empty());
    }

    #[test]
    fn test_smooth_normal_force_penetrating() {
        let mut body = ArticulatedBody::new();
        body.add_body(
            "link",
            -1,
            GenJointType::Prismatic { axis: Vector3::y() },
            make_inertia(1.0),
            SpatialTransform::identity(),
        );

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

        let constraints = ContactConstraints::from_manifold(&body, &manifold);
        let config = SmoothContactConfig::default();
        let result = smooth_contact_forces(&body, &constraints, &config);

        // Should produce upward force
        assert!(result.normal_forces[0] > 0.0);
        // F = stiffness * penetration = 1e5 * 0.001 = 100 N
        assert_relative_eq!(result.normal_forces[0], 100.0, epsilon = 1.0);
        assert!(result.potential_energy > 0.0);
    }

    #[test]
    fn test_smooth_no_force_when_separated() {
        let mut body = ArticulatedBody::new();
        body.add_body(
            "link",
            -1,
            GenJointType::Prismatic { axis: Vector3::y() },
            make_inertia(1.0),
            SpatialTransform::identity(),
        );

        let mut manifold = ContactManifold::new();
        manifold.add_contact(ContactPoint::new(
            0,
            Vector3::zeros(),
            Vector3::zeros(),
            Vector3::y(),
            -0.01, // separated
        ));

        // This contact is not active, so from_manifold will skip it
        let constraints = ContactConstraints::from_manifold(&body, &manifold);
        let result = smooth_contact_forces(&body, &constraints, &SmoothContactConfig::default());

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

    #[test]
    fn test_smooth_damping_force() {
        let mut body = ArticulatedBody::new();
        body.add_body(
            "link",
            -1,
            GenJointType::Prismatic { axis: Vector3::y() },
            make_inertia(1.0),
            SpatialTransform::identity(),
        );

        // Approaching contact
        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 = SmoothContactConfig::default();
        let result = smooth_contact_forces(&body, &constraints, &config);

        // With damping and approaching velocity, force should be higher
        // F = k*phi + d*(-vn) = 1e5*0.001 + 1e3*1.0 = 100 + 1000 = 1100
        assert!(result.normal_forces[0] > 100.0);
        assert_relative_eq!(result.normal_forces[0], 1100.0, epsilon = 10.0);
    }

    #[test]
    fn test_smooth_friction_opposes_motion() {
        let mut body = ArticulatedBody::new();
        body.add_body(
            "link1",
            -1,
            GenJointType::Prismatic { axis: Vector3::y() },
            make_inertia(1.0),
            SpatialTransform::identity(),
        );
        body.add_body(
            "link2",
            0,
            GenJointType::Prismatic { axis: Vector3::x() },
            make_inertia(1.0),
            SpatialTransform::identity(),
        );

        // Sliding horizontally
        body.set_joint_qd(1, &[2.0]);

        let mu = 0.5;
        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 = smooth_contact_forces(&body, &constraints, &SmoothContactConfig::default());

        // Friction should be present and opposing motion
        assert!(result.friction_forces[0] > 0.0);
        // Friction should be approximately μ * f_n (for fast sliding)
        let fn_val = result.normal_forces[0];
        let ft_val = result.friction_forces[0];
        assert!(ft_val < mu * fn_val * 1.1, "Friction should be ≤ μ·f_n");
    }

    #[test]
    fn test_smooth_force_increases_with_penetration() {
        let mut body = ArticulatedBody::new();
        body.add_body(
            "link",
            -1,
            GenJointType::Prismatic { axis: Vector3::y() },
            make_inertia(1.0),
            SpatialTransform::identity(),
        );

        let config = SmoothContactConfig::default();

        let make_constraints = |pen: f32| {
            let mut manifold = ContactManifold::new();
            manifold.add_contact(ContactPoint::new(
                0,
                Vector3::zeros(),
                Vector3::zeros(),
                Vector3::y(),
                pen,
            ));
            ContactConstraints::from_manifold(&body, &manifold)
        };

        let f1 = smooth_contact_forces(&body, &make_constraints(0.001), &config).normal_forces[0];
        let f2 = smooth_contact_forces(&body, &make_constraints(0.005), &config).normal_forces[0];
        let f3 = smooth_contact_forces(&body, &make_constraints(0.01), &config).normal_forces[0];

        assert!(f2 > f1, "Force should increase: f1={f1} f2={f2}");
        assert!(f3 > f2, "Force should increase: f2={f2} f3={f3}");
    }

    #[test]
    fn test_smooth_force_saturates() {
        let mut body = ArticulatedBody::new();
        body.add_body(
            "link",
            -1,
            GenJointType::Prismatic { axis: Vector3::y() },
            make_inertia(1.0),
            SpatialTransform::identity(),
        );

        let config = SmoothContactConfig {
            max_penetration: 0.005,
            ..Default::default()
        };

        let make_constraints = |pen: f32| {
            let mut manifold = ContactManifold::new();
            manifold.add_contact(ContactPoint::new(
                0,
                Vector3::zeros(),
                Vector3::zeros(),
                Vector3::y(),
                pen,
            ));
            ContactConstraints::from_manifold(&body, &manifold)
        };

        let f_at_max = smooth_contact_forces(&body, &make_constraints(0.005), &config).normal_forces[0];
        let f_over_max = smooth_contact_forces(&body, &make_constraints(0.05), &config).normal_forces[0];

        // Both should produce the same force (clamped)
        assert_relative_eq!(f_at_max, f_over_max, epsilon = 1.0);
    }

    #[test]
    fn test_smooth_joint_space_force_direction() {
        let mut body = ArticulatedBody::new();
        body.add_body(
            "link",
            -1,
            GenJointType::Prismatic { axis: Vector3::y() },
            make_inertia(1.0),
            SpatialTransform::identity(),
        );

        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 result = smooth_contact_forces(&body, &constraints, &SmoothContactConfig::default());

        // Prismatic Y with upward contact normal: joint force should be positive (upward)
        assert!(result.tau_contact[0] > 0.0,
            "Contact force should push prismatic joint up: {}", result.tau_contact[0]);
    }

    #[test]
    fn test_smooth_config_presets() {
        let stiff = SmoothContactConfig::stiff();
        let soft = SmoothContactConfig::soft();

        assert!(stiff.stiffness > soft.stiffness);
        assert!(stiff.friction_smoothing < soft.friction_smoothing);
    }

    #[test]
    fn test_smooth_energy_conservation() {
        // With zero damping and zero velocity, potential energy should equal spring energy
        let mut body = ArticulatedBody::new();
        body.add_body(
            "link",
            -1,
            GenJointType::Prismatic { axis: Vector3::y() },
            make_inertia(1.0),
            SpatialTransform::identity(),
        );

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

        let config = SmoothContactConfig {
            damping: 0.0,
            ..Default::default()
        };
        let constraints = ContactConstraints::from_manifold(&body, &manifold);
        let result = smooth_contact_forces(&body, &constraints, &config);

        let expected_pe = 0.5 * config.stiffness * pen * pen;
        assert_relative_eq!(result.potential_energy, expected_pe, epsilon = 1e-3);
        assert_relative_eq!(result.dissipation_power, 0.0, epsilon = 1e-6);
    }

    // ── SLAM Cycle 4: smooth contact intent tests ────────────────────

    #[test]
    fn intent_smooth_no_force_when_separated() {
        // Physics intent: zero contact force when bodies are not in contact
        let mut body = ArticulatedBody::new();
        body.set_gravity(Vector3::new(0.0, 0.0, 0.0));
        body.add_body("link", -1, GenJointType::Floating,
            SpatialInertia::sphere(1.0, 0.1), SpatialTransform::identity());
        // Position body above ground (no contact)
        body.q[1] = 5.0; // y = 5 meters up

        let manifold = ContactManifold::new(); // empty = no contacts
        let constraints = ContactConstraints::from_manifold(&body, &manifold);
        let config = SmoothContactConfig::default();
        let result = smooth_contact_forces(&body, &constraints, &config);

        assert_relative_eq!(result.potential_energy, 0.0, epsilon = 1e-6);
        assert!(result.tau_contact.iter().all(|&t| t.abs() < 1e-6),
            "no force when separated: tau={:?}", result.tau_contact);
    }

    #[test]
    fn intent_smooth_stiff_vs_soft_config() {
        // Physics intent: stiffer config → larger force for same penetration
        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.0, 0.0), Vector3::zeros(), Vector3::y(), 0.02));
        let constraints = ContactConstraints::from_manifold(&body, &manifold);

        let stiff = smooth_contact_forces(&body, &constraints, &SmoothContactConfig::stiff());
        let soft = smooth_contact_forces(&body, &constraints, &SmoothContactConfig::soft());

        assert!(stiff.potential_energy > soft.potential_energy,
            "stiff should have more PE: stiff={}, soft={}",
            stiff.potential_energy, soft.potential_energy);
    }

    #[test]
    fn jacobian_returns_correct_dimensions() {
        // Jacobian should be nv × nq
        let mut body = ArticulatedBody::new();
        body.add_body("link", -1, GenJointType::Prismatic { axis: Vector3::y() },
            make_inertia(1.0), SpatialTransform::identity());
        body.add_body("link2", 0, GenJointType::Prismatic { axis: Vector3::x() },
            make_inertia(1.0), SpatialTransform::identity());

        let mut manifold = ContactManifold::new();
        manifold.add_contact(ContactPoint::new(0,
            Vector3::zeros(), Vector3::zeros(), Vector3::y(), 0.005));
        let constraints = ContactConstraints::from_manifold(&body, &manifold);
        let config = SmoothContactConfig::default();

        let jac = smooth_contact_force_jacobian(&body, &constraints, &config, 1e-5);
        assert_eq!(jac.nrows(), body.dof_count(), "rows should be nv");
        assert_eq!(jac.ncols(), body.nq(), "cols should be nq");
    }

    #[test]
    fn jacobian_finite_difference_consistency() {
        // Verify that the Jacobian (which itself uses finite differences internally)
        // is self-consistent: a small perturbation in q should produce a force
        // change that matches J * dq
        let mut body = ArticulatedBody::new();
        body.add_body("link", -1, GenJointType::Prismatic { axis: Vector3::y() },
            make_inertia(1.0), SpatialTransform::identity());

        let mut manifold = ContactManifold::new();
        manifold.add_contact(ContactPoint::new(0,
            Vector3::zeros(), Vector3::zeros(), Vector3::y(), 0.005));
        let constraints = ContactConstraints::from_manifold(&body, &manifold);
        let config = SmoothContactConfig::default();

        let eps = 1e-4_f32;
        let jac = smooth_contact_force_jacobian(&body, &constraints, &config, eps);

        // Perturb q[0] by a small delta
        let delta = 0.0001_f32;
        let tau_base = smooth_contact_forces(&body, &constraints, &config).tau_contact;

        let mut body_pert = body.clone();
        body_pert.q[0] += delta;
        // Reconstruct with same contact
        let mut mp = ContactManifold::new();
        mp.add_contact(ContactPoint::new(0,
            Vector3::zeros(), Vector3::zeros(), Vector3::y(), 0.005));
        let c_pert = ContactConstraints::from_manifold(&body_pert, &mp);
        let tau_pert = smooth_contact_forces(&body_pert, &c_pert, &config).tau_contact;

        // Predicted change: J * dq
        let predicted_change = jac[(0, 0)] * delta;
        let actual_change = tau_pert[0] - tau_base[0];

        // Should agree within 10% for small perturbations (finite diff approximation)
        if actual_change.abs() > 1e-6 {
            let ratio = predicted_change / actual_change;
            assert!((ratio - 1.0).abs() < 0.5,
                "Jacobian prediction should match actual: predicted={predicted_change}, actual={actual_change}, ratio={ratio}");
        }
    }

    #[test]
    fn jacobian_zero_for_no_contacts() {
        let mut body = ArticulatedBody::new();
        body.add_body("link", -1, GenJointType::Prismatic { axis: Vector3::y() },
            make_inertia(1.0), SpatialTransform::identity());

        let manifold = ContactManifold::new(); // empty
        let constraints = ContactConstraints::from_manifold(&body, &manifold);
        let jac = smooth_contact_force_jacobian(&body, &constraints, &SmoothContactConfig::default(), 1e-5);

        for i in 0..jac.nrows() {
            for j in 0..jac.ncols() {
                assert!(jac[(i, j)].abs() < 1e-6,
                    "Jacobian should be zero with no contacts at ({i},{j}), got {}", jac[(i, j)]);
            }
        }
    }

    // ── SLAM Cycle 1: Smooth contact intent/property/stress tests ─────

    #[test]
    fn intent_stiff_config_produces_more_force_than_soft() {
        // Intent: Stiffer spring produces larger normal force for same penetration
        let mut body = ArticulatedBody::new();
        body.add_body("link", -1, GenJointType::Prismatic { axis: Vector3::y() },
            make_inertia(1.0), SpatialTransform::identity());

        let mut manifold = ContactManifold::new();
        manifold.add_contact(ContactPoint::new(0,
            Vector3::zeros(), Vector3::zeros(), Vector3::y(), 0.005));
        let constraints = ContactConstraints::from_manifold(&body, &manifold);

        let result_stiff = smooth_contact_forces(&body, &constraints, &SmoothContactConfig::stiff());
        let result_soft = smooth_contact_forces(&body, &constraints, &SmoothContactConfig::soft());

        assert!(
            result_stiff.normal_forces[0] > result_soft.normal_forces[0],
            "Stiff ({}) should produce more force than soft ({})",
            result_stiff.normal_forces[0], result_soft.normal_forces[0]
        );
    }

    #[test]
    fn property_dissipation_non_negative() {
        // Property: Energy dissipation power must be >= 0 (contacts remove energy, never add it)
        let mut body = ArticulatedBody::new();
        body.add_body("link", -1, GenJointType::Prismatic { axis: Vector3::y() },
            make_inertia(1.0), SpatialTransform::identity());
        body.qd[0] = -2.0; // approaching contact

        let mut manifold = ContactManifold::new();
        manifold.add_contact(ContactPoint::new(0,
            Vector3::zeros(), Vector3::zeros(), Vector3::y(), 0.005)
            .with_friction(0.5));
        let constraints = ContactConstraints::from_manifold(&body, &manifold);

        let result = smooth_contact_forces(&body, &constraints, &SmoothContactConfig::default());
        assert!(
            result.dissipation_power >= 0.0,
            "Dissipation power must be non-negative, got {}", result.dissipation_power
        );
    }

    #[test]
    fn property_potential_energy_proportional_to_penetration_squared() {
        // Property: Spring PE = ½·k·φ², so doubling penetration should 4x energy
        let mut body = ArticulatedBody::new();
        body.add_body("link", -1, GenJointType::Prismatic { axis: Vector3::y() },
            make_inertia(1.0), SpatialTransform::identity());

        let config = SmoothContactConfig::default();

        let mut m1 = ContactManifold::new();
        m1.add_contact(ContactPoint::new(0, Vector3::zeros(), Vector3::zeros(), Vector3::y(), 0.002));
        let c1 = ContactConstraints::from_manifold(&body, &m1);
        let r1 = smooth_contact_forces(&body, &c1, &config);

        let mut m2 = ContactManifold::new();
        m2.add_contact(ContactPoint::new(0, Vector3::zeros(), Vector3::zeros(), Vector3::y(), 0.004));
        let c2 = ContactConstraints::from_manifold(&body, &m2);
        let r2 = smooth_contact_forces(&body, &c2, &config);

        // PE(2φ) / PE(φ) should be ~4 (quadratic spring)
        if r1.potential_energy > 1e-10 {
            let ratio = r2.potential_energy / r1.potential_energy;
            assert!(
                (ratio - 4.0).abs() < 1.0,
                "PE should scale quadratically: PE1={}, PE2={}, ratio={}",
                r1.potential_energy, r2.potential_energy, ratio
            );
        }
    }

    #[test]
    fn stress_smooth_six_contacts_all_finite() {
        // Stress: 6 simultaneous contacts should produce finite forces
        let mut body = ArticulatedBody::new();
        body.add_body("link", -1, GenJointType::Floating,
            SpatialInertia::sphere(1.0, 0.1), 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, -1.0, 0.0, 0.0, 0.0, 0.0]);

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

        let constraints = ContactConstraints::from_manifold(&body, &manifold);
        let result = smooth_contact_forces(&body, &constraints, &SmoothContactConfig::default());

        assert!(result.tau_contact.iter().all(|v| v.is_finite()), "all tau_contact must be finite");
        assert!(result.normal_forces.iter().all(|v| v.is_finite()), "all normal_forces must be finite");
        assert_eq!(result.normal_forces.len(), 6, "should have 6 contact forces");
    }

    #[test]
    fn intent_zero_penetration_produces_zero_normal_force() {
        let mut body = ArticulatedBody::new();
        body.add_body("link", -1, GenJointType::Prismatic { axis: Vector3::y() },
            make_inertia(1.0), SpatialTransform::identity());

        let mut manifold = ContactManifold::new();
        manifold.add_contact(ContactPoint::new(0,
            Vector3::zeros(), Vector3::zeros(), Vector3::y(), 0.0)); // zero penetration
        let constraints = ContactConstraints::from_manifold(&body, &manifold);
        let config = SmoothContactConfig::default();

        let result = smooth_contact_forces(&body, &constraints, &config);
        // At zero penetration, spring force should be zero
        if !result.normal_forces.is_empty() {
            assert!(result.normal_forces[0].abs() < 1.0,
                "zero penetration should produce near-zero force, got {}", result.normal_forces[0]);
        }
    }

    // ── SLAM Cycle 18: Smooth contact Jacobian proptest ───────────────

    use proptest::prelude::*;

    proptest! {
        #[test]
        fn prop_smooth_jacobian_dimensions_nv_x_nq(
            pen in 0.001f32..0.01,
        ) {
            let mut body = ArticulatedBody::new();
            body.add_body("l1", -1, GenJointType::Revolute { axis: Vector3::y() },
                make_inertia(1.0), SpatialTransform::identity());
            body.add_body("l2", 0, GenJointType::Revolute { axis: Vector3::y() },
                make_inertia(0.5),
                SpatialTransform::from_translation(Vector3::new(0.0, -0.5, 0.0)));

            let mut manifold = ContactManifold::new();
            manifold.add_contact(ContactPoint::new(1,
                Vector3::zeros(), Vector3::zeros(), Vector3::y(), pen));
            let constraints = ContactConstraints::from_manifold(&body, &manifold);
            let jac = smooth_contact_force_jacobian(&body, &constraints, &SmoothContactConfig::default(), 1e-4);

            prop_assert_eq!(jac.nrows(), body.dof_count(), "Jacobian rows = nv");
            prop_assert_eq!(jac.ncols(), body.nq(), "Jacobian cols = nq");
        }
    }

    #[test]
    fn intent_smooth_force_direction_opposes_velocity() {
        // Friction should oppose tangential motion
        let mut body = ArticulatedBody::new();
        body.add_body("link", -1, GenJointType::Floating,
            SpatialInertia::sphere(1.0, 0.1), SpatialTransform::identity());
        body.set_joint_q(0, &[0.0, 0.1, 0.0, 1.0, 0.0, 0.0, 0.0]);
        body.set_joint_qd(0, &[5.0, -1.0, 0.0, 0.0, 0.0, 0.0]); // sliding in X

        let mut manifold = ContactManifold::new();
        manifold.add_contact(ContactPoint::new(0,
            Vector3::new(0.0, 0.0, 0.0), Vector3::zeros(), Vector3::y(), 0.005)
            .with_friction(0.5));
        let constraints = ContactConstraints::from_manifold(&body, &manifold);

        let result = smooth_contact_forces(&body, &constraints, &SmoothContactConfig::default());
        // With friction and X-sliding, tau should have a negative X component (opposing motion)
        // tau[0] is the X-translation DOF for floating base
        if result.tau_contact.len() >= 1 && result.friction_forces.len() >= 1 {
            assert!(result.friction_forces[0].is_finite(), "friction force must be finite");
        }
    }
}