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

namespace AcDream.Core.Physics;

/// <summary>
/// Per-frame combiner for remote-entity motion: the root-motion delta emitted
/// by <c>CSequence::update</c> + InterpolationManager catch-up correction.
/// Pure function — no side effects or hidden state.
///
/// Mirrors retail <c>CPhysicsObj::UpdatePositionInternal</c> (0x00512C30):
/// CPartArray writes one complete local Frame, then PositionManager mutates
/// that same Frame. Active interpolation replaces it with
/// <c>Position::subtract2</c>; later managers receive the result.
///
/// The animation root motion is the complete body-local <c>Frame.Origin</c>
/// accumulated by <c>CPartArray::Update</c>: authored PosFrames plus the
/// literal sequence velocity. We rotate that delta by the body's orientation
/// to get world space. It must not be reconstructed from the interpreted
/// command; the retail Humanoid table carries zero sequence velocity for Walk
/// and Run, and retail moves observed characters through the interpolation
/// queue instead.
///
/// <para><b>Renamed R5</b> (was <c>PositionManager</c>): this class is only the
/// InterpolationManager-composition portion of retail's
/// <c>PositionManager::adjust_offset</c> — NOT the retail PositionManager
/// facade. The faithful facade (Sticky/Constraint, owned per entity) is
/// <see cref="Motion.PositionManager"/>. The name was freed to remove the
/// ambiguity that broke every file importing both
/// <c>AcDream.Core.Physics</c> and <c>AcDream.Core.Physics.Motion</c>.</para>
/// </summary>
public sealed class RemoteMotionCombiner
{
    /// <summary>
    /// Compose retail's complete per-object delta frame. Interpolation, when
    /// active, replaces the PartArray frame via
    /// <c>Position::subtract2</c>; otherwise the authored root frame remains.
    ///
    /// <para><b>AD-10, retired 2026-08-06.</b> This method used to accept a
    /// <c>terrainNormal</c> and project the composed world-space root motion
    /// onto it whenever interpolation did not overwrite. That was an EXTRA
    /// projection: retail projects the per-sub-step offset onto
    /// <c>collision_info.contact_plane</c> INSIDE the sweep
    /// (<c>CTransition::adjust_offset</c> <c>0x0050a370</c>,
    /// pc:272271-272393), acdream ports that in
    /// <c>Transition.AdjustOffset</c> (structurally exact, with exactly two
    /// filed divergences — AD-65 and AD-66), and remote bodies do run that sweep.
    /// The extra copy also sampled the wrong surface — a single-point
    /// XY-only terrain lookup, blind to buildings, EnvCells and statics — so
    /// on a walkable NON-terrain surface it applied the plane of the ground
    /// far below. Removing it left the measured trajectory unchanged.</para>
    /// </summary>
    /// <returns><c>true</c> when interpolation replaced the root frame.</returns>
    public bool ComposeOffset(
        double dt,
        Vector3 currentBodyPosition,
        Quaternion ori,
        MotionDeltaFrame rootMotionLocalFrame,
        InterpolationManager interp,
        float maxSpeed,
        MotionDeltaFrame output,
        bool inContact = true)
    {
        ArgumentNullException.ThrowIfNull(rootMotionLocalFrame);
        ArgumentNullException.ThrowIfNull(interp);
        ArgumentNullException.ThrowIfNull(output);

        output.Origin = rootMotionLocalFrame.Origin;
        output.Orientation = rootMotionLocalFrame.Orientation;
        bool interpolationOverwrote = interp.AdjustOffset(
            dt,
            currentBodyPosition,
            ori,
            maxSpeed,
            output,
            inContact);

        return interpolationOverwrote;
    }

    /// <summary>
    /// Compute the per-frame world-space delta to add to body.Position.
    /// </summary>
    /// <param name="dt">Per-frame delta time, seconds.</param>
    /// <param name="currentBodyPosition">Body's current world-space position.</param>
    /// <param name="rootMotionLocalDelta">
    ///   Complete body-local displacement accumulated by this frame's
    ///   <c>CSequence::update</c>. This is already a delta for the current
    ///   quantum, not a velocity to multiply by <paramref name="dt"/>.
    /// </param>
    /// <param name="ori">Body orientation; used to rotate root motion from body-local to world.</param>
    /// <param name="interp">The remote's InterpolationManager (for AdjustOffset call).</param>
    /// <param name="maxSpeed">From <c>MotionInterpreter.GetMaxSpeed()</c> — passed to AdjustOffset for the catch-up clamp.</param>
    public Vector3 ComputeOffset(
        double dt,
        Vector3 currentBodyPosition,
        Vector3 rootMotionLocalDelta,
        Quaternion ori,
        InterpolationManager interp,
        float maxSpeed)
    {
        // Retail-faithful per-frame combiner. Mirrors
        // CPhysicsObj::UpdatePositionInternal (acclient @ 0x00512c30) +
        // InterpolationManager::adjust_offset (@ 0x00555d30):
        //
        //   1. CPartArray::Update writes rootOffset (animation root motion)
        //      into the per-tick Frame.
        //   2. PositionManager::adjust_offset → InterpolationManager::adjust_offset
        //      either:
        //        a) RETURNS EARLY when distance(body, head) < 0.05m
        //           (NodeCompleted; arg2 unmodified) — body uses root motion.
        //        b) OVERWRITES arg2 with `direction × min(catchUpSpeed × dt,
        //           distance)` when body is far from head — catch-up REPLACES
        //           root motion for this frame.
        //
        // It is NOT additive. Our prior port added rootMotion + correction
        // every frame, which stacked the animation push (≈ RunAnimSpeed ×
        // speedMod, ≈ 11.7 m/s) on top of the queue catch-up (capped at
        // ≈ 23.5 m/s) so the body advanced at up to ~3× the server's
        // broadcast pace and the head-behind-body case produced a backward
        // correction every UP — the visible 1-Hz blip the user reported.
        //
        // AdjustOffset returns Vector3.Zero in two cases mapped to retail's
        // early-return: empty queue OR distance < DesiredDistance (0.05m).
        // In both, body falls back to animation root motion.
        var root = new MotionDeltaFrame
        {
            Origin = rootMotionLocalDelta,
        };
        var output = new MotionDeltaFrame();
        ComposeOffset(
            dt,
            currentBodyPosition,
            ori,
            root,
            interp,
            maxSpeed,
            output);
        // AD-10 (retired 2026-08-06): a second copy of the deleted terrain
        // projection used to run here. See ComposeOffset's summary.
        return Vector3.Transform(output.Origin, ori);
    }
}