acdream/src/AcDream.Core/Physics/InterpolationManager.cs

using System;
using System.Collections.Generic;
using System.Numerics;
using AcDream.Core.Physics.Motion;

namespace AcDream.Core.Physics;

// ─────────────────────────────────────────────────────────────────────────────
// InterpolationManager — retail CPhysicsObj interpolation queue.
//
// Source spec: docs/research/2026-05-04-l3-port/04-interp-manager.md
// Retail addresses (Sept-2013 EoR PDB):
//   InterpolationManager::InterpolateTo   acclient @ 0x00555B20
//   InterpolationManager::adjust_offset   acclient @ 0x00555D30
//   InterpolationManager::UseTime         acclient @ 0x00555F20
//   InterpolationManager::NodeCompleted   acclient @ 0x005559A0
//   InterpolationManager::StopInterpolating acclient @ 0x00555950
//
// FIFO Position-waypoint queue (cap 20). The compatibility overload returns
// only its world-space origin, while the production overload carries retail's
// complete relative Frame from Position::subtract2, including orientation.
//
// Bug fixes applied vs prior port (audit § 7):
//   #1: progress_quantum accumulates dt (not step magnitude).
//   #3: far-branch Enqueue sets node_fail_counter = 4 → immediate next-tick
//       blip-to-tail. Triggered by distance > AutonomyBlipDistance (100 m).
//   #4: secondary stall test ports the retail formula verbatim:
//       cumulative_progress / progress_quantum / dt < 0.30.
//   #5: tail-prune is a tail-walking loop (collapses multiple stale entries).
// ─────────────────────────────────────────────────────────────────────────────

/// <summary>Internal queue node. type=1 = Position waypoint (only kind we use).</summary>
internal sealed class InterpolationNode
{
    public Vector3 TargetPosition;
    public Quaternion TargetOrientation = Quaternion.Identity;
}

/// <summary>
/// Per-remote-entity position interpolation queue. Caller enqueues server
/// position updates and calls <see cref="AdjustOffset"/> once per physics
/// tick to get the per-frame correction delta.
/// </summary>
public sealed class InterpolationManager
{
    // ── public constants (retail binary values) ───────────────────────────────

    /// <summary>Maximum waypoints held before oldest (head) is dropped.</summary>
    public const int QueueCap = 20;

    /// <summary>
    /// Catch-up gain: catchUpSpeed = motionMaxSpeed × this modifier.
    /// Retail MAX_INTERPOLATED_VELOCITY_MOD (@ 0x00555D30 line 353122).
    /// </summary>
    public const float MaxInterpolatedVelocityMod = 2.0f;

    /// <summary>
    /// Fallback catch-up speed (m/s) when motion-table max speed is
    /// unavailable. Retail MAX_INTERPOLATED_VELOCITY (@ 0x40f00000 line 353137).
    /// </summary>
    public const float MaxInterpolatedVelocity = 7.5f;

    /// <summary>
    /// Per-5-frame stall progress threshold (meters).
    /// Retail MIN_DISTANCE_TO_REACH_POSITION (@ 0x00555E42).
    /// </summary>
    public const float MinDistanceToReachPosition = 0.20f;

    /// <summary>
    /// Reach + duplicate-prune radius (meters).
    /// Retail DESIRED_DISTANCE (@ 0x00555D30).
    /// </summary>
    public const float DesiredDistance = 0.05f;

    /// <summary>
    /// Number of ticks per stall progress check window.
    /// Retail frame_counter threshold (@ 0x00555E14).
    /// </summary>
    public const int StallCheckFrameInterval = 5;

    /// <summary>
    /// Secondary stall ratio threshold — port verbatim from retail.
    /// Audit notes the formula has odd units (1/sec); not our bug to fix.
    /// Retail CREATURE_FAILED_INTERPOLATION_PERCENTAGE (@ 0x00555E73).
    /// </summary>
    public const float StallProgressMinFraction = 0.30f;

    /// <summary>
    /// Stall-fail counter threshold. Blip fires when fail count EXCEEDS this
    /// value (4+, not 3). Retail UseTime check (@ 0x00555F39): fail &gt; 3.
    /// </summary>
    public const int StallFailCountThreshold = 3;

