//! Digital humans / avatars — `wai.world.sim.skeletal0` (Phase 4).
//!
//! A telepresence stream is a live world of one avatar. The bones are
//! nodes; an animation / expression op stream sets joint transforms; the
//! sink skins a `wai.mesh.glb` (its skeletal armature + skin weights) by
//! the deterministic pose. There is no solver — like the state-feed, the
//! `step` is the identity; the pose changes only when a signed op sets a
//! joint. Conformance is therefore **pose-equivalence**: the per-bone
//! local transforms hash identically on every sink — a special case of
//! replay-equivalence. The forward-kinematics composition into world
//! space, and the skinning of the mesh, are the sink's presentation job
//! (a pure function of the hashed locals + the hierarchy), exactly where
//! `wai.mesh.glb` already sits — never in the state hash.
//!
//! The floor stays trig-free: a joint rotation is carried as a
//! quaternion (`Fx` x,y,z,w), and quaternion → rotation-matrix is `+ − ×`
//! only. Axis-angle authoring (which needs sin/cos) happens once,
//! off-line, in whatever produces the op bytes — the canonical artifact
//! is the `Fx` quaternion, and every sink replays it exactly.

use super::fixed::Fx;
use super::graph::WorldState;
use super::op::{mint_node_id, CausalOp};
use super::rng::Xoshiro256pp;
use super::sim::{Resolve, Sim};
use super::{SimContract, WorldError};

/// The reference skeletal sim capability.
pub const CAP_SIM_SKELETAL0: &str = "wai.world.sim.skeletal0";

/// Mutation kinds registered by skeletal0. All are LWW — a transform set
/// rides the canonical total order (last canonical writer wins); there
/// is no commutative transform set, so no SUM kind.
pub mod skel_mutation {
    /// `parent[16] | translation[3] | rotation[4] | scale[3]`
    /// (16 + 10×`i64` Fx = 96 bytes). Target MUST equal
    /// `mint_node_id(actor, lamport)`. `parent` is another bone's node
    /// id, or all-zero for a root.
    pub const CREATE: u16 = 0x0001;
    /// Empty payload — remove the bone.
    pub const DELETE: u16 = 0x0002;
    /// `rotation[4]` (4×`i64` Fx quaternion x,y,z,w) — the animation
    /// driver: set a joint's local rotation.
    pub const SET_ROTATION: u16 = 0x0010;
    /// `translation[3]` (3×`i64` Fx).
    pub const SET_TRANSLATION: u16 = 0x0011;
    /// `scale[3]` (3×`i64` Fx).
    pub const SET_SCALE: u16 = 0x0012;
}

/// A bone: a parent reference + a local TRS transform. Canonical field
/// order (and therefore hash order) is exactly this struct order:
/// `parent`, `translation`, `rotation`, `scale`.
#[derive(Clone, Copy, Debug, PartialEq)]
pub struct Bone {
    /// Parent bone's node id, or `[0; 16]` for a root.
    pub parent: [u8; 16],
    pub translation: [Fx; 3],
    /// Local rotation quaternion `(x, y, z, w)`.
    pub rotation: [Fx; 4],
    pub scale: [Fx; 3],
}

impl Default for Bone {
    fn default() -> Self {
        Bone {
            parent: [0u8; 16],
            translation: [Fx::ZERO; 3],
            rotation: [Fx::ZERO, Fx::ZERO, Fx::ZERO, Fx::ONE], // identity quat
            scale: [Fx::ONE; 3],
        }
    }
}

/// `wai.world.sim.skeletal0` — the identity-step avatar registrant.
pub struct Skeletal0;

impl Sim for Skeletal0 {
    type Node = Bone;

