//! Load-time orientation and animation corrections for source bodies.
//!
//! Applied once per body after loading, inside `preview::posed_worlds`, so
//! the live view, stills, production and equipment all see the same pose.
//!
//! * **Forward axis.** The renderer's convention is "character faces −Z"
//!   (yaw 0 = front). A body whose feet point to +Z (toes in front of the
//!   ankles, the glTF default) is turned 180° about Y.
//! * **Detached feet.** IK rigs often keep the foot bones under the root
//!   instead of under the shin. When such a foot chain starts exactly at the
//!   end of an animated leaf bone and has no animation of its own, it follows
//!   that bone rigidly (as forward kinematics would).
//! * **Front/back mirrored motion.** When knees bend the wrong way relative
//!   to the body's own front in almost every frame, the motion is mirrored
//!   front/back (per-joint world deltas reflected through the frontal plane);
//!   the rest pose is unchanged.
use crate::preview::{self, affine_inverse, mul, Mat4, PreviewCharacter};

#[derive(Debug, Clone, Default, PartialEq)]
pub struct BodyFixups {
    /// Turn the whole body 180° about +Y (source faces +Z).
    pub turn_180: bool,
    /// Reflect the motion front/back (source-local Z) around the rest pose.
    pub mirror_motion: bool,
    /// (node, driver node): node's subtree follows the driver rigidly.
    pub follow: Vec<(usize, usize)>,
    /// Human-readable (Dutch) explanations for the editor/render log.
    pub notes: Vec<String>,
}

impl BodyFixups {
    pub fn is_identity(&self) -> bool {
        !self.turn_180 && !self.mirror_motion && self.follow.is_empty()
    }
}

/// Which way a body faces, as decided by `detect`.
#[derive(Debug, Clone, Copy, PartialEq, Eq)]
pub enum Forward {
    MinusZ,
    PlusZ,
}

fn diag(x: f32, y: f32, z: f32) -> Mat4 {
    let mut m = preview::identity();
    m[0] = x;
    m[5] = y;
    m[10] = z;
    m
}

fn descendants(character: &PreviewCharacter, root: usize) -> Vec<usize> {
    let mut out = vec![root];
    let mut i = 0;
    while i < out.len() {
        let n = out[i];
        for (c, node) in character.nodes.iter().enumerate() {
            if node.parent == Some(n) && !out.contains(&c) {
                out.push(c);
            }
        }
        i += 1;
    }
    out
}

fn is_ancestor(character: &PreviewCharacter, a: usize, mut n: usize) -> bool {
    let mut guard = 0;
    while let Some(p) = character.nodes.get(n).and_then(|x| x.parent) {
        if p == a {
            return true;
        }
        n = p;
        guard += 1;
        if guard > character.nodes.len() {
            break;
        }
    }
    false
}

/// Apply `character.fixups` to raw posed world matrices.
pub fn apply(character: &PreviewCharacter, worlds: Vec<Mat4>) -> Vec<Mat4> {
    apply_with(character, &character.fixups, worlds)
}

fn apply_with(character: &PreviewCharacter, f: &BodyFixups, mut w: Vec<Mat4>) -> Vec<Mat4> {
    if f.is_identity() {
        return w;
    }
    let rest = preview::rest_worlds(character);
    for &(node, driver) in &f.follow {
        let (Some(rd), Some(rn)) = (affine_inverse(&rest[driver]), affine_inverse(&w[node])) else {
            continue;
        };
        let delta = mul(w[driver], rd);
        let new_node = mul(delta, rest[node]);
        for d in descendants(character, node) {
            w[d] = mul(new_node, mul(rn, w[d]));
        }
    }
    if f.mirror_motion {
        let s = diag(1., 1., -1.);
        for (i, m) in w.iter_mut().enumerate() {
            if let Some(ri) = affine_inverse(&rest[i]) {
                *m = mul(s, mul(*m, mul(ri, mul(s, rest[i]))));
            }
        }
    }
    if f.turn_180 {
        let r = diag(-1., 1., -1.);
        for m in w.iter_mut() {
            *m = mul(r, *m);
        }
    }
    w
}

