animsmith-core 0.1.0

//! Pipeline-mechanical clip transforms, ported from the incubating
//! bake's Python: frame-window slicing, hold-extension, and gait-anchor
//! rotation. Scope rule (DESIGN.md §1): animsmith may rewrite a clip
//! only in ways whose correctness its own checks can verify.

use crate::metrics::foot_cycle_metrics;
use crate::model::{Clip, Interpolation, Skeleton, Track, TrackValues};
use crate::profile::ResolvedRoles;
use crate::sample::{TrackSample, sample_clip, sample_track};

/// Keep only the keys inside `[start, end]` seconds (with a half-frame
/// epsilon at `fps` absorbing float drift from earlier retimings) and
/// retime them so the window starts at 0. Cubic tangent triplets move
/// with their keys. The clip duration becomes `end - start`.
///
/// Boundary keys are snapped to the window, not carried past it: keys
/// within the epsilon of `start` clamp to 0 and keys within it of `end`
/// clamp to the new duration. When several keys land on a boundary, the
/// one closest to the original boundary is kept and the rest dropped —
/// so the output has at most one key at 0 and one at the end, stays
/// time-monotonic, and round-trips its declared duration.
///
/// # Panics
///
/// Panics if a hand-built track violates the loader invariant that
/// `values` contains one value per key for linear/step tracks, or one
/// tangent-value-tangent triplet per key for cubic-spline tracks.
pub fn slice(clip: &mut Clip, start_s: f64, end_s: f64, fps: f64) {
    let eps = (0.5 / fps) as f32;
    let (start, end) = (start_s as f32, end_s as f32);
    let duration = (end - start).max(0.0);
    for track in &mut clip.tracks {
        // (key index, retimed+clamped time), in original key order.
        let mut kept: Vec<(usize, f32)> = (0..track.key_count())
            .filter(|&k| track.times[k] >= start - eps && track.times[k] <= end + eps)
            .map(|k| (k, (track.times[k] - start).clamp(0.0, duration)))
            .collect();

        // Drop boundary duplicates: at t=0 keep the last (closest to
        // `start`); at t=duration keep the first (closest to `end`).
        // Interior times are already distinct and monotonic.
        kept.retain({
            let times: Vec<f32> = kept.iter().map(|&(_, t)| t).collect();
            let mut i = 0;
            move |_| {
                let t = times[i];
                let keep = if t <= 0.0 {
                    times.get(i + 1).is_none_or(|&next| next > 0.0)
                } else if t >= duration {
                    i == 0 || times[i - 1] < duration
                } else {
                    true
                };
                i += 1;
                keep
            }
        });

        track.times = kept.iter().map(|&(_, t)| t).collect();
        let per_key = match track.interpolation {
            Interpolation::CubicSpline => 3,
            _ => 1,
        };
        match &mut track.values {
            TrackValues::Vec3s(v) => {
                let old = std::mem::take(v);
                *v = kept
                    .iter()
                    .flat_map(|&(k, _)| old[k * per_key..(k + 1) * per_key].to_vec())
                    .collect();
            }
            TrackValues::Quats(v) => {
                let old = std::mem::take(v);
                *v = kept
                    .iter()
                    .flat_map(|&(k, _)| old[k * per_key..(k + 1) * per_key].to_vec())
                    .collect();
            }
        }
    }
    clip.duration_s = (end_s - start_s).max(0.0);
    clip.tracks.retain(|t| t.key_count() > 0);
}

/// Append one key per track duplicating its final value `hold_s`
/// seconds after its last key (a linear hold — charge/block poses).
/// The clip duration extends to the longest held end.
///
/// # Panics
///
/// Panics if a hand-built track violates the loader invariant that each
/// key has a corresponding stored value (or cubic-spline triplet).
pub fn hold_extend(clip: &mut Clip, hold_s: f64) {
    for track in &mut clip.tracks {
        let Some(&last) = track.times.last() else {
            continue;
        };
        let key = track.key_count() - 1;
        track.times.push(last + hold_s as f32);
        let value_index = track.value_index(key);
        match &mut track.values {
            TrackValues::Vec3s(v) => {
                let value = v[value_index];
                match track.interpolation {
                    Interpolation::CubicSpline => {
                        // Zero tangents: a flat Hermite hold. Also zero
                        // the previous key's out-tangent so the hold
                        // segment stays flat.
                        v[key * 3 + 2] = glam::Vec3::ZERO;
                        v.extend_from_slice(&[glam::Vec3::ZERO, value, glam::Vec3::ZERO]);
                    }
                    _ => v.push(value),
                }
            }
            TrackValues::Quats(v) => {
                let value = v[value_index];
                match track.interpolation {
                    Interpolation::CubicSpline => {
                        v[key * 3 + 2] = glam::Quat::from_xyzw(0.0, 0.0, 0.0, 0.0);
                        v.extend_from_slice(&[
                            glam::Quat::from_xyzw(0.0, 0.0, 0.0, 0.0),
                            value,
                            glam::Quat::from_xyzw(0.0, 0.0, 0.0, 0.0),
                        ]);
                    }
                    _ => v.push(value),
                }
            }
        }
        clip.duration_s = clip.duration_s.max((last + hold_s as f32) as f64);
    }
}

