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.
    /// </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,
        Vector3? terrainNormal = null,
        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);

        if (!interpolationOverwrote
            && terrainNormal.HasValue
            && terrainNormal.Value.Z > 0.01f)
        {
            Vector3 rootMotionWorld = Vector3.Transform(output.Origin, ori);
            Vector3 normal = terrainNormal.Value;
            rootMotionWorld -= normal * Vector3.Dot(rootMotionWorld, normal);
            output.Origin = MoveToMath.GlobalToLocalVec(ori, rootMotionWorld);
        }

        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>
    /// <param name="terrainNormal">
    /// Optional local terrain plane normal at the body's current XY. When
    /// supplied AND the queue-empty / head-reached fallback path runs, the
    /// world-space anim root motion is projected onto the plane so XY motion
    /// produces a corresponding Z change on slopes. Without this, the
    /// fallback advances XY at the locomotion cycle's pace but leaves Z at
    /// the last UP's reported Z — visible as a ~5 Hz staircase on slopes
    /// (the rate of server UpdatePositions). Mirrors retail's
    /// <c>CTransition::adjust_offset</c> contact-plane projection
    /// (named-retail acclient_2013_pseudo_c.txt:272296-272346) for grounded
    /// motion, applied here at the queue-empty boundary instead of inside
    /// the sweep. Pass <c>null</c> on flat ground / when no terrain sample
    /// is available — projection is a no-op when normal == +Z.
    /// </param>
    public Vector3 ComputeOffset(
        double dt,
        Vector3 currentBodyPosition,
        Vector3 rootMotionLocalDelta,
        Quaternion ori,
        InterpolationManager interp,
        float maxSpeed,
        Vector3? terrainNormal = null)
    {
        // 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,
            terrainNormal: null);
        Vector3 rootMotionWorld = Vector3.Transform(output.Origin, ori);

        // Slope projection (queue-empty fallback only). Locomotion cycles
        // bake Z=0 in body-local, so without projection the body's Z stays
        // at the last UP's reported value while XY advances at the running
        // pace — visible ~5 Hz staircase between UPs on hills. Projecting
        // the world-space anim motion onto the local terrain plane gives
        // it a Z component proportional to slope × forward speed, so the
        // body follows the terrain mesh smoothly. No-op on flat ground
        // (normal ≈ +Z, dot ≈ 0) so it can't regress the M2 flat-ground
        // verification.
        if (terrainNormal.HasValue && terrainNormal.Value.Z > 0.01f)
        {
            Vector3 N = terrainNormal.Value;
            float into = Vector3.Dot(rootMotionWorld, N);
            rootMotionWorld -= N * into;
        }
        return rootMotionWorld;
    }
}