fn point(m: &Mat4, p: [f32; 3]) -> [f32; 3] {
    [
        m[0] * p[0] + m[4] * p[1] + m[8] * p[2] + m[12],
        m[1] * p[0] + m[5] * p[1] + m[9] * p[2] + m[13],
        m[2] * p[0] + m[6] * p[1] + m[10] * p[2] + m[14],
    ]
}

/// Rest-pose vertex positions (bind positions through the rest skin).
fn rest_positions(character: &PreviewCharacter) -> Vec<[f32; 3]> {
    if character.is_static() || character.joint_nodes.is_empty() {
        return character.positions.clone();
    }
    let rest = preview::rest_worlds(character);
    let skin: Vec<Mat4> = character
        .joint_nodes
        .iter()
        .zip(&character.inverse_binds)
        .map(|(n, ib)| mul(rest[*n], *ib))
        .collect();
    character
        .positions
        .iter()
        .enumerate()
        .map(|(i, p)| {
            let w = character
                .weights
                .get(i)
                .copied()
                .unwrap_or([1., 0., 0., 0.]);
            let j = character.joints.get(i).copied().unwrap_or([0; 4]);
            let mut out = [0.; 3];
            let mut sum = 0.;
            for k in 0..4 {
                if w[k] > 0. {
                    if let Some(m) = skin.get(j[k] as usize) {
                        let q = point(m, *p);
                        for a in 0..3 {
                            out[a] += w[k] * q[a];
                        }
                        sum += w[k];
                    }
                }
            }
            if sum > 0. {
                out.map(|v| v / sum)
            } else {
                *p
            }
        })
        .collect()
}

/// Feet heuristic: the vertices of the lowest 4% of the body (feet) extend
/// further forward than those at 6–10% (ankles/shins). None when unclear
/// (meshes under 1000 vertices such as box proxies, fewer than 12 vertices per
/// band, or a difference under 1% of height).
pub fn detect_forward(positions: &[[f32; 3]]) -> Option<Forward> {
    let (lo, hi) = positions
        .iter()
        .fold((f32::INFINITY, f32::NEG_INFINITY), |(a, b), p| {
            (a.min(p[1]), b.max(p[1]))
        });
    let h = hi - lo;
    if !(h.is_finite() && h > 0.) || positions.len() < 1000 {
        return None;
    }
    let mean = |a: f32, b: f32| {
        let v: Vec<f32> = positions
            .iter()
            .filter(|p| p[1] >= lo + a * h && p[1] <= lo + b * h)
            .map(|p| p[2])
            .collect();
        // Too few vertices (box-like proxies) give no reliable feet shape.
        (v.len() >= 12).then(|| v.iter().sum::<f32>() / v.len() as f32)
    };
    let feet = mean(0., 0.04)?;
    let ankles = mean(0.06, 0.10)?;
    let d = feet - ankles;
    if d > 0.01 * h {
        Some(Forward::PlusZ)
    } else if d < -0.01 * h {
        Some(Forward::MinusZ)
    } else {
        None
    }
}

fn channel_static(ch: &preview::AnimChannel) -> bool {
    let n = ch.property.width();
    let first: Vec<f32> = ch.values.iter().take(n).copied().collect();
    ch.values
        .chunks(n.max(1))
        .all(|k| k.iter().zip(&first).all(|(a, b)| (a - b).abs() < 1e-5))
}

fn node_animated(character: &PreviewCharacter, node: usize) -> bool {
    character.clips.iter().any(|c| {
        c.channels
            .iter()
            .any(|ch| ch.node == node && !channel_static(ch))
    })
}

