featherstone 0.1.0

//! Recursive Newton-Euler Algorithm (RNEA) — O(n) Inverse Dynamics
//!
//! Given joint positions (q), velocities (qd), and accelerations (qdd),
//! compute the required joint torques (tau) in O(n) time.
//!
//! Two-pass algorithm (Featherstone Ch.5):
//! 1. **Forward pass** (base->tip): propagate velocities and accelerations
//! 2. **Backward pass** (tip->base): compute forces and project onto joint axes
//!
//! Used for:
//! - Gravity compensation: tau = RNEA(q, 0, 0)
//! - Coriolis/centrifugal forces: C(q,qd) = RNEA(q, qd, 0) - RNEA(q, 0, 0)
//! - Computed-torque control: tau = RNEA(q, qd, qdd_desired)
//! - Bias forces: C(q,qd) = RNEA(q, qd, 0) (includes gravity)

use nalgebra::DVector;

use super::body::ArticulatedBody;
use super::spatial::{SpatialTransform, SpatialVector};

/// Cached intermediate data from an RNEA pass.
///
/// Stores per-body velocities, accelerations, and transforms for reuse
/// by the contact solver and other algorithms.
#[derive(Clone, Debug)]
pub struct RneaData {
    /// Per-body spatial velocity (body frame)
    pub v: Vec<SpatialVector>,
    /// Per-body spatial acceleration (body frame)
    pub a: Vec<SpatialVector>,
    /// Per-body net force (body frame)
    pub f: Vec<SpatialVector>,
    /// Per-body transform from parent: X_i = X_tree * X_joint
    pub x_up: Vec<SpatialTransform>,
    /// Motion subspace columns (cached)
    pub s: Vec<Vec<SpatialVector>>,
}

impl RneaData {
    /// Allocate RNEA data for n bodies
    pub fn new(n: usize) -> Self {
        Self {
            v: vec![SpatialVector::zero(); n],
            a: vec![SpatialVector::zero(); n],
            f: vec![SpatialVector::zero(); n],
            x_up: vec![SpatialTransform::identity(); n],
            s: vec![Vec::new(); n],
        }
    }
}

/// Run the Recursive Newton-Euler Algorithm (inverse dynamics).
///
/// Computes joint torques tau from q, qd, qdd, f_ext.
/// Returns the torque vector (dimension = dof_count).
///
/// Also returns cached intermediate data for reuse.
pub fn rnea_inverse_dynamics(body: &ArticulatedBody) -> (DVector<f32>, RneaData) {
    let n = body.body_count();
    let nv = body.dof_count();
    let mut tau = DVector::zeros(nv);

    if n == 0 {
        return (tau, RneaData::new(0));
    }

    let mut data = RneaData::new(n);

    // ========================================================================
    // Pass 1: Forward (base -> tip) — compute velocities and accelerations
    // ========================================================================
    for i in body.iter_base_to_tip() {
        let bd = &body.bodies[i];

        // Joint transform
        let x_j = bd.joint_type.transform(body.joint_q(i));
        data.x_up[i] = x_j.compose(&bd.x_tree);

        // Motion subspace
        data.s[i] = bd.joint_type.motion_subspace(body.joint_q(i));
        let _dof = bd.joint_type.dof();

        // Joint velocity: v_j = S * qd
        let qd_slice = body.joint_qd(i);
        let v_j = spatial_linear_combination(&data.s[i], qd_slice);

        // Joint acceleration: a_j = S * qdd
        let qdd_slice = joint_qdd_slice(body, i);
        let a_j = spatial_linear_combination(&data.s[i], qdd_slice);

        if bd.parent < 0 {
            // Root body: parent velocity = 0, parent acceleration = gravity
            data.v[i] = v_j.clone();
            // a_i = X_i * a_0 + S * qdd + c_i
            // where a_0 = gravity (base acceleration), c_i = v_i x v_j (Coriolis)
            let a_gravity = data.x_up[i].apply_motion(&body.gravity);
            let c_i = data.v[i].cross_motion(&v_j);
            data.a[i] = a_gravity.add(&a_j).add(&c_i);
        } else {
            let parent = bd.parent as usize;
            // v_i = X_i * v_parent + v_j
            let v_parent = data.x_up[i].apply_motion(&data.v[parent]);
            data.v[i] = v_parent.add(&v_j);
            // a_i = X_i * a_parent + S * qdd + v_i x v_j
            let a_parent = data.x_up[i].apply_motion(&data.a[parent]);
            let c_i = data.v[i].cross_motion(&v_j);
            data.a[i] = a_parent.add(&a_j).add(&c_i);
        }
    }

    // ========================================================================
    // Pass 2: Backward (tip -> base) — compute forces and torques
    // ========================================================================
    for i in body.iter_tip_to_base() {
        let bd = &body.bodies[i];

        // f_i = I_i * a_i + v_i x* (I_i * v_i) - f_ext_i
        let i_a = bd.inertia.mul_vector(&data.a[i]);
        let i_v = bd.inertia.mul_vector(&data.v[i]);
        let gyroscopic = data.v[i].cross_force(&i_v);
        data.f[i] = data.f[i].add(&i_a).add(&gyroscopic).sub(&body.f_ext[i]);

        // Project onto joint axes: tau_i = S_i^T * f_i
        let dof = bd.joint_type.dof();
        if dof > 0 {
            let v_idx = bd.v_index;
            for j in 0..dof {
                tau[v_idx + j] = data.s[i][j].dot(&data.f[i]);
            }
        }

        // Propagate force to parent: f_parent += X_i^{-T} * f_i
        if bd.parent >= 0 {
            let parent = bd.parent as usize;
            let x_inv = data.x_up[i].inverse();
            data.f[parent] = data.f[parent].add(&x_inv.apply_force(&data.f[i]));
        }
    }

    (tau, data)
}

