acdream/tests/AcDream.Core.Tests/Physics/InterpolationManagerTests.cs

using System;
using System.Numerics;
using AcDream.Core.Physics;
using Xunit;

namespace AcDream.Core.Tests.Physics;

// ─────────────────────────────────────────────────────────────────────────────
// InterpolationManagerTests — covers the retail CPhysicsObj interpolation
// queue port (L.3.1 Task 1).
//
// Source addresses tested:
//   CPhysicsObj::InterpolateTo          acclient @ 0x005104F0  (Enqueue)
//   InterpolationManager::adjust_offset acclient @ 0x00555D30  (AdjustOffset)
//   InterpolationManager::UseTime       acclient @ 0x00555F20  (blip-to-tail)
// ─────────────────────────────────────────────────────────────────────────────

public sealed class InterpolationManagerTests
{
    // ── helpers ───────────────────────────────────────────────────────────────

    /// <summary>Origin used as the "body is here" position in most tests.</summary>
    private static readonly Vector3 BodyOrigin = Vector3.Zero;

    /// <summary>A position clearly outside DesiredDistance (= 0.05 m).</summary>
    private static readonly Vector3 FarTarget = new Vector3(10f, 0f, 0f);

    private static InterpolationManager Make() => new InterpolationManager();

    // =========================================================================
    // Queue mechanics
    // =========================================================================

    [Fact]
    public void Enqueue_AddsNode_QueueBecomesActive()
    {
        var mgr = Make();
        Assert.False(mgr.IsActive);

        mgr.Enqueue(FarTarget, heading: 0f, isMovingTo: true);

        Assert.True(mgr.IsActive);
    }

    [Fact]
    public void Enqueue_DropsOldestWhenAtCap20()
    {
        var mgr = Make();

        // Fill the queue to cap with distinct positions spaced far enough
        // apart to avoid the duplicate-prune threshold (DesiredDistance = 0.05).
        for (int i = 0; i < InterpolationManager.QueueCap; i++)
        {
            mgr.Enqueue(new Vector3(i * 1f, 0f, 0f), heading: 0f, isMovingTo: true);
        }

        // Sanity: queue is at cap before the 21st enqueue.
        Assert.Equal(InterpolationManager.QueueCap, mgr.Count);

        // The 21st enqueue must drop the oldest (x=0) and keep the count at cap.
        mgr.Enqueue(new Vector3(100f, 0f, 0f), heading: 0f, isMovingTo: true);

        // Count must still be QueueCap — not QueueCap+1.
        Assert.Equal(InterpolationManager.QueueCap, mgr.Count);

        // The head (oldest surviving node) must now be x=1 (the second-original
        // position), not x=0 (which was dropped).  Verify by driving the body
        // to exactly x=1 — AdjustOffset must pop that node (distance < DesiredDistance)
        // and return zero, confirming x=1 is the head.
        var bodyAtSecondOriginal = new Vector3(1f, 0f, 0f);
        var result = mgr.AdjustOffset(
            dt: 0.016,
            currentBodyPosition: bodyAtSecondOriginal,
            maxSpeedFromMinterp: 10f);

        // Reached head (dist ≈ 0) → zero delta + node popped.
        Assert.Equal(Vector3.Zero, result);
        // One node was consumed; count must now be QueueCap - 1.
        Assert.Equal(InterpolationManager.QueueCap - 1, mgr.Count);
    }

    [Fact]
    public void Enqueue_AtCap20_HeadIsSecondOriginal()
    {
        // Complementary test for the cap overflow: after 21 enqueues the
        // second-enqueued position (x=1) must be at the head, not x=0.
        var mgr = Make();
        for (int i = 0; i < InterpolationManager.QueueCap; i++)
        {
            mgr.Enqueue(new Vector3(i * 1f, 0f, 0f), heading: 0f, isMovingTo: true);
        }
        mgr.Enqueue(new Vector3(100f, 0f, 0f), heading: 0f, isMovingTo: true);

        // Place the body far away from x=0 but RIGHT on x=1.  If x=0 were the
        // head the result would be non-zero (body is 1 m away from x=0).
        // If x=1 is the head the distance is 0 → pop → zero return.
        var bodyAtX1 = new Vector3(1f, 0f, 0f);
        var delta = mgr.AdjustOffset(dt: 0.016, currentBodyPosition: bodyAtX1, maxSpeedFromMinterp: 10f);

        Assert.Equal(Vector3.Zero, delta);
    }

