using Godot;

// Smooths one tracked pose for the physics tick. Tracking updates at the display rate (90 Hz) but
// physics runs at 144 Hz: sampled raw, every second or third tick sees the pose frozen and the next
// one jumped, and every velocity feed-forward downstream (muscles, hand guides, ground control)
// turns that into jerks — the step-2.1 "wobbling more and more / sliding around" in the headset.
// Each new sample is timestamped; between samples the pose is extrapolated with the (smoothed)
// sample velocity. Works in the PLAY-SPACE-LOCAL frame, so moving the origin never looks like motion.
public sealed class PoseFilter
{
    public const float MaxExtrapolation = 1.5f / 90f;  // s
    public const float VelocitySmoothing = 0.3f;       // EMA weight of each new sample
    public const float JumpDistance = 0.5f;            // m between samples = recenter, not motion

    private Transform3D _last;
    private double _tLast;
    private bool _has;

    public Vector3 Velocity { get; private set; }         // play-space-local m/s
    public Vector3 AngularVelocity { get; private set; }  // play-space-local rad/s

    public void Reset() => _has = false;

    public Transform3D Sample(Transform3D raw, double now)
    {
        if (!_has)
        {
            _has = true;
            _last = raw;
            _tLast = now;
            Velocity = AngularVelocity = Vector3.Zero;
            return raw;
        }
        if (raw != _last)
        {
            float interval = (float)(now - _tLast);
            if (interval > 1e-4f)
            {
                if (raw.Origin.DistanceTo(_last.Origin) > JumpDistance)
                {
                    Velocity = AngularVelocity = Vector3.Zero;
                }
                else
                {
                    Vector3 v = (raw.Origin - _last.Origin) / interval;
                    Vector3 w = BodyMath.RotationError(raw.Basis.GetRotationQuaternion(), _last.Basis.GetRotationQuaternion()) / interval;
                    Velocity = Velocity.Lerp(v, VelocitySmoothing);
                    AngularVelocity = AngularVelocity.Lerp(w, VelocitySmoothing);
                }
            }
            _last = raw;
            _tLast = now;
        }
        float age = Mathf.Min((float)(now - _tLast), MaxExtrapolation);
        Basis spin = AngularVelocity.LengthSquared() > 1e-10f
            ? new Basis(AngularVelocity.Normalized(), AngularVelocity.Length() * age) : Basis.Identity;
        return new Transform3D(spin * _last.Basis.Orthonormalized(), _last.Origin + Velocity * age);
    }
}

// The per-physics-tick body pipeline, shared by PlayerRig (headset) and RagdollSpike so the
// regression runs exactly what the game runs: filter the tracked poses → play-space correction
// (push-back / ground follow / falls) → True Body solve → (re)build → ragdoll muscles.
// Locomotion (moving the origin by the stick, turning, teleports) happens before Tick.
public sealed class RagdollDriver
{
    public readonly PoseFilter Head = new(), Left = new(), Right = new();
    public TrueBodySolver Solver;
    public PlayerRagdoll Ragdoll;
    public bool RebuildRequested = true;
    private double _now;

    // origin = play space (modified by the correction); raw* = origin-local tracked poses as last
    // reported (repeat between tracking updates); moveVelocity = intended origin velocity (stick),
    // the only origin motion that is real hand motion for the guides' feed-forward.
    public void Tick(BodyProfile profile, ref Transform3D origin, Transform3D rawHead, Transform3D rawLeft, Transform3D rawRight,
        Vector3 moveVelocity, float dt)
    {
        _now += dt;
        Transform3D head = Head.Sample(rawHead, _now), left = Left.Sample(rawLeft, _now), right = Right.Sample(rawRight, _now);
        if (Ragdoll.Built && !RebuildRequested)
            origin.Origin += Ragdoll.OriginCorrection((origin * head).Origin, origin.Origin.Y, dt);
        Transform3D h = origin * head, l = origin * left, r = origin * right;
        Solver.Solve(profile, h, l, r, origin.Origin.Y, dt);
        if (RebuildRequested || !Ragdoll.Built)
        {
            Ragdoll.Build(profile, Solver, l, r);
            RebuildRequested = false;
        }
        Vector3 lv = origin.Basis * Left.Velocity + moveVelocity, rv = origin.Basis * Right.Velocity + moveVelocity;
        Ragdoll.Step(Solver, l, r, lv, rv, origin.Origin.Y, dt);
    }
}