/// Compute gravity compensation torques: tau = RNEA(q, 0, 0).
///
/// Returns the torques needed to hold the current configuration against gravity
/// with zero velocity and zero acceleration.
pub fn gravity_compensation(body: &ArticulatedBody) -> DVector<f32> {
    // Create a temporary body with zero velocity and acceleration
    let mut temp = body.clone();
    temp.qd.fill(0.0);
    temp.qdd.fill(0.0);
    temp.clear_external_forces();
    let (tau, _) = rnea_inverse_dynamics(&temp);
    tau
}

/// Compute Coriolis + centrifugal + gravity forces: C(q, qd).
///
/// C = RNEA(q, qd, 0) — the torques needed to produce zero acceleration
/// at the current state. Includes gravity terms.
pub fn bias_forces_rnea(body: &ArticulatedBody) -> DVector<f32> {
    let mut temp = body.clone();
    temp.qdd.fill(0.0);
    let (tau, _) = rnea_inverse_dynamics(&temp);
    tau
}

/// Compute pure Coriolis + centrifugal forces (no gravity): C(q, qd) - g(q).
pub fn coriolis_forces(body: &ArticulatedBody) -> DVector<f32> {
    let bias = bias_forces_rnea(body);
    let grav = gravity_compensation(body);
    bias - grav
}

// ============================================================================
// Helpers
// ============================================================================

/// Get qdd slice for body i's joint (using v_index)
fn joint_qdd_slice(body: &ArticulatedBody, i: usize) -> &[f32] {
    let bd = &body.bodies[i];
    let dof = bd.joint_type.dof();
    if dof == 0 {
        return &[];
    }
    &body.qdd.as_slice()[bd.v_index..bd.v_index + dof]
}

/// Compute spatial linear combination: sum S_k * c_k
fn spatial_linear_combination(s: &[SpatialVector], coeffs: &[f32]) -> SpatialVector {
    let mut result = SpatialVector::zero();
    for (k, s_col) in s.iter().enumerate() {
        if k < coeffs.len() {
            result = result.add(&s_col.scale(coeffs[k]));
        }
    }
    result
}

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

#[cfg(test)]
mod tests {
    use super::*;
    use super::super::aba::aba_forward_dynamics;
    use super::super::body::GenJointType;
    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_rnea_empty() {
        let body = ArticulatedBody::new();
        let (tau, _) = rnea_inverse_dynamics(&body);
        assert_eq!(tau.len(), 0);
    }