    [Fact]
    public void Enqueue_PrunesDuplicateWithinDesiredDistance()
    {
        var mgr = Make();
        var basePos = new Vector3(5f, 0f, 0f);

        mgr.Enqueue(basePos, heading: 0f, isMovingTo: true);

        // Within DesiredDistance (0.05) — must be ignored.
        var nearDuplicate = basePos + new Vector3(0.01f, 0f, 0f);
        mgr.Enqueue(nearDuplicate, heading: 0f, isMovingTo: true);

        // Confirm duplicate was not added: driving the body to basePos should
        // exhaust the queue in one pop, leaving it empty.
        // Position body exactly AT the target so AdjustOffset pops the head node.
        var result = mgr.AdjustOffset(dt: 0.016, currentBodyPosition: basePos, maxSpeedFromMinterp: 10f);

        Assert.Equal(Vector3.Zero, result);         // reached → pop
        Assert.False(mgr.IsActive);                 // only one node existed
    }

    [Fact]
    public void Clear_EmptiesQueueAndResetsCounters()
    {
        var mgr = Make();
        mgr.Enqueue(FarTarget, heading: 0f, isMovingTo: true);
        Assert.True(mgr.IsActive);

        mgr.Clear();

        Assert.False(mgr.IsActive);

        // After Clear, AdjustOffset must return zero (no stale state).
        var delta = mgr.AdjustOffset(dt: 0.016, currentBodyPosition: BodyOrigin, maxSpeedFromMinterp: 4f);
        Assert.Equal(Vector3.Zero, delta);
    }

    // =========================================================================
    // AdjustOffset math
    // =========================================================================

    [Fact]
    public void AdjustOffset_EmptyQueue_ReturnsZero()
    {
        var mgr   = Make();
        var delta = mgr.AdjustOffset(dt: 0.016, currentBodyPosition: BodyOrigin, maxSpeedFromMinterp: 4f);

        Assert.Equal(Vector3.Zero, delta);
    }

    [Fact]
    public void AdjustOffset_ReachesNodeWithinDesiredDistance_PopsHead()
    {
        var mgr    = Make();
        var target = new Vector3(0.02f, 0f, 0f); // within DesiredDistance (0.05)

        mgr.Enqueue(target, heading: 0f, isMovingTo: true);

        // Body is at origin; distance = 0.02 < 0.05 → should pop and return zero.
        var delta = mgr.AdjustOffset(dt: 0.016, currentBodyPosition: BodyOrigin, maxSpeedFromMinterp: 4f);

        Assert.Equal(Vector3.Zero, delta);
        Assert.False(mgr.IsActive, "Head node should have been popped after being reached");
    }

    [Fact]
    public void AdjustOffset_ClampedToCatchUpSpeed_2xMotionMax()
    {
        var mgr       = Make();
        float maxSpeed = 4.0f;          // motion-table max speed
        double dt      = 0.5;           // large dt to make the math clear
        // target is far enough that there's no overshoot clamping
        var target = new Vector3(100f, 0f, 0f);
        mgr.Enqueue(target, heading: 0f, isMovingTo: true);

        var delta = mgr.AdjustOffset(dt, currentBodyPosition: BodyOrigin, maxSpeedFromMinterp: maxSpeed);

        // Expected step = catchUpSpeed * dt = (maxSpeed * 2.0) * dt = 4.0
        float expectedStep = maxSpeed * InterpolationManager.MaxInterpolatedVelocityMod * (float)dt;
        Assert.Equal(expectedStep, delta.Length(), precision: 4);
    }

    [Fact]
    public void AdjustOffset_FallbackSpeed_WhenMinterpZero()
    {
        var mgr   = Make();
        double dt = 0.5;
        var target = new Vector3(100f, 0f, 0f);
        mgr.Enqueue(target, heading: 0f, isMovingTo: true);

        // maxSpeedFromMinterp = 0 → fallback to MaxInterpolatedVelocity (7.5)
        var delta = mgr.AdjustOffset(dt, currentBodyPosition: BodyOrigin, maxSpeedFromMinterp: 0f);

        float expectedStep = InterpolationManager.MaxInterpolatedVelocity * (float)dt;
        Assert.Equal(expectedStep, delta.Length(), precision: 4);
    }

