acdream/src/AcDream.Core/Physics/Motion/StickyManager.cs

using System;
using System.Numerics;

namespace AcDream.Core.Physics.Motion;

/// <summary>
/// R5 — verbatim port of retail's <c>StickyManager</c> (acclient.h:31518,
/// struct #3466; decomp block 0x00555400-0x00555866,
/// <c>r5-positionmanager-sticky-decomp.md</c>). Makes the owning object
/// FOLLOW a target object at a bounded gap: each tick
/// <see cref="AdjustOffset"/> steers the mover horizontally toward the
/// target's live (or last-known) position and turns it to face the target,
/// speed- and turn-rate-limited. A 1-second watchdog (<see cref="UseTime"/>)
/// drops the stick if no target-position update arrives.
///
/// <para>Owned by <see cref="PositionManager"/> (lazily created on first
/// <see cref="PositionManager.StickTo"/>). Establishes its target-tracking
/// subscription through the owning <see cref="IPhysicsObjHost"/>'s
/// <c>set_target</c> (→ <see cref="TargetManager"/>); receives the target's
/// live position back through <see cref="HandleUpdateTarget"/>, fanned out from
/// <c>CPhysicsObj::HandleUpdateTarget</c> → <see cref="PositionManager"/>.</para>
///
/// <para>The dense x87 back half of retail's <c>adjust_offset</c> is decoded
/// against ACE's <c>StickyManager.cs</c> (the two constants
/// <see cref="StickyRadius"/>=0.3 and <see cref="StickyTime"/>=1.0 are ACE's,
/// verified against the retail mush structure — see the port-plan §2a).</para>
/// </summary>
public sealed class StickyManager
{
    /// <summary>Retail <c>StickyRadius</c> const (ACE: 0.3f) — the desired
    /// follow gap subtracted from the cylinder distance.</summary>
    public const float StickyRadius = 0.3f;

    /// <summary>Retail <c>StickyTime</c> const (ACE: 1.0f) — the one-shot grace
    /// window: if no target update refreshes the stick within this many
    /// seconds of <see cref="StickTo"/>, <see cref="UseTime"/> drops it.</summary>
    public const float StickyTime = 1.0f;

    /// <summary>Retail <c>get_max_speed</c> multiplier for the follow speed
    /// (ACE: ×5 — the follower catches up faster than a normal walk/run).</summary>
    private const float FollowSpeedFactor = 5.0f;

    /// <summary>Retail fallback follow speed when no motion interpreter exists
    /// (ACE: 15.0f, i.e. the <c>MAX_VELOCITY</c> constant the mush loads).</summary>
    private const float FallbackFollowSpeed = 15.0f;

    private readonly IPhysicsObjHost _host;

    /// <summary>+0x00 retail <c>target_id</c> — the object we are stuck to
    /// (0 = not stuck).</summary>
    public uint TargetId { get; private set; }

    /// <summary>+0x04 retail <c>target_radius</c> — the target's cylinder
    /// radius (from <c>CPartArray::GetRadius</c> of the stuck-to object).</summary>
    public float TargetRadius { get; private set; }

    /// <summary>+0x08 retail <c>target_position</c> — last-known target
    /// position (from <see cref="HandleUpdateTarget"/>), used when the live
    /// <c>GetObjectA</c> resolve fails.</summary>
    public Position TargetPosition { get; private set; }

    /// <summary>+0x54 retail <c>initialized</c> — false until the first
    /// <c>Ok</c> target update arrives (gates <see cref="AdjustOffset"/> and
    /// the <see cref="UseTime"/> timeout).</summary>
    public bool Initialized { get; private set; }

    /// <summary>+0x58 retail <c>sticky_timeout_time</c> — the wall-clock
    /// deadline set once at <see cref="StickTo"/> time (now + 1 s).</summary>
    public double StickyTimeoutTime { get; private set; }

    public StickyManager(IPhysicsObjHost host)
        => _host = host ?? throw new ArgumentNullException(nameof(host));

    /// <summary>
    /// Retail <c>StickyManager::UnStick</c> (0x00555400). No-op if not stuck;
    /// otherwise the standard 4-step teardown: clear <see cref="TargetId"/> +
    /// <see cref="Initialized"/>, tell the host to <c>clear_target</c> (drop
    /// the voyeur subscription), then <c>interrupt_current_movement</c>.
    /// </summary>
    public void UnStick()
    {
        if (TargetId == 0)
            return;

        TargetId = 0;
        Initialized = false;
        _host.ClearTarget();
        _host.InterruptCurrentMovement();
    }

    /// <summary>
    /// Retail <c>StickyManager::StickTo</c> (0x00555710). Begin following
    /// <paramref name="objectId"/>. If already stuck, tears the old stick down
    /// first (same 4-step sequence as <see cref="UnStick"/>). Sets the 1 s
    /// timeout deadline and registers as a voyeur of the target via the host's
    /// <c>set_target(context=0, objectId, radius=0.5, quantum=0.5)</c> — the
    /// live target position then arrives asynchronously through
    /// <see cref="HandleUpdateTarget"/>.
    /// </summary>
    /// <param name="objectId">Retail <c>arg2</c> — target object id.</param>
    /// <param name="targetRadius">Retail <c>arg3</c> — the target's cylinder
    /// radius (feeds <see cref="AdjustOffset"/>'s distance math).</param>
    /// <param name="targetHeight">Retail <c>arg4</c> — accepted for call-shape
    /// parity but UNUSED in the body (matches retail + ACE; the height feeds
    /// the caller-side geometry only).</param>
    public void StickTo(uint objectId, float targetRadius, float targetHeight)
    {
        _ = targetHeight; // retail/ACE: arg4 is read nowhere in this body.

        if (TargetId != 0)
        {
            // Inlined 4-step teardown of the previous stick (retail 0x00555716).
            TargetId = 0;
            Initialized = false;
            _host.ClearTarget();
            _host.InterruptCurrentMovement();
        }

        TargetRadius = targetRadius;
        TargetId = objectId;
        Initialized = false;
        StickyTimeoutTime = _host.CurTime + StickyTime;
        // set_target(context_id=0, objectId, radius=0.5, quantum=0.5).
        _host.SetTarget(0, objectId, 0.5f, 0.5);
    }

