sharira 1.1.0

use hisab::{Quat, Vec3};
use serde::{Deserialize, Serialize};
use tracing::{instrument, trace};

/// Unique bone identifier within a skeleton.
#[derive(Debug, Clone, Copy, PartialEq, Eq, Hash, Serialize, Deserialize)]
pub struct BoneId(pub u16);

/// A bone in a skeleton hierarchy.
#[derive(Debug, Clone, Serialize, Deserialize)]
pub struct Bone {
    pub id: BoneId,
    pub name: String,
    pub parent: Option<BoneId>,
    pub length: f32,          // meters
    pub mass: f32,            // kg
    pub local_position: Vec3, // offset from parent joint (meters)
    pub local_rotation: Quat, // local orientation relative to parent
}

/// A complete skeleton (bone hierarchy).
#[derive(Debug, Clone, Serialize, Deserialize)]
pub struct Skeleton {
    pub name: String,
    pub bones: Vec<Bone>,
}

impl Bone {
    /// Create a new bone.
    #[must_use]
    pub fn new(
        id: BoneId,
        name: impl Into<String>,
        length: f32,
        mass: f32,
        parent: Option<BoneId>,
    ) -> Self {
        Self {
            id,
            name: name.into(),
            parent,
            length,
            mass,
            local_position: Vec3::ZERO,
            local_rotation: Quat::IDENTITY,
        }
    }

    /// Create a new bone with position.
    #[must_use]
    pub fn with_position(mut self, position: Vec3) -> Self {
        self.local_position = position;
        self
    }

    /// Create a new bone with rotation.
    #[must_use]
    pub fn with_rotation(mut self, rotation: Quat) -> Self {
        self.local_rotation = rotation;
        self
    }
}

impl Skeleton {
    /// Create an empty skeleton.
    #[must_use]
    pub fn new(name: impl Into<String>) -> Self {
        Self {
            name: name.into(),
            bones: Vec::new(),
        }
    }

    /// Add a bone to the skeleton.
    pub fn add_bone(&mut self, bone: Bone) {
        trace!(bone_id = bone.id.0, name = %bone.name, "adding bone");
        self.bones.push(bone);
    }

    /// Immutable access to bones.
    #[must_use]
    #[inline]
    pub fn bones(&self) -> &[Bone] {
        &self.bones
    }

    /// Find a bone by name.
    #[must_use]
    #[inline]
    pub fn find_bone(&self, name: &str) -> Option<&Bone> {
        self.bones.iter().find(|b| b.name == name)
    }

    /// Find a bone by ID.
    #[must_use]
    #[inline]
    pub fn get_bone(&self, id: BoneId) -> Option<&Bone> {
        self.bones.iter().find(|b| b.id == id)
    }

    /// Total mass of all bones.
    #[must_use]
    pub fn total_mass(&self) -> f32 {
        self.bones.iter().map(|b| b.mass).sum()
    }

    /// Number of bones.
    #[must_use]
    #[inline]
    pub fn bone_count(&self) -> usize {
        self.bones.len()
    }

    /// Get all root bones (no parent).
    #[must_use]
    pub fn roots(&self) -> Vec<&Bone> {
        self.bones.iter().filter(|b| b.parent.is_none()).collect()
    }

    /// Get children of a bone.
    #[must_use]
    pub fn children(&self, parent_id: BoneId) -> Vec<&Bone> {
        self.bones
            .iter()
            .filter(|b| b.parent == Some(parent_id))
            .collect()
    }

    /// Chain from bone to root (inclusive).
    #[must_use]
    #[instrument(skip(self), fields(skeleton = %self.name))]
    pub fn chain_to_root(&self, bone_id: BoneId) -> Vec<BoneId> {
        let mut chain = vec![bone_id];
        let mut current = bone_id;
        while let Some(bone) = self.get_bone(current) {
            if let Some(parent) = bone.parent {
                chain.push(parent);
                current = parent;
            } else {
                break;
            }
        }
        chain
    }
}

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

    fn test_skeleton() -> Skeleton {
        Skeleton {
            name: "test".into(),
            bones: vec![
                Bone {
                    id: BoneId(0),
                    name: "root".into(),
                    parent: None,
                    length: 0.5,
                    mass: 10.0,
                    local_position: Vec3::ZERO,
                    local_rotation: Quat::IDENTITY,
                },
                Bone {
                    id: BoneId(1),
                    name: "spine".into(),
                    parent: Some(BoneId(0)),
                    length: 0.4,
                    mass: 8.0,
                    local_position: Vec3::new(0.0, 0.5, 0.0),
                    local_rotation: Quat::IDENTITY,
                },
                Bone {
                    id: BoneId(2),
                    name: "head".into(),
                    parent: Some(BoneId(1)),
                    length: 0.2,
                    mass: 5.0,
                    local_position: Vec3::new(0.0, 0.4, 0.0),
                    local_rotation: Quat::IDENTITY,
                },
                Bone {
                    id: BoneId(3),
                    name: "left_arm".into(),
                    parent: Some(BoneId(1)),
                    length: 0.6,
                    mass: 4.0,
                    local_position: Vec3::new(-0.2, 0.3, 0.0),
                    local_rotation: Quat::IDENTITY,
                },
                Bone {
                    id: BoneId(4),
                    name: "right_arm".into(),
                    parent: Some(BoneId(1)),
                    length: 0.6,
                    mass: 4.0,
                    local_position: Vec3::new(0.2, 0.3, 0.0),
                    local_rotation: Quat::IDENTITY,
                },
            ],
        }
    }

    #[test]
    fn find_bone_by_name() {
        let s = test_skeleton();
        assert!(s.find_bone("spine").is_some());
        assert!(s.find_bone("tail").is_none());
    }

    #[test]
    fn total_mass() {
        let s = test_skeleton();
        assert!((s.total_mass() - 31.0).abs() < 0.01);
    }

    #[test]
    fn bone_count() {
        assert_eq!(test_skeleton().bone_count(), 5);
    }

    #[test]
    fn single_root() {
        let s = test_skeleton();
        assert_eq!(s.roots().len(), 1);
        assert_eq!(s.roots()[0].name, "root");
    }

    #[test]
    fn spine_has_children() {
        let s = test_skeleton();
        let children = s.children(BoneId(1));
        assert_eq!(children.len(), 3); // head, left_arm, right_arm
    }

    #[test]
    fn chain_to_root() {
        let s = test_skeleton();
        let chain = s.chain_to_root(BoneId(2)); // head → spine → root
        assert_eq!(chain, vec![BoneId(2), BoneId(1), BoneId(0)]);
    }

    #[test]
    fn root_chain_is_self() {
        let s = test_skeleton();
        let chain = s.chain_to_root(BoneId(0));
        assert_eq!(chain, vec![BoneId(0)]);
    }
}