    fn capability(&self) -> &'static str {
        CAP_SIM_SKELETAL0
    }

    fn resolve_kind(&self, mutation_kind: u16) -> Option<Resolve> {
        use skel_mutation::*;
        match mutation_kind {
            CREATE | DELETE | SET_ROTATION | SET_TRANSLATION | SET_SCALE => Some(Resolve::Lww),
            _ => None,
        }
    }

    fn apply(&self, world: &mut WorldState<Bone>, op: &CausalOp) -> Result<(), WorldError> {
        use skel_mutation::*;
        match op.mutation_kind {
            CREATE => {
                if op.target != mint_node_id(&op.actor, op.lamport) {
                    return Err(WorldError::BadNodeMint);
                }
                if op.mutation.len() != 16 + 10 * 8 {
                    return Err(WorldError::BadMutation);
                }
                let mut parent = [0u8; 16];
                parent.copy_from_slice(&op.mutation[..16]);
                let v = take_fx::<10>(&op.mutation[16..])?;
                world.nodes.insert(
                    op.target,
                    Bone {
                        parent,
                        translation: [v[0], v[1], v[2]],
                        rotation: [v[3], v[4], v[5], v[6]],
                        scale: [v[7], v[8], v[9]],
                    },
                );
            }
            DELETE => {
                if !op.mutation.is_empty() {
                    return Err(WorldError::BadMutation);
                }
                world.nodes.remove(&op.target);
            }
            SET_ROTATION => {
                let v = take_fx::<4>(&op.mutation)?;
                if let Some(b) = world.nodes.get_mut(&op.target) {
                    b.rotation = v;
                }
            }
            SET_TRANSLATION => {
                let v = take_fx::<3>(&op.mutation)?;
                if let Some(b) = world.nodes.get_mut(&op.target) {
                    b.translation = v;
                }
            }
            SET_SCALE => {
                let v = take_fx::<3>(&op.mutation)?;
                if let Some(b) = world.nodes.get_mut(&op.target) {
                    b.scale = v;
                }
            }
            k => return Err(WorldError::UnknownMutationKind(k)),
        }
        Ok(())
    }

    /// No physics — the pose is set by ops, never integrated. (FK and
    /// skinning are the sink's presentation job, not the state.)
    fn step(
        &self,
        _world: &mut WorldState<Bone>,
        _contract: &SimContract,
        _tick: u64,
        _rng: &mut Xoshiro256pp,
    ) {
    }

    fn serialize_node(&self, node: &Bone, out: &mut Vec<u8>) {
        out.extend_from_slice(&node.parent);
        for v in node
            .translation
            .iter()
            .chain(node.rotation.iter())
            .chain(node.scale.iter())
        {
            out.extend_from_slice(&v.to_bits().to_le_bytes());
        }
    }

    fn parse_node(&self, bytes: &[u8]) -> Result<Bone, WorldError> {
        if bytes.len() != 16 + 10 * 8 {
            return Err(WorldError::BadMutation);
        }
        let mut parent = [0u8; 16];
        parent.copy_from_slice(&bytes[..16]);
        let v = take_fx::<10>(&bytes[16..])?;
        Ok(Bone {
            parent,
            translation: [v[0], v[1], v[2]],
            rotation: [v[3], v[4], v[5], v[6]],
            scale: [v[7], v[8], v[9]],
        })
    }
}

/// Encode a CREATE payload: `parent | translation | rotation | scale`.
pub fn pack_bone(parent: &[u8; 16], t: &[Fx; 3], r: &[Fx; 4], s: &[Fx; 3]) -> Vec<u8> {
    let mut out = Vec::with_capacity(16 + 80);
    out.extend_from_slice(parent);
    for v in t.iter().chain(r.iter()).chain(s.iter()) {
        out.extend_from_slice(&v.to_bits().to_le_bytes());
    }
    out
}

/// Encode a packed `Fx` vector (for SET_ROTATION / SET_TRANSLATION /
/// SET_SCALE payloads).
pub fn pack_fx(values: &[Fx]) -> Vec<u8> {
    let mut out = Vec::with_capacity(values.len() * 8);
    for v in values {
        out.extend_from_slice(&v.to_bits().to_le_bytes());
    }
    out
}

// ---------------------------------------------------------------------------
// Forward kinematics — PRESENTATION ONLY (never enters the state hash).
// A pure function of the hashed local transforms + the hierarchy: every
// sink that agrees on the pose agrees on these world positions. Output is
// authoring-float for the GPU, like WorldSink::particles().
// ---------------------------------------------------------------------------