    /// <summary>
    /// Retail <c>StickyManager::UseTime</c> (0x00555610). The 1 s watchdog: if
    /// <c>Timer::cur_time &gt;= sticky_timeout_time</c>, force-unstick (same
    /// 4-step teardown). The deadline is set once in <see cref="StickTo"/> and
    /// NOT refreshed by <see cref="HandleUpdateTarget"/> (retail + ACE) — a
    /// stick survives at most 1 s of wall-clock unless re-issued.
    /// </summary>
    public void UseTime()
    {
        if (TargetId == 0)
            return;

        // Strictly AFTER the deadline (retail 0x00555626 `test ah,0x41` —
        // C0|C3 clear = cur_time > timeout; ACE `>` too), not >=.
        if (_host.CurTime > StickyTimeoutTime)
        {
            TargetId = 0;
            Initialized = false;
            _host.ClearTarget();
            _host.InterruptCurrentMovement();
        }
    }

    /// <summary>
    /// Retail <c>StickyManager::HandleUpdateTarget</c> (0x00555780). The
    /// target-position callback (fanned out from
    /// <c>CPhysicsObj::HandleUpdateTarget</c> → <see cref="PositionManager"/>).
    /// Ignores updates whose <see cref="TargetInfo.ObjectId"/> doesn't match
    /// our <see cref="TargetId"/>. On <see cref="TargetStatus.Ok"/>: cache the
    /// target position and mark <see cref="Initialized"/>. On any other status
    /// (lost/exit/teleport): tear the stick down (4-step).
    /// </summary>
    public void HandleUpdateTarget(TargetInfo info)
    {
        if (info.ObjectId != TargetId)
            return;

        if (info.Status == TargetStatus.Ok)
        {
            Initialized = true;
            TargetPosition = info.TargetPosition;
            return;
        }

        if (TargetId != 0)
        {
            TargetId = 0;
            Initialized = false;
            _host.ClearTarget();
            _host.InterruptCurrentMovement();
        }
    }

    /// <summary>
    /// Retail <c>StickyManager::adjust_offset</c> (0x00555430). Writes this
    /// tick's follow steering into the shared <paramref name="offset"/>
    /// accumulator: a speed-clamped horizontal position delta toward the
    /// target plus a bounded turn to face it. No-op unless stuck AND
    /// initialized. See port-plan §2a for the x87-mush decode.
    /// </summary>
    /// <param name="offset">The per-tick delta frame
    /// (<see cref="PositionManager.AdjustOffset"/>'s shared accumulator).</param>
    /// <param name="quantum">Elapsed time this tick, seconds.</param>
    public void AdjustOffset(MotionDeltaFrame offset, double quantum)
    {
        if (TargetId == 0 || !Initialized)
            return;

        var self = _host.Position;
        var target = _host.GetObjectA(TargetId);
        var targetPos = target != null ? target.Position : TargetPosition;

        // offset = local-frame, Z-flattened vector from self to target.
        Vector3 worldOffset = targetPos.Frame.Origin - self.Frame.Origin;   // Position::get_offset
        Vector3 local = MoveToMath.GlobalToLocalVec(self.Frame.Orientation, worldOffset);
        local.Z = 0f;
        offset.Origin = local;

        // Signed horizontal cylinder distance past the 0.3 m stick gap.
        float dist = MoveToMath.CylinderDistanceNoZ(
            _host.Radius, self.Frame.Origin, TargetRadius, targetPos.Frame.Origin) - StickyRadius;

        if (MoveToMath.NormalizeCheckSmall(ref offset.Origin))
            offset.Origin = Vector3.Zero;

        // Follow speed = 5× own max locomotion speed (catch up), fallback 15.
        float speed = 0f;
        float? maxSpeed = _host.MinterpMaxSpeed;
        if (maxSpeed.HasValue)
            speed = maxSpeed.Value * FollowSpeedFactor;
        if (speed < MoveToMath.Epsilon)
            speed = FallbackFollowSpeed;

        // Don't overshoot: clamp the per-tick step to the remaining (signed)
        // distance — a negative dist inverts the direction (back off).
        float delta = speed * (float)quantum;
        if (delta >= MathF.Abs(dist))
            delta = dist;
        offset.Origin *= delta;

        // Bounded turn to face the target (relative heading this tick).
        float curHeading = MoveToMath.GetHeading(self.Frame.Orientation);
        float targetHeading = MoveToMath.PositionHeading(self.Frame.Origin, targetPos.Frame.Origin);
        float heading = targetHeading - curHeading;
        if (MathF.Abs(heading) < MoveToMath.Epsilon)
            heading = 0f;
        if (heading < -MoveToMath.Epsilon)
            heading += 360f;
        offset.SetHeading(heading);
    }
}