featherstone 0.1.0

//! URDF/MJCF to ArticulatedBody conversion
//!
//! Bridges the existing robot loaders (urdf_rs, MJCF parser) to the native
//! articulated body solver. Converts joint types, inertial parameters, and
//! kinematic tree topology.

use std::collections::HashMap;

use nalgebra::{Matrix3, Vector3};

use super::body::ArticulatedBody;
use super::error::Error;
use super::joint::GenJoint;
use super::limits::JointLimits;
use super::spatial::{SpatialInertia, SpatialTransform};

/// Result of URDF conversion: articulated body + per-joint limits.
pub struct UrdfConversion {
    /// The articulated body tree
    pub body: ArticulatedBody,
    /// Joint limits per body index (only for bodies with limits)
    pub limits: HashMap<usize, JointLimits>,
    /// Mapping from body index to URDF link name
    pub link_names: Vec<String>,
    /// Mapping from body index to URDF joint name
    pub joint_names: Vec<String>,
}

/// Convert a urdf_rs Robot to an ArticulatedBody.
///
/// Builds the parent-indexed tree from URDF's joint parent/child link references.
/// Maps joint types, extracts inertial parameters, and preserves joint limits.
///
/// # Errors
///
/// Returns [`Error::EmptyUrdf`] if the robot has no links, or
/// [`Error::MissingLink`] if a joint references a child link that doesn't exist.
pub fn articulated_body_from_urdf(robot: &urdf_rs::Robot) -> Result<UrdfConversion, Error> {
    let mut body = ArticulatedBody::new();
    let mut limits_map = HashMap::new();
    let mut link_names = Vec::new();
    let mut joint_names = Vec::new();

    if robot.links.is_empty() {
        return Err(Error::EmptyUrdf);
    }

    // Build link name → index mapping for joint parent/child lookup
    let link_index: HashMap<String, usize> = robot
        .links
        .iter()
        .enumerate()
        .map(|(i, link)| (link.name.clone(), i))
        .collect();

    // Find the root link (link that is never a child of any joint)
    let child_links: std::collections::HashSet<&str> = robot
        .joints
        .iter()
        .map(|j| j.child.link.as_str())
        .collect();

    let root_link = robot
        .links
        .iter()
        .find(|l| !child_links.contains(l.name.as_str()))
        .unwrap_or(&robot.links[0]);

    // Build parent→children adjacency from joints
    // joint.parent.link → joint → joint.child.link
    let mut children_of: HashMap<String, Vec<usize>> = HashMap::new();
    for (ji, joint) in robot.joints.iter().enumerate() {
        children_of
            .entry(joint.parent.link.clone())
            .or_default()
            .push(ji);
    }

    // BFS/DFS from root to build the articulated body tree
    // Map: URDF link name → body index in ArticulatedBody
    let mut link_to_body: HashMap<String, usize> = HashMap::new();

    // Add root link as a fixed body (no joint to parent)
    let root_inertia = urdf_inertial_to_spatial(&root_link.inertial);
    let root_idx = body.add_body(
        &root_link.name,
        -1,
        GenJoint::Fixed,
        root_inertia,
        SpatialTransform::identity(),
    );
    link_to_body.insert(root_link.name.clone(), root_idx);
    link_names.push(root_link.name.clone());
    joint_names.push(String::new()); // root has no joint

    // BFS queue
    let mut queue = std::collections::VecDeque::new();
    queue.push_back(root_link.name.clone());

    while let Some(parent_link_name) = queue.pop_front() {
        let parent_body_idx = link_to_body[&parent_link_name];

        if let Some(child_joints) = children_of.get(&parent_link_name) {
            for &ji in child_joints {
                let joint = &robot.joints[ji];
                let child_link_name = &joint.child.link;

                // Find child link
                let child_link_idx = match link_index.get(child_link_name) {
                    Some(&idx) => idx,
                    None => {
                        return Err(Error::MissingLink {
                            joint: joint.name.clone(),
                            link: child_link_name.clone(),
                        });
                    }
                };
                let child_link = &robot.links[child_link_idx];

                // Convert joint type
                let gen_joint = urdf_joint_to_gen_joint(&joint.joint_type, &joint.axis.xyz);

                // Convert inertial
                let inertia = urdf_inertial_to_spatial(&child_link.inertial);

                // Convert joint origin transform
                let x_tree = urdf_origin_to_transform(&joint.origin);

                // Add body
                let body_idx = body.add_body(
                    child_link_name,
                    parent_body_idx as i32,
                    gen_joint,
                    inertia,
                    x_tree,
                );

                // Extract joint limits
                let joint_dof = body.bodies[body_idx].joint_type.dof();
                if joint_dof > 0 {
                    let jl = urdf_limit_to_joint_limits(&joint.limit, joint_dof);
                    limits_map.insert(body_idx, jl);
                }

                link_to_body.insert(child_link_name.clone(), body_idx);
                link_names.push(child_link_name.clone());
                joint_names.push(joint.name.clone());

                queue.push_back(child_link_name.clone());
            }
        }
    }

    Ok(UrdfConversion {
        body,
        limits: limits_map,
        link_names,
        joint_names,
    })
}