/// World-space joint position of `bone`, composing the parent chain in
/// the `Fx` floor (translation + quaternion-rotation + scale, all
/// `+ − ×`). Returns the world translation as exact `Fx` (the caller
/// converts to f32 for display). `lookup` resolves a parent id to its
/// bone; a missing / cyclic parent terminates the chain at root.
pub fn world_translation(
    bone: &Bone,
    lookup: &impl Fn(&[u8; 16]) -> Option<Bone>,
) -> [Fx; 3] {
    // Walk root→leaf, applying each ancestor's TRS to the accumulated
    // point. Collect the chain first (root last) with a visited guard.
    let mut chain = vec![*bone];
    let mut seen = std::collections::BTreeSet::new();
    let mut cur = *bone;
    while cur.parent != [0u8; 16] {
        if !seen.insert(cur.parent) {
            break; // cycle guard — deterministic termination
        }
        match lookup(&cur.parent) {
            Some(p) => {
                chain.push(p);
                cur = p;
            }
            None => break,
        }
    }
    // The local origin of `bone` transformed up through its ancestors.
    let mut point = [Fx::ZERO; 3];
    for b in &chain {
        // scale, then rotate, then translate (TRS), into the parent frame.
        let scaled = [point[0] * b.scale[0], point[1] * b.scale[1], point[2] * b.scale[2]];
        let rotated = quat_rotate(&b.rotation, &scaled);
        point = [
            rotated[0] + b.translation[0],
            rotated[1] + b.translation[1],
            rotated[2] + b.translation[2],
        ];
    }
    point
}

/// Rotate a vector by a quaternion `(x, y, z, w)` — `v + 2w(q×v) +
/// 2(q×(q×v))`, the standard expansion, `+ − ×` only (no normalization,
/// no trig). An unnormalized quaternion scales the result, deterministic
/// either way.
fn quat_rotate(q: &[Fx; 4], v: &[Fx; 3]) -> [Fx; 3] {
    let (x, y, z, w) = (q[0], q[1], q[2], q[3]);
    let two = Fx::from_int(2);
    // u = q.xyz
    let u = [x, y, z];
    // t = 2 * cross(u, v)
    let t = [
        two * (u[1] * v[2] - u[2] * v[1]),
        two * (u[2] * v[0] - u[0] * v[2]),
        two * (u[0] * v[1] - u[1] * v[0]),
    ];
    // v + w*t + cross(u, t)
    let ct = [
        u[1] * t[2] - u[2] * t[1],
        u[2] * t[0] - u[0] * t[2],
        u[0] * t[1] - u[1] * t[0],
    ];
    [
        v[0] + w * t[0] + ct[0],
        v[1] + w * t[1] + ct[1],
        v[2] + w * t[2] + ct[2],
    ]
}

fn take_fx<const N: usize>(bytes: &[u8]) -> Result<[Fx; N], WorldError> {
    if bytes.len() != N * 8 {
        return Err(WorldError::BadMutation);
    }
    let mut out = [Fx::ZERO; N];
    for (i, chunk) in bytes.chunks_exact(8).enumerate() {
        out[i] = Fx::from_bits(i64::from_le_bytes(chunk.try_into().unwrap()));
    }
    Ok(out)
}

#[cfg(test)]
mod tests {
    use super::*;
    use crate::world::container::WorldContainer;
    use crate::world::replay::{collect_checkpoints, Ingest, LiveWorld};
    use crate::world::{SimContract, NUMERIC_FIXED64, RESOLVE_LWW};
    use ed25519_dalek::SigningKey;

    fn contract() -> SimContract {
        SimContract {
            tick_rate: 30,
            numeric: NUMERIC_FIXED64.into(),
            rng_alg: "xoshiro256++".into(),
            rng_seed: [0u8; 32],
            sim: CAP_SIM_SKELETAL0.into(),
            resolve_default: RESOLVE_LWW.into(),
            checkpoint_every: 1,
        }
    }
    fn key(b: u8) -> SigningKey {
        SigningKey::from_bytes(&[b; 32])
    }
    fn empty(ops: Vec<CausalOp>) -> WorldContainer {
        let c = contract();
        WorldContainer {
            contract_bytes: c.to_canonical_json(),
            contract: c,
            graph_bytes: WorldState::<Bone>::new().canonical_bytes(&Skeletal0),
            assets: vec![],
            ops,
            seal: None,
        }
    }
    const IDENT: [Fx; 4] = [Fx::ZERO, Fx::ZERO, Fx::ZERO, Fx::ONE];

