featherstone 0.1.0

//! Time integration methods for articulated body dynamics
//!
//! Provides multiple integration schemes for stepping the articulated body
//! state forward in time given the computed accelerations.
//!
//! Available integrators:
//! - **Semi-implicit Euler**: Fast, stable for stiff contacts (default)
//! - **Symplectic Euler**: Energy-conserving for conservative systems
//! - **RK4**: High accuracy for smooth dynamics, 4 ABA evaluations per step
//! - **Velocity Verlet**: Good energy conservation with 2nd-order accuracy
//!
//! All integrators handle the dual q/v indexing for quaternion joints.

use nalgebra::{UnitQuaternion, Quaternion, Vector3};

use super::aba::aba_forward_dynamics;
use super::body::ArticulatedBody;
use super::limits::JointLimits;

/// Integration method selection.
#[derive(Clone, Copy, Debug, PartialEq, Eq, Default)]
pub enum IntegrationMethod {
    /// Semi-implicit Euler (symplectic): update velocity first, then position.
    /// 1st-order, good stability for contacts. O(n) per step.
    #[default]
    SemiImplicitEuler,
    /// Explicit (forward) Euler: update position first, then velocity.
    /// 1st-order, less stable. Included for comparison only.
    ExplicitEuler,
    /// Classic 4th-order Runge-Kutta. 4th-order accuracy.
    /// Requires 4 ABA evaluations per step — use for smooth dynamics.
    RK4,
    /// Velocity Verlet (Störmer-Verlet): 2nd-order, symplectic.
    /// Requires 2 ABA evaluations per step.
    VelocityVerlet,
    /// Implicit Euler (backward Euler): unconditionally stable for stiff systems.
    /// Uses Newton iteration to solve the implicit equation.
    /// 1st-order, high damping. Best for tendons, muscles, stiff contacts.
    ImplicitEuler,
}

/// Integrator configuration.
#[derive(Clone, Debug)]
pub struct IntegratorConfig {
    /// Integration method
    pub method: IntegrationMethod,
    /// Timestep (seconds)
    pub dt: f32,
    /// Per-joint limits (optional, indexed by body index)
    pub joint_limits: Vec<Option<JointLimits>>,
    /// Velocity damping factor (1.0 = no damping, 0.99 = slight damping)
    pub velocity_damping: f32,
    /// Whether to normalize quaternion joints after integration
    pub normalize_quaternions: bool,
}

impl Default for IntegratorConfig {
    fn default() -> Self {
        Self {
            method: IntegrationMethod::SemiImplicitEuler,
            dt: 0.001,
            joint_limits: Vec::new(),
            velocity_damping: 1.0,
            normalize_quaternions: true,
        }
    }
}

/// Step the articulated body forward by one timestep.
///
/// This is the main entry point. Calls ABA for forward dynamics, applies
/// joint limits, and integrates the state.
pub fn step(body: &mut ArticulatedBody, config: &IntegratorConfig) {
    match config.method {
        IntegrationMethod::SemiImplicitEuler => step_semi_implicit_euler(body, config),
        IntegrationMethod::ExplicitEuler => step_explicit_euler(body, config),
        IntegrationMethod::RK4 => step_rk4(body, config),
        IntegrationMethod::VelocityVerlet => step_velocity_verlet(body, config),
        IntegrationMethod::ImplicitEuler => step_implicit_euler(body, config),
    }
}

// ============================================================================
// Split-step phases (used by backend for MuJoCo-style step ordering)
// ============================================================================

/// Phase A: Compute joint accelerations via ABA with limit forces.
///
/// Saves user torques, applies transient joint-limit forces, runs
/// Articulated Body Algorithm (ABA) forward dynamics, then restores
/// user torques. After this call, `body.qdd` contains the joint
/// accelerations.
pub fn compute_accelerations(body: &mut ArticulatedBody, config: &IntegratorConfig) {
    let tau_user = body.tau.clone();
    apply_limit_forces(body, config);
    aba_forward_dynamics(body);
    body.tau = tau_user;
}

/// Phase B: Update velocities from accelerations, apply damping and limits.
///
/// Integrates `qd += qdd * dt`, applies velocity damping, and clamps
/// to joint velocity limits. Call after `compute_accelerations()`.
pub fn update_velocities(body: &mut ArticulatedBody, config: &IntegratorConfig) {
    let dt = config.dt as f64;
    let nv = body.dof_count();
    // f64 compensated summation — compute in double, store in float.
    // The compensation accumulates the f64→f32 truncation residual across steps.
    // When enough sub-ULP increments accumulate, they push qd past the next ULP.
    for i in 0..nv {
        let qd = body.qd[i] as f64;
        let qdd = body.qdd[i] as f64;
        let comp = body.qd_compensation[i] as f64;
        // Add accumulated residual to this step's increment
        let y = qdd * dt + comp;
        let t = qd + y;
        let stored = t as f32;
        // Track what was lost in the f64→f32 truncation
        body.qd_compensation[i] = (t - stored as f64) as f32;
        body.qd[i] = stored;
    }
    if config.velocity_damping < 1.0 {
        // Scale both velocity and compensation to preserve Kahan accuracy
        let d = config.velocity_damping;
        body.qd *= d;
        body.qd_compensation *= d;
    }
    apply_velocity_limits(body, config);
}

/// Phase C: Integrate positions from velocities, apply limits and normalization.
///
/// Integrates `q += qd * dt` with exponential-map quaternion integration,
/// then clamps to joint position limits and normalizes quaternions.
/// Call after `update_velocities()` and any velocity-level contact corrections.
pub fn update_positions(body: &mut ArticulatedBody, config: &IntegratorConfig) {
    integrate_positions(body, config.dt);
    apply_position_limits(body, config);
    if config.normalize_quaternions {
        body.normalize_quaternions();
    }
}