/// Convert a URDF joint type to a GenJoint.
pub fn urdf_joint_to_gen_joint(joint_type: &urdf_rs::JointType, axis_xyz: &[f64; 3]) -> GenJoint {
    let axis = Vector3::new(axis_xyz[0] as f32, axis_xyz[1] as f32, axis_xyz[2] as f32);
    let axis = if axis.norm() > 1e-6 {
        axis.normalize()
    } else {
        Vector3::z() // default axis
    };

    match joint_type {
        urdf_rs::JointType::Revolute => GenJoint::Revolute { axis },
        urdf_rs::JointType::Continuous => GenJoint::Revolute { axis }, // no limits
        urdf_rs::JointType::Prismatic => GenJoint::Prismatic { axis },
        urdf_rs::JointType::Fixed => GenJoint::Fixed,
        urdf_rs::JointType::Floating => GenJoint::Floating,
        urdf_rs::JointType::Planar => GenJoint::Planar { normal: axis },
        urdf_rs::JointType::Spherical => GenJoint::Spherical,
    }
}

/// Convert URDF inertial to SpatialInertia.
pub fn urdf_inertial_to_spatial(inertial: &urdf_rs::Inertial) -> SpatialInertia {
    let mass = inertial.mass.value as f32;

    if mass <= 0.0 {
        return SpatialInertia::zero();
    }

    // COM offset from link frame
    let com = Vector3::new(
        inertial.origin.xyz[0] as f32,
        inertial.origin.xyz[1] as f32,
        inertial.origin.xyz[2] as f32,
    );

    // Inertia tensor (symmetric, 6 unique values)
    let i = &inertial.inertia;
    let inertia_matrix = Matrix3::new(
        i.ixx as f32, i.ixy as f32, i.ixz as f32,
        i.ixy as f32, i.iyy as f32, i.iyz as f32,
        i.ixz as f32, i.iyz as f32, i.izz as f32,
    );

    // Handle COM-referenced vs origin-referenced inertia
    // URDF inertia is specified at the link's inertial frame (COM + optional rotation)
    // If there's a rotation in the inertial origin, we need to rotate the inertia tensor
    let rpy = &inertial.origin.rpy;
    if rpy[0].abs() > 1e-10 || rpy[1].abs() > 1e-10 || rpy[2].abs() > 1e-10 {
        // Rotate inertia tensor: I' = R * I * R^T
        let rot = rpy_to_rotation(rpy[0] as f32, rpy[1] as f32, rpy[2] as f32);
        let rotated_inertia = rot * inertia_matrix * rot.transpose();
        SpatialInertia::from_mass_inertia(mass, com, rotated_inertia)
    } else {
        SpatialInertia::from_mass_inertia(mass, com, inertia_matrix)
    }
}