    #[test]
    fn test_gravity_compensation_single_pendulum() {
        // Pendulum: mass at distance L from joint, gravity -Y
        // Gravity torque at angle θ around Z: τ = m*g*L*cos(θ) for small θ
        // At θ=0 (arm along X): τ_grav = -m*g*L (gravity tries to rotate CW)
        let mass = 1.0_f32;
        let length = 0.5_f32;
        let g = 9.81_f32;

        let mut body = ArticulatedBody::new();
        body.set_gravity(Vector3::new(0.0, -g, 0.0));

        let inertia = SpatialInertia::from_mass_inertia(
            mass,
            Vector3::new(length, 0.0, 0.0),
            Matrix3::from_diagonal(&Vector3::new(0.001, 0.001, 0.001)),
        );

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

        let tau = gravity_compensation(&body);

        // At θ=0: gravity tries to rotate CW (negative Z torque)
        // RNEA returns the torque needed to produce zero acceleration,
        // which is the positive compensation torque: +m*g*L
        let expected = mass * g * length;
        assert_relative_eq!(tau[0], expected, epsilon = 0.01);
    }

    #[test]
    fn test_rnea_aba_round_trip() {
        // RNEA(ABA output) should reproduce the input torques
        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)),
            );
        }

        // Set some state
        body.set_joint_q(0, &[0.3]);
        body.set_joint_q(1, &[-0.2]);
        body.set_joint_qd(0, &[1.0]);
        body.set_joint_qd(1, &[-0.5]);
        body.set_joint_tau(0, &[0.5]);
        body.set_joint_tau(1, &[-0.3]);
        body.set_joint_tau(2, &[0.1]);

        let original_tau = body.tau.clone();

        // ABA: compute qdd from tau
        aba_forward_dynamics(&mut body);
        let _qdd = body.qdd.clone();

        // RNEA: compute tau from qdd (should recover original tau)
        let (tau_recovered, _) = rnea_inverse_dynamics(&body);

        for i in 0..body.dof_count() {
            assert_relative_eq!(
                tau_recovered[i],
                original_tau[i],
                epsilon = 0.01
            );
        }
    }

    #[test]
    fn test_coriolis_zero_at_zero_velocity() {
        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_q(0, &[0.5]);
        // qd = 0 (default)

        let c = coriolis_forces(&body);

        for i in 0..c.len() {
            assert_relative_eq!(c[i], 0.0, epsilon = 1e-6);
        }
    }

    #[test]
    fn test_gravity_compensation_balances_aba() {
        // If we apply gravity compensation torques, ABA should give zero acceleration
        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_q(0, &[0.3]);
        body.set_joint_q(1, &[-0.5]);
        body.set_joint_q(2, &[0.7]);

        // Compute gravity compensation
        let tau_grav = gravity_compensation(&body);

        // Apply as torques
        body.tau = tau_grav;

        // ABA should give zero acceleration (gravity balanced)
        aba_forward_dynamics(&mut body);

        for i in 0..body.dof_count() {
            assert_relative_eq!(body.qdd[i], 0.0, epsilon = 1e-4);
        }
    }

    #[test]
    fn test_rnea_zero_gravity_zero_state() {
        // No gravity, zero state → zero torques
        let mut body = ArticulatedBody::new();
        body.set_gravity(Vector3::zeros());

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

        let (tau, _) = rnea_inverse_dynamics(&body);
        assert_relative_eq!(tau[0], 0.0, epsilon = 1e-8);
    }

    #[test]
    fn test_rnea_with_acceleration() {
        // Single link, no gravity: τ = I * qdd
        let mut body = ArticulatedBody::new();
        body.set_gravity(Vector3::zeros());

        let i_zz = 0.1_f32;
        body.add_body(
            "link",
            -1,
            GenJointType::Revolute { axis: Vector3::z() },
            SpatialInertia::from_mass_inertia_at_com(
                1.0,
                Matrix3::from_diagonal(&Vector3::new(i_zz, i_zz, i_zz)),
            ),
            SpatialTransform::identity(),
        );

        // Set desired acceleration
        body.qdd[0] = 10.0;

        let (tau, _) = rnea_inverse_dynamics(&body);

        // τ = I_zz * qdd = 0.1 * 10 = 1.0
        assert_relative_eq!(tau[0], 1.0, epsilon = 0.01);
    }

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

        body.add_body(
            "link1",
            -1,
            GenJointType::Revolute { axis: Vector3::z() },
            make_inertia(1.0),
            SpatialTransform::identity(),
        );
        body.add_body(
            "link2",
            0,
            GenJointType::Revolute { axis: Vector3::z() },
            make_inertia(1.0),
            SpatialTransform::from_translation(Vector3::new(0.3, 0.0, 0.0)),
        );

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

        let (_, data) = rnea_inverse_dynamics(&body);

        // Verify cached data has correct dimensions
        assert_eq!(data.v.len(), 2);
        assert_eq!(data.a.len(), 2);
        assert_eq!(data.f.len(), 2);

        // Root velocity should be non-zero (qd=1.0 around Z)
        assert!(data.v[0].angular().z.abs() > 0.5);
    }

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

        body.add_body(
            "base",
            -1,
            GenJointType::Floating,
            SpatialInertia::sphere(5.0, 0.2),
            SpatialTransform::identity(),
        );
        body.add_body(
            "arm",
            0,
            GenJointType::Revolute { axis: Vector3::z() },
            make_inertia(1.0),
            SpatialTransform::from_translation(Vector3::new(0.3, 0.0, 0.0)),
        );

        let (tau, _) = rnea_inverse_dynamics(&body);
        assert_eq!(tau.len(), 7); // 6 + 1

        // All values should be finite
        for i in 0..tau.len() {
            assert!(tau[i].is_finite(), "tau[{i}] = {}", tau[i]);
        }
    }

    #[test]
    fn test_rnea_external_force() {
        let mut body = ArticulatedBody::new();
        body.set_gravity(Vector3::zeros());

        body.add_body(
            "link",
            -1,
            GenJointType::Revolute { axis: Vector3::z() },
            SpatialInertia::from_mass_inertia_at_com(
                1.0,
                Matrix3::from_diagonal(&Vector3::new(0.1, 0.1, 0.1)),
            ),
            SpatialTransform::identity(),
        );

        // Apply external torque around Z
        body.set_external_force(
            0,
            SpatialVector::new(Vector3::new(0.0, 0.0, 5.0), Vector3::zeros()),
        );

        let (tau, _) = rnea_inverse_dynamics(&body);

        // External force should reduce required torque
        // With qdd=0 and no gravity, tau should counteract external force: τ = -f_ext
        assert_relative_eq!(tau[0], -5.0, epsilon = 0.1);
    }

    // ======================================================================
    // Intent / property tests
    // ======================================================================

    #[test]
    fn test_rnea_aba_consistency() {
        // Intent: RNEA(ABA_output) should recover the applied torques
        // τ = RNEA(q, qd, ABA(q, qd, τ))
        use crate::aba::aba_forward_dynamics;

        let mut body = ArticulatedBody::new();
        body.set_gravity(Vector3::new(0.0, -9.81, 0.0));

        body.add_body(
            "link1", -1,
            GenJointType::Revolute { axis: Vector3::z() },
            SpatialInertia::from_mass_inertia_at_com(
                1.0,
                Matrix3::from_diagonal(&Vector3::new(0.1, 0.1, 0.1)),
            ),
            SpatialTransform::identity(),
        );
        body.add_body(
            "link2", 0,
            GenJointType::Revolute { axis: Vector3::z() },
            SpatialInertia::from_mass_inertia_at_com(
                0.5,
                Matrix3::from_diagonal(&Vector3::new(0.05, 0.05, 0.05)),
            ),
            SpatialTransform::from_translation(Vector3::new(0.3, 0.0, 0.0)),
        );

        // Apply known torques
        body.tau[0] = 5.0;
        body.tau[1] = -2.0;

        // Forward dynamics: compute qdd from tau
        aba_forward_dynamics(&mut body);

        // Inverse dynamics: recover tau from qdd
        let (tau_recovered, _) = rnea_inverse_dynamics(&body);

        assert_relative_eq!(tau_recovered[0], 5.0, epsilon = 0.1);
        assert_relative_eq!(tau_recovered[1], -2.0, epsilon = 0.1);
    }

    #[test]
    fn test_rnea_zero_state_finite() {
        // Intent: RNEA at zero state should produce finite torques
        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() },
                SpatialInertia::from_mass_inertia_at_com(
                    1.0,
                    Matrix3::from_diagonal(&Vector3::new(0.01, 0.01, 0.01)),
                ),
                SpatialTransform::from_translation(Vector3::new(0.3, 0.0, 0.0)),
            );
        }

        let (tau, _) = rnea_inverse_dynamics(&body);
        for (i, &t) in tau.iter().enumerate() {
            assert!(t.is_finite(), "tau[{i}] should be finite: {t}");
        }
    }

    // ── SLAM Cycle 7: RNEA intent tests ──────────────────────────────

    #[test]
    fn intent_rnea_gravity_compensation_balances_gravity() {
        // Intent: RNEA(q, 0, 0) gives torques that hold a static posture
        // Applying these torques via ABA should yield zero acceleration
        use crate::aba::aba_forward_dynamics;

        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() },
            make_inertia(2.0), SpatialTransform::identity());
        body.q[0] = 0.5; // tilted posture

        // Get gravity compensation torques
        let (tau_grav, _) = rnea_inverse_dynamics(&body);

        // Apply them and check ABA gives ~zero acceleration
        body.tau[0] = tau_grav[0];
        aba_forward_dynamics(&mut body);

        assert!(
            body.qdd[0].abs() < 0.1,
            "gravity-compensated torque should yield near-zero accel: qdd={}",
            body.qdd[0]
        );
    }

    #[test]
    fn intent_rnea_zero_state_zero_gravity_gives_zero_torques() {
        // Intent: no motion + no gravity → zero required torques
        let mut body = ArticulatedBody::new();
        body.set_gravity(Vector3::new(0.0, 0.0, 0.0));
        body.add_body("l1", -1, GenJointType::Revolute { axis: Vector3::z() },
            make_inertia(1.0), SpatialTransform::identity());

        let (tau, _) = rnea_inverse_dynamics(&body);
        assert!(tau[0].abs() < 1e-6, "zero state + zero gravity → zero torque: {}", tau[0]);
    }

    #[test]
    fn intent_rnea_aba_roundtrip_multi_dof() {
        // Intent: RNEA(q, qd, qdd) gives tau → ABA(q, qd, tau) gives back qdd
        // This verifies RNEA and ABA are consistent (inverse/forward dynamics agreement)
        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() },
            make_inertia(1.0), SpatialTransform::identity());
        body.add_body("l2", 0, GenJointType::Revolute { axis: Vector3::z() },
            make_inertia(0.5), SpatialTransform::from_translation(Vector3::new(0.0, 0.5, 0.0)));

        body.q[0] = 0.3;
        body.q[1] = -0.2;
        body.qd[0] = 1.0;
        body.qd[1] = -0.5;

        // First: compute forward dynamics to get natural accelerations
        aba_forward_dynamics(&mut body);
        let qdd_aba = vec![body.qdd[0], body.qdd[1]];

        // Now: use RNEA to compute the torques needed for those accelerations
        // We need to set qdd first
        body.qdd[0] = qdd_aba[0];
        body.qdd[1] = qdd_aba[1];
        body.tau = DVector::zeros(2); // clear tau before RNEA

        // RNEA with the ABA accelerations should give zero external torques
        // because ABA computed natural (tau=0) accelerations
        let (tau_rnea, _) = rnea_inverse_dynamics(&body);

        for i in 0..2 {
            assert!(tau_rnea[i].abs() < 0.5,
                "RNEA/ABA roundtrip: tau[{i}]={} should be near zero (natural dynamics)",
                tau_rnea[i]);
        }
    }

    #[test]
    fn intent_rnea_bias_forces_grow_with_velocity() {
        // Physics intent: bias forces (C*qd) grow with velocity for a spinning body
        // Use a non-symmetric inertia on a floating base to ensure gyroscopic effects
        use nalgebra::Matrix3;

        let mut body_slow = ArticulatedBody::new();
        body_slow.set_gravity(Vector3::zeros());
        // Non-symmetric inertia guarantees gyroscopic coupling
        let inertia = SpatialInertia::from_mass_inertia_at_com(
            2.0,
            Matrix3::new(1.0, 0.0, 0.0,
                         0.0, 2.0, 0.0,
                         0.0, 0.0, 3.0),
        );
        body_slow.add_body("body", -1, GenJointType::Floating, inertia, SpatialTransform::identity());

        // Floating joint DOF order: qd[0..3]=linear, qd[3..6]=angular
        body_slow.qd[3] = 1.0; // wx
        body_slow.qd[4] = 0.5; // wy
        body_slow.qd[5] = 0.3; // wz

        let mut body_fast = body_slow.clone();
        body_fast.qd[3] = 10.0;
        body_fast.qd[4] = 5.0;
        body_fast.qd[5] = 3.0;

        let (tau_slow, _) = rnea_inverse_dynamics(&body_slow);
        let (tau_fast, _) = rnea_inverse_dynamics(&body_fast);

        let mag_slow: f32 = tau_slow.iter().map(|t| t * t).sum::<f32>().sqrt();
        let mag_fast: f32 = tau_fast.iter().map(|t| t * t).sum::<f32>().sqrt();

        assert!(mag_fast > mag_slow,
            "higher angular velocity should produce larger gyroscopic torques: slow={mag_slow}, fast={mag_fast}");
    }

    // ── SLAM Cycle 1: RNEA intent/property tests ─────────────────────

    #[test]
    fn property_rnea_linear_in_qdd() {
        // Property: τ should scale linearly with qdd (for zero qd)
        let mut body = ArticulatedBody::new();
        body.set_gravity(Vector3::zeros());
        body.add_body("link", -1,
            GenJointType::Revolute { axis: Vector3::z() },
            SpatialInertia::from_mass_inertia_at_com(1.0, Matrix3::identity() * 0.1),
            SpatialTransform::identity());

        body.qdd[0] = 1.0;
        let (tau1, _) = rnea_inverse_dynamics(&body);

        body.qdd[0] = 3.0;
        let (tau3, _) = rnea_inverse_dynamics(&body);

        // tau3 should be ~3 * tau1
        if tau1[0].abs() > 1e-8 {
            let ratio = tau3[0] / tau1[0];
            assert!((ratio - 3.0).abs() < 0.1,
                "RNEA should be linear in qdd: tau(1)={}, tau(3)={}, ratio={ratio}", tau1[0], tau3[0]);
        }
    }

    #[test]
    fn intent_gravity_compensation_produces_finite_torques() {
        // Intent: For any valid configuration, gravity compensation should produce finite torques
        let mut body = ArticulatedBody::new();
        body.set_gravity(Vector3::new(0.0, -9.81, 0.0));
        for i in 0..5 {
            let parent = if i == 0 { -1 } else { (i - 1) as i32 };
            body.add_body(&format!("l{i}"), parent,
                GenJointType::Revolute { axis: Vector3::z() },
                SpatialInertia::sphere(1.0, 0.1),
                SpatialTransform::from_translation(Vector3::new(0.0, -0.3, 0.0)));
        }
        // Random-ish config
        body.q[0] = 0.5; body.q[1] = -0.8; body.q[2] = 1.2;

        let tau = gravity_compensation(&body);
        for i in 0..tau.len() {
            assert!(tau[i].is_finite(), "gravity_comp tau[{i}]={} must be finite", tau[i]);
        }
    }

    // ── SLAM Cycle 7: RNEA proptests ──────────────────────────────────

    use proptest::prelude::*;

    proptest! {
        #[test]
        fn prop_rnea_output_always_finite(
            q in -2.0f32..2.0,
            qd in -5.0f32..5.0,
            qdd in -10.0f32..10.0,
        ) {
            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() },
                SpatialInertia::from_mass_inertia_at_com(1.0, Matrix3::identity() * 0.1),
                SpatialTransform::identity());
            body.q[0] = q;
            body.qd[0] = qd;
            body.qdd[0] = qdd;

            let (tau, _) = rnea_inverse_dynamics(&body);
            prop_assert!(tau[0].is_finite(), "RNEA tau must be finite for q={q}, qd={qd}, qdd={qdd}");
        }

        #[test]
        fn prop_gravity_compensation_finite(
            q in -3.0f32..3.0,
        ) {
            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() },
                SpatialInertia::sphere(1.0, 0.1),
                SpatialTransform::from_translation(Vector3::new(0.0, -0.5, 0.0)));
            body.q[0] = q;

            let tau = gravity_compensation(&body);
            prop_assert!(tau[0].is_finite(), "gravity comp must be finite at q={q}");
        }
    }

    // ── SLAM Cycle 9: Determinism and boundary tests ──────────────────

    #[test]
    fn determinism_rnea_same_input_same_output() {
        let make = || {
            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() },
                make_inertia(2.0), SpatialTransform::identity());
            body.add_body("l2", 0, GenJointType::Revolute { axis: Vector3::z() },
                make_inertia(1.0), SpatialTransform::from_translation(Vector3::new(0.0, -0.5, 0.0)));
            body.q[0] = 0.7;
            body.q[1] = -0.3;
            body.qd[0] = 2.0;
            body.qd[1] = -1.5;
            body.qdd[0] = 0.5;
            body.qdd[1] = -0.2;
            body
        };

        let (tau1, _) = rnea_inverse_dynamics(&make());
        let (tau2, _) = rnea_inverse_dynamics(&make());

        for i in 0..tau1.len() {
            assert_eq!(tau1[i], tau2[i], "RNEA must be deterministic at tau[{i}]");
        }
    }

    #[test]
    fn edge_rnea_large_velocity_produces_finite_torques() {
        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.qd[0] = 1000.0; // very large velocity

        let (tau, _) = rnea_inverse_dynamics(&body);
        assert!(tau[0].is_finite(), "large velocity should still give finite torque: {}", tau[0]);
    }

    #[test]
    fn intent_coriolis_nonzero_for_multi_link_with_velocity() {
        // Multi-link chain with significant offset and velocity SHOULD have nonzero Coriolis.
        // Use larger bodies with offset COM for stronger centripetal effects.
        let mut body = ArticulatedBody::new();
        body.set_gravity(Vector3::zeros());
        let inertia = SpatialInertia::from_mass_inertia(
            1.0, Vector3::new(0.0, -0.25, 0.0),
            Matrix3::from_diagonal(&Vector3::new(0.1, 0.01, 0.1)),
        );
        body.add_body("l1", -1, GenJointType::Revolute { axis: Vector3::z() },
            inertia.clone(), SpatialTransform::identity());
        body.add_body("l2", 0, GenJointType::Revolute { axis: Vector3::z() },
            inertia,
            SpatialTransform::from_translation(Vector3::new(0.0, -0.5, 0.0)));
        body.q[0] = 0.5;  // nonzero q needed — Coriolis depends on sin(q)
        body.q[1] = -0.3;
        body.qd[0] = 5.0;
        body.qd[1] = -3.0;

        let c = coriolis_forces(&body);
        let mag: f32 = c.iter().map(|v| v * v).sum::<f32>().sqrt();
        assert!(mag > 1e-6,
            "multi-link with velocity+nonzero q should have nonzero Coriolis: ||c||={mag}");
    }

    #[test]
    fn intent_coriolis_zero_for_single_revolute() {
        // Single revolute with no branching has zero coriolis forces
        // (coriolis only appears with multi-body coupling or asymmetric inertia)
        let mut body = ArticulatedBody::new();
        body.set_gravity(Vector3::zeros());
        body.add_body("link", -1, GenJointType::Revolute { axis: Vector3::z() },
            make_inertia(1.0), SpatialTransform::identity());
        body.qd[0] = 5.0;

        let c = coriolis_forces(&body);
        assert!(c[0].abs() < 1e-4,
            "single revolute should have ~zero coriolis: {}", c[0]);
    }
}