using System.Numerics;

namespace AcDream.Core.Physics;

/// <summary>
/// Per-frame combiner for remote-entity motion: animation root motion
/// + InterpolationManager catch-up correction. Pure function — no
/// side effects, no hidden state.
///
/// Mirrors retail CPhysicsObj::UpdateObjectInternal (acclient @ 0x00513730):
///   rootOffset = CPartArray::Update(dt)             // animation
///   PositionManager::adjust_offset(rootOffset)      // adds correction
///   frame.origin += rootOffset
///
/// In acdream the animation root motion is sourced from
/// AnimationSequencer.CurrentVelocity (body-local velocity from the
/// active locomotion cycle). We rotate that by the body's orientation
/// to get a world-space delta, then add the InterpolationManager's
/// world-space correction.
/// </summary>
public sealed class PositionManager
{
    /// <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="seqVel">
    ///   Body-local velocity from the active animation cycle
    ///   (from <c>AnimationSequencer.CurrentVelocity</c>); pass
    ///   <c>Vector3.Zero</c> if the entity has no sequencer or is on a
    ///   non-locomotion cycle.
    /// </param>
    /// <param name="ori">Body orientation; used to rotate seqVel 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 seqVel,
        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.
        Vector3 correction = interp.AdjustOffset(dt, currentBodyPosition, maxSpeed);
        if (correction.LengthSquared() > 0f)
            return correction;

        Vector3 rootMotionLocal = seqVel * (float)dt;
        Vector3 rootMotionWorld = Vector3.Transform(rootMotionLocal, 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;
    }
}