fn node_matches_rest(character: &PreviewCharacter, node: usize) -> bool {
    // Static channels must also equal the rest pose (else the bone is posed).
    let n = &character.nodes[node];
    character.clips.iter().all(|c| {
        c.channels.iter().filter(|ch| ch.node == node).all(|ch| {
            let rest: Vec<f32> = match ch.property {
                preview::ChannelProperty::Translation => n.translation.to_vec(),
                preview::ChannelProperty::Rotation => n.rotation.to_vec(),
                preview::ChannelProperty::Scale => n.scale.to_vec(),
            };
            let k = &ch.values[..rest.len().min(ch.values.len())];
            let dot: f32 = k.iter().zip(&rest).map(|(a, b)| a * b).sum();
            match ch.property {
                preview::ChannelProperty::Rotation => dot.abs() > 0.9999,
                _ => k.iter().zip(&rest).all(|(a, b)| (a - b).abs() < 1e-4),
            }
        })
    })
}

/// Detect all fixups for a freshly loaded body.
pub fn detect(character: &PreviewCharacter) -> BodyFixups {
    let mut f = BodyFixups::default();
    let positions = rest_positions(character);
    let forward = detect_forward(&positions);
    if character.is_static() {
        return f;
    }
    let rest = preview::rest_worlds(character);
    let (lo, hi) = positions
        .iter()
        .fold((f32::INFINITY, f32::NEG_INFINITY), |(a, b), p| {
            (a.min(p[1]), b.max(p[1]))
        });
    let h = (hi - lo).max(1e-3);
    let joints = &character.joint_nodes;
    let has_joint_child = |n: usize| joints.iter().any(|c| character.nodes[*c].parent == Some(n));
    // Detached feet.
    for &driver in joints {
        if has_joint_child(driver) || !node_animated(character, driver) {
            continue;
        }
        let Some(inv) = affine_inverse(&rest[driver]) else {
            continue;
        };
        for &cand in joints {
            if cand == driver
                || is_ancestor(character, cand, driver)
                || is_ancestor(character, driver, cand)
            {
                continue;
            }
            let o = [rest[cand][12], rest[cand][13], rest[cand][14]];
            let l = point(&inv, o);
            // Rest origin on the driver's bone axis (+Y), beyond its head.
            let len = (rest[driver][4].powi(2) + rest[driver][5].powi(2) + rest[driver][6].powi(2))
                .sqrt();
            let along = l[1] * len;
            if !(l[0].abs() * len < 0.003 * h
                && l[2].abs() * len < 0.003 * h
                && along > 0.05 * h
                && along < 0.5 * h)
            {
                continue;
            }
            // Climb to the topmost ancestor that is not above the driver.
            let mut top = cand;
            while let Some(p) = character.nodes[top].parent {
                if is_ancestor(character, p, driver) || p == driver {
                    break;
                }
                top = p;
            }
            let subtree = descendants(character, top);
            if subtree
                .iter()
                .any(|n| node_animated(character, *n) || !node_matches_rest(character, *n))
            {
                continue;
            }
            if !f.follow.iter().any(|(n, _)| *n == top) {
                f.follow.push((top, driver));
                f.notes.push(format!(
                    "{} volgt {} (losse voetketen zonder eigen animatie)",
                    character.nodes[top].name, character.nodes[driver].name
                ));
            }
        }
    }
    // Knee direction vs. the body's own front.
    let front = match forward {
        Some(Forward::PlusZ) => 1.,
        _ => -1.,
    };
    let mut legs: Vec<(usize, usize, [f32; 3])> = Vec::new(); // (thigh node, shin node, ankle in shin space)
    for &(node, driver) in &f.follow {
        if let (Some(p), Some(inv)) = (
            character.nodes[driver].parent,
            affine_inverse(&rest[driver]),
        ) {
            legs.push((
                p,
                driver,
                point(&inv, [rest[node][12], rest[node][13], rest[node][14]]),
            ));
        }
    }
    let (mut good, mut bad) = (0, 0);
    if !legs.is_empty() {
        let partial = BodyFixups {
            follow: f.follow.clone(),
            ..Default::default()
        };
        for clip in &character.clips {
            let frames = ((clip.duration * 24.).ceil() as usize).max(1);
            for i in 0..frames {
                let w = apply_with(
                    character,
                    &partial,
                    preview::raw_posed_worlds(character, &clip.name, i as f32 / 24.),
                );
                for (thigh, shin, ankle_local) in &legs {
                    let hip = [w[*thigh][12], w[*thigh][13], w[*thigh][14]];
                    let knee = [w[*shin][12], w[*shin][13], w[*shin][14]];
                    let ankle = point(&w[*shin], *ankle_local);
                    let dy = ankle[1] - hip[1];
                    if dy.abs() < 1e-4 {
                        continue;
                    }
                    let t = (knee[1] - hip[1]) / dy;
                    let line_z = hip[2] + t * (ankle[2] - hip[2]);
                    let off = (knee[2] - line_z) * front;
                    if off > 0.02 * h {
                        good += 1;
                    } else if off < -0.02 * h {
                        bad += 1;
                    }
                }
            }
        }
    }
    if bad >= 4 && bad > 3 * good {
        f.mirror_motion = true;
        f.notes.push(format!(
            "beweging voor/achter gespiegeld: knieën bogen in {bad} van {} gemeten standen de verkeerde kant op",
            bad + good
        ));
    }
    if forward == Some(Forward::PlusZ) {
        f.turn_180 = true;
        f.notes.push("lichaam kijkt naar +Z (tenen voor de enkels): 180° gedraaid zodat yaw 0 de voorkant toont".into());
    }
    f
}