    /// <summary>
    /// Distance threshold (meters) above which an Enqueue is treated as a far
    /// jump and pre-arms an immediate blip. Retail outdoor value; indoor is
    /// 20 m. Bug #3 fix from audit § 7.
    /// </summary>
    public const float AutonomyBlipDistance = 100.0f;

    /// <summary>
    /// Sentinel for original_distance before the first window baseline is
    /// taken. Retail value (@ 0x00555D30 ctor) is 999999f.
    /// </summary>
    public const float OriginalDistanceSentinel = 999999f;

    private const float FEpsilon = 0.0002f;

    // ── internals (retail field names in comments) ────────────────────────────

    private readonly LinkedList<InterpolationNode> _queue = new();   // position_queue

    private int   _frameCounter      = 0;                            // frame_counter
    private float _progressQuantum   = 0f;                           // progress_quantum (sum of dt)
    private float _originalDistance  = OriginalDistanceSentinel;     // original_distance
    private int   _failCount         = 0;                            // node_fail_counter
    private bool  _keepHeading;                                      // keep_heading

    // ── public API ────────────────────────────────────────────────────────────

    /// <summary>True when the queue holds at least one waypoint.</summary>
    public bool IsActive => _queue.Count > 0;

    /// <summary>Current waypoint count (visible to tests for cap verification).</summary>
    internal int Count => _queue.Count;

    /// <summary>
    /// Bug B (2026-08-04) read-only diagnostic view for the
    /// <c>ACDREAM_PROBE_REMOTE_SLIDE</c> family. The queue depth plus the
    /// live <c>node_fail_counter</c> is what lets a reader see blip producer
    /// Candidate 2 ARMING (fail count climbing toward
    /// <see cref="StallFailCountThreshold"/>) from the per-packet
    /// <c>[remote-slide-up]</c> line, before it fires. Pure read; no
    /// production consumer. TEMPORARY — strip with the probe family.
    /// </summary>
    public (int Depth, int FailCount) DiagnosticInterpolationState
        => (_queue.Count, _failCount);

    /// <summary>
    /// Stop interpolating: drain queue and reset all stall state to sentinel
    /// values. Retail StopInterpolating (@ 0x00555950).
    /// </summary>
    public void Clear()
    {
        _queue.Clear();
        _frameCounter     = 0;
        _progressQuantum  = 0f;
        _originalDistance = OriginalDistanceSentinel;
        _failCount        = 0;
    }

    /// <summary>
    /// Enqueue a new server-authoritative waypoint. Implements retail
    /// <c>InterpolateTo</c> branching:
    /// <list type="bullet">
    ///   <item><b>Already-close</b>: if distance(body, target) ≤
    ///   <see cref="DesiredDistance"/>, queue is wiped (StopInterpolating)
    ///   and no node is enqueued.</item>
    ///   <item><b>Far</b>: if distance(reference, target) &gt;
    ///   <see cref="AutonomyBlipDistance"/>, enqueue and set
    ///   node_fail_counter = StallFailCountThreshold + 1 — pre-arms an
    ///   immediate blip on the next AdjustOffset call.</item>
    ///   <item><b>Near</b>: tail-prune loop collapses adjacent stale entries
    ///   within <see cref="DesiredDistance"/>; cap at 20 (head eviction);
    ///   enqueue.</item>
    /// </list>
    /// </summary>
    /// <param name="targetPosition">Server-reported world position.</param>
    /// <param name="heading">Server-reported heading (radians).</param>
    /// <param name="isMovingTo">True when body is currently following an MTP.</param>
    /// <param name="currentBodyPosition">
    /// Body's current world position. Used for the already-close check (versus
    /// body) and as the fallback distance reference when the queue is empty.
    /// Pass <c>null</c> if not available — far/near classification falls back
    /// to "near" (no pre-armed blip).
    /// </param>
    public Quaternion? Enqueue(
        Vector3 targetPosition,
        float heading,
        bool isMovingTo,
        Vector3? currentBodyPosition = null)
        => Enqueue(
            targetPosition,
            Quaternion.CreateFromAxisAngle(Vector3.UnitZ, heading),
            isMovingTo,
            currentBodyPosition,
            currentBodyOrientation: null);

