featherstone 0.1.0

//! Articulated Body Algorithm (ABA) — O(n) Forward Dynamics
//!
//! Given joint positions (q), velocities (qd), and applied torques (tau),
//! compute joint accelerations (qdd) in O(n) time.
//!
//! Three-pass algorithm (Featherstone Ch.7):
//! 1. **Forward pass** (base->tip): compute per-body velocities and bias forces
//! 2. **Backward pass** (tip->base): compute articulated inertias and projected forces
//! 3. **Forward pass** (base->tip): compute accelerations
//!
//! Gravity is modeled as a fictitious upward acceleration of the base (a_0 = -g),
//! following Featherstone's convention.

use nalgebra::{DMatrix, DVector};

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

/// Intermediate data for a single ABA pass (cached between calls for reuse)
#[derive(Clone, Debug)]
pub struct AbaData {
    /// Per-body velocity (spatial)
    pub v: Vec<SpatialVector>,
    /// Per-body bias acceleration (Coriolis + centrifugal)
    pub c: Vec<SpatialVector>,
    /// Per-body articulated inertia
    pub i_a: Vec<SpatialInertia>,
    /// Per-body articulated bias force
    pub p_a: Vec<SpatialVector>,
    /// Per-body acceleration (spatial)
    pub a: 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>>,
    /// D_i = S_i^T * I_A_i * S_i (joint-space inertia, dof x dof per joint)
    pub d: Vec<DMatrix<f32>>,
    /// U_i = I_A_i * S_i (6 x dof per joint)
    pub u: Vec<Vec<SpatialVector>>,
    /// u_i = tau_i - S_i^T * p_A_i (joint-space bias, dof per joint)
    pub uu: Vec<DVector<f32>>,
}

impl AbaData {
    /// Allocate ABA data for n bodies
    pub fn new(n: usize) -> Self {
        Self {
            v: vec![SpatialVector::zero(); n],
            c: vec![SpatialVector::zero(); n],
            i_a: vec![SpatialInertia::zero(); n],
            p_a: vec![SpatialVector::zero(); n],
            a: vec![SpatialVector::zero(); n],
            x_up: vec![SpatialTransform::identity(); n],
            s: vec![Vec::new(); n],
            d: vec![DMatrix::zeros(0, 0); n],
            u: vec![Vec::new(); n],
            uu: vec![DVector::zeros(0); n],
        }
    }
}