    [Fact]
    public void AdjustOffset_OvershootProtection_StepClampedToDistance()
    {
        var mgr   = Make();
        float maxSpeed = 10f;
        double dt      = 1.0;       // step = 2*10*1.0 = 20 >> actual distance

        // Place target just 0.5 m away — inside the step distance.
        var target = new Vector3(0.5f, 0f, 0f);
        mgr.Enqueue(target, heading: 0f, isMovingTo: true);

        var delta = mgr.AdjustOffset(dt, currentBodyPosition: BodyOrigin, maxSpeedFromMinterp: maxSpeed);

        // Step should be clamped to dist (0.5), not the unclamped 20.
        Assert.Equal(0.5f, delta.Length(), precision: 4);
    }

    // =========================================================================
    // Stall detection
    // =========================================================================

    [Fact]
    public void AdjustOffset_StallCounterIncrementsEachFrame()
    {
        // Run 4 frames (< StallCheckFrameInterval = 5) with a body that does
        // not move — the queue should still be active (no blip yet).
        var mgr    = Make();
        var target = new Vector3(10f, 0f, 0f);
        mgr.Enqueue(target, heading: 0f, isMovingTo: true);

        // Body does NOT move — we pass the same fixed position each frame.
        for (int i = 0; i < 4; i++)
        {
            mgr.AdjustOffset(dt: 0.016, currentBodyPosition: BodyOrigin, maxSpeedFromMinterp: 4f);
        }

        // After 4 frames (<5) the stall check hasn't fired yet, queue intact.
        Assert.True(mgr.IsActive);
    }

    [Fact]
    public void AdjustOffset_NoProgressMarksFail_AfterFiveFrames()
    {
        // Body stays at origin every frame — zero real progress.
        // After 5 frames the stall check fires and _failCount increments (to 1).
        // Queue must still be alive (blip only at > StallFailCountThreshold = 3).
        var mgr    = Make();
        var target = new Vector3(50f, 0f, 0f);
        mgr.Enqueue(target, heading: 0f, isMovingTo: true);

        for (int i = 0; i < InterpolationManager.StallCheckFrameInterval; i++)
        {
            mgr.AdjustOffset(dt: 0.016, currentBodyPosition: BodyOrigin, maxSpeedFromMinterp: 4f);
        }

        // 1 fail < StallFailCountThreshold (3), so queue is still active.
        Assert.True(mgr.IsActive);
    }

    [Fact]
    public void AdjustOffset_GoodProgressResetsFailCount()
    {
        // Simulate: body truly advances toward target each frame.
        // After each check-interval the fail counter should reset to 0
        // (because progress ≥ 30% of expected).
        var mgr      = Make();
        var origin   = Vector3.Zero;
        var target   = new Vector3(50f, 0f, 0f);
        float maxSpd = 4f;
        double dt    = 0.016;
        mgr.Enqueue(target, heading: 0f, isMovingTo: true);

        // Run 5 frames, advancing the body by the actual delta returned each time.
        Vector3 bodyPos = origin;
        for (int i = 0; i < InterpolationManager.StallCheckFrameInterval; i++)
        {
            var delta = mgr.AdjustOffset(dt, currentBodyPosition: bodyPos, maxSpeedFromMinterp: maxSpd);
            bodyPos += delta; // body truly moves
        }

        // After 5 frames of genuine progress, queue must still be active
        // (no blip) and _failCount should have been reset to 0 (no way to read
        // it directly, but we verify indirectly: we'd need 3×5=15 more frames
        // of stalling to blip — a further 5-frame no-progress window at this
        // point should only bring _failCount to 1, not trigger a blip).
        Assert.True(mgr.IsActive);
    }