    /// <summary>
    /// Complete-frame overload of retail <c>InterpolateTo</c>. The node keeps
    /// the target quaternion; a near enqueue assigns
    /// <paramref name="isMovingTo"/> to retail's manager-wide
    /// <c>keep_heading</c> flag. The far branch deliberately retains the
    /// manager's prior flag, matching the retail early return.
    /// </summary>
    public Quaternion? Enqueue(
        Vector3 targetPosition,
        Quaternion targetOrientation,
        bool isMovingTo,
        Vector3? currentBodyPosition = null,
        Quaternion? currentBodyOrientation = null)
    {
        // Retail compares dist against either the tail's stored position
        // (if tail exists AND tail->type == 1) or the body's m_position.
        Vector3 reference;
        bool haveTail = _queue.Last is { } tail;
        if (haveTail)
        {
            reference = _queue.Last!.Value.TargetPosition;
        }
        else if (currentBodyPosition.HasValue)
        {
            reference = currentBodyPosition.Value;
        }
        else
        {
            reference = targetPosition; // dist = 0 → near branch
        }

        float dist = Vector3.Distance(reference, targetPosition);

        // Far branch (retail line 352918, dist > GetAutonomyBlipDistance):
        if (dist > AutonomyBlipDistance)
        {
            // The far branch does not assign keep_heading from arg3. It uses
            // the manager's existing flag when storing this Position.
            EnqueueRaw(
                targetPosition,
                StoreTargetOrientation(
                    targetOrientation,
                    currentBodyOrientation,
                    _keepHeading));
            // Pre-arm immediate blip on next AdjustOffset (audit § 7 #3).
            _failCount = StallFailCountThreshold + 1;
            return null;
        }

        // Near & already-close branch (retail line 352962):
        // distance(body, target) ≤ DesiredDistance → wipe queue, no enqueue.
        if (currentBodyPosition.HasValue)
        {
            float bodyDist = Vector3.Distance(currentBodyPosition.Value, targetPosition);
            if (bodyDist <= DesiredDistance)
            {
                Clear();
                // InterpolateTo 0x00555C08 calls CPhysicsObj::set_heading
                // with the target Frame's heading. It does not install the
                // target's pitch/roll at this already-close seam.
                return isMovingTo
                    ? null
                    : MoveToMath.SetHeading(
                        targetOrientation,
                        MoveToMath.GetHeading(targetOrientation));
            }
        }

        // Near & not-close branch:
        // 1. Tail-prune loop — collapse all consecutive stale tail entries
        //    within DesiredDistance of the new target (audit § 7 #5).
        while (_queue.Last is { } stale &&
               Vector3.Distance(stale.Value.TargetPosition, targetPosition) <= DesiredDistance)
        {
            _queue.RemoveLast();
        }

        // 2. Cap at 20 — drop head (audit § 7 #6).
        if (_queue.Count >= QueueCap)
            _queue.RemoveFirst();

        // 3. Append.
        _keepHeading = isMovingTo;
        EnqueueRaw(
            targetPosition,
            StoreTargetOrientation(
                targetOrientation,
                currentBodyOrientation,
                _keepHeading));
        return null;
    }

    private void EnqueueRaw(
        Vector3 target,
        Quaternion targetOrientation)
    {
        _queue.AddLast(new InterpolationNode
        {
            TargetPosition = target,
            TargetOrientation = targetOrientation,
        });
    }

    private static Quaternion StoreTargetOrientation(
        Quaternion targetOrientation,
        Quaternion? currentBodyOrientation,
        bool keepHeading)
    {
        if (!keepHeading || currentBodyOrientation is not { } current)
            return targetOrientation;

        // InterpolateTo stores the object's current heading into the node
        // when keep_heading is active. Frame::set_heading intentionally
        // discards the target Position's pitch/roll at this seam.
        return MoveToMath.SetHeading(
            targetOrientation,
            MoveToMath.GetHeading(current));
    }