/// Outcome of [`align_gait_anchor`].
#[derive(Debug, Clone)]
#[non_exhaustive]
pub struct GaitAlignOutcome {
    /// The measured stride-anchor phase before rotation.
    pub phase_before: f64,
    /// The phase after rotation (should sit near 0).
    pub phase_after: f64,
    /// Loop-seam ratio after rotation (the chosen candidate's wrap).
    pub seam_after: Option<f64>,
    /// The whole-frame offset (−1/0/+1) that produced the cleanest wrap.
    pub frame_offset: i32,
}

/// Rotate a cyclic clip in time so its measured stride anchor (the
/// trough of the L−R foot-height fundamental) lands at clip time 0.
///
/// Semantics ported from the reference bake: the cycle period is
/// `duration + 1/fps` (an open loop's wrap step is a real frame of the
/// stride); the shift is quantized to whole frames so every resample
/// lands on an existing key; each animated channel keeps its times and
/// gets its output values replaced by the channel sampled at
/// `(t + shift) mod period`. Constant channels are rotation-invariant
/// and left alone; a non-constant CUBICSPLINE channel cannot be
/// resampled losslessly, so alignment refuses (naming it) rather than
/// rotate the rest of the rig around it. Because a ±1-frame shift stays
/// inside phase tolerance but moves *where the wrap lands*, all three
/// candidates are tried and the one with the cleanest wrap (lowest seam
/// ratio) wins.
///
/// # Errors
///
/// Returns an error when the clip has no measurable stride anchor, the
/// left-right foot amplitude is too small to define a stable phase, a
/// non-constant cubic-spline track would need lossy resampling, or no
/// tested rotation candidate remains measurable.
///
/// # Panics
///
/// Panics if `roles` contains bone indices outside `skeleton`, or if a
/// hand-built clip violates the loader key/value-count invariants.
pub fn align_gait_anchor(
    skeleton: &Skeleton,
    clip: &mut Clip,
    roles: &ResolvedRoles,
    fps: f64,
) -> Result<GaitAlignOutcome, String> {
    let measure = |c: &Clip| -> Option<(f64, Option<f64>, f64)> {
        let frames = crate::metrics::metric_frame_count(c)?;
        let grid = sample_clip(skeleton, c, frames);
        let m = foot_cycle_metrics(&grid, roles, crate::metrics::MIN_STRIDE_STEP_M)?;
        Some((m.gait_phase?, m.loop_seam_ratio, m.lr_amplitude_m))
    };
    let Some((phase_before, _, amplitude)) = measure(clip) else {
        return Err(
            "no usable stride anchor (hips/foot roles unresolved or clip too short)".into(),
        );
    };
    if amplitude < 0.03 {
        return Err(format!(
            "no usable stride anchor (L−R amplitude {amplitude:.4} m) — a ring clip must \
             alternate its feet for anchor alignment to mean anything"
        ));
    }

    // Refuse rather than rotate part of a clip: a channel we cannot
    // resample coherently (a non-constant CUBICSPLINE track) would be
    // left in place while its siblings shift, desynchronizing the rig.
    // Constant tracks are rotation-invariant and safely skipped.
    let unrotatable: Vec<String> = clip
        .tracks
        .iter()
        .filter(|t| t.interpolation == Interpolation::CubicSpline && !is_constant_track(t))
        .map(|t| format!("{} bone {}", t.property.as_str(), t.bone))
        .collect();
    if !unrotatable.is_empty() {
        return Err(format!(
            "cannot gait-anchor: these animated tracks need lossless resampling that is \
             not yet supported ({}); retime them to LINEAR first",
            unrotatable.join(", ")
        ));
    }

    let original = clip.clone();
    let mut best: Option<(f64, GaitAlignOutcome, Clip)> = None;
    for frame_offset in [0i32, -1, 1] {
        let mut candidate = original.clone();
        rotate_values(&mut candidate, phase_before, fps, frame_offset);
        let Some((phase_after, seam_after, _)) = measure(&candidate) else {
            continue;
        };
        // Rank by wrap cleanliness; a missing seam (no stride at the
        // wrap) should not happen on a ring clip — rank it last.
        let rank = seam_after.unwrap_or(f64::MAX);
        if best.as_ref().is_none_or(|(r, _, _)| rank < *r) {
            best = Some((
                rank,
                GaitAlignOutcome {
                    phase_before,
                    phase_after,
                    seam_after,
                    frame_offset,
                },
                candidate,
            ));
        }
    }
    let Some((_, outcome, rotated)) = best else {
        return Err("no rotation candidate was measurable".into());
    };
    *clip = rotated;
    Ok(outcome)
}