/// Convert URDF joint origin (xyz + rpy) to SpatialTransform.
fn urdf_origin_to_transform(origin: &urdf_rs::Pose) -> SpatialTransform {
    let translation = Vector3::new(
        origin.xyz[0] as f32,
        origin.xyz[1] as f32,
        origin.xyz[2] as f32,
    );
    let rotation = rpy_to_rotation(
        origin.rpy[0] as f32,
        origin.rpy[1] as f32,
        origin.rpy[2] as f32,
    );
    SpatialTransform::from_rotation_translation(rotation, translation)
}

/// Convert URDF joint limits to JointLimits.
fn urdf_limit_to_joint_limits(limit: &urdf_rs::JointLimit, dof: usize) -> JointLimits {
    if dof == 1 {
        JointLimits::from_urdf_params(
            limit.lower as f32,
            limit.upper as f32,
            limit.velocity as f32,
            limit.effort as f32,
        )
    } else {
        // Multi-DOF: replicate same limits for each DOF
        JointLimits {
            lower: vec![limit.lower as f32; dof],
            upper: vec![limit.upper as f32; dof],
            max_velocity: vec![limit.velocity as f32; dof],
            max_effort: vec![limit.effort as f32; dof],
            ..Default::default()
        }
    }
}

/// Convert RPY (Roll-Pitch-Yaw) Euler angles to a rotation matrix.
/// Convention: R = Rz(yaw) * Ry(pitch) * Rx(roll)
fn rpy_to_rotation(roll: f32, pitch: f32, yaw: f32) -> Matrix3<f32> {
    let (sr, cr) = roll.sin_cos();
    let (sp, cp) = pitch.sin_cos();
    let (sy, cy) = yaw.sin_cos();

    Matrix3::new(
        cy * cp,
        cy * sp * sr - sy * cr,
        cy * sp * cr + sy * sr,
        sy * cp,
        sy * sp * sr + cy * cr,
        sy * sp * cr - cy * sr,
        -sp,
        cp * sr,
        cp * cr,
    )
}

