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

namespace AcDream.Core.Tests.Physics.Motion;

/// <summary>
/// R4-V2 — <c>HandleMoveToPosition</c> Phase 1 aux-turn steering
/// (<c>00529d80</c>, raw 307187-307280) per r4-moveto-decomp.md §6b: the
/// 20°/340° deadband, direction pick (diff ≥ 180 → TurnLeft else TurnRight),
/// no-redundant-reissue, and the "stop aux while animating" branch.
/// </summary>
public sealed class MoveToManagerAuxTurnTests
{
    /// <summary>Drives a manager into an active MoveToPosition (heading
    /// already settled so BeginMoveForward runs on the first BeginNextNode),
    /// then drains the interp's pending_motions queue via a synthetic
    /// MotionDone callback — standing in for "the WalkForward/RunForward
    /// animation-table dispatch cycle has started/completed" the way a real
    /// AnimationSequencer would signal it. Without this, MotionsPending()
    /// stays true forever (BeginMoveForward's own _DoMotion dispatch
    /// enqueues a node that nothing else in this bare harness ever pops),
    /// and HandleMoveToPosition's Phase 1 would perpetually take the
    /// "animating, stop aux" branch — never exercising the deadband/turn
    /// logic this test file targets.</summary>
    private static MoveToManagerHarness ArmMoving(float initialHeading, Vector3 targetXY)
    {
        var h = new MoveToManagerHarness();
        h.WorldPosition = new Position(1u, Vector3.Zero, Quaternion.Identity);
        h.Heading = initialHeading;

        var p = new MovementParameters { DistanceToObject = 0.6f, UseSpheres = false };
        h.Manager.MoveToPosition(new Position(1u, targetXY, Quaternion.Identity), p);

        h.DrainPendingMotions();

        return h;
    }

    [Fact]
    public void WithinDeadband_NoAuxTurnIssued()
    {
        // Target due east (heading 90); mover already facing 85 -> diff 5,
        // inside [0,20] deadband.
        var h = ArmMoving(initialHeading: 90f, targetXY: new Vector3(20f, 0f, 0f));
        h.Heading = 85f; // simulate drift after the initial turn-to-face completed

        h.Manager.HandleMoveToPosition();

        Assert.Equal(0u, h.Manager.AuxCommand);
    }

    [Fact]
    public void JustOutsideDeadband_Positive_IssuesTurnRight()
    {
        var h = ArmMoving(initialHeading: 90f, targetXY: new Vector3(20f, 0f, 0f));
        h.Heading = 40f; // diff = 90-40 = 50 -> outside [0,20]∪[340,360)

        h.Manager.HandleMoveToPosition();

        Assert.Equal(MotionCommand.TurnRight, h.Manager.AuxCommand);
    }

    [Fact]
    public void DiffAtOrAbove180_IssuesTurnLeft()
    {
        var h = ArmMoving(initialHeading: 90f, targetXY: new Vector3(20f, 0f, 0f));
        h.Heading = 300f; // diff = 90-300 = -210 -> +360 = 150... need >=180 for TurnLeft; pick 260.
        h.Heading = 260f; // diff = 90-260=-170 -> +360=190 (>=180) -> TurnLeft

        h.Manager.HandleMoveToPosition();

        Assert.Equal(MotionCommand.TurnLeft, h.Manager.AuxCommand);
    }

    [Fact]
    public void DeadbandUpperBoundary_340_NoTurn()
    {
        var h = ArmMoving(initialHeading: 0f, targetXY: new Vector3(0f, 20f, 0f)); // target heading 0
        h.Heading = 20f; // diff = 0-20=-20 -> +360=340 -> boundary INCLUSIVE (diff >= 340)

        h.Manager.HandleMoveToPosition();

        Assert.Equal(0u, h.Manager.AuxCommand);
    }

    [Fact]
    public void DeadbandLowerBoundary_20_NoTurn()
    {
        var h = ArmMoving(initialHeading: 0f, targetXY: new Vector3(0f, 20f, 0f)); // target heading 0
        h.Heading = -20f % 360f; // normalize below
        h.Heading = 340f; // diff = 0-340 = -340 -> +360=20 -> boundary INCLUSIVE (diff <= 20)

        h.Manager.HandleMoveToPosition();

        Assert.Equal(0u, h.Manager.AuxCommand);
    }

    [Fact]
    public void NoRedundantReissue_SameDirectionTwice_DoesNotRedispatch()
    {
        var h = ArmMoving(initialHeading: 90f, targetXY: new Vector3(20f, 0f, 0f));
        h.Heading = 40f; // outside deadband -> TurnRight

        h.Manager.HandleMoveToPosition();
        uint firstAux = h.Manager.AuxCommand;
        Assert.Equal(MotionCommand.TurnRight, firstAux);

        // Drain the interp's pending_motions queue (the TurnRight dispatch
        // just enqueued a node) so Phase 1's "not animating" branch runs
        // again on the second tick — otherwise MotionsPending() would stay
        // true and Phase 1 would take the "animating, stop aux" branch
        // instead of exercising the redundant-reissue guard this test
        // targets.
        h.DrainPendingMotions();

        int stopCallsBefore = h.StopCompletelyCalls; // unrelated counter, just for isolation

        // Second tick, still outside deadband, same direction -> _DoMotion
        // should NOT be re-issued (turn != AuxCommand test fails since
        // AuxCommand already equals TurnRight) — assert AuxCommand is
        // unchanged (still TurnRight) as the observable proxy.
        h.Manager.HandleMoveToPosition();

        Assert.Equal(MotionCommand.TurnRight, h.Manager.AuxCommand);
        Assert.Equal(stopCallsBefore, h.StopCompletelyCalls);
    }

    [Fact]
    public void AnimatingMotionsPending_StopsAuxTurn_DoesNotStartNew()
    {
        var h = ArmMoving(initialHeading: 90f, targetXY: new Vector3(20f, 0f, 0f));
        h.Heading = 40f;
        h.Manager.HandleMoveToPosition();
        Assert.Equal(MotionCommand.TurnRight, h.Manager.AuxCommand);

        // Simulate an animation-table motion still pending by queueing a
        // node onto the REAL MotionInterpreter's pending_motions.
        h.Interp.AddToQueue(0, MotionCommand.WalkForward, 0);
        Assert.True(h.Interp.MotionsPending());

        h.Manager.HandleMoveToPosition();

        Assert.Equal(0u, h.Manager.AuxCommand);
    }
}