    /// <summary>
    /// Compute the per-frame world-space correction delta. Combines the retail
    /// <c>UseTime</c> blip-check (fail_count &gt; 3 → snap to tail, clear queue)
    /// with the per-frame <c>adjust_offset</c> step computation.
    ///
    /// Returns <see cref="Vector3.Zero"/> when:
    ///   • queue is empty,
    ///   • head reached (distance &lt; <see cref="DesiredDistance"/>) — head pops,
    ///   • dt is invalid (≤ 0 or NaN).
    ///
    /// Returns the snap delta (tail − currentBodyPosition) when fail_count
    /// exceeds <see cref="StallFailCountThreshold"/>, then clears the queue.
    /// </summary>
    /// <param name="dt">Frame delta time (seconds).</param>
    /// <param name="currentBodyPosition">Current world-space body position.</param>
    /// <param name="maxSpeedFromMinterp">
    /// Max motion-table speed for this entity's current cycle (m/s).
    /// Pass 0 to use the <see cref="MaxInterpolatedVelocity"/> fallback.
    /// </param>
    public Vector3 AdjustOffset(
        double dt,
        Vector3 currentBodyPosition,
        float maxSpeedFromMinterp,
        bool inContact = true)
    {
        if (!inContact)
            return Vector3.Zero;

        InterpolationStep step = ComputeStep(
            dt,
            currentBodyPosition,
            maxSpeedFromMinterp);
        return step.Overwrites ? step.WorldOrigin : Vector3.Zero;
    }

    /// <summary>
    /// Retail <c>InterpolationManager::adjust_offset</c> complete-Frame path
    /// (0x00555D30). When interpolation is active it replaces both components
    /// of <paramref name="offset"/> with <c>Position::subtract2</c>'s relative
    /// target frame, then scales only Origin to the catch-up step. A MoveTo
    /// node keeps heading by replacing the relative rotation with identity.
    /// When the queue is empty or a node completes, the incoming PartArray
    /// frame remains untouched.
    /// </summary>
    public bool AdjustOffset(
        double dt,
        Vector3 currentBodyPosition,
        Quaternion currentBodyOrientation,
        float maxSpeedFromMinterp,
        MotionDeltaFrame offset,
        bool inContact = true)
    {
        ArgumentNullException.ThrowIfNull(offset);
        if (!inContact)
            return false;

        InterpolationStep step = ComputeStep(
            dt,
            currentBodyPosition,
            maxSpeedFromMinterp);
        if (!step.Overwrites)
            return false;

        offset.Origin = MoveToMath.GlobalToLocalVec(
            currentBodyOrientation,
            step.WorldOrigin);
        offset.Orientation = _keepHeading
            ? Quaternion.Identity
            : FrameOps.SetRotate(
                offset.Origin,
                Quaternion.Identity,
                Quaternion.Inverse(currentBodyOrientation)
                * step.TargetOrientation);
        return true;
    }

