featherstone 0.1.0

//! Error types for the featherstone crate.

use std::fmt;

/// Errors that can occur during featherstone operations.
#[derive(Clone, Debug)]
pub enum Error {
    /// URDF has no links defined.
    EmptyUrdf,
    /// A URDF joint references a child link that does not exist.
    MissingLink {
        /// The joint name referencing the missing link.
        joint: String,
        /// The missing link name.
        link: String,
    },
    /// EPA failed to converge (degenerate or near-degenerate geometry).
    EpaFailed,
    /// A matrix that should be invertible is singular or near-singular.
    SingularMatrix {
        /// Description of which matrix failed.
        context: &'static str,
    },
}

impl fmt::Display for Error {
    fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result {
        match self {
            Error::EmptyUrdf => write!(f, "URDF contains no links"),
            Error::MissingLink { joint, link } => {
                write!(f, "joint '{}' references missing link '{}'", joint, link)
            }
            Error::EpaFailed => write!(f, "EPA failed to converge"),
            Error::SingularMatrix { context } => {
                write!(f, "singular matrix: {}", context)
            }
        }
    }
}

impl std::error::Error for Error {}

/// Convenience alias for featherstone results.
pub type Result<T> = std::result::Result<T, Error>;

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

    #[test]
    fn error_display_empty_urdf() {
        let e = Error::EmptyUrdf;
        assert_eq!(format!("{e}"), "URDF contains no links");
    }

    #[test]
    fn error_display_missing_link() {
        let e = Error::MissingLink {
            joint: "j1".into(),
            link: "child_link".into(),
        };
        let msg = format!("{e}");
        assert!(msg.contains("j1"), "should mention joint name");
        assert!(msg.contains("child_link"), "should mention link name");
    }

    #[test]
    fn error_display_epa_failed() {
        let e = Error::EpaFailed;
        assert_eq!(format!("{e}"), "EPA failed to converge");
    }

    #[test]
    fn error_display_singular_matrix() {
        let e = Error::SingularMatrix { context: "mass matrix" };
        let msg = format!("{e}");
        assert!(msg.contains("singular matrix"), "should mention singular");
        assert!(msg.contains("mass matrix"), "should mention context");
    }

    #[test]
    fn error_implements_std_error_trait() {
        let e: Box<dyn std::error::Error> = Box::new(Error::EmptyUrdf);
        // If it compiles, it implements the trait
        assert!(!e.to_string().is_empty());
    }

    #[test]
    fn error_is_clone_and_debug() {
        let e = Error::MissingLink { joint: "j".into(), link: "l".into() };
        let cloned = e.clone();
        let dbg = format!("{:?}", cloned);
        assert!(dbg.contains("MissingLink"));
    }

    #[test]
    fn result_type_alias_works() {
        fn ok_result() -> Result<i32> { Ok(42) }
        fn err_result() -> Result<i32> { Err(Error::EpaFailed) }
        assert_eq!(ok_result().unwrap(), 42);
        assert!(err_result().is_err());
    }

    // ── SLAM Cycle 11: Additional error coverage ──────────────────────

    #[test]
    fn error_singular_matrix_different_contexts() {
        let e1 = Error::SingularMatrix { context: "inertia" };
        let e2 = Error::SingularMatrix { context: "Jacobian" };
        assert_ne!(format!("{e1}"), format!("{e2}"),
            "different contexts should produce different messages");
    }

    #[test]
    fn error_clone_preserves_content() {
        let e = Error::SingularMatrix { context: "test" };
        let c = e.clone();
        assert_eq!(format!("{e}"), format!("{c}"), "clone should preserve display");
    }
}