// ============================================================================
// Full integration methods
// ============================================================================

/// Semi-implicit Euler: qd += qdd*dt, q += qd*dt
///
/// Updates velocity first (using current acceleration), then position
/// (using updated velocity). This is symplectic and provides good
/// energy behavior for Hamiltonian systems.
fn step_semi_implicit_euler(body: &mut ArticulatedBody, config: &IntegratorConfig) {
    compute_accelerations(body, config);
    update_velocities(body, config);
    update_positions(body, config);
}

/// Explicit (forward) Euler: q += qd*dt, qd += qdd*dt
fn step_explicit_euler(body: &mut ArticulatedBody, config: &IntegratorConfig) {
    let dt = config.dt;

    let tau_user = body.tau.clone();
    apply_limit_forces(body, config);
    aba_forward_dynamics(body);
    body.tau = tau_user;

    // Save current velocity
    let qd_old = body.qd.clone();

    // Update positions first (using old velocity)
    integrate_positions(body, dt);

    // Update velocities with f64 compensated summation
    let nv = body.dof_count();
    let dt64 = dt as f64;
    for i in 0..nv {
        let increment = body.qdd[i] as f64 * dt64;
        let comp = body.qd_compensation[i] as f64;
        let y = increment + comp;
        let t = qd_old[i] as f64 + y;
        let stored = t as f32;
        body.qd_compensation[i] = (t - stored as f64) as f32;
        body.qd[i] = stored;
    }

    if config.velocity_damping < 1.0 {
        let d = config.velocity_damping;
        body.qd *= d;
        body.qd_compensation *= d;
    }

    apply_velocity_limits(body, config);
    apply_position_limits(body, config);

    if config.normalize_quaternions {
        body.normalize_quaternions();
    }
}

/// Classic 4th-order Runge-Kutta
fn step_rk4(body: &mut ArticulatedBody, config: &IntegratorConfig) {
    let dt = config.dt;
    let nv = body.dof_count();

    // Save initial state (including Kahan compensation for position)
    let q0 = body.q.clone();
    let qd0 = body.qd.clone();
    let comp0 = body.q_compensation.clone();

    let tau_user = body.tau.clone();

    // k1: evaluate at current state
    apply_limit_forces(body, config);
    aba_forward_dynamics(body);
    body.tau = tau_user.clone();
    let k1_qd = body.qd.clone();
    let k1_qdd = body.qdd.clone();

    // k2: evaluate at t + dt/2 with k1
    // Reset q and compensation to initial state before each intermediate evaluation
    body.q = q0.clone();
    body.q_compensation = comp0.clone();
    body.qd = k1_qd.clone();
    integrate_positions(body, dt / 2.0);
    for i in 0..nv {
        body.qd[i] = qd0[i] + k1_qdd[i] * dt / 2.0;
    }
    apply_limit_forces(body, config);
    aba_forward_dynamics(body);
    body.tau = tau_user.clone();
    let k2_qd = body.qd.clone();
    let k2_qdd = body.qdd.clone();

    // k3: evaluate at t + dt/2 with k2
    body.q = q0.clone();
    body.q_compensation = comp0.clone();
    body.qd = k2_qd.clone();
    integrate_positions(body, dt / 2.0);
    for i in 0..nv {
        body.qd[i] = qd0[i] + k2_qdd[i] * dt / 2.0;
    }
    apply_limit_forces(body, config);
    aba_forward_dynamics(body);
    body.tau = tau_user.clone();
    let k3_qd = body.qd.clone();
    let k3_qdd = body.qdd.clone();

    // k4: evaluate at t + dt with k3
    body.q = q0.clone();
    body.q_compensation = comp0.clone();
    body.qd = k3_qd.clone();
    integrate_positions(body, dt);
    for i in 0..nv {
        body.qd[i] = qd0[i] + k3_qdd[i] * dt;
    }
    apply_limit_forces(body, config);
    aba_forward_dynamics(body);
    body.tau = tau_user;
    let k4_qdd = body.qdd.clone();

    // body.qd currently holds the k4 evaluation velocity (qd0 + k3_qdd * dt)
    let k4_qd = body.qd.clone();

    // Combine positions: use weighted-average velocity for integration
    // to handle quaternion joints via exponential map properly.
    // Restore compensation to initial state for the actual step.
    body.q = q0;
    body.q_compensation = comp0;
    let qd_avg = (&k1_qd + &k2_qd * 2.0 + &k3_qd * 2.0 + &k4_qd) * (1.0 / 6.0);
    body.qd = qd_avg;
    integrate_positions(body, dt);

    // Combine velocities: qd = qd0 + (dt/6)*(k1_qdd + 2*k2_qdd + 2*k3_qdd + k4_qdd)
    // Direct assignment — reset compensation since this is a fresh computed value
    for i in 0..nv {
        body.qd[i] = qd0[i]
            + dt / 6.0 * (k1_qdd[i] + 2.0 * k2_qdd[i] + 2.0 * k3_qdd[i] + k4_qdd[i]);
        body.qd_compensation[i] = 0.0;
    }

    if config.velocity_damping < 1.0 {
        body.qd *= config.velocity_damping;
    }
    apply_velocity_limits(body, config);
    apply_position_limits(body, config);

    if config.normalize_quaternions {
        body.normalize_quaternions();
    }
}