    private InterpolationStep ComputeStep(
        double dt,
        Vector3 currentBodyPosition,
        float maxSpeedFromMinterp)
    {
        // dt sanity guard — protects PhysicsBody.Position from NaN poisoning.
        if (dt <= 0 || double.IsNaN(dt))
            return default;

        if (_queue.First is null)
            return default;

        // Distance to head node (retail line 353083).
        var head = _queue.First.Value;
        float dist = Vector3.Distance(head.TargetPosition, currentBodyPosition);

        // Reach test (retail line 353089): dist ≤ DESIRED_DISTANCE → pop and
        // re-baseline. NodeCompleted(1) advances to next head, also resets the
        // window state.
        if (dist <= DesiredDistance)
        {
            NodeCompleted(popHead: true, currentBodyPosition);
            return default;
        }

        // Catch-up speed (retail line 353122 + 353128 fallback).
        float scaled    = maxSpeedFromMinterp * MaxInterpolatedVelocityMod;
        float catchUp   = scaled > FEpsilon ? scaled : MaxInterpolatedVelocity;

        // Accumulate progress_quantum (audit § 7 #1: SUM OF DT, not step).
        _progressQuantum += (float)dt;
        _frameCounter++;

        // 5-frame stall window check (retail line 353146).
        if (_frameCounter >= StallCheckFrameInterval)
        {
            float cumulative = _originalDistance - dist;

            // Primary check (retail line 353150-353166):
            //   cumulative >= MIN_DISTANCE_TO_REACH_POSITION (0.20)
            bool primaryPass = cumulative >= MinDistanceToReachPosition;

            // Secondary check (retail line 353169-353172, audit § 7 #4):
            //   cumulative > F_EPSILON
            //   AND (cumulative / progress_quantum / dt) >= 0.30
            //
            // Port verbatim despite weird units; audit notes this may be a
            // Turbine bug or x87-stack misread by Binary Ninja. Mirroring bytes.
            bool secondaryPass = false;
            if (cumulative > FEpsilon && _progressQuantum > 0f && dt > 0)
            {
                float ratio = (cumulative / _progressQuantum) / (float)dt;
                secondaryPass = ratio >= StallProgressMinFraction;
            }

            if (!primaryPass && !secondaryPass)
            {
                _failCount++;
            }
            else
            {
                _failCount = 0;
            }

            // Re-baseline window regardless of pass/fail.
            _frameCounter     = 0;
            _progressQuantum  = 0f;
            _originalDistance = dist;
        }
        else if (_originalDistance >= OriginalDistanceSentinel - 0.5f)
        {
            // First call after Clear / new motion: seed the baseline so the
            // first 5-frame window's cumulative is computed against frame-0
            // distance, not the 999999f sentinel. Retail handles this via
            // the sentinel itself — the sentinel produces a huge cumulative
            // that always passes — but we use a baseline-seeded approach so
            // the secondary check has sane progress_quantum behavior.
            _originalDistance = dist;
        }

        // Retail UseTime blip check (@ 0x00555F39): fail_count > 3 → snap to
        // tail, clear queue. Placed AFTER the stall window logic so it fires
        // in the same tick as both:
        //   (a) the just-incremented fail_count from a stall window pass, AND
        //   (b) a far-branch Enqueue pre-arm (fail_count = 4 set externally).
        // Retail splits this into a separate UseTime call; we collapse it.
        if (_failCount > StallFailCountThreshold)
        {
            InterpolationNode tail = _queue.Last!.Value;
            Vector3 tailDelta = tail.TargetPosition - currentBodyPosition;
            // Bug B (2026-08-04) blip producer CANDIDATE 2 — observation only.
            // docs/research/2026-08-04-bug-b-remote-slide-diagnosis.md §2
            // establishes this snap as a FAITHFUL port of retail
            // InterpolationManager::UseTime @0x00555f20 firing correctly on a
            // body frozen upstream, and rules it explicitly out of scope for
            // any fix. The call reads only values already computed on this
            // line and is self-guarded on ProbeRemoteSlideEnabled, so it
            // changes neither the branch nor its result. TEMPORARY — strip
            // with the ACDREAM_PROBE_REMOTE_SLIDE family.
            PhysicsDiagnostics.LogRemoteSlideStallSnap(
                failCount: _failCount,
                threshold: StallFailCountThreshold,
                queueDepth: _queue.Count,
                bodyPosition: currentBodyPosition,
                tailPosition: tail.TargetPosition,
                distanceToHead: dist);
            Clear();
            return new InterpolationStep(
                true,
                tailDelta,
                tail.TargetOrientation);
        }

        // Per-frame step magnitude (retail line 353218).
        float step = catchUp * (float)dt;
        // No-overshoot scaling (retail line 353231): if step would overshoot
        // dist, clamp to dist.
        if (step > dist)
            step = dist;

        // Direction × step.
        Vector3 delta = ((head.TargetPosition - currentBodyPosition) / dist) * step;
        return new InterpolationStep(
            true,
            delta,
            head.TargetOrientation);
    }

    private readonly record struct InterpolationStep(
        bool Overwrites,
        Vector3 WorldOrigin,
        Quaternion TargetOrientation);

    /// <summary>
    /// Retail NodeCompleted (@ 0x005559A0). popHead=true after head reached;
    /// popHead=false during stall fail (re-baseline only). For our collapsed
    /// architecture we always re-baseline on pop.
    /// </summary>
    private void NodeCompleted(bool popHead, Vector3 currentBodyPosition)
    {
        _frameCounter    = 0;
        _progressQuantum = 0f;

        if (popHead && _queue.First != null)
        {
            _queue.RemoveFirst();
        }

        // Re-baseline on the new head, or reset to sentinel if queue empty.
        if (_queue.First is { } newHead)
        {
            _originalDistance = Vector3.Distance(newHead.Value.TargetPosition, currentBodyPosition);
        }
        else
        {
            _originalDistance = OriginalDistanceSentinel;
        }
    }
}