/// Detect and store the fixups on a loaded body. Static bodies (no bones)
/// get their +Z-facing geometry turned directly.
pub fn finalize(mut character: PreviewCharacter) -> PreviewCharacter {
    if character.is_static() {
        if detect_forward(&character.positions) == Some(Forward::PlusZ) {
            for p in character
                .positions
                .iter_mut()
                .chain(character.normals.iter_mut())
            {
                p[0] = -p[0];
                p[2] = -p[2];
            }
            character.fixups.notes.push(
                "lichaam kijkt naar +Z (tenen voor de enkels): 180° gedraaid zodat yaw 0 de voorkant toont".into(),
            );
        }
        return character;
    }
    character.fixups = detect(&character);
    character
}

/// Forward axis of a body's rest pose (None when unclear).
pub fn forward_of(character: &PreviewCharacter) -> Option<Forward> {
    detect_forward(&rest_positions(character))
}

#[cfg(test)]
mod tests {
    use super::*;
    use crate::preview::{
        AnimChannel, BodyRig, ChannelProperty, Interpolation, PreviewClip, SceneNode,
    };

    fn node(name: &str, parent: Option<usize>, t: [f32; 3], r: [f32; 4]) -> SceneNode {
        SceneNode {
            name: name.into(),
            parent,
            translation: t,
            rotation: r,
            scale: [1.; 3],
        }
    }

    /// Leg whose foot control hangs under the root (IK style), with a knee
    /// that bends by `knee_deg` (negative = shin back = natural for −Z front).
    fn ik_leg(knee_deg: f32) -> PreviewCharacter {
        let half = (knee_deg.to_radians() / 2.).sin_cos();
        let nodes = vec![
            node("root", None, [0., 0., 0.], [0., 0., 0., 1.]),
            node("thigh", Some(0), [0., 1., 0.], [1., 0., 0., 0.]),
            node("calf", Some(1), [0., 0.5, 0.], [0., 0., 0., 1.]),
            node("footControl", Some(0), [0., 0.05, 0.], [0., 0., 0., 1.]),
            node("toes", Some(3), [0., 0., -0.1], [0., 0., 0., 1.]),
        ];
        let clip = PreviewClip {
            name: "walk".into(),
            duration: 1.,
            channels: vec![AnimChannel {
                node: 2,
                property: ChannelProperty::Rotation,
                times: vec![0., 0.5, 1.],
                values: vec![0., 0., 0., 1., half.0, 0., 0., half.1, 0., 0., 0., 1.],
                interpolation: Interpolation::Linear,
            }],
            skipped: Vec::new(),
        };
        PreviewCharacter {
            nodes,
            joint_nodes: (0..5).collect(),
            joint_names: ["root", "thigh", "calf", "footControl", "toes"]
                .map(String::from)
                .to_vec(),
            inverse_binds: vec![preview::identity(); 5],
            positions: vec![[0., 0., 0.], [0., 1.8, 0.]],
            normals: vec![[0., 1., 0.]; 2],
            source_normals_complete: true,
            joints: vec![[0; 4]; 2],
            weights: vec![[1., 0., 0., 0.]; 2],
            indices: Vec::new(),
            base_color: [1.; 4],
            colors: Vec::new(),
            uvs: Vec::new(),
            surfaces: Vec::new(),
            clips: vec![clip],
            rig: BodyRig::Skinned,
            fixups: BodyFixups::default(),
        }
    }