/// Helper to build a minimal URDF Robot programmatically (test-only).
#[cfg(test)]
pub fn make_test_urdf() -> urdf_rs::Robot {
    urdf_rs::Robot {
        name: "test_robot".to_string(),
        links: vec![
            urdf_rs::Link {
                name: "base_link".to_string(),
                inertial: urdf_rs::Inertial {
                    origin: urdf_rs::Pose {
                        xyz: urdf_rs::Vec3([0.0, 0.0, 0.0]),
                        rpy: urdf_rs::Vec3([0.0, 0.0, 0.0]),
                    },
                    mass: urdf_rs::Mass { value: 5.0 },
                    inertia: urdf_rs::Inertia {
                        ixx: 0.1, ixy: 0.0, ixz: 0.0,
                        iyy: 0.1, iyz: 0.0, izz: 0.1,
                    },
                },
                visual: vec![],
                collision: vec![],
            },
            urdf_rs::Link {
                name: "link1".to_string(),
                inertial: urdf_rs::Inertial {
                    origin: urdf_rs::Pose {
                        xyz: urdf_rs::Vec3([0.15, 0.0, 0.0]),
                        rpy: urdf_rs::Vec3([0.0, 0.0, 0.0]),
                    },
                    mass: urdf_rs::Mass { value: 1.0 },
                    inertia: urdf_rs::Inertia {
                        ixx: 0.01, ixy: 0.0, ixz: 0.0,
                        iyy: 0.01, iyz: 0.0, izz: 0.01,
                    },
                },
                visual: vec![],
                collision: vec![],
            },
            urdf_rs::Link {
                name: "link2".to_string(),
                inertial: urdf_rs::Inertial {
                    origin: urdf_rs::Pose {
                        xyz: urdf_rs::Vec3([0.1, 0.0, 0.0]),
                        rpy: urdf_rs::Vec3([0.0, 0.0, 0.0]),
                    },
                    mass: urdf_rs::Mass { value: 0.5 },
                    inertia: urdf_rs::Inertia {
                        ixx: 0.005, ixy: 0.0, ixz: 0.0,
                        iyy: 0.005, iyz: 0.0, izz: 0.005,
                    },
                },
                visual: vec![],
                collision: vec![],
            },
        ],
        joints: vec![
            urdf_rs::Joint {
                name: "joint1".to_string(),
                joint_type: urdf_rs::JointType::Revolute,
                origin: urdf_rs::Pose {
                    xyz: urdf_rs::Vec3([0.0, 0.0, 0.1]),
                    rpy: urdf_rs::Vec3([0.0, 0.0, 0.0]),
                },
                parent: urdf_rs::LinkName {
                    link: "base_link".to_string(),
                },
                child: urdf_rs::LinkName {
                    link: "link1".to_string(),
                },
                axis: urdf_rs::Axis {
                    xyz: urdf_rs::Vec3([0.0, 0.0, 1.0]),
                },
                limit: urdf_rs::JointLimit {
                    lower: -1.57,
                    upper: 1.57,
                    effort: 100.0,
                    velocity: 5.0,
                },
                dynamics: None,
                mimic: None,
                safety_controller: None,
            },
            urdf_rs::Joint {
                name: "joint2".to_string(),
                joint_type: urdf_rs::JointType::Revolute,
                origin: urdf_rs::Pose {
                    xyz: urdf_rs::Vec3([0.3, 0.0, 0.0]),
                    rpy: urdf_rs::Vec3([0.0, 0.0, 0.0]),
                },
                parent: urdf_rs::LinkName {
                    link: "link1".to_string(),
                },
                child: urdf_rs::LinkName {
                    link: "link2".to_string(),
                },
                axis: urdf_rs::Axis {
                    xyz: urdf_rs::Vec3([0.0, 0.0, 1.0]),
                },
                limit: urdf_rs::JointLimit {
                    lower: -(std::f32::consts::PI as f64),
                    upper: std::f32::consts::PI as f64,
                    effort: 50.0,
                    velocity: 10.0,
                },
                dynamics: None,
                mimic: None,
                safety_controller: None,
            },
        ],
        materials: vec![],
    }
}

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

#[cfg(test)]
mod tests {
    use super::*;
    use approx::assert_relative_eq;

    #[test]
    fn test_basic_urdf_conversion() {
        let robot = make_test_urdf();
        let result = articulated_body_from_urdf(&robot).unwrap();

        // 3 bodies: base_link (fixed root), link1, link2
        assert_eq!(result.body.body_count(), 3);
        // 2 revolute joints = 2 DOF
        assert_eq!(result.body.dof_count(), 2);
    }

    #[test]
    fn test_urdf_link_names() {
        let robot = make_test_urdf();
        let result = articulated_body_from_urdf(&robot).unwrap();

        assert_eq!(result.body.body_by_name("base_link"), Some(0));
        assert_eq!(result.body.body_by_name("link1"), Some(1));
        assert_eq!(result.body.body_by_name("link2"), Some(2));
    }

    #[test]
    fn test_urdf_topology() {
        let robot = make_test_urdf();
        let result = articulated_body_from_urdf(&robot).unwrap();

        assert!(result.body.is_root(0)); // base_link
        assert_eq!(result.body.parent(1), 0); // link1 → base_link
        assert_eq!(result.body.parent(2), 1); // link2 → link1
    }

