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.
//
// Ports:
//   CPhysicsObj::InterpolateTo          (acclient @ 0x005104F0)
//   InterpolationManager::adjust_offset (acclient @ 0x00555D30)
//   InterpolationManager::UseTime       (acclient @ 0x00555F20) — stall/blip
//
// FIFO position-waypoint queue (cap 20). On each physics tick the caller
// passes current body position + max-speed from the motion table; we return
// the delta vector to apply to the body for this frame.
//
// Queue semantics:
//   - Head = next target.  Body walks toward head at catch-up speed.
//   - Tail = most-recent server position.  On stall we blip directly to tail
//     (retail UseTime @ 0x00555F20: copies tail_ position, calls
//     CPhysicsObj::SetPositionSimple, then StopInterpolating).
//
// Constants verified from named binary at the addresses cited above (not
// guesses):
//   MAX_INTERPOLATED_VELOCITY_MOD = 2.0
//   MAX_INTERPOLATED_VELOCITY     = 7.5
//   MIN_DISTANCE_TO_REACH_POSITION = 0.20  (unused here — kept for reference)
//   DESIRED_DISTANCE               = 0.05
// ─────────────────────────────────────────────────────────────────────────────

/// <summary>
/// Waypoint used internally by <see cref="InterpolationManager"/>.
/// </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 is dropped.</summary>
    public const int QueueCap = 20;

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

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

    /// <summary>
    /// Per-5-frame stall progress threshold (meters).  Body must advance at
    /// least this far in <see cref="StallCheckFrameInterval"/> frames or
    /// it counts as a stall tick.
    /// Retail MIN_DISTANCE_TO_REACH_POSITION (@ 0x00555D30).
    /// </summary>
    public const float MinDistanceToReachPosition = 0.20f;

    /// <summary>
    /// Reach + duplicate-prune radius (meters).  Node is popped when
    /// distance to its target falls below this value; new enqueues within
    /// this distance of the tail are ignored.
    /// Retail DESIRED_DISTANCE (@ 0x00555D30).
    /// </summary>
    public const float DesiredDistance = 0.05f;

    /// <summary>
    /// Number of ticks between stall progress checks.
    /// Retail StallCheckFrameInterval (@ 0x00555D30).
    /// </summary>
    public const int StallCheckFrameInterval = 5;

    /// <summary>
    /// Minimum fraction of the expected advance that counts as "real
    /// progress" in a stall check window.  Below this fraction the
    /// fail counter increments.
    /// Retail StallProgressMinFraction (@ 0x00555D30).
    /// </summary>
    public const float StallProgressMinFraction = 0.30f;

    /// <summary>
    /// Number of consecutive stall-check failures before the body is
    /// blipped to the tail of the queue.
    /// Retail StallFailCountForBlip (@ 0x00555D30).
    /// </summary>
    public const int StallFailCountForBlip = 3;

    // ── internals ─────────────────────────────────────────────────────────────

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

    private int   _failFrameCounter     = 0;
    private float _failDistanceLastCheck = 0f;
    private int   _failCount            = 0;

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

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

    /// <summary>
    /// Stop interpolating: clear the queue and reset all stall counters.
    /// Retail StopInterpolating / destructor cleanup.
    /// </summary>
    public void Clear()
    {
        _queue.Clear();
        _failFrameCounter     = 0;
        _failDistanceLastCheck = 0f;
        _failCount            = 0;
    }

    /// <summary>
    /// Enqueue a new server-authoritative position waypoint.
    ///
    /// <para>
    /// Step 1: Duplicate-prune — if the new target is within
    /// <see cref="DesiredDistance"/> of the current tail, ignore it.<br/>
    /// Step 2: Cap — if the queue is already at <see cref="QueueCap"/>,
    /// drop the oldest (head) entry.<br/>
    /// Step 3/4: Append a new <see cref="InterpolationNode"/>.
    /// </para>
    ///
    /// Retail CPhysicsObj::InterpolateTo (@ 0x005104F0).
    /// </summary>
    /// <param name="targetPosition">Server-reported world position.</param>
    /// <param name="heading">Server-reported heading (radians, AC convention).</param>
    /// <param name="isMovingTo">True when the body is in motion — gates heading validity.</param>
    public void Enqueue(Vector3 targetPosition, float heading, bool isMovingTo)
    {
        // Step 1: duplicate-prune
        if (_queue.Last is { } last)
        {
            if (Vector3.Distance(targetPosition, last.Value.TargetPosition) < DesiredDistance)
                return;
        }

        // Step 2: enforce cap
        if (_queue.Count >= QueueCap)
            _queue.RemoveFirst();

        // Steps 3+4: add node
        var node = new InterpolationNode
        {
            TargetPosition = targetPosition,
            Heading        = heading,
            IsHeadingValid = isMovingTo,
        };
        _queue.AddLast(node);
    }

    /// <summary>
    /// Compute the per-frame position correction delta.
    ///
    /// <para>
    /// Returns <see cref="Vector3.Zero"/> when the queue is empty or when
    /// the head node has been reached.  Returns a snap delta (tail −
    /// currentBodyPosition) after <see cref="StallFailCountForBlip"/>
    /// consecutive stall failures, then clears the queue.
    /// </para>
    ///
    /// Retail InterpolationManager::adjust_offset (@ 0x00555D30) +
    /// UseTime stall/blip (@ 0x00555F20).
    /// </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), as
    /// reported by MotionInterpreter.  Pass 0 if unavailable; the fallback
    /// <see cref="MaxInterpolatedVelocity"/> will be used.
    /// </param>
    /// <returns>World-space delta to apply to the body this frame.</returns>
    public Vector3 AdjustOffset(double dt, Vector3 currentBodyPosition, float maxSpeedFromMinterp)
    {
        // Step 1: empty queue → no correction
        if (_queue.First is null)
            return Vector3.Zero;

        // Step 2: peek head
        var headNode = _queue.First.Value;

        // Step 3: distance to head target
        float dist = (headNode.TargetPosition - currentBodyPosition).Length();

        // Step 4: reached node
        if (dist < DesiredDistance)
        {
            _queue.RemoveFirst();
            return Vector3.Zero;
        }

        // Step 5: compute catch-up speed
        float scaled      = maxSpeedFromMinterp * MaxInterpolatedVelocityMod;
        float catchUpSpeed = scaled > 1e-6f ? scaled : MaxInterpolatedVelocity;

        // Step 6: step magnitude (no overshoot)
        float step = catchUpSpeed * (float)dt;
        if (step > dist)
            step = dist;

        // Step 7: direction × step
        Vector3 delta = ((headNode.TargetPosition - currentBodyPosition) / dist) * step;

        // Step 8: stall detection
        _failFrameCounter++;
        if (_failFrameCounter >= StallCheckFrameInterval)
        {
            float progress = _failDistanceLastCheck - dist;
            float expected = catchUpSpeed * (float)dt * StallCheckFrameInterval;

            if (progress < StallProgressMinFraction * expected)
            {
                _failCount++;
                if (_failCount > StallFailCountForBlip)
                {
                    // Blip-to-tail: retail UseTime (@ 0x00555F20) reads
                    // position_queue.tail_, copies its position to a local,
                    // calls CPhysicsObj::SetPositionSimple, then
                    // StopInterpolating.  Snap target is the TAIL (the most
                    // recent server position), not the head.
                    Vector3 tailPos = _queue.Last!.Value.TargetPosition;
                    Clear();
                    return tailPos - currentBodyPosition;
                }
            }
            else
            {
                _failCount = 0;
            }

            _failDistanceLastCheck = dist;
            _failFrameCounter      = 0;
        }

        // Step 9: return per-frame delta
        return delta;
    }
}