    #[test]
    fn detached_foot_follows_the_shin() {
        let mut leg = ik_leg(-40.);
        leg.fixups = detect(&leg);
        assert_eq!(leg.fixups.follow, vec![(3, 2)]);
        assert!(!leg.fixups.mirror_motion && !leg.fixups.turn_180);
        let w = preview::posed_worlds(&leg, "walk", 0.5);
        // Foot origin = shin end (0.45 m along the shin), toes keep their offset.
        let end = point(&w[2], [0., 0.45, 0.]);
        for a in 0..3 {
            assert!(
                (w[3][12 + a] - end[a]).abs() < 1e-4,
                "{:?} vs {end:?}",
                &w[3][12..15]
            );
        }
        assert!(
            end[2] > 0.2,
            "shin swings back (+Z) for a −Z front: {end:?}"
        );
        let toes = [w[4][12], w[4][13], w[4][14]];
        assert!((toes[2] - end[2] + 0.1).abs() < 0.05 || (toes[1] - end[1]).abs() > 0.01);
        // At rest nothing moves.
        let r = preview::posed_worlds(&leg, "", 0.);
        assert!((r[3][13] - 0.05).abs() < 1e-5);
    }

    #[test]
    fn backwards_knees_mirror_the_motion_front_back() {
        let mut leg = ik_leg(40.);
        leg.fixups = detect(&leg);
        assert!(leg.fixups.mirror_motion, "{:?}", leg.fixups.notes);
        let w = preview::posed_worlds(&leg, "walk", 0.5);
        let ankle = [w[3][12], w[3][13], w[3][14]];
        assert!(
            ankle[2] > 0.2,
            "after the mirror the shin swings back: {ankle:?}"
        );
        // The rest pose is unchanged by the mirror.
        let r = preview::posed_worlds(&leg, "", 0.);
        assert!((r[2][13] - 0.5).abs() < 1e-5 && r[2][14].abs() < 1e-5);
    }

    #[test]
    fn plus_z_bodies_turn_and_test_bodies_stay() {
        let mut feet = Vec::new();
        for i in 0..1200 {
            let f = i as f32 / 1200.;
            // Feet (lowest 4%) reach +Z, ankles (6–10% of height) sit behind.
            if i % 3 == 0 {
                feet.push([f * 0.1, 0.02, 0.1 * f]);
            } else if i % 3 == 1 {
                feet.push([f * 0.1, 0.16, -0.05]);
            } else {
                feet.push([f * 0.1, 0.2 + 1.5 * f, 0.]);
            }
        }
        assert_eq!(detect_forward(&feet), Some(Forward::PlusZ));
        let mirrored: Vec<[f32; 3]> = feet.iter().map(|p| [p[0], p[1], -p[2]]).collect();
        assert_eq!(detect_forward(&mirrored), Some(Forward::MinusZ));
        for f in [
            "test-body-male.glb",
            "test-body-female.glb",
            "test-body-male.fbx",
        ] {
            let b = preview::load_body_file(
                &std::path::Path::new(env!("CARGO_MANIFEST_DIR"))
                    .join("tests/fixtures/bodies")
                    .join(f),
            )
            .unwrap();
            assert!(b.fixups.is_identity(), "{f}: {:?}", b.fixups.notes);
        }
    }
}