    // A 2-bone arm: a root at the origin and an elbow offset +1 in y. An
    // animation op stream rotates the elbow over ticks. Replaying the
    // sealed log reproduces the exact pose hash — pose-equivalence.
    #[test]
    fn arm_animation_replays_exactly() {
        let k = key(1);
        let root = mint_node_id(&k.verifying_key().to_bytes(), 1);
        let elbow = mint_node_id(&k.verifying_key().to_bytes(), 2);
        let z = [Fx::ZERO; 3];
        let one = [Fx::ONE; 3];

        // a 90° turn about z, authored once as a quaternion (sin/cos here,
        // off-line — the op carries Fx, the floor never sees trig).
        let h = std::f64::consts::FRAC_PI_4; // half-angle of 90°
        let q90 = [
            Fx::from_bits((0.0_f64 * 4294967296.0) as i64),
            Fx::from_bits((0.0_f64 * 4294967296.0) as i64),
            Fx::from_bits((h.sin() * 4294967296.0).round() as i64),
            Fx::from_bits((h.cos() * 4294967296.0).round() as i64),
        ];

        let ops = vec![
            CausalOp::sign(&k, 1, 1, vec![], root, skel_mutation::CREATE, pack_bone(&[0u8; 16], &z, &IDENT, &one)),
            CausalOp::sign(&k, 2, 1, vec![], elbow, skel_mutation::CREATE, pack_bone(&root, &[Fx::ZERO, Fx::ONE, Fx::ZERO], &IDENT, &one)),
            CausalOp::sign(&k, 3, 5, vec![], elbow, skel_mutation::SET_ROTATION, pack_fx(&q90)),
        ];

        let sealed = empty(ops.clone());
        let a = collect_checkpoints(&Skeletal0, &sealed, 5).unwrap();

        // shuffled arrival converges to the same final pose hash
        let mut live = LiveWorld::open(Skeletal0, empty(vec![])).unwrap();
        for i in [2usize, 0, 1] {
            let outcome = live.ingest(ops[i].clone()).unwrap();
            assert!(matches!(outcome, Ingest::Applied | Ingest::Buffered));
        }
        let hpose = live.advance_to(5).unwrap();
        assert_eq!(hpose, a.last().unwrap().hash);

        // FK puts the elbow's child origin where the rotation moved it:
        // the elbow sits at y=1; rotating its local frame 90° about z does
        // not move its own origin, but a point +1 x in elbow-space lands
        // near (0,1±) in world. Spot-check the elbow's world origin = (0,1,0).
        let bones = live.state().nodes.clone();
        let lookup = |id: &[u8; 16]| bones.get(id).copied();
        let eb = bones.get(&elbow).unwrap();
        let wp = world_translation(eb, &lookup);
        assert_eq!(wp[1], Fx::ONE); // y stays 1
        assert_eq!(wp[0], Fx::ZERO);
    }

    // Setting a joint rotation changes the pose hash; setting it back
    // restores it — the hash is a faithful function of the pose.
    #[test]
    fn pose_hash_tracks_rotation() {
        let k = key(2);
        let b = mint_node_id(&k.verifying_key().to_bytes(), 1);
        let z = [Fx::ZERO; 3];
        let one = [Fx::ONE; 3];
        let create = CausalOp::sign(&k, 1, 1, vec![], b, skel_mutation::CREATE, pack_bone(&[0u8; 16], &z, &IDENT, &one));

        let mut live = LiveWorld::open(Skeletal0, empty(vec![create])).unwrap();
        let h0 = live.advance_to(1).unwrap();

        let spin = [Fx::ZERO, Fx::ONE, Fx::ZERO, Fx::ZERO]; // 180° about y
        live.ingest(CausalOp::sign(&k, 2, 2, vec![], b, skel_mutation::SET_ROTATION, pack_fx(&spin))).unwrap();
        let h1 = live.advance_to(2).unwrap();
        assert_ne!(h0, h1);

        live.ingest(CausalOp::sign(&k, 3, 3, vec![], b, skel_mutation::SET_ROTATION, pack_fx(&IDENT))).unwrap();
        let h2 = live.advance_to(3).unwrap();
        assert_eq!(h0, h2); // back to the identity pose → same hash
    }

    #[test]
    fn quat_rotate_identity_and_180() {
        // identity quat leaves a vector unchanged
        let v = [Fx::from_int(1), Fx::from_int(2), Fx::from_int(3)];
        assert_eq!(quat_rotate(&IDENT, &v), v);
        // 180° about y: (x,y,z) -> (-x, y, -z)
        let qy = [Fx::ZERO, Fx::ONE, Fx::ZERO, Fx::ZERO];
        let r = quat_rotate(&qy, &v);
        assert_eq!(r, [Fx::from_int(-1), Fx::from_int(2), Fx::from_int(-3)]);
    }
}