/// Run the Articulated Body Algorithm (forward dynamics).
///
/// Computes `body.qdd` from `body.q`, `body.qd`, `body.tau`, and `body.f_ext`.
///
/// # Returns
/// Reference to the computed accelerations `body.qdd`.
pub fn aba_forward_dynamics(body: &mut ArticulatedBody) -> &DVector<f32> {
    let n = body.body_count();
    if n == 0 {
        return &body.qdd;
    }

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

    // ========================================================================
    // Pass 1: Forward (base -> tip) — compute velocities and bias forces
    // ========================================================================
    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));
        // Full transform from parent to this body
        data.x_up[i] = x_j.compose(&bd.x_tree);

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

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

        if bd.parent < 0 {
            // Root body
            data.v[i] = v_j;
            data.c[i] = SpatialVector::zero(); // No Coriolis at root with zero parent velocity
        } else {
            let parent = bd.parent as usize;
            // v_i = X_i * v_parent + v_j
            let v_parent_transformed = data.x_up[i].apply_motion(&data.v[parent]);
            data.v[i] = v_parent_transformed.add(&v_j);
            // Coriolis acceleration: c_i = v_i x v_j
            data.c[i] = data.v[i].cross_motion(&v_j);
        }

        // Initialize articulated inertia with rigid body inertia
        data.i_a[i] = bd.inertia.clone();

        // Bias force: p_A = v x I*v - f_ext
        let iv = data.i_a[i].mul_vector(&data.v[i]);
        data.p_a[i] = data.v[i].cross_force(&iv).sub(&body.f_ext[i]);
    }

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

        if dof > 0 {
            // U_i = I_A_i * S_i
            data.u[i] = data.s[i]
                .iter()
                .map(|s_col| data.i_a[i].mul_vector(s_col))
                .collect();

            // D_i = S_i^T * I_A_i * S_i (dof x dof)
            let mut d_mat = DMatrix::zeros(dof, dof);
            for r in 0..dof {
                for c in 0..dof {
                    d_mat[(r, c)] = data.s[i][r].dot(&data.u[i][c]);
                }
            }
            data.d[i] = d_mat;

            // u_i = tau_i - S_i^T * p_A_i
            let mut uu_vec = DVector::zeros(dof);
            let v_idx = bd.v_index;
            for j in 0..dof {
                let tau_j = body.tau[v_idx + j];
                let st_pa = data.s[i][j].dot(&data.p_a[i]);
                uu_vec[j] = tau_j - st_pa;
            }
            data.uu[i] = uu_vec;

            // Propagate to parent:
            // I_A_parent += X_i* (I_A_i - U_i * D_i^{-1} * U_i^T) X_i^T
            // p_A_parent += X_i* (p_A_i + I_a * c_i + U_i * D_i^{-1} * u_i)
            if bd.parent >= 0 {
                let parent = bd.parent as usize;

                // Compute D^{-1} * u
                let d_inv_u = solve_dof_system(&data.d[i], &data.uu[i]);

                // Compute I_a' = I_A - U * D^{-1} * U^T (rank-dof update)
                let i_a_reduced = subtract_rank_update(&data.i_a[i], &data.u[i], &data.d[i]);

                // p_a' = p_A + I^a * c + U * D^{-1} * u  (Featherstone Alg. 7.1)
                // I^a = I^A - U*D^{-1}*U^T (reduced articulated inertia, NOT full I^A)
                let ia_c = i_a_reduced.mul_vector(&data.c[i]);
                let u_dinv_u = linear_combination(&data.u[i], &d_inv_u);
                let p_a_new = data.p_a[i].add(&ia_c).add(&u_dinv_u);

                // Transform to parent frame and accumulate
                let x_inv = data.x_up[i].inverse();
                let i_a_parent = x_inv.apply_inertia(&i_a_reduced);
                let p_a_parent = x_inv.apply_force(&p_a_new);

                data.i_a[parent] = data.i_a[parent].add(&i_a_parent);
                data.p_a[parent] = data.p_a[parent].add(&p_a_parent);
            }
        } else if bd.parent >= 0 {
            // Fixed joint: just propagate inertia and bias force directly
            let parent = bd.parent as usize;
            let x_inv = data.x_up[i].inverse();
            let ia_c = data.i_a[i].mul_vector(&data.c[i]);
            let p_a_new = data.p_a[i].add(&ia_c);

            data.i_a[parent] = data.i_a[parent].add(&x_inv.apply_inertia(&data.i_a[i]));
            data.p_a[parent] = data.p_a[parent].add(&x_inv.apply_force(&p_a_new));
        }
    }

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

        // Parent acceleration (base acceleration = gravity)
        let a_parent = if bd.parent < 0 {
            body.gravity.clone()
        } else {
            data.a[bd.parent as usize].clone()
        };

        // Transform parent acceleration to this frame
        let a_parent_local = data.x_up[i].apply_motion(&a_parent);

        if dof > 0 {
            // qdd = D^{-1} * (u - U^T * (a_parent_local + c))
            let a_plus_c = a_parent_local.add(&data.c[i]);
            let mut rhs = data.uu[i].clone();
            for j in 0..dof {
                rhs[j] -= data.u[i][j].dot(&a_plus_c);
            }
            let qdd_joint = solve_dof_system(&data.d[i], &rhs);

            // Store joint accelerations (using v_index for velocity-space indexing)
            let v_idx = bd.v_index;
            for j in 0..dof {
                body.qdd[v_idx + j] = qdd_joint[j];
            }

            // Body acceleration: a = a_parent_local + c + S * qdd
            let s_qdd = linear_combination(&data.s[i], &qdd_joint);
            data.a[i] = a_parent_local.add(&data.c[i]).add(&s_qdd);
        } else {
            // Fixed joint: just propagate
            data.a[i] = a_parent_local.add(&data.c[i]);
        }
    }

    &body.qdd
}