/// True when `track` holds the same pose at every time — such a track
/// is invariant under time rotation, so it can be skipped safely. For
/// CUBICSPLINE this means the keyed values agree *and* every in/out
/// tangent is zero: equal values with non-zero tangents still describe
/// an animated Hermite curve (the sampler interpolates through the
/// tangents), so that track must be rotated or refused, never skipped.
/// A short or inconsistent value array reads as animated (never wrongly
/// skipped).
fn is_constant_track(track: &Track) -> bool {
    let n = track.key_count();
    if n <= 1 {
        return true;
    }
    let cubic = track.interpolation == Interpolation::CubicSpline;
    fn constant<T: Copy + PartialEq>(v: &[T], n: usize, cubic: bool, zero: T) -> bool {
        let value = |k: usize| if cubic { 3 * k + 1 } else { k };
        let Some(&first) = v.get(value(0)) else {
            return false;
        };
        (0..n).all(|k| {
            // Keyed value matches, and for cubic the in/out tangents
            // (indices 3k and 3k+2) are zero — a genuinely flat hold.
            v.get(value(k)) == Some(&first)
                && (!cubic || (v.get(3 * k) == Some(&zero) && v.get(3 * k + 2) == Some(&zero)))
        })
    }
    match &track.values {
        TrackValues::Vec3s(v) => constant(v, n, cubic, glam::Vec3::ZERO),
        TrackValues::Quats(v) => constant(v, n, cubic, glam::Quat::from_xyzw(0.0, 0.0, 0.0, 0.0)),
    }
}

/// Replace each animated channel's output values with the channel
/// sampled at `(t + shift) mod period`; times untouched. Constant
/// tracks (rotation-invariant) are skipped; non-constant CUBICSPLINE
/// tracks are refused upstream in [`align_gait_anchor`].
///
/// Uniform-framing assumption: the cycle period is `duration + 1/fps`,
/// i.e. every track is framed on the same uniform `1/fps` grid and the
/// open-loop wrap step spans exactly one such frame. On a clip with
/// irregular key spacing — or tracks framed at different rates — that
/// period is only approximate, the quantized shift may miss an existing
/// key by a fraction of a frame, and the resample stops being exactly
/// lossless. animsmith's pipeline emits uniformly-framed clips, so the
/// assumption holds for the inputs this transform is applied to.
fn rotate_values(clip: &mut Clip, phase: f64, fps: f64, frame_offset: i32) {
    let duration = clip
        .tracks
        .iter()
        .map(Track::end_time)
        .fold(0.0f32, f32::max) as f64;
    if duration <= 0.0 {
        return;
    }
    let period = duration + 1.0 / fps;
    let mut shift = ((phase * period * fps).round() + frame_offset as f64) / fps;
    shift = shift.rem_euclid(period);

    for track in &mut clip.tracks {
        // Constant tracks (any key count) are invariant; cubic tracks
        // reaching here are constant, so the zip below only touches
        // LINEAR/STEP values. Non-constant short tracks (e.g. a 2-key
        // root ramp) are now rotated instead of silently left behind.
        if is_constant_track(track) {
            continue;
        }
        let sampled: Vec<TrackSample> = track
            .times
            .iter()
            .map(|&t| sample_track(track, ((t as f64 + shift) % period) as f32))
            .collect();
        match &mut track.values {
            TrackValues::Vec3s(v) => {
                for (slot, s) in v.iter_mut().zip(&sampled) {
                    if let TrackSample::Vec3(x) = s {
                        *slot = *x;
                    }
                }
            }
            TrackValues::Quats(v) => {
                for (slot, s) in v.iter_mut().zip(&sampled) {
                    if let TrackSample::Quat(q) = s {
                        *slot = *q;
                    }
                }
            }
        }
    }
}