/// Velocity Verlet (Störmer-Verlet): 2nd-order symplectic
fn step_velocity_verlet(body: &mut ArticulatedBody, config: &IntegratorConfig) {
    let dt = config.dt;
    let nv = body.dof_count();
    let tau_user = body.tau.clone();

    // Step 1: compute acceleration at current state
    apply_limit_forces(body, config);
    aba_forward_dynamics(body);
    body.tau = tau_user.clone();
    let qdd_old = body.qdd.clone();

    // Step 2: update position using half-kick velocity: qd_half = qd + 0.5*qdd*dt
    // Then integrate positions with proper quaternion handling
    let qd_saved = body.qd.clone();
    for i in 0..nv {
        body.qd[i] += 0.5 * qdd_old[i] * dt;
    }
    integrate_positions(body, dt);
    // Restore velocity for step 4
    body.qd = qd_saved;

    // Step 3: compute new acceleration at new position
    apply_limit_forces(body, config);
    aba_forward_dynamics(body);
    body.tau = tau_user;

    // Step 4: update velocity with f64 compensated summation: qd += 0.5*(qdd_old + qdd_new)*dt
    for i in 0..nv {
        let increment = 0.5_f64 * (qdd_old[i] as f64 + body.qdd[i] as f64) * dt as f64;
        let comp = body.qd_compensation[i] as f64;
        let y = increment + comp;
        let t = body.qd[i] as f64 + y;
        let stored = t as f32;
        body.qd_compensation[i] = (t - stored as f64) as f32;
        body.qd[i] = stored;
    }

    if config.velocity_damping < 1.0 {
        let d = config.velocity_damping;
        body.qd *= d;
        body.qd_compensation *= d;
    }
    apply_velocity_limits(body, config);
    apply_position_limits(body, config);

    if config.normalize_quaternions {
        body.normalize_quaternions();
    }
}

/// Implicit (backward) Euler: unconditionally stable for stiff systems.
///
/// Solves q_{n+1} = q_n + qd_{n+1} * dt, qd_{n+1} = qd_n + qdd(q_{n+1}) * dt
/// via Newton iteration. More expensive per step (2-5 ABA evals) but allows
/// larger timesteps for stiff dynamics (tendons, muscles, high-stiffness contacts).
fn step_implicit_euler(body: &mut ArticulatedBody, config: &IntegratorConfig) {
    let dt = config.dt;
    let nv = body.dof_count();

    // Store initial velocity (qd_0) for the implicit equation: qd_{n+1} = qd_0 + qdd(q_{n+1}, qd_{n+1}) * dt
    let qd0 = body.qd.clone();

    // Predictor: semi-implicit Euler as initial guess
    let tau_user = body.tau.clone();
    apply_limit_forces(body, config);
    aba_forward_dynamics(body);
    body.tau = tau_user.clone();

    // Initial guess: forward Euler prediction
    for i in 0..nv {
        body.qd[i] = qd0[i] + body.qdd[i] * dt;
    }

    // Newton corrector: iterate to find consistent qd_{n+1} = qd_0 + qdd(q_{n+1}, qd_{n+1}) * dt
    for _newton_iter in 0..3 {
        // Save ALL position state (including both Kahan compensators)
        let q_saved = body.q.clone();
        let q_comp_saved = body.q_compensation.clone();
        let qd_comp_saved = body.qd_compensation.clone();
        integrate_positions(body, dt);

        // Re-evaluate dynamics at new (q, qd)
        body.tau = tau_user.clone();
        apply_limit_forces(body, config);
        aba_forward_dynamics(body);
        body.tau = tau_user.clone();

        // Direct implicit Euler: qd_{n+1} = qd_0 + qdd(q_{n+1}, qd_{n+1}) * dt
        for i in 0..nv {
            body.qd[i] = qd0[i] + body.qdd[i] * dt;
        }

        // Restore ALL position state for next iteration
        body.q = q_saved;
        body.q_compensation = q_comp_saved;
        body.qd_compensation = qd_comp_saved;
    }

    // Reset velocity compensation — converged velocity is a fresh value
    body.qd_compensation.fill(0.0);

    // Final position update with converged velocity
    if config.velocity_damping < 1.0 {
        body.qd *= config.velocity_damping;
    }
    apply_velocity_limits(body, config);
    integrate_positions(body, dt);
    apply_position_limits(body, config);
    if config.normalize_quaternions {
        body.normalize_quaternions();
    }
}

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