    #[test]
    fn test_urdf_inertia_values() {
        let robot = make_test_urdf();
        let result = articulated_body_from_urdf(&robot).unwrap();

        // base_link mass
        assert_relative_eq!(result.body.bodies[0].inertia.mass, 5.0, epsilon = 1e-6);
        // link1 mass
        assert_relative_eq!(result.body.bodies[1].inertia.mass, 1.0, epsilon = 1e-6);
        // link2 mass
        assert_relative_eq!(result.body.bodies[2].inertia.mass, 0.5, epsilon = 1e-6);

        // Inertias should be valid
        for b in &result.body.bodies {
            assert!(b.inertia.is_valid(), "Inertia for {} should be valid", b.name);
        }
    }

    #[test]
    fn test_urdf_joint_limits() {
        let robot = make_test_urdf();
        let result = articulated_body_from_urdf(&robot).unwrap();

        // link1 (body 1) should have limits
        let lim1 = result.limits.get(&1).expect("link1 should have limits");
        assert_relative_eq!(lim1.lower[0], -1.57, epsilon = 1e-6);
        assert_relative_eq!(lim1.upper[0], 1.57, epsilon = 1e-6);
        assert_relative_eq!(lim1.max_effort[0], 100.0, epsilon = 1e-6);
        assert_relative_eq!(lim1.max_velocity[0], 5.0, epsilon = 1e-6);
    }

    #[test]
    fn test_urdf_joint_origin_transform() {
        let robot = make_test_urdf();
        let result = articulated_body_from_urdf(&robot).unwrap();

        // joint1 origin: xyz=[0, 0, 0.1]
        let x_tree_1 = &result.body.bodies[1].x_tree;
        assert_relative_eq!(x_tree_1.translation.z, 0.1, epsilon = 1e-6);

        // joint2 origin: xyz=[0.3, 0, 0]
        let x_tree_2 = &result.body.bodies[2].x_tree;
        assert_relative_eq!(x_tree_2.translation.x, 0.3, epsilon = 1e-6);
    }

    #[test]
    fn test_urdf_joint_type_mapping() {
        // Revolute
        let gj = urdf_joint_to_gen_joint(&urdf_rs::JointType::Revolute, &[0.0, 0.0, 1.0]);
        assert!(matches!(gj, GenJoint::Revolute { .. }));
        assert_eq!(gj.dof(), 1);

        // Continuous = Revolute without limits
        let gj = urdf_joint_to_gen_joint(&urdf_rs::JointType::Continuous, &[0.0, 1.0, 0.0]);
        assert!(matches!(gj, GenJoint::Revolute { .. }));

        // Prismatic
        let gj = urdf_joint_to_gen_joint(&urdf_rs::JointType::Prismatic, &[1.0, 0.0, 0.0]);
        assert!(matches!(gj, GenJoint::Prismatic { .. }));
        assert_eq!(gj.dof(), 1);

        // Fixed
        let gj = urdf_joint_to_gen_joint(&urdf_rs::JointType::Fixed, &[0.0, 0.0, 1.0]);
        assert!(matches!(gj, GenJoint::Fixed));
        assert_eq!(gj.dof(), 0);

        // Floating
        let gj = urdf_joint_to_gen_joint(&urdf_rs::JointType::Floating, &[0.0, 0.0, 1.0]);
        assert!(matches!(gj, GenJoint::Floating));
        assert_eq!(gj.dof(), 6);
    }

    #[test]
    fn test_urdf_inertial_with_com_offset() {
        let inertial = urdf_rs::Inertial {
            origin: urdf_rs::Pose {
                xyz: urdf_rs::Vec3([0.5, 0.0, 0.0]),
                rpy: urdf_rs::Vec3([0.0, 0.0, 0.0]),
            },
            mass: urdf_rs::Mass { value: 2.0 },
            inertia: urdf_rs::Inertia {
                ixx: 0.01, ixy: 0.0, ixz: 0.0,
                iyy: 0.01, iyz: 0.0, izz: 0.01,
            },
        };

        let si = urdf_inertial_to_spatial(&inertial);
        assert_relative_eq!(si.mass, 2.0, epsilon = 1e-6);
        assert!(si.is_valid());
    }

