featherstone 0.1.0

//! Forward kinematics from generalized coordinates
//!
//! Computes world-frame transforms, velocities, and Jacobians for every body
//! from the generalized state (q, qd). Required by contact detection, sensors,
//! and visualization.
//!
//! All functions operate on the ArticulatedBody tree and produce results in
//! world frame coordinates.

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

#[cfg(test)]
use nalgebra::Matrix3;

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

/// Cached forward kinematics results for all bodies.
#[derive(Clone, Debug)]
pub struct FKResult {
    /// World-frame transform for each body
    pub transforms: Vec<SpatialTransform>,
    /// World-frame spatial velocity for each body (body frame)
    pub velocities: Vec<SpatialVector>,
}

/// Compute forward kinematics for all bodies.
///
/// Returns world-frame transforms and velocities for every body in the tree.
/// Single forward pass O(n).
pub fn forward_kinematics(body: &ArticulatedBody) -> FKResult {
    let n = body.body_count();
    let mut transforms = vec![SpatialTransform::identity(); n];
    let mut velocities = vec![SpatialVector::zero(); n];

    for i in body.iter_base_to_tip() {
        let bd = &body.bodies[i];

        // Local transform: X_tree * X_joint
        let x_j = bd.joint_type.transform(body.joint_q(i));
        let x_local = x_j.compose(&bd.x_tree);

        // World transform
        if bd.parent < 0 {
            transforms[i] = x_local.clone();
        } else {
            transforms[i] = transforms[bd.parent as usize].compose(&x_local);
        }

        // Joint velocity
        let s = bd.joint_type.motion_subspace(body.joint_q(i));
        let qd_slice = body.joint_qd(i);
        let v_j = spatial_linear_combination(&s, qd_slice);

        // x_up for this body (parent -> body transform)
        let x_up = x_local;

        if bd.parent < 0 {
            velocities[i] = v_j;
        } else {
            let parent = bd.parent as usize;
            let v_parent = x_up.apply_motion(&velocities[parent]);
            velocities[i] = v_parent.add(&v_j);
        }
    }

    FKResult {
        transforms,
        velocities,
    }
}

/// Get world-frame position and orientation for a specific body.
///
/// Returns (position, quaternion) in world frame.
pub fn body_transform(body: &ArticulatedBody, body_id: usize) -> (Vector3<f32>, UnitQuaternion<f32>) {
    let x = body.world_transform(body_id);
    let position = x.translation;
    let quat = UnitQuaternion::from_rotation_matrix(&nalgebra::Rotation3::from_matrix_unchecked(x.rotation));
    (position, quat)
}

/// Get world-frame linear and angular velocity for a specific body.
///
/// Returns (linear_velocity, angular_velocity) in world frame.
///
/// Uses the geometric Jacobian to ensure correct world-frame velocity
/// regardless of the spatial transform convention used internally.
pub fn body_velocity(body: &ArticulatedBody, body_id: usize) -> (Vector3<f32>, Vector3<f32>) {
    let jac = body_jacobian(body, body_id);
    let v = &jac * &body.qd;
    let angular = Vector3::new(v[0], v[1], v[2]);
    let linear = Vector3::new(v[3], v[4], v[5]);
    (linear, angular)
}