/// Integrate generalized positions: q += qd * dt.
///
/// Handles quaternion joints specially: integrates angular velocity
/// into quaternion using exponential map.
///
/// Uses Kahan compensated summation for scalar position updates to reduce
/// f32 round-off error during long simulations (hundreds of timesteps).
/// This brings projectile range accuracy from ~0.15% to ~0.01% vs MuJoCo.
fn integrate_positions(body: &mut ArticulatedBody, dt: f32) {
    for i in 0..body.body_count() {
        let bd = &body.bodies[i];
        let dof = bd.joint_type.dof();

        if dof == 0 {
            continue;
        }

        match &bd.joint_type {
            super::body::GenJointType::Spherical => {
                // Quaternion integration via exponential map (no Kahan needed —
                // quaternion multiplication doesn't accumulate additive error)
                let q_idx = bd.q_index;
                let v_idx = bd.v_index;

                let qw = body.q[q_idx];
                let qi = body.q[q_idx + 1];
                let qj = body.q[q_idx + 2];
                let qk = body.q[q_idx + 3];
                let current = UnitQuaternion::new_normalize(
                    Quaternion::new(qw, qi, qj, qk),
                );

                let wx = body.qd[v_idx] * dt;
                let wy = body.qd[v_idx + 1] * dt;
                let wz = body.qd[v_idx + 2] * dt;

                let angle = (wx * wx + wy * wy + wz * wz).sqrt();
                let delta = if angle > 1e-10 {
                    let axis = Vector3::new(wx, wy, wz) / angle;
                    UnitQuaternion::from_axis_angle(
                        &nalgebra::Unit::new_normalize(axis),
                        angle,
                    )
                } else {
                    UnitQuaternion::identity()
                };

                let new_q = current * delta;
                body.q[q_idx] = new_q.w;
                body.q[q_idx + 1] = new_q.i;
                body.q[q_idx + 2] = new_q.j;
                body.q[q_idx + 3] = new_q.k;
            }
            super::body::GenJointType::Floating => {
                // First 3 DOF: translational — use Kahan compensated summation
                let q_idx = bd.q_index;
                let v_idx = bd.v_index;

                for k in 0..3 {
                    let idx = q_idx + k;
                    // f64 Kahan for translational position
                    let q = body.q[idx] as f64;
                    let qd = body.qd[v_idx + k] as f64;
                    let comp = body.q_compensation[idx] as f64;
                    let y = qd * (dt as f64) + comp;
                    let t = q + y;
                    let stored = t as f32;
                    body.q_compensation[idx] = (t - stored as f64) as f32;
                    body.q[idx] = stored;
                }

                // Next 3 DOF: rotational (quaternion at q_idx+3..q_idx+7)
                // No Kahan needed — quaternion multiplication, not accumulation
                let qw = body.q[q_idx + 3];
                let qi = body.q[q_idx + 4];
                let qj = body.q[q_idx + 5];
                let qk = body.q[q_idx + 6];
                let current = UnitQuaternion::new_normalize(
                    Quaternion::new(qw, qi, qj, qk),
                );

                let wx = body.qd[v_idx + 3] * dt;
                let wy = body.qd[v_idx + 4] * dt;
                let wz = body.qd[v_idx + 5] * dt;

                let angle = (wx * wx + wy * wy + wz * wz).sqrt();
                let delta = if angle > 1e-10 {
                    let axis = Vector3::new(wx, wy, wz) / angle;
                    UnitQuaternion::from_axis_angle(
                        &nalgebra::Unit::new_normalize(axis),
                        angle,
                    )
                } else {
                    UnitQuaternion::identity()
                };

                let new_q = current * delta;
                body.q[q_idx + 3] = new_q.w;
                body.q[q_idx + 4] = new_q.i;
                body.q[q_idx + 5] = new_q.j;
                body.q[q_idx + 6] = new_q.k;
            }
            _ => {
                // Scalar integration for revolute, prismatic, etc.
                // Uses Kahan compensated summation to reduce f32 round-off
                let q_idx = bd.q_index;
                let v_idx = bd.v_index;
                for k in 0..dof {
                    let idx = q_idx + k;
                    // f64 Kahan for scalar position DOFs
                    let q = body.q[idx] as f64;
                    let qd = body.qd[v_idx + k] as f64;
                    let comp = body.q_compensation[idx] as f64;
                    let y = qd * (dt as f64) + comp;
                    let t = q + y;
                    let stored = t as f32;
                    body.q_compensation[idx] = (t - stored as f64) as f32;
                    body.q[idx] = stored;
                }
            }
        }
    }
}

/// Apply joint limit barrier forces to tau.
fn apply_limit_forces(body: &mut ArticulatedBody, config: &IntegratorConfig) {
    for (i, limits_opt) in config.joint_limits.iter().enumerate() {
        if let Some(limits) = limits_opt {
            if i < body.body_count() {
                let forces = limits.compute_limit_force(body.joint_q(i), body.joint_qd(i));
                let bd = &body.bodies[i];
                let v_idx = bd.v_index;
                let dof = bd.joint_type.dof();
                for (k, &f) in forces.iter().enumerate().take(dof.min(forces.len())) {
                    body.tau[v_idx + k] += f;
                }
            }
        }
    }
}

/// Clamp joint velocities to limits.
fn apply_velocity_limits(body: &mut ArticulatedBody, config: &IntegratorConfig) {
    for (i, limits_opt) in config.joint_limits.iter().enumerate() {
        if let Some(limits) = limits_opt {
            if i < body.body_count() {
                let clamped = limits.clamp_velocity(body.joint_qd(i));
                let bd = &body.bodies[i];
                let v_idx = bd.v_index;
                let dof = bd.joint_type.dof();
                for (k, &c) in clamped.iter().enumerate().take(dof.min(clamped.len())) {
                    body.qd[v_idx + k] = c;
                    body.qd_compensation[v_idx + k] = 0.0;
                }
            }
        }
    }
}

/// Hard position clamping — safety net after integration.
///
/// If a joint exceeds its hard limits (despite soft barrier forces), clamp the
/// position to the limit and zero out velocity directed into the limit. This
/// prevents divergence when barrier stiffness/damping is too high for the timestep.
fn apply_position_limits(body: &mut ArticulatedBody, config: &IntegratorConfig) {
    for (i, limits_opt) in config.joint_limits.iter().enumerate() {
        if let Some(limits) = limits_opt {
            if !limits.enabled || i >= body.body_count() {
                continue;
            }
            let bd = &body.bodies[i];
            let q_idx = bd.q_index;
            let v_idx = bd.v_index;
            let dof = bd.joint_type.dof();
            for k in 0..dof.min(limits.lower.len()) {
                let lo = limits.lower[k];
                let hi = limits.upper[k];
                if lo == f32::NEG_INFINITY && hi == f32::INFINITY {
                    continue;
                }
                if body.q[q_idx + k] < lo {
                    body.q[q_idx + k] = lo;
                    body.q_compensation[q_idx + k] = 0.0;
                    if body.qd[v_idx + k] < 0.0 {
                        body.qd[v_idx + k] = 0.0;
                        body.qd_compensation[v_idx + k] = 0.0;
                    }
                } else if body.q[q_idx + k] > hi {
                    body.q[q_idx + k] = hi;
                    body.q_compensation[q_idx + k] = 0.0;
                    if body.qd[v_idx + k] > 0.0 {
                        body.qd[v_idx + k] = 0.0;
                        body.qd_compensation[v_idx + k] = 0.0;
                    }
                }
            }
        }
    }
}

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