    #[test]
    fn test_urdf_zero_mass_link() {
        let inertial = urdf_rs::Inertial {
            origin: urdf_rs::Pose {
                xyz: urdf_rs::Vec3([0.0, 0.0, 0.0]),
                rpy: urdf_rs::Vec3([0.0, 0.0, 0.0]),
            },
            mass: urdf_rs::Mass { value: 0.0 },
            inertia: urdf_rs::Inertia {
                ixx: 0.0, ixy: 0.0, ixz: 0.0,
                iyy: 0.0, iyz: 0.0, izz: 0.0,
            },
        };

        let si = urdf_inertial_to_spatial(&inertial);
        assert_relative_eq!(si.mass, 0.0, epsilon = 1e-10);
    }

    #[test]
    fn test_rpy_to_rotation_identity() {
        let r = rpy_to_rotation(0.0, 0.0, 0.0);
        assert!((r - Matrix3::identity()).norm() < 1e-6);
    }

    #[test]
    fn test_rpy_to_rotation_90deg_yaw() {
        let r = rpy_to_rotation(0.0, 0.0, std::f32::consts::FRAC_PI_2);
        // 90° yaw: X → Y
        let p = r * Vector3::x();
        assert_relative_eq!(p.y, 1.0, epsilon = 1e-5);
        assert!(p.x.abs() < 1e-5);
    }

    #[test]
    fn test_converted_body_runs_aba() {
        use super::super::aba::aba_forward_dynamics;

        let robot = make_test_urdf();
        let mut result = articulated_body_from_urdf(&robot).unwrap();

        result.body.set_gravity(Vector3::new(0.0, -9.81, 0.0));
        aba_forward_dynamics(&mut result.body);

        // Should produce finite accelerations
        for i in 0..result.body.dof_count() {
            assert!(
                result.body.qdd[i].is_finite(),
                "qdd[{i}] should be finite"
            );
        }
    }

    #[test]
    fn test_puma_like_6dof() {
        // Build a 6-DOF serial chain (PUMA-560 like)
        let mut links = vec![urdf_rs::Link {
            name: "base".to_string(),
            inertial: urdf_rs::Inertial {
                origin: urdf_rs::Pose {
                    xyz: urdf_rs::Vec3([0.0, 0.0, 0.0]),
                    rpy: urdf_rs::Vec3([0.0, 0.0, 0.0]),
                },
                mass: urdf_rs::Mass { value: 10.0 },
                inertia: urdf_rs::Inertia {
                    ixx: 0.5, ixy: 0.0, ixz: 0.0,
                    iyy: 0.5, iyz: 0.0, izz: 0.5,
                },
            },
            visual: vec![],
            collision: vec![],
        }];

        let mut joints = vec![];

        for i in 0..6 {
            let link_name = format!("link{i}");
            let parent_name = if i == 0 {
                "base".to_string()
            } else {
                format!("link{}", i - 1)
            };

            links.push(urdf_rs::Link {
                name: link_name.clone(),
                inertial: urdf_rs::Inertial {
                    origin: urdf_rs::Pose {
                        xyz: urdf_rs::Vec3([0.1, 0.0, 0.0]),
                        rpy: urdf_rs::Vec3([0.0, 0.0, 0.0]),
                    },
                    mass: urdf_rs::Mass { value: 1.0 },
                    inertia: urdf_rs::Inertia {
                        ixx: 0.01, ixy: 0.0, ixz: 0.0,
                        iyy: 0.01, iyz: 0.0, izz: 0.01,
                    },
                },
                visual: vec![],
                collision: vec![],
            });

            // Alternate Z and Y axes like a real robot
            let axis = if i % 2 == 0 {
                [0.0, 0.0, 1.0]
            } else {
                [0.0, 1.0, 0.0]
            };

            joints.push(urdf_rs::Joint {
                name: format!("joint{i}"),
                joint_type: urdf_rs::JointType::Revolute,
                origin: urdf_rs::Pose {
                    xyz: urdf_rs::Vec3([0.2, 0.0, 0.0]),
                    rpy: urdf_rs::Vec3([0.0, 0.0, 0.0]),
                },
                parent: urdf_rs::LinkName {
                    link: parent_name,
                },
                child: urdf_rs::LinkName {
                    link: link_name,
                },
                axis: urdf_rs::Axis {
                    xyz: urdf_rs::Vec3(axis),
                },
                limit: urdf_rs::JointLimit {
                    lower: -(std::f32::consts::PI as f64),
                    upper: std::f32::consts::PI as f64,
                    effort: 100.0,
                    velocity: 5.0,
                },
                dynamics: None,
                mimic: None,
                safety_controller: None,
            });
        }

        let robot = urdf_rs::Robot {
            name: "puma".to_string(),
            links,
            joints,
            materials: vec![],
        };

        let result = articulated_body_from_urdf(&robot).unwrap();

        assert_eq!(result.body.body_count(), 7); // base + 6 links
        assert_eq!(result.body.dof_count(), 6);  // 6 revolute joints
        assert_eq!(result.limits.len(), 6);       // All joints have limits
    }