// ============================================================================
// Helper functions
// ============================================================================

/// Compute joint velocity: v_j = sum S_k * qd_k
fn compute_joint_velocity(s: &[SpatialVector], qd: &[f32]) -> SpatialVector {
    let mut result = SpatialVector::zero();
    for (k, s_col) in s.iter().enumerate() {
        if k < qd.len() {
            result = result.add(&s_col.scale(qd[k]));
        }
    }
    result
}

/// Compute linear combination: sum v_k * c_k
fn linear_combination(vectors: &[SpatialVector], coeffs: &DVector<f32>) -> SpatialVector {
    let mut result = SpatialVector::zero();
    for (k, v) in vectors.iter().enumerate() {
        if k < coeffs.len() {
            result = result.add(&v.scale(coeffs[k]));
        }
    }
    result
}

/// Solve D * x = b for small DOF systems (1, 3, or 6)
fn solve_dof_system(d: &DMatrix<f32>, b: &DVector<f32>) -> DVector<f32> {
    let n = d.nrows();
    if n == 0 {
        return DVector::zeros(0);
    }
    if n == 1 {
        // Scalar case
        let d_val = d[(0, 0)];
        if d_val.abs() > 1e-6 {
            return DVector::from_element(1, b[0] / d_val);
        }
        return DVector::zeros(1);
    }
    // General case: LU decomposition
    if let Some(lu) = nalgebra::linalg::LU::new(d.clone()).try_inverse() {
        lu * b
    } else {
        // Regularize if singular
        let reg = d + DMatrix::identity(n, n) * 1e-8;
        if let Some(inv) = nalgebra::linalg::LU::new(reg).try_inverse() {
            inv * b
        } else {
            DVector::zeros(n)
        }
    }
}

