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

using System;
using System.Collections.Generic;
using System.Numerics;

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). Each physics tick the caller passes
// current body position + max-speed from the motion table; we return the
// world-space delta vector to apply to the body for this frame.
//
// Public C# API kept Vector3-based for compatibility with PositionManager and
// GameWindow callsites; retail-spec method names are documented inline. The
// retail Frame mutation pattern collapses to "return a Vector3 delta" because
// adjust_offset's offset Frame is rotation-zero (translation-only) for this
// queue's purposes — see audit 04-interp-manager.md § 4.
//
// 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 float   Heading;
    public bool    IsHeadingValid;
}

/// <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

    // ── 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>
    /// 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 void Enqueue(
        Vector3 targetPosition,
        float heading,
        bool isMovingTo,
        Vector3? currentBodyPosition = 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)
        {
            EnqueueRaw(targetPosition, heading, isMovingTo);
            // Pre-arm immediate blip on next AdjustOffset (audit § 7 #3).
            _failCount = StallFailCountThreshold + 1;
            return;
        }

        // 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();
                return;
            }
        }

        // 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.
        EnqueueRaw(targetPosition, heading, isMovingTo);
    }

    private void EnqueueRaw(Vector3 target, float heading, bool isMovingTo)
    {
        _queue.AddLast(new InterpolationNode
        {
            TargetPosition = target,
            Heading        = heading,
            IsHeadingValid = isMovingTo,
        });
    }

    /// <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)
    {
        // dt sanity guard — protects PhysicsBody.Position from NaN poisoning.
        if (dt <= 0 || double.IsNaN(dt))
            return Vector3.Zero;

        if (_queue.First is null)
            return Vector3.Zero;

        // 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 Vector3.Zero;
        }

        // 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)
        {
            Vector3 tailPos = _queue.Last!.Value.TargetPosition;
            Clear();
            return tailPos - currentBodyPosition;
        }

        // 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 delta;
    }

    /// <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;
        }
    }
}