    // ── SLAM Cycle 10: Error path tests ──────────────────────────────

    #[test]
    fn error_empty_urdf_returns_error() {
        let robot = urdf_rs::Robot {
            name: "empty".to_string(),
            links: vec![],
            joints: vec![],
            materials: vec![],
        };
        let result = articulated_body_from_urdf(&robot);
        assert!(result.is_err(), "empty URDF should return error");
    }

    #[test]
    fn error_urdf_single_link_produces_one_body() {
        // A URDF with just a root link and no joints should produce 1 body.
        let robot = urdf_rs::Robot {
            name: "single_link".to_string(),
            links: vec![urdf_rs::Link {
                name: "base_link".to_string(),
                inertial: urdf_rs::Inertial {
                    origin: urdf_rs::Pose {
                        xyz: urdf_rs::Vec3([0.0, 0.0, 0.0]),
                        rpy: urdf_rs::Vec3([0.0, 0.0, 0.0]),
                    },
                    mass: urdf_rs::Mass { value: 1.0 },
                    inertia: urdf_rs::Inertia {
                        ixx: 0.01, ixy: 0.0, ixz: 0.0,
                        iyy: 0.01, iyz: 0.0, izz: 0.01,
                    },
                },
                visual: vec![],
                collision: vec![],
            }],
            joints: vec![],
            materials: vec![],
        };

        let result = articulated_body_from_urdf(&robot).unwrap();
        assert_eq!(result.body.body_count(), 1, "single link should produce 1 body");
        assert_eq!(result.body.dof_count(), 0, "single fixed root has 0 DOF");
        assert_eq!(result.link_names.len(), 1);
        assert_eq!(result.link_names[0], "base_link");
    }

    #[test]
    fn intent_urdf_joint_types_all_have_correct_dof() {
        // Create joints of each URDF type, verify dof() matches expected.
        let cases: Vec<(urdf_rs::JointType, usize)> = vec![
            (urdf_rs::JointType::Revolute, 1),
            (urdf_rs::JointType::Continuous, 1),  // maps to Revolute
            (urdf_rs::JointType::Prismatic, 1),
            (urdf_rs::JointType::Fixed, 0),
            (urdf_rs::JointType::Floating, 6),
            (urdf_rs::JointType::Planar, 3),
            (urdf_rs::JointType::Spherical, 3),
        ];

        for (jtype, expected_dof) in &cases {
            let gen = urdf_joint_to_gen_joint(jtype, &[0.0, 0.0, 1.0]);
            assert_eq!(
                gen.dof(), *expected_dof,
                "URDF joint type {:?} should have DOF={}, got {}",
                jtype, expected_dof, gen.dof()
            );
        }
    }

}