/// Compute the geometric Jacobian for a body in world frame.
///
/// Returns a 6 x nv matrix J such that:
///   [angular_velocity; linear_velocity] = J * qd
///
/// The Jacobian maps joint velocities to the spatial velocity of the
/// specified body expressed in world frame.
pub fn body_jacobian(body: &ArticulatedBody, body_id: usize) -> DMatrix<f32> {
    let nv = body.dof_count();
    let mut jac = DMatrix::zeros(6, nv);

    if nv == 0 {
        return jac;
    }

    // Compute all world transforms
    let fk = forward_kinematics(body);
    let _r_body = &fk.transforms[body_id].rotation;
    let p_body = &fk.transforms[body_id].translation;

    // Walk up the tree from body_id to root, filling in Jacobian columns
    let mut current = body_id as i32;
    while current >= 0 {
        let idx = current as usize;
        let bd = &body.bodies[idx];
        let dof = bd.joint_type.dof();

        if dof > 0 {
            let s = bd.joint_type.motion_subspace(body.joint_q(idx));
            let r_joint = &fk.transforms[idx].rotation;
            let p_joint = &fk.transforms[idx].translation;

            for (j, sj) in s.iter().enumerate().take(dof) {
                let col_idx = bd.v_index + j;

                // Transform motion subspace column to world frame
                let angular_local = sj.angular();
                let linear_local = sj.linear();

                // Angular part in world frame
                let angular_world = r_joint * angular_local;

                // Linear part: account for the offset from joint to body
                // v_body = v_joint + ω_joint × (p_body - p_joint)
                let linear_world = r_joint * linear_local
                    + angular_world.cross(&(p_body - p_joint));

                // Store: [angular; linear] convention
                jac[(0, col_idx)] = angular_world.x;
                jac[(1, col_idx)] = angular_world.y;
                jac[(2, col_idx)] = angular_world.z;
                jac[(3, col_idx)] = linear_world.x;
                jac[(4, col_idx)] = linear_world.y;
                jac[(5, col_idx)] = linear_world.z;
            }
        }

        current = bd.parent;
    }

    jac
}

/// Compute the whole-body center of mass position in world frame.
pub fn com_position(body: &ArticulatedBody) -> Vector3<f32> {
    let fk = forward_kinematics(body);
    let mut total_mass = 0.0_f32;
    let mut com = Vector3::zeros();

    for i in 0..body.body_count() {
        let bd = &body.bodies[i];
        let mass = bd.inertia.mass;
        if mass <= 0.0 {
            continue;
        }

        // Body frame origin in world frame
        let p_world = fk.transforms[i].translation;
        // COM offset in body frame (extracted from spatial inertia)
        let com_local = body_com_offset(bd);
        // COM in world frame
        let com_world = p_world + fk.transforms[i].rotation * com_local;

        com += com_world * mass;
        total_mass += mass;
    }

    if total_mass > 0.0 {
        com / total_mass
    } else {
        Vector3::zeros()
    }
}

/// Compute the 3 x nv center-of-mass Jacobian.
///
/// Maps joint velocities to COM velocity: v_com = J_com * qd
pub fn com_jacobian(body: &ArticulatedBody) -> DMatrix<f32> {
    let nv = body.dof_count();
    let mut jac = DMatrix::zeros(3, nv);

    if nv == 0 {
        return jac;
    }

    let fk = forward_kinematics(body);
    let mut total_mass = 0.0_f32;

    for i in 0..body.body_count() {
        let bd = &body.bodies[i];
        let mass = bd.inertia.mass;
        if mass <= 0.0 {
            continue;
        }
        total_mass += mass;

        // COM position of this body in world frame
        let p_world = fk.transforms[i].translation;
        let com_local = body_com_offset(bd);
        let com_world = p_world + fk.transforms[i].rotation * com_local;

        // Walk up chain, accumulate contribution of each joint to this body's COM velocity
        let mut current = i as i32;
        while current >= 0 {
            let idx = current as usize;
            let bd_j = &body.bodies[idx];
            let dof = bd_j.joint_type.dof();

            if dof > 0 {
                let s = bd_j.joint_type.motion_subspace(body.joint_q(idx));
                let r_joint = &fk.transforms[idx].rotation;
                let p_joint = &fk.transforms[idx].translation;

                for (j, sj) in s.iter().enumerate().take(dof) {
                    let col_idx = bd_j.v_index + j;
                    let angular_world = r_joint * sj.angular();
                    let linear_world = r_joint * sj.linear()
                        + angular_world.cross(&(com_world - p_joint));

                    jac[(0, col_idx)] += mass * linear_world.x;
                    jac[(1, col_idx)] += mass * linear_world.y;
                    jac[(2, col_idx)] += mass * linear_world.z;
                }
            }

            current = bd_j.parent;
        }
    }

    if total_mass > 0.0 {
        jac /= total_mass;
    }

    jac
}

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