/// Compute I_A - U * D^{-1} * U^T as a rank update to spatial inertia
fn subtract_rank_update(
    i_a: &SpatialInertia,
    u_cols: &[SpatialVector],
    d: &DMatrix<f32>,
) -> SpatialInertia {
    let dof = u_cols.len();
    if dof == 0 {
        return i_a.clone();
    }

    // Compute D^{-1}
    let d_inv = if dof == 1 {
        let d_val = d[(0, 0)];
        if d_val.abs() > 1e-6 {
            DMatrix::from_element(1, 1, 1.0 / d_val)
        } else {
            DMatrix::zeros(1, 1)
        }
    } else {
        nalgebra::linalg::LU::new(d.clone())
            .try_inverse()
            .unwrap_or_else(|| DMatrix::zeros(dof, dof))
    };

    // Build U as 6 x dof matrix
    let mut u_mat = DMatrix::zeros(6, dof);
    for (j, col) in u_cols.iter().enumerate() {
        for r in 0..6 {
            u_mat[(r, j)] = col.data[r];
        }
    }

    // Update: I_A - U * D^{-1} * U^T
    let update = &u_mat * &d_inv * u_mat.transpose();

    let mut result_data = i_a.data;
    for r in 0..6 {
        for c in 0..6 {
            result_data[(r, c)] -= update[(r, c)];
        }
    }

    SpatialInertia {
        data: result_data,
        mass: i_a.mass,
    }
}

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

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

    #[test]
    fn test_aba_free_fall() {
        // Single body with floating joint: should accelerate at gravity
        let mut body = ArticulatedBody::new();
        body.set_gravity(Vector3::new(0.0, -9.81, 0.0));

        body.add_body(
            "free",
            -1,
            GenJointType::Floating,
            SpatialInertia::sphere(1.0, 0.1),
            SpatialTransform::identity(),
        );

        aba_forward_dynamics(&mut body);

        // DOFs: [x, y, z, rx, ry, rz]
        // y acceleration should be -9.81 (gravity)
        assert_relative_eq!(body.qdd[0], 0.0, epsilon = 1e-6); // x
        assert_relative_eq!(body.qdd[1], -9.81, epsilon = 1e-4); // y (gravity)
        assert_relative_eq!(body.qdd[2], 0.0, epsilon = 1e-6); // z
        assert_relative_eq!(body.qdd[3], 0.0, epsilon = 1e-6); // rx
        assert_relative_eq!(body.qdd[4], 0.0, epsilon = 1e-6); // ry
        assert_relative_eq!(body.qdd[5], 0.0, epsilon = 1e-6); // rz
    }

    #[test]
    fn test_aba_single_pendulum_gravity_torque() {
        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(),
        );

        aba_forward_dynamics(&mut body);

        assert!(body.qdd[0] < 0.0, "Pendulum should accelerate under gravity: {}", body.qdd[0]);
        assert!(body.qdd[0].abs() > 1.0, "Acceleration should be significant");
    }

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

        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.5, 0.0, 0.0)),
        );

        aba_forward_dynamics(&mut body);

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

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

        let inertia = SpatialInertia::from_mass_inertia_at_com(
            1.0,
            Matrix3::from_diagonal(&Vector3::new(0.1, 0.1, 0.1)),
        );

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

        body.set_joint_tau(0, &[1.0]);
        aba_forward_dynamics(&mut body);

        // qdd = tau / I_zz = 1.0 / 0.1 = 10.0
        assert_relative_eq!(body.qdd[0], 10.0, epsilon = 0.1);
    }

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

        let inertia = SpatialInertia::from_mass_inertia_at_com(
            1.0,
            Matrix3::from_diagonal(&Vector3::new(0.01, 0.01, 0.01)),
        );

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

        body.set_joint_tau(1, &[1.0]);
        aba_forward_dynamics(&mut body);

        assert!(body.qdd[1] > 0.0, "Distal joint should accelerate: qdd[1]={}", body.qdd[1]);
        assert!(body.qdd[0].is_finite(), "Joint 0 should be finite");
    }

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

        body.add_body(
            "slider",
            -1,
            GenJointType::Prismatic { axis: Vector3::new(0.0, -1.0, 0.0) },
            SpatialInertia::sphere(1.0, 0.1),
            SpatialTransform::identity(),
        );

        aba_forward_dynamics(&mut body);

        assert_relative_eq!(body.qdd[0], 9.81, epsilon = 0.1);
    }

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

        body.add_body(
            "base",
            -1,
            GenJointType::Fixed,
            make_inertia(1.0),
            SpatialTransform::identity(),
        );
        body.add_body(
            "arm",
            0,
            GenJointType::Revolute { axis: Vector3::z() },
            make_inertia(1.0),
            SpatialTransform::from_translation(Vector3::new(0.5, 0.0, 0.0)),
        );

        assert_eq!(body.dof_count(), 1);

        body.set_joint_tau(1, &[0.5]);
        aba_forward_dynamics(&mut body);

        assert!(body.qdd[0] > 0.0, "Applied torque should produce acceleration");
    }

    #[test]
    fn test_aba_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(),
        );

        body.set_external_force(
            0,
            SpatialVector::new(Vector3::new(0.0, 0.0, 2.0), Vector3::zeros()),
        );

        aba_forward_dynamics(&mut body);

        assert!(body.qdd[0].abs() > 0.1, "External force should cause acceleration");
    }

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

        // Floating base with spherical child
        body.add_body(
            "base",
            -1,
            GenJointType::Floating,
            SpatialInertia::sphere(1.0, 0.1),
            SpatialTransform::identity(),
        );
        body.add_body(
            "ball",
            0,
            GenJointType::Spherical,
            SpatialInertia::sphere(0.5, 0.05),
            SpatialTransform::from_translation(Vector3::new(0.3, 0.0, 0.0)),
        );

        assert_eq!(body.dof_count(), 9); // 6 + 3
        assert_eq!(body.nq(), 11); // 7 + 4

        aba_forward_dynamics(&mut body);

        // Base should accelerate downward
        assert_relative_eq!(body.qdd[1], -9.81, epsilon = 0.5);
        // All accelerations should be finite
        for i in 0..body.dof_count() {
            assert!(body.qdd[i].is_finite(), "qdd[{i}] should be finite: {}", body.qdd[i]);
        }
    }

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

    #[test]
    fn test_aba_symmetric_torques_produce_opposite_accelerations() {
        // Intent: equal and opposite torques on two identical pendulums
        // should produce equal and opposite accelerations
        let mut body1 = ArticulatedBody::new();
        body1.set_gravity(Vector3::zeros());
        body1.add_body(
            "link", -1,
            GenJointType::Revolute { axis: Vector3::z() },
            SpatialInertia::sphere(1.0, 0.1),
            SpatialTransform::identity(),
        );

        let mut body2 = body1.clone();

        body1.tau[0] = 5.0;
        body2.tau[0] = -5.0;

        aba_forward_dynamics(&mut body1);
        aba_forward_dynamics(&mut body2);

        assert_relative_eq!(body1.qdd[0], -body2.qdd[0], epsilon = 1e-5);
    }

    #[test]
    fn test_aba_heavier_body_accelerates_less() {
        // Intent: F=ma — heavier body should have smaller acceleration
        let make_pendulum = |mass: f32| {
            let mut body = ArticulatedBody::new();
            body.set_gravity(Vector3::zeros());
            body.add_body(
                "link", -1,
                GenJointType::Revolute { axis: Vector3::z() },
                SpatialInertia::sphere(mass, 0.1),
                SpatialTransform::identity(),
            );
            body.tau[0] = 10.0;
            body
        };

        let mut light = make_pendulum(1.0);
        let mut heavy = make_pendulum(10.0);

        aba_forward_dynamics(&mut light);
        aba_forward_dynamics(&mut heavy);

        assert!(
            light.qdd[0].abs() > heavy.qdd[0].abs(),
            "Light body (qdd={}) should accelerate more than heavy (qdd={})",
            light.qdd[0], heavy.qdd[0]
        );
    }

    #[test]
    fn test_aba_all_accelerations_finite_multi_link() {
        // Intent: ABA should produce finite accelerations for complex trees
        let mut body = ArticulatedBody::new();
        body.set_gravity(Vector3::new(0.0, -9.81, 0.0));

        // Build a 5-link chain
        for i in 0..5 {
            let parent = if i == 0 { -1 } else { (i - 1) as i32 };
            body.add_body(
                format!("link{i}"),
                parent,
                GenJointType::Revolute { axis: Vector3::z() },
                SpatialInertia::sphere(0.5, 0.05),
                SpatialTransform::from_translation(Vector3::new(0.2, 0.0, 0.0)),
            );
        }

        aba_forward_dynamics(&mut body);

        for i in 0..body.dof_count() {
            assert!(body.qdd[i].is_finite(), "qdd[{i}] should be finite: {}", body.qdd[i]);
        }
    }

    // ── SLAM Cycle 1: Intent tests ──────────────────────────────────

    #[test]
    fn intent_aba_double_mass_halves_acceleration() {
        // Physics intent: F=ma → doubling mass halves acceleration for same torque
        let make_pendulum = |mass: f32| -> ArticulatedBody {
            let mut body = ArticulatedBody::new();
            body.set_gravity(Vector3::new(0.0, 0.0, 0.0));
            body.add_body(
                "link", -1, GenJointType::Revolute { axis: Vector3::z() },
                make_inertia(mass),
                SpatialTransform::identity(),
            );
            body
        };

        let mut body1 = make_pendulum(1.0);
        body1.tau[0] = 1.0;
        aba_forward_dynamics(&mut body1);
        let acc1 = body1.qdd[0];

        let mut body2 = make_pendulum(2.0);
        body2.tau[0] = 1.0;
        aba_forward_dynamics(&mut body2);
        let acc2 = body2.qdd[0];

        assert!(
            (acc2 / acc1 - 0.5).abs() < 0.15,
            "doubling mass should halve accel: acc1={}, acc2={}, ratio={}",
            acc1, acc2, acc2 / acc1
        );
    }

    #[test]
    fn intent_aba_symmetric_torques_produce_opposite_accelerations() {
        let make_pendulum = || -> ArticulatedBody {
            let mut body = ArticulatedBody::new();
            body.set_gravity(Vector3::new(0.0, 0.0, 0.0));
            body.add_body(
                "link", -1, GenJointType::Revolute { axis: Vector3::z() },
                make_inertia(1.0),
                SpatialTransform::identity(),
            );
            body
        };

        let mut body_pos = make_pendulum();
        body_pos.tau[0] = 5.0;
        aba_forward_dynamics(&mut body_pos);

        let mut body_neg = make_pendulum();
        body_neg.tau[0] = -5.0;
        aba_forward_dynamics(&mut body_neg);

        assert!(
            (body_pos.qdd[0] + body_neg.qdd[0]).abs() < 1e-5,
            "symmetric torques should produce opposite accel: {} vs {}",
            body_pos.qdd[0], body_neg.qdd[0]
        );
    }

    #[test]
    fn intent_aba_rnea_roundtrip_recovers_torques() {
        use crate::rnea::rnea_inverse_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() },
            make_inertia(2.0),
            SpatialTransform::identity(),
        );
        body.add_body(
            "link2", 0, GenJointType::Revolute { axis: Vector3::z() },
            make_inertia(1.0),
            SpatialTransform::from_translation(Vector3::new(0.0, -1.0, 0.0)),
        );

        let tau_original = [3.0_f32, -1.5];
        body.tau[0] = tau_original[0];
        body.tau[1] = tau_original[1];
        body.qd[0] = 0.5;
        body.qd[1] = -0.3;

        aba_forward_dynamics(&mut body);

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

        for i in 0..2 {
            assert!(
                (tau_recovered[i] - tau_original[i]).abs() < 0.5,
                "RNEA should recover torque[{}]: original={}, recovered={}",
                i, tau_original[i], tau_recovered[i]
            );
        }
    }

    // ── SLAM Cycle 10: Determinism + stress tests ────────────────────

    #[test]
    fn determinism_aba_same_input_same_output() {
        // Physics intent: ABA is deterministic — same state → same acceleration
        let make_body = || {
            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, -1.0, 0.0)));
            body.q[0] = 0.3;
            body.q[1] = -0.5;
            body.qd[0] = 1.0;
            body.qd[1] = -0.7;
            body.tau[0] = 2.5;
            body.tau[1] = -1.0;
            body
        };

        let mut body_a = make_body();
        let mut body_b = make_body();

        aba_forward_dynamics(&mut body_a);
        aba_forward_dynamics(&mut body_b);

        for i in 0..body_a.dof_count() {
            assert_eq!(body_a.qdd[i], body_b.qdd[i],
                "ABA must be deterministic: qdd[{}] differs", i);
        }
    }

    #[test]
    fn stress_aba_1000_steps_no_nan() {
        // Stress: run ABA + semi-implicit Euler 1000 times, verify no NaN/Inf
        use crate::integrator::{step, IntegratorConfig, IntegrationMethod};

        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.tau[0] = 0.1; // small constant torque

        let config = IntegratorConfig {
            dt: 0.001,
            method: IntegrationMethod::SemiImplicitEuler,
            ..Default::default()
        };

        for s in 0..1000 {
            step(&mut body, &config);
            for i in 0..body.q.len() {
                assert!(body.q[i].is_finite(), "q[{}] NaN at step {}", i, s);
            }
            for i in 0..body.qd.len() {
                assert!(body.qd[i].is_finite(), "qd[{}] NaN at step {}", i, s);
            }
        }
    }

    #[test]
    fn stress_aba_branching_tree_100_steps() {
        // Stress: 4-body branching tree, 100 steps, no NaN
        use crate::integrator::{step, IntegratorConfig, IntegrationMethod};

        let mut body = ArticulatedBody::new();
        body.set_gravity(Vector3::new(0.0, -9.81, 0.0));
        body.add_body("torso", -1, GenJointType::Floating,
            SpatialInertia::sphere(5.0, 0.2), SpatialTransform::identity());
        body.add_body("left", 0, GenJointType::Revolute { axis: Vector3::z() },
            make_inertia(1.0), SpatialTransform::from_translation(Vector3::new(-0.5, 0.0, 0.0)));
        body.add_body("right", 0, GenJointType::Revolute { axis: Vector3::z() },
            make_inertia(1.0), SpatialTransform::from_translation(Vector3::new(0.5, 0.0, 0.0)));
        body.add_body("head", 0, GenJointType::Revolute { axis: Vector3::x() },
            make_inertia(0.5), SpatialTransform::from_translation(Vector3::new(0.0, 0.5, 0.0)));

        let config = IntegratorConfig {
            dt: 0.004,
            method: IntegrationMethod::SemiImplicitEuler,
            ..Default::default()
        };

        for s in 0..100 {
            step(&mut body, &config);
            for i in 0..body.q.len() {
                assert!(body.q[i].is_finite(), "q[{}] NaN at step {} (branching tree)", i, s);
            }
        }
    }

    #[test]
    fn stress_aba_20_body_chain_all_finite() {
        // Stress: 20-link revolute chain with varied initial q, all qdd must be finite
        let mut body = ArticulatedBody::new();
        body.set_gravity(Vector3::new(0.0, -9.81, 0.0));

        // Hardcoded "random-ish" q values for each of the 20 links
        let q_values: [f32; 20] = [
            0.3, -0.7, 1.2, -0.1, 0.8, -1.5, 0.05, 0.9, -0.4, 1.1,
            -0.6, 0.2, -1.0, 0.55, -0.35, 1.4, -0.85, 0.15, -1.3, 0.65,
        ];

        for i in 0..20 {
            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(0.5),
                SpatialTransform::from_translation(Vector3::new(0.15, 0.0, 0.0)),
            );
        }

        // Set initial joint positions
        for i in 0..20 {
            body.q[i] = q_values[i];
        }

        aba_forward_dynamics(&mut body);

        assert_eq!(body.dof_count(), 20);
        for i in 0..body.dof_count() {
            assert!(
                body.qdd[i].is_finite(),
                "qdd[{i}] is not finite ({}) in 20-link chain",
                body.qdd[i]
            );
        }
    }

    // ── SLAM Cycle 5: ABA proptest ───────────────────────────────────

    use proptest::prelude::*;

    proptest! {
        #[test]
        fn prop_aba_double_torque_doubles_acceleration(
            tau in 0.1f32..10.0,
        ) {
            // F=ma: doubling applied torque should double resulting acceleration
            let run = |t: f32| -> f32 {
                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.tau[0] = t;
                aba_forward_dynamics(&mut body);
                body.qdd[0]
            };

            let qdd1 = run(tau);
            let qdd2 = run(2.0 * tau);

            if qdd1.abs() > 1e-6 {
                let ratio = qdd2 / qdd1;
                prop_assert!((ratio - 2.0).abs() < 0.1,
                    "2x torque should give 2x accel: ratio={ratio}");
            }
        }

        #[test]
        fn prop_aba_output_always_finite(
            q in -2.0f32..2.0,
            qd in -5.0f32..5.0,
            tau 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() },
                make_inertia(1.0), SpatialTransform::identity());
            body.q[0] = q;
            body.qd[0] = qd;
            body.tau[0] = tau;
            aba_forward_dynamics(&mut body);
            prop_assert!(body.qdd[0].is_finite(),
                "qdd must be finite for q={q}, qd={qd}, tau={tau}");
        }
    }

    #[test]
    fn intent_aba_zero_torque_zero_gravity_zero_velocity_zero_acceleration() {
        // With no gravity, no torques, no velocity, ABA should produce zero acceleration
        let mut body = ArticulatedBody::new();
        body.set_gravity(Vector3::zeros());

        for i in 0..5 {
            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.3, 0.0, 0.0)),
            );
        }

        // Ensure all inputs are zero (they should be by default, but be explicit)
        for i in 0..body.dof_count() {
            body.q[i] = 0.0;
            body.qd[i] = 0.0;
            body.tau[i] = 0.0;
        }

        aba_forward_dynamics(&mut body);

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