#[cfg(test)]
mod tests {
    use super::*;
    use super::super::body::GenJointType;
    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)
    }

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

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

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

        body
    }

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

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

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

        // Simulate 1 second of free fall
        for _ in 0..1000 {
            step(&mut body, &config);
        }

        let t = 1.0;
        let expected_y = -0.5 * 9.81 * t * t; // -4.905
        assert_relative_eq!(body.q[0], expected_y, epsilon = 0.1);
    }

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

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

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

        for _ in 0..1000 {
            step(&mut body, &config);
        }

        let expected_y = -0.5 * 9.81 * 1.0;
        assert_relative_eq!(body.q[0], expected_y, epsilon = 0.1);
    }

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

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

        let config = IntegratorConfig {
            method: IntegrationMethod::RK4,
            dt: 0.01, // larger timestep — RK4 should still be accurate
            ..Default::default()
        };

        for _ in 0..100 {
            step(&mut body, &config);
        }

        let expected_y = -0.5 * 9.81 * 1.0;
        // RK4 should be very accurate for constant acceleration
        assert_relative_eq!(body.q[0], expected_y, epsilon = 0.01);
    }

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

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

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

        for _ in 0..1000 {
            step(&mut body, &config);
        }

        let expected_y = -0.5 * 9.81 * 1.0;
        assert_relative_eq!(body.q[0], expected_y, epsilon = 0.05);
    }

    #[test]
    fn test_pendulum_energy_conservation() {
        // Undamped pendulum should approximately conserve energy
        let mut body = make_pendulum();
        body.set_joint_q(0, &[0.3]); // Start displaced

        let config = IntegratorConfig {
            method: IntegrationMethod::SemiImplicitEuler,
            dt: 0.0001, // Small dt for energy conservation
            ..Default::default()
        };

        // Compute initial energy
        let initial_energy = pendulum_energy(&body);

        for _ in 0..10000 {
            step(&mut body, &config);
        }

        let final_energy = pendulum_energy(&body);

        // Energy should be approximately conserved (within 1%)
        let energy_drift = ((final_energy - initial_energy) / initial_energy).abs();
        assert!(
            energy_drift < 0.01,
            "Energy drift: {energy_drift:.4} (initial={initial_energy:.4}, final={final_energy:.4})"
        );
    }

    #[test]
    fn test_integrator_with_limits() {
        let mut body = make_pendulum();
        body.set_joint_q(0, &[1.5]); // Start near limit

        let limits = JointLimits::single(-1.57, 1.57)
            .with_barrier(5000.0, 500.0, 0.05);

        let config = IntegratorConfig {
            dt: 0.001,
            joint_limits: vec![Some(limits)],
            ..Default::default()
        };

        let mut max_q = 0.0_f32;
        for _ in 0..1000 {
            step(&mut body, &config);
            max_q = max_q.max(body.q[0].abs());
        }

        // Joint should stay near limits
        assert!(max_q < 2.0, "Joint exceeded limits: max_q={max_q}");
    }

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

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

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

        let config = IntegratorConfig {
            dt: 0.001,
            velocity_damping: 0.99,
            ..Default::default()
        };

        for _ in 0..1000 {
            step(&mut body, &config);
        }

        // Velocity should have decreased significantly
        assert!(body.qd[0].abs() < 1.0, "Velocity should decay: {}", body.qd[0]);
    }

    #[test]
    fn test_multi_link_integration() {
        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.1]);

        let config = IntegratorConfig::default();

        // Should simulate without panicking
        for _ in 0..100 {
            step(&mut body, &config);
        }

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

    #[test]
    fn test_rk4_spherical_joint_quaternion_normalization() {
        // Spherical joints use quaternion representation (4 pos params, 3 vel DOF).
        // The integrator must use exponential map integration to keep quaternions normalized.
        let mut body = ArticulatedBody::new();
        body.set_gravity(Vector3::new(0.0, -9.81, 0.0));

        body.add_body(
            "ball",
            -1,
            GenJointType::Spherical,
            make_inertia(1.0),
            SpatialTransform::identity(),
        );

        // Give it angular velocity
        body.set_joint_qd(0, &[1.0, 0.5, -0.3]);

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

        for _ in 0..100 {
            step(&mut body, &config);
        }

        // Quaternion should remain normalized (q[0..4] = w,x,y,z)
        let qw = body.q[0];
        let qx = body.q[1];
        let qy = body.q[2];
        let qz = body.q[3];
        let norm = (qw * qw + qx * qx + qy * qy + qz * qz).sqrt();
        assert!(
            (norm - 1.0).abs() < 0.01,
            "Quaternion should be normalized after RK4, got norm = {norm}"
        );
    }

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

        body.add_body(
            "ball",
            -1,
            GenJointType::Spherical,
            make_inertia(1.0),
            SpatialTransform::identity(),
        );

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

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

        for _ in 0..100 {
            step(&mut body, &config);
        }

        let qw = body.q[0];
        let qx = body.q[1];
        let qy = body.q[2];
        let qz = body.q[3];
        let norm = (qw * qw + qx * qx + qy * qy + qz * qz).sqrt();
        assert!(
            (norm - 1.0).abs() < 0.01,
            "Quaternion should be normalized after Velocity Verlet, got norm = {norm}"
        );
    }

    /// Compute pendulum total energy (kinetic + potential).
    fn pendulum_energy(body: &ArticulatedBody) -> f32 {
        let q = body.q[0];
        let qd = body.qd[0];

        // Pendulum: m=1, L=0.3 (COM offset), g=9.81
        let m = 1.0;
        let l = 0.3;
        let g = 9.81;
        let i_total = 0.01 + m * l * l; // I_zz + m*L^2

        let ke = 0.5 * i_total * qd * qd;
        // COM at (L,0,0) rotated by θ about Z → height = L*sin(θ)
        let pe = m * g * l * q.sin();

        ke + pe
    }

    // ======================================================================
    // Intent tests: integrator correctness properties
    // ======================================================================

    #[test]
    fn test_all_integrators_produce_finite_results() {
        // Intent: every integrator should produce finite state for a simple system
        let methods = [
            IntegrationMethod::SemiImplicitEuler,
            IntegrationMethod::ExplicitEuler,
            IntegrationMethod::RK4,
            IntegrationMethod::VelocityVerlet,
        ];

        for method in &methods {
            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::from_translation(Vector3::new(0.3, 0.0, 0.0)),
            );

            let config = IntegratorConfig {
                method: *method,
                dt: 0.01,
                ..Default::default()
            };

            for s in 0..50 {
                step(&mut body, &config);
                assert!(body.q[0].is_finite(), "{method:?} produced NaN q at step {s}");
                assert!(body.qd[0].is_finite(), "{method:?} produced NaN qd at step {s}");
            }
        }
    }

    #[test]
    fn test_rk4_more_accurate_than_euler() {
        // Intent: RK4 should have smaller error than explicit Euler for the same step
        // Ground truth: free-fall for 0.1s → θ_exact uses analytical solution
        let make_body = || {
            let mut body = ArticulatedBody::new();
            body.set_gravity(Vector3::new(0.0, -9.81, 0.0));
            body.add_body(
                "link", -1,
                GenJointType::Prismatic { axis: Vector3::y() },
                make_inertia(1.0),
                SpatialTransform::identity(),
            );
            body
        };

        let dt = 0.01;
        let steps = 10;

        let mut body_euler = make_body();
        let euler_config = IntegratorConfig {
            method: IntegrationMethod::ExplicitEuler,
            dt,
            ..Default::default()
        };
        for _ in 0..steps {
            step(&mut body_euler, &euler_config);
        }

        let mut body_rk4 = make_body();
        let rk4_config = IntegratorConfig {
            method: IntegrationMethod::RK4,
            dt,
            ..Default::default()
        };
        for _ in 0..steps {
            step(&mut body_rk4, &rk4_config);
        }

        // Analytical: y = -0.5*g*t² = -0.5*9.81*0.01 = -0.04905
        let t = dt * steps as f32;
        let analytical_y = -0.5 * 9.81 * t * t;

        let euler_err = (body_euler.q[0] - analytical_y).abs();
        let rk4_err = (body_rk4.q[0] - analytical_y).abs();

        assert!(
            rk4_err <= euler_err + 1e-6,
            "RK4 error ({rk4_err}) should be <= Euler error ({euler_err})"
        );
    }

    // ── SLAM Cycle 6: Integrator intent tests ───────────────────────

    #[test]
    fn intent_integrator_state_dimensions_preserved() {
        // Intent: integration must not change state vector dimensions
        let mut body = ArticulatedBody::new();
        body.set_gravity(Vector3::new(0.0, -9.81, 0.0));
        body.add_body("l1", -1, GenJointType::Revolute { axis: Vector3::z() },
            SpatialInertia::sphere(1.0, 0.1), SpatialTransform::identity());
        body.add_body("l2", 0, GenJointType::Revolute { axis: Vector3::z() },
            SpatialInertia::sphere(1.0, 0.1),
            SpatialTransform::from_translation(Vector3::new(0.0, -1.0, 0.0)));

        let nq_before = body.q.len();
        let nv_before = body.qd.len();

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

        assert_eq!(body.q.len(), nq_before, "q dimension changed after step");
        assert_eq!(body.qd.len(), nv_before, "qd dimension changed after step");
    }

    #[test]
    fn intent_integrator_all_methods_produce_finite_values() {
        // Intent: no integration method should produce NaN/Inf
        let methods = [
            IntegrationMethod::SemiImplicitEuler,
            IntegrationMethod::ExplicitEuler,
            IntegrationMethod::VelocityVerlet,
            IntegrationMethod::RK4,
        ];

        for method in &methods {
            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::identity());
            body.tau[0] = 1.0;
            body.qd[0] = 0.5;

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

            for _ in 0..100 {
                step(&mut body, &config);
            }

            for i in 0..body.q.len() {
                assert!(body.q[i].is_finite(),
                    "{:?}: q[{}] is not finite after 100 steps: {}", method, i, body.q[i]);
            }
            for i in 0..body.qd.len() {
                assert!(body.qd[i].is_finite(),
                    "{:?}: qd[{}] is not finite after 100 steps: {}", method, i, body.qd[i]);
            }
        }
    }

    #[test]
    fn intent_integrator_damping_reduces_velocity() {
        // Intent: velocity damping should reduce speed over time
        let mut body = ArticulatedBody::new();
        body.set_gravity(Vector3::new(0.0, 0.0, 0.0)); // no gravity
        body.add_body("link", -1, GenJointType::Revolute { axis: Vector3::z() },
            SpatialInertia::sphere(1.0, 0.1), SpatialTransform::identity());
        body.qd[0] = 10.0; // initial velocity

        let config = IntegratorConfig {
            dt: 0.01,
            method: IntegrationMethod::SemiImplicitEuler,
            velocity_damping: 0.99, // 1% loss per step
            ..Default::default()
        };

        let v_before = body.qd[0].abs();
        for _ in 0..100 {
            step(&mut body, &config);
        }
        let v_after = body.qd[0].abs();

        assert!(v_after < v_before,
            "damping should reduce velocity: before={}, after={}", v_before, v_after);
    }

    // ====================================================================
    // Cross-module intent tests: ABA + integrator pipeline
    // ====================================================================

    #[test]
    fn intent_free_fall_body_accelerates_under_gravity() {
        // Physics intent: a body under gravity should gain downward velocity
        let mut body = ArticulatedBody::new();
        body.set_gravity(Vector3::new(0.0, -9.81, 0.0));
        body.add_body("link", -1, GenJointType::Prismatic { axis: Vector3::y() },
            SpatialInertia::sphere(1.0, 0.1), SpatialTransform::identity());

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

        let pos_before = body.q[0];
        for _ in 0..100 {
            step(&mut body, &config);
        }
        let pos_after = body.q[0];

        // Should have fallen (negative Y displacement)
        assert!(pos_after < pos_before,
            "body should fall under gravity: before={}, after={}", pos_before, pos_after);
        // Should have non-trivial velocity
        assert!(body.qd[0] < -1.0,
            "should have downward velocity after 1s of free fall, got qd={}", body.qd[0]);
    }

    #[test]
    fn intent_all_integrators_agree_on_free_fall_direction() {
        // All integration methods should produce the same sign of displacement for free fall
        let methods = [
            IntegrationMethod::SemiImplicitEuler,
            IntegrationMethod::ExplicitEuler,
            IntegrationMethod::RK4,
            IntegrationMethod::VelocityVerlet,
        ];

        for method in &methods {
            let mut body = ArticulatedBody::new();
            body.set_gravity(Vector3::new(0.0, -9.81, 0.0));
            body.add_body("link", -1, GenJointType::Prismatic { axis: Vector3::y() },
                SpatialInertia::sphere(1.0, 0.1), SpatialTransform::identity());

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

            for _ in 0..100 {
                step(&mut body, &config);
            }

            assert!(body.q[0] < 0.0,
                "{:?}: body should fall (q < 0), got q={}", method, body.q[0]);
            assert!(body.qd[0] < 0.0,
                "{:?}: should have negative velocity, got qd={}", method, body.qd[0]);
        }
    }

    #[test]
    fn intent_rk4_more_accurate_than_euler_for_oscillator() {
        // For a simple pendulum, RK4 should be more accurate than Euler
        // after many steps (less energy drift)
        fn run_pendulum(method: IntegrationMethod, steps: usize) -> f32 {
            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.5), SpatialTransform::identity());
            body.q[0] = 0.5; // initial angle

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

            for _ in 0..steps {
                step(&mut body, &config);
            }

            // Return kinetic energy proxy (v^2)
            let ke = 0.5 * body.qd[0] * body.qd[0];
            // Return potential energy proxy
            let pe = 9.81 * 0.5 * (1.0 - body.q[0].cos());
            ke + pe
        }

        // Run both for 1000 steps
        let initial_energy = 9.81 * 0.5 * (1.0 - 0.5_f32.cos());
        let euler_energy = run_pendulum(IntegrationMethod::SemiImplicitEuler, 1000);
        let rk4_energy = run_pendulum(IntegrationMethod::RK4, 1000);

        let euler_drift = (euler_energy - initial_energy).abs();
        let rk4_drift = (rk4_energy - initial_energy).abs();

        // RK4 should have less energy drift (more accurate)
        assert!(rk4_drift < euler_drift + 0.1,
            "RK4 should be at least as accurate as Euler: rk4_drift={rk4_drift}, euler_drift={euler_drift}");
    }

    #[test]
    fn intent_zero_gravity_preserves_state() {
        // With no gravity and no applied forces, a body at rest should stay at rest
        let mut body = ArticulatedBody::new();
        body.set_gravity(Vector3::zeros());
        body.add_body("link", -1, GenJointType::Prismatic { axis: Vector3::y() },
            SpatialInertia::sphere(1.0, 0.1), SpatialTransform::identity());
        body.q[0] = 3.0;
        body.qd[0] = 0.0;

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

        for _ in 0..100 {
            step(&mut body, &config);
        }

        assert!((body.q[0] - 3.0).abs() < 1e-6,
            "body at rest in zero-g should not move: q={}", body.q[0]);
        assert!(body.qd[0].abs() < 1e-6,
            "velocity should remain zero: qd={}", body.qd[0]);
    }

    // ── SLAM Cycle 1: Integrator intent/property/stress tests ─────────

    #[test]
    fn intent_implicit_euler_more_stable_large_dt() {
        // Intent: Implicit Euler should remain stable with larger dt where explicit may diverge
        let mut body_impl = ArticulatedBody::new();
        body_impl.set_gravity(Vector3::new(0.0, -9.81, 0.0));
        body_impl.add_body("link", -1,
            GenJointType::Revolute { axis: Vector3::z() },
            SpatialInertia::sphere(1.0, 0.1), SpatialTransform::identity());
        body_impl.q[0] = 0.5;

        let config = IntegratorConfig {
            dt: 0.05, // large dt
            method: IntegrationMethod::ImplicitEuler,
            ..Default::default()
        };

        for _ in 0..200 {
            step(&mut body_impl, &config);
        }

        // Implicit Euler should produce finite bounded results at large dt
        assert!(body_impl.q[0].is_finite(), "implicit Euler q should be finite at dt=0.05");
        assert!(body_impl.qd[0].is_finite(), "implicit Euler qd should be finite at dt=0.05");
        assert!(body_impl.q[0].abs() < 100.0, "implicit Euler should not diverge");
    }

    #[test]
    fn stress_integrator_1000_steps_three_link_chain() {
        // Stress: 3-link chain, 1000 steps, all values remain finite
        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::sphere(1.0, 0.1),
                SpatialTransform::from_translation(Vector3::new(0.0, -0.5, 0.0)),
            );
        }
        body.q[0] = 0.3;
        body.q[1] = -0.2;

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

        for _ in 0..1000 {
            step(&mut body, &config);
        }

        for i in 0..body.dof_count() {
            assert!(body.q[i].is_finite(), "q[{i}]={} must be finite after 1000 steps", body.q[i]);
            assert!(body.qd[i].is_finite(), "qd[{i}]={} must be finite", body.qd[i]);
        }
    }

    #[test]
    fn intent_velocity_verlet_second_order() {
        // Intent: Velocity Verlet error should scale as dt^2 (second order)
        // Run same problem at dt and dt/2, error should shrink by ~4x
        fn run_free_fall(dt: f32, steps: usize) -> f32 {
            let mut body = ArticulatedBody::new();
            body.set_gravity(Vector3::new(0.0, -9.81, 0.0));
            body.add_body("link", -1,
                GenJointType::Prismatic { axis: Vector3::y() },
                SpatialInertia::sphere(1.0, 0.1), SpatialTransform::identity());

            let config = IntegratorConfig {
                dt,
                method: IntegrationMethod::VelocityVerlet,
                ..Default::default()
            };
            for _ in 0..steps {
                step(&mut body, &config);
            }
            body.q[0]
        }

        let t_end = 1.0;
        let analytical = -0.5 * 9.81 * t_end * t_end; // y = -½gt²

        let q_coarse = run_free_fall(0.01, 100);      // dt = 0.01, 100 steps
        let q_fine = run_free_fall(0.005, 200);        // dt = 0.005, 200 steps

        let err_coarse = (q_coarse - analytical).abs();
        let err_fine = (q_fine - analytical).abs();

        // For 2nd order method, halving dt should reduce error by ~4x (ratio ~3-5 with tolerance)
        if err_coarse > 1e-6 {
            let ratio = err_coarse / (err_fine + 1e-10);
            assert!(ratio > 2.0,
                "Verlet should be 2nd order: err_coarse={err_coarse}, err_fine={err_fine}, ratio={ratio}");
        }
    }

    #[test]
    fn property_all_methods_agree_on_direction() {
        // Property: All integration methods should agree on the direction of motion
        // (a body falling under gravity moves in -Y for all methods)
        let methods = [
            IntegrationMethod::SemiImplicitEuler,
            IntegrationMethod::ExplicitEuler,
            IntegrationMethod::RK4,
            IntegrationMethod::VelocityVerlet,
            IntegrationMethod::ImplicitEuler,
        ];

        for method in &methods {
            let mut body = ArticulatedBody::new();
            body.set_gravity(Vector3::new(0.0, -9.81, 0.0));
            body.add_body("link", -1,
                GenJointType::Prismatic { axis: Vector3::y() },
                SpatialInertia::sphere(1.0, 0.1), SpatialTransform::identity());

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

            for _ in 0..100 {
                step(&mut body, &config);
            }

            assert!(body.q[0] < 0.0,
                "{:?}: body should fall (q[0]<0), got q[0]={}", method, body.q[0]);
            assert!(body.qd[0] < 0.0,
                "{:?}: velocity should be negative, got qd[0]={}", method, body.qd[0]);
        }
    }

    #[test]
    fn intent_kahan_compensation_arrays_exist() {
        // Intent: After stepping, Kahan compensation arrays should have same dimension as state
        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::identity());

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

        step(&mut body, &config);

        assert_eq!(body.q_compensation.len(), body.q.len(),
            "q_compensation dimension must match q");
        assert_eq!(body.qd_compensation.len(), body.qd.len(),
            "qd_compensation dimension must match qd");
    }

    #[test]
    fn test_velocity_verlet_spherical_quaternion_stays_unit() {
        let mut body = ArticulatedBody::new();
        body.set_gravity(Vector3::zeros());
        body.add_body("link", -1, GenJointType::Spherical,
            SpatialInertia::sphere(1.0, 0.1), SpatialTransform::identity());
        body.qd[0] = 1.0; // rotating
        body.qd[1] = 0.5;

        let config = IntegratorConfig {
            dt: 0.01,
            method: IntegrationMethod::VelocityVerlet,
            normalize_quaternions: true,
            ..Default::default()
        };

        for _ in 0..200 {
            step(&mut body, &config);
        }

        let norm: f32 = body.q.iter().take(4).map(|x| x * x).sum::<f32>().sqrt();
        assert!((norm - 1.0).abs() < 0.01,
            "spherical quaternion norm should be 1 after 200 Verlet steps: {norm}");
    }

    // ── SLAM Cycle 21: Integrator proptest ────────────────────────────

    use proptest::prelude::*;

    proptest! {
        #[test]
        fn prop_step_preserves_state_dimensions(
            q0 in -2.0f32..2.0,
            dt in 0.0001f32..0.01,
        ) {
            let mut body = ArticulatedBody::new();
            body.set_gravity(Vector3::new(0.0, -9.81, 0.0));
            body.add_body("l1", -1,
                GenJointType::Revolute { axis: Vector3::z() },
                SpatialInertia::sphere(1.0, 0.1), SpatialTransform::identity());
            body.add_body("l2", 0,
                GenJointType::Revolute { axis: Vector3::z() },
                SpatialInertia::sphere(0.5, 0.1),
                SpatialTransform::from_translation(Vector3::new(0.0, -0.5, 0.0)));
            body.q[0] = q0;

            let nq_before = body.nq();
            let nv_before = body.dof_count();

            let config = IntegratorConfig {
                dt,
                method: IntegrationMethod::SemiImplicitEuler,
                ..Default::default()
            };
            step(&mut body, &config);

            prop_assert_eq!(body.nq(), nq_before, "nq changed after step");
            prop_assert_eq!(body.dof_count(), nv_before, "nv changed after step");
            prop_assert_eq!(body.q.len(), nq_before);
            prop_assert_eq!(body.qd.len(), nv_before);
        }
    }
}