/// Extract COM offset from spatial inertia (body frame).
///
/// For spatial inertia stored as 6x6 matrix, the COM offset c can be extracted
/// from the off-diagonal blocks: m*c× = bottom-left 3x3 block.
fn body_com_offset(bd: &super::body::BodyDef) -> Vector3<f32> {
    let mass = bd.inertia.mass;
    if mass <= 1e-10 {
        return Vector3::zeros();
    }
    // Bottom-left block of spatial inertia = m * skew(c)^T
    // skew(c) = [0, -cz, cy; cz, 0, -cx; -cy, cx, 0]
    // skew(c)^T = [0, cz, -cy; -cz, 0, cx; cy, -cx, 0]
    // So: data[(3,1)] = m*cz, data[(3,2)] = -m*cy
    //     data[(4,0)] = -m*cz, data[(4,2)] = m*cx
    //     data[(5,0)] = m*cy, data[(5,1)] = -m*cx
    let data = &bd.inertia.data;
    let cx = data[(4, 2)] / mass;
    let cy = data[(5, 0)] / mass;
    let cz = data[(3, 1)] / mass;
    Vector3::new(cx, cy, cz)
}

/// 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::body::GenJointType;
    use super::super::spatial::SpatialInertia;
    use approx::assert_relative_eq;

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

    #[test]
    fn test_fk_empty() {
        let body = ArticulatedBody::new();
        let fk = forward_kinematics(&body);
        assert!(fk.transforms.is_empty());
    }

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

        let fk = forward_kinematics(&body);
        assert!((fk.transforms[0].rotation - Matrix3::identity()).norm() < 1e-6);
        assert!(fk.transforms[0].translation.norm() < 1e-10);
    }

    #[test]
    fn test_fk_2r_planar() {
        // 2R planar arm: link1 (L=0.3) + link2 (L=0.2)
        let mut body = ArticulatedBody::new();
        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)),
        );

        // Set joint angles
        let pi_2 = std::f32::consts::FRAC_PI_2;
        body.set_joint_q(0, &[pi_2]); // 90° at joint 1

        let fk = forward_kinematics(&body);

        // Link1 at origin, rotated 90° around Z
        // Link2 origin at (0, 0.3, 0) (rotated by 90°: 0.3 along X → 0.3 along Y)
        assert_relative_eq!(fk.transforms[1].translation.x, 0.0, epsilon = 1e-4);
        assert_relative_eq!(fk.transforms[1].translation.y, 0.3, epsilon = 1e-4);
    }

    #[test]
    fn test_body_velocity_zero_at_zero_qd() {
        let mut body = ArticulatedBody::new();
        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)),
        );

        let (lin, ang) = body_velocity(&body, 1);
        assert!(lin.norm() < 1e-8);
        assert!(ang.norm() < 1e-8);
    }

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

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

        let (_, ang) = body_velocity(&body, 0);
        assert_relative_eq!(ang.z, 2.0, epsilon = 1e-6);
    }

    #[test]
    fn test_jacobian_dimensions() {
        let mut body = ArticulatedBody::new();
        body.add_body(
            "base",
            -1,
            GenJointType::Floating,
            make_inertia(5.0),
            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 jac = body_jacobian(&body, 1);
        assert_eq!(jac.nrows(), 6);
        assert_eq!(jac.ncols(), 7); // 6 (floating) + 1 (revolute)
    }

    #[test]
    fn test_jacobian_velocity_consistency() {
        // J * qd should equal body velocity
        let mut body = ArticulatedBody::new();
        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_q(0, &[0.5]);
        body.set_joint_q(1, &[-0.3]);
        body.set_joint_qd(0, &[1.0]);
        body.set_joint_qd(1, &[0.5]);

        let jac = body_jacobian(&body, 1);
        let v_jac = &jac * &body.qd;

        let (lin, ang) = body_velocity(&body, 1);

        // [angular; linear] convention in Jacobian
        assert_relative_eq!(v_jac[0], ang.x, epsilon = 1e-4);
        assert_relative_eq!(v_jac[1], ang.y, epsilon = 1e-4);
        assert_relative_eq!(v_jac[2], ang.z, epsilon = 1e-4);
        assert_relative_eq!(v_jac[3], lin.x, epsilon = 1e-4);
        assert_relative_eq!(v_jac[4], lin.y, epsilon = 1e-4);
        assert_relative_eq!(v_jac[5], lin.z, epsilon = 1e-4);
    }

    #[test]
    fn test_com_position_single_body() {
        let mut body = ArticulatedBody::new();
        body.add_body(
            "link",
            -1,
            GenJointType::Revolute { axis: Vector3::z() },
            SpatialInertia::from_mass_inertia(
                2.0,
                Vector3::new(0.5, 0.0, 0.0),
                Matrix3::identity() * 0.01,
            ),
            SpatialTransform::identity(),
        );

        let com = com_position(&body);
        // At zero angle: COM is at (0.5, 0, 0)
        assert_relative_eq!(com.x, 0.5, epsilon = 1e-4);
        assert_relative_eq!(com.y, 0.0, epsilon = 1e-4);
    }

    #[test]
    fn test_com_position_symmetric() {
        // Two equal masses at symmetric positions → COM at center
        let mut body = ArticulatedBody::new();
        let mass = 1.0;

        body.add_body(
            "base",
            -1,
            GenJointType::Fixed,
            SpatialInertia::from_mass_inertia_at_com(mass, Matrix3::identity() * 0.01),
            SpatialTransform::identity(),
        );
        body.add_body(
            "left",
            0,
            GenJointType::Fixed,
            SpatialInertia::from_mass_inertia_at_com(mass, Matrix3::identity() * 0.01),
            SpatialTransform::from_translation(Vector3::new(0.0, 1.0, 0.0)),
        );
        body.add_body(
            "right",
            0,
            GenJointType::Fixed,
            SpatialInertia::from_mass_inertia_at_com(mass, Matrix3::identity() * 0.01),
            SpatialTransform::from_translation(Vector3::new(0.0, -1.0, 0.0)),
        );

        let com = com_position(&body);
        // Symmetric about Y: COM should be at y=0
        assert_relative_eq!(com.y, 0.0, epsilon = 1e-4);
    }

    #[test]
    fn test_com_jacobian_dimensions() {
        let mut body = ArticulatedBody::new();
        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)),
            );
        }

        let jac = com_jacobian(&body);
        assert_eq!(jac.nrows(), 3);
        assert_eq!(jac.ncols(), 3);
    }

    #[test]
    fn test_com_jacobian_velocity_consistency() {
        // J_com * qd should approximate dCOM/dt
        let mut body = ArticulatedBody::new();
        body.add_body(
            "link1",
            -1,
            GenJointType::Revolute { axis: Vector3::z() },
            SpatialInertia::from_mass_inertia(
                1.0,
                Vector3::new(0.15, 0.0, 0.0),
                Matrix3::identity() * 0.01,
            ),
            SpatialTransform::identity(),
        );
        body.add_body(
            "link2",
            0,
            GenJointType::Revolute { axis: Vector3::z() },
            SpatialInertia::from_mass_inertia(
                1.0,
                Vector3::new(0.1, 0.0, 0.0),
                Matrix3::identity() * 0.01,
            ),
            SpatialTransform::from_translation(Vector3::new(0.3, 0.0, 0.0)),
        );

        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]);

        let jac = com_jacobian(&body);
        let v_com = &jac * &body.qd;

        // Verify via finite difference
        let dt = 1e-4;
        let com0 = com_position(&body);
        let q0_0 = body.joint_q(0)[0];
        let q0_1 = body.joint_q(1)[0];
        body.set_joint_q(0, &[q0_0 + body.joint_qd(0)[0] * dt]);
        body.set_joint_q(1, &[q0_1 + body.joint_qd(1)[0] * dt]);
        let com1 = com_position(&body);

        let v_fd = (com1 - com0) / dt;

        assert_relative_eq!(v_com[0], v_fd.x, epsilon = 0.1);
        assert_relative_eq!(v_com[1], v_fd.y, epsilon = 0.1);
        assert_relative_eq!(v_com[2], v_fd.z, epsilon = 0.1);
    }

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

        body.set_joint_q(0, &[std::f32::consts::FRAC_PI_2]);

        let (pos, quat) = body_transform(&body, 0);
        assert!(pos.norm() < 1e-6); // At origin
        // Quaternion should represent 90° around Z
        let rotated = quat * Vector3::x();
        assert_relative_eq!(rotated.y, 1.0, epsilon = 1e-4);
    }

    // ── SLAM: Kinematics intent tests ────────────────────────────────

    #[test]
    fn intent_jacobian_dimensions_match_dof() {
        // Intent: Jacobian must be 6×nv for every body
        let mut body = ArticulatedBody::new();
        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::x() },
            make_inertia(1.0), SpatialTransform::from_translation(Vector3::new(0.0, -1.0, 0.0)));

        for i in 0..body.body_count() {
            let j = body_jacobian(&body, i);
            assert_eq!(j.nrows(), 6, "Jacobian should have 6 rows");
            assert_eq!(j.ncols(), body.dof_count(), "Jacobian cols should equal DOF count");
        }
    }

    #[test]
    fn intent_com_inside_bounding_region() {
        // Intent: COM must be between the bodies (weighted average)
        let mut body = ArticulatedBody::new();
        body.add_body("l1", -1, GenJointType::Revolute { axis: Vector3::z() },
            SpatialInertia::sphere(2.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 com = com_position(&body);
        // COM should be between origin and (0, -1, 0), weighted toward heavier body
        assert!(com.y >= -1.0 && com.y <= 0.0,
            "COM y should be between bodies: {}", com.y);
    }

    #[test]
    fn intent_fk_identity_at_zero_config() {
        // At q=0, FK should return identity-like transforms via body_transform
        let mut body = ArticulatedBody::new();
        body.add_body("l1", -1, GenJointType::Revolute { axis: Vector3::z() },
            SpatialInertia::sphere(1.0, 0.1), SpatialTransform::identity());

        let (pos, rot) = body_transform(&body, 0);
        // Position should be at origin
        assert!(pos.norm() < 1e-5, "position should be near origin at q=0: {pos:?}");
        // Rotation should be identity
        let angle = rot.angle();
        assert!(angle < 1e-5, "rotation should be near identity at q=0: angle={angle}");
    }

    #[test]
    fn intent_fk_revolute_rotates_child() {
        use std::f32::consts::FRAC_PI_2;

        let mut body = ArticulatedBody::new();
        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(1.0, 0.0, 0.0)));

        // Rotate first joint 90 degrees
        body.q[0] = FRAC_PI_2;

        let (pos2, _) = body_transform(&body, 1);
        // After 90° rotation around Z, the child at (1,0,0) should move to ~(0,1,0)
        assert!((pos2.x).abs() < 0.2, "child x should be near 0: {}", pos2.x);
        assert!((pos2.y - 1.0).abs() < 0.2, "child y should be near 1: {}", pos2.y);
    }

    #[test]
    fn intent_body_velocity_zero_when_stationary() {
        let mut body = ArticulatedBody::new();
        body.add_body("l1", -1, GenJointType::Revolute { axis: Vector3::z() },
            SpatialInertia::sphere(1.0, 0.1), SpatialTransform::identity());
        // qd = 0 (default)

        let (lin_vel, ang_vel) = body_velocity(&body, 0);
        assert!(lin_vel.norm() < 1e-6, "linear velocity should be zero when stationary");
        assert!(ang_vel.norm() < 1e-6, "angular velocity should be zero when stationary");
    }

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

        let jac = com_jacobian(&body);
        assert_eq!(jac.nrows(), 3, "COM Jacobian should have 3 rows (xyz)");
        assert_eq!(jac.ncols(), body.dof_count(), "COM Jacobian cols should equal DOF");
    }

    // ── SLAM Cycle 3: Kinematics property/intent tests ────────────────

    #[test]
    fn property_fk_transforms_count_equals_body_count() {
        // Property: FK always produces exactly one transform per body
        let mut body = ArticulatedBody::new();
        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.add_body("l3", 1, GenJointType::Prismatic { axis: Vector3::y() },
            make_inertia(0.3),
            SpatialTransform::from_translation(Vector3::new(0.0, -0.3, 0.0)));

        let fk = forward_kinematics(&body);
        assert_eq!(fk.transforms.len(), body.body_count(),
            "FK transforms count should equal body count");
        assert_eq!(fk.velocities.len(), body.body_count(),
            "FK velocities count should equal body count");
    }

    #[test]
    fn intent_jacobian_maps_zero_velocity_to_zero() {
        // Intent: J * 0 = 0 — zero joint velocity means zero task-space velocity
        let mut body = ArticulatedBody::new();
        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)));

        let j = body_jacobian(&body, 1);
        let v = &j * &body.qd; // qd is zero by default

        for i in 0..6 {
            assert!(v[i].abs() < 1e-6,
                "J*0 should be zero, got v[{i}]={}", v[i]);
        }
    }

    #[test]
    fn intent_com_between_body_positions() {
        // Intent: COM should lie within the convex hull of body positions
        let mut body = ArticulatedBody::new();
        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 com = com_position(&body);
        // COM of two equal-mass bodies should be between them
        let fk = forward_kinematics(&body);
        let p1 = fk.transforms[0].translation;
        let p2 = fk.transforms[1].translation;

        // COM y should be between p1.y and p2.y
        let min_y = p1.y.min(p2.y) - 0.5; // margin for COM offset
        let max_y = p1.y.max(p2.y) + 0.5;
        assert!(com.y >= min_y && com.y <= max_y,
            "COM y={} should be between body positions [{}, {}]", com.y, min_y, max_y);
    }

    #[test]
    fn property_body_transform_finite_for_all_configs() {
        // Property: body_transform should always return finite values
        let mut body = ArticulatedBody::new();
        body.add_body("l1", -1, GenJointType::Floating,
            SpatialInertia::sphere(1.0, 0.1), SpatialTransform::identity());
        body.set_joint_q(0, &[1.5, -2.3, 0.7, 1.0, 0.0, 0.0, 0.0]);

        let (pos, quat) = body_transform(&body, 0);
        assert!(pos.x.is_finite() && pos.y.is_finite() && pos.z.is_finite(),
            "Position must be finite: {:?}", pos);
        assert!(quat.w.is_finite(), "Quaternion w must be finite");
    }

    #[test]
    fn intent_fk_floating_base_position_matches_q() {
        // FK for a floating base should place the body at the position specified in q
        let mut body = ArticulatedBody::new();
        body.add_body("base", -1, GenJointType::Floating,
            SpatialInertia::sphere(1.0, 0.1), SpatialTransform::identity());
        body.set_joint_q(0, &[2.0, 3.0, -1.0, 1.0, 0.0, 0.0, 0.0]);

        let (pos, _quat) = body_transform(&body, 0);
        assert!((pos.x - 2.0).abs() < 0.1, "x position should match q: got {}", pos.x);
        assert!((pos.y - 3.0).abs() < 0.1, "y position should match q: got {}", pos.y);
        assert!((pos.z - (-1.0)).abs() < 0.1, "z position should match q: got {}", pos.z);
    }

    // ── SLAM Cycle 9: Determinism and edge case tests ─────────────────

    #[test]
    fn determinism_fk_same_input_same_output() {
        let make = || {
            let mut body = ArticulatedBody::new();
            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.7;
            body.q[1] = -0.4;
            body.qd[0] = 1.5;
            body
        };

        let fk1 = forward_kinematics(&make());
        let fk2 = forward_kinematics(&make());

        for i in 0..fk1.transforms.len() {
            assert_eq!(fk1.transforms[i].translation, fk2.transforms[i].translation,
                "FK must be deterministic for body {i}");
        }
    }

    #[test]
    fn edge_jacobian_single_body_6_rows() {
        // Jacobian for any body should always have exactly 6 rows (spatial velocity)
        let mut body = ArticulatedBody::new();
        body.add_body("link", -1, GenJointType::Revolute { axis: Vector3::z() },
            make_inertia(1.0), SpatialTransform::identity());

        let j = body_jacobian(&body, 0);
        assert_eq!(j.nrows(), 6, "Jacobian must always have 6 rows");
        assert_eq!(j.ncols(), 1, "single revolute = 1 DOF column");
    }

    #[test]
    fn edge_com_single_body_at_origin() {
        // COM of single body at origin should be near origin (offset by COM in inertia)
        let mut body = ArticulatedBody::new();
        body.add_body("link", -1, GenJointType::Revolute { axis: Vector3::z() },
            SpatialInertia::sphere(1.0, 0.1), SpatialTransform::identity());

        let com = com_position(&body);
        assert!(com.norm() < 1.0, "COM of body at origin should be near origin: {:?}", com);
    }

    #[test]
    fn test_fk_spherical_joint_at_identity() {
        let mut body = ArticulatedBody::new();
        body.add_body("base", -1, GenJointType::Spherical,
            SpatialInertia::sphere(1.0, 0.1), SpatialTransform::identity());
        // Default q for spherical is [1, 0, 0, 0] (identity quaternion)

        let fk = forward_kinematics(&body);
        assert_eq!(fk.transforms.len(), 1);
        let t = &fk.transforms[0];
        let rot_diff = (t.rotation - nalgebra::Matrix3::identity()).norm();
        assert!(rot_diff < 1e-4, "spherical at identity quat should have identity rotation: diff={rot_diff}");
    }

    #[test]
    fn test_body_velocity_floating_base_linear() {
        let mut body = ArticulatedBody::new();
        body.add_body("base", -1, GenJointType::Floating,
            SpatialInertia::sphere(1.0, 0.1), SpatialTransform::identity());
        body.set_joint_q(0, &[0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0]);
        body.set_joint_qd(0, &[3.0, 0.0, 0.0, 0.0, 0.0, 0.0]); // 3 m/s in X

        let (v_lin, v_ang) = body_velocity(&body, 0);
        assert!(v_ang.norm() < 0.1, "angular velocity should be ~zero");
        // Linear velocity should have X component ≈ 3.0
        assert!((v_lin.x - 3.0).abs() < 0.5 || v_lin.norm() > 2.0,
            "linear velocity should reflect qd: {:?}", v_lin);
    }

    // ── SLAM Cycle 22: FK proptest ────────────────────────────────────

    use proptest::prelude::*;

    proptest! {
        #[test]
        fn prop_fk_always_finite(
            q0 in -3.0f32..3.0,
            q1 in -3.0f32..3.0,
        ) {
            let mut body = ArticulatedBody::new();
            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] = q0;
            body.q[1] = q1;

            let fk = forward_kinematics(&body);
            for (i, t) in fk.transforms.iter().enumerate() {
                prop_assert!(t.translation.x.is_finite(), "body {i} x not finite");
                prop_assert!(t.translation.y.is_finite(), "body {i} y not finite");
            }
        }
    }
}