    [Fact]
    public void AdjustOffset_3FailsTriggersBlipToTail()
    {
        // Need > StallFailCountThreshold (3) failures.
        // Each failure requires one stall-check window (5 frames of no progress).
        // So we need 4 × 5 = 20 frames with the body frozen at origin.
        //
        // Also enqueue a SECOND node (the tail) different from the first, so we
        // can verify the snap is to the tail, not the head.
        var mgr  = Make();
        var head = new Vector3(10f, 0f, 0f);
        var tail = new Vector3(30f, 0f, 0f);

        mgr.Enqueue(head, heading: 0f, isMovingTo: true);
        mgr.Enqueue(tail, heading: 0f, isMovingTo: true);

        // 4 stall-check windows × 5 frames each = 20 frames, body never moves.
        Vector3? blipDelta = null;
        const int totalFrames = (InterpolationManager.StallFailCountThreshold + 1)
                              * InterpolationManager.StallCheckFrameInterval;

        for (int i = 0; i < totalFrames; i++)
        {
            var delta = mgr.AdjustOffset(dt: 0.016, currentBodyPosition: BodyOrigin, maxSpeedFromMinterp: 4f);
            if (delta.Length() > 1f) // blip delta will be >> normal per-frame step
            {
                blipDelta = delta;
                break;
            }
        }

        // Blip must have fired.
        Assert.NotNull(blipDelta);

        // Blip delta = tailPos − currentBodyPosition = (30,0,0) − (0,0,0)
        Assert.Equal(tail.X, blipDelta!.Value.X, precision: 4);
        Assert.Equal(tail.Y, blipDelta!.Value.Y, precision: 4);
        Assert.Equal(tail.Z, blipDelta!.Value.Z, precision: 4);

        // Queue must be cleared after blip (retail StopInterpolating).
        Assert.False(mgr.IsActive);
    }

    // =========================================================================
    // New tests: I-1 first-window false-positive guard, I-3 dt guard, I-5 cap
    // =========================================================================

    [Fact]
    public void AdjustOffset_FirstWindow_DoesNotFalseFail()
    {
        // Before the fix, _distanceAtWindowStart defaulted to 0, so on the
        // first 5-frame window cumulative_progress = 0 - dist = -dist < 0.20
        // → every new motion sequence triggered a spurious stall fail.
        //
        // After the fix, the baseline is seeded from the first call, so
        // cumulative_progress = dist(frame0) - dist(frame4) which for a body
        // that hasn't moved yet is ≈ 0.  That is still < MIN (0.20), but the
        // _failCount starts at 0 and must be > 3 (not == 1) to blip.  The key
        // assertion is that after exactly ONE stall window the queue is still
        // alive (fail count == 1, blip requires > 3).
        var mgr = Make();
        mgr.Enqueue(new Vector3(50f, 0f, 0f), heading: 0f, isMovingTo: true);

        // Run exactly one check-window (5 frames) with the body frozen.
        for (int i = 0; i < InterpolationManager.StallCheckFrameInterval; i++)
        {
            mgr.AdjustOffset(dt: 0.016, currentBodyPosition: BodyOrigin, maxSpeedFromMinterp: 4f);
        }

        // One window fail → _failCount == 1, far below StallFailCountThreshold (3).
        // Queue must still be active; no spurious blip on first window.
        Assert.True(mgr.IsActive,
            "First stall window must NOT trigger a blip (would require > 3 consecutive failures).");
    }

    [Fact]
    public void AdjustOffset_DtZeroOrNegative_ReturnsZero()
    {
        var mgr = Make();
        mgr.Enqueue(FarTarget, heading: 0f, isMovingTo: true);

        // dt == 0 → guard fires, return zero, no side-effects.
        var deltaZero = mgr.AdjustOffset(dt: 0.0, currentBodyPosition: BodyOrigin, maxSpeedFromMinterp: 4f);
        Assert.Equal(Vector3.Zero, deltaZero);

        // dt < 0 → guard fires, return zero.
        var deltaNeg = mgr.AdjustOffset(dt: -1.0, currentBodyPosition: BodyOrigin, maxSpeedFromMinterp: 4f);
        Assert.Equal(Vector3.Zero, deltaNeg);

        // dt = NaN → guard fires, return zero.
        var deltaNaN = mgr.AdjustOffset(dt: double.NaN, currentBodyPosition: BodyOrigin, maxSpeedFromMinterp: 4f);
        Assert.Equal(Vector3.Zero, deltaNaN);

        // Queue must still be intact (guards did not consume or corrupt state).
        Assert.True(mgr.IsActive);
    }
}