using System;
using System.Numerics;

namespace AcDream.Core.Physics;

/// <summary>
/// Per-tick steering for server-controlled remote creatures while a
/// MoveToObject (movementType 6) or MoveToPosition (movementType 7) packet
/// is the active locomotion source.
///
/// <para>
/// Replaces the 882a07c-era "hold body Velocity at zero during MoveTo"
/// stabilizer. With the full MoveTo path payload now captured on
/// <see cref="AcDream.Core.Net.Messages.CreateObject.MoveToPathData"/>,
/// the body solver has the destination + heading + thresholds it needs to
/// run the retail per-tick loop instead of waiting for sparse
/// UpdatePosition snap corrections.
/// </para>
///
/// <para>
/// Retail references:
/// <list type="bullet">
/// <item><description>
/// <c>MoveToManager::HandleMoveToPosition</c> (<c>0x00529d80</c>) — the
/// per-tick driver. Computes heading-to-target, fires an aux
/// <c>TurnLeft</c>/<c>TurnRight</c> command when |delta| &gt; 20°, snaps
/// orientation when within tolerance, and tests arrival via
/// <c>dist &lt;= min_distance</c> (chase) or
/// <c>dist &gt;= distance_to_object</c> (flee).
/// </description></item>
/// <item><description>
/// <c>MoveToManager::_DoMotion</c> / <c>_StopMotion</c> route turn
/// commands through <c>CMotionInterp::DoInterpretedMotion</c> — i.e.
/// MoveToManager itself does NOT touch the body. The body's actual
/// velocity comes from <c>CMotionInterp::apply_current_movement</c>
/// reading <c>InterpretedState.ForwardCommand = RunForward</c> and
/// emitting <c>velocity.Y = RunAnimSpeed × speedMod</c>, transformed by
/// the body's orientation.
/// </description></item>
/// </list>
/// </para>
///
/// <para>
/// Acdream port scope: minimum viable subset. We skip target re-tracking
/// (server re-emits MoveTo every ~1 s with refreshed Origin), sticky/
/// StickTo, fail-distance progress detector, and the sphere-cylinder
/// distance variant — all server-side concerns the local body doesn't need
/// to model. We DO port heading-to-target, the ±20° aux-turn tolerance
/// (with ACE's <c>set_heading(true)</c> snap-on-aligned fudge), and
/// arrival detection via <c>min_distance</c>.
/// </para>
///
/// <para>
/// ACE divergence: ACE swaps the chase/flee arrival predicates
/// (<c>dist &lt;= DistanceToObject</c> vs retail's <c>dist &lt;= MinDistance</c>).
/// We follow retail.
/// </para>
/// </summary>
public static class RemoteMoveToDriver
{
    /// <summary>
    /// Heading tolerance below which we snap orientation directly to the
    /// target heading (ACE's <c>set_heading(target, true)</c>
    /// server-tic-rate fudge). Above tolerance we rotate at
    /// <see cref="TurnRateRadPerSec"/>. Retail value (line 307251 of
    /// <c>acclient_2013_pseudo_c.txt</c>) is 20°.
    /// </summary>
    public const float HeadingSnapToleranceRad = 20.0f * MathF.PI / 180.0f;

    /// <summary>
    /// Default angular rate for in-motion heading correction when delta
    /// exceeds <see cref="HeadingSnapToleranceRad"/>. Picked to match
    /// ACE's <c>TurnSpeed</c> default of <c>π/2</c> rad/s for monsters;
    /// when the per-creature value differs, the future port can wire it
    /// in via the <c>TurnSpeed</c> field on InterpretedMotionState.
    /// </summary>
    public const float TurnRateRadPerSec = MathF.PI / 2.0f;

    /// <summary>
    /// Float-comparison slack for the arrival predicate. With
    /// <c>min_distance == 0</c> in a chase packet, exact equality is
    /// unreachable due to integration wobble; this epsilon prevents the
    /// driver from over-shooting by a sub-meter and snap-flipping back.
    /// </summary>
    public const float ArrivalEpsilon = 0.05f;

    /// <summary>
    /// Maximum staleness (seconds) of the most recent MoveTo packet
    /// before the driver gives up steering. ACE re-emits MoveTo at ~1 Hz
    /// during active chase; if no fresh packet arrives for this long,
    /// the entity has likely either left our streaming view, switched
    /// to a non-MoveTo motion the server's broadcast didn't reach us
    /// for, or had its move cancelled server-side without our seeing
    /// the cancel UM. In any of those cases, continuing to drive the
    /// body toward a stale destination produces the "monster runs in
    /// place after popping back into view" symptom (2026-04-28).
    /// 1.5 s gives us comfortable margin over the ~1 s emit cadence
    /// while still failing fast on real loss-of-state.
    /// </summary>
    public const double StaleDestinationSeconds = 1.5;

    public enum DriveResult
    {
        /// <summary>Within arrival window — caller should zero velocity.</summary>
        Arrived,
        /// <summary>Steering active — caller should let
        /// <c>apply_current_movement</c> set body velocity from the cycle.</summary>
        Steering,
    }

    /// <summary>
    /// Steer body orientation toward <paramref name="destinationWorld"/>
    /// and report whether the body has arrived or should keep running.
    /// Pure function — emits the updated orientation via
    /// <paramref name="newOrientation"/> (the input is not mutated; the
    /// caller assigns the new value back to its body).
    /// </summary>
    /// <param name="minDistance">
    /// <c>min_distance</c> from the wire's MovementParameters block —
    /// retail's <c>HandleMoveToPosition</c> chase-arrival threshold.
    /// </param>
    /// <param name="distanceToObject">
    /// <c>distance_to_object</c> from the wire — ACE's chase-arrival
    /// threshold (default 0.6 m, the melee range). The actual arrival
    /// gate is <c>max(minDistance, distanceToObject)</c>: retail-faithful
    /// when retail sends <c>min_distance</c> &gt; 0, ACE-compatible when
    /// ACE puts the value in <c>distance_to_object</c> with
    /// <c>min_distance == 0</c>. Without this, ACE's <c>min_distance==0</c>
    /// chase packets never arrive — the body keeps re-targeting around
    /// the player at melee range and visibly oscillates between facings,
    /// which is the user-reported "monster keeps running in different
    /// directions when it should be attacking" symptom (2026-04-28).
    /// </param>
    public static DriveResult Drive(
        Vector3 bodyPosition,
        Quaternion bodyOrientation,
        Vector3 destinationWorld,
        float minDistance,
        float distanceToObject,
        float dt,
        bool moveTowards,
        out Quaternion newOrientation)
    {
        // Horizontal distance only — server owns Z, our body Z is
        // hard-snapped to the latest UpdatePosition.
        float dx = destinationWorld.X - bodyPosition.X;
        float dy = destinationWorld.Y - bodyPosition.Y;
        float dist = MathF.Sqrt(dx * dx + dy * dy);

        // Arrival predicate per retail MoveToManager::HandleMoveToPosition
        // (acclient_2013_pseudo_c.txt:307289-307320) and ACE
        // MoveToManager.cs:476:
        //
        //   chase (MoveTowards): dist <= distance_to_object
        //   flee  (MoveAway):    dist >= min_distance
        //
        // (My earlier <c>max(MinDistance, DistanceToObject)</c> was a
        // defensive guess; cross-checked with two independent research
        // agents against the named retail decomp + ACE port + holtburger,
        // the chase threshold is unambiguously DistanceToObject —
        // MinDistance is the FLEE arrival threshold. ACE's wire defaults
        // give MinDistance=0, DistanceToObject=0.6 — the body should stop
        // at melee range, not run to zero.)
        float arrivalThreshold = moveTowards ? distanceToObject : minDistance;
        if (moveTowards && dist <= arrivalThreshold + ArrivalEpsilon)
        {
            newOrientation = bodyOrientation;
            return DriveResult.Arrived;
        }
        if (!moveTowards && dist >= arrivalThreshold - ArrivalEpsilon)
        {
            newOrientation = bodyOrientation;
            return DriveResult.Arrived;
        }

        // Degenerate — already on target horizontally; preserve heading.
        if (dist < 1e-4f)
        {
            newOrientation = bodyOrientation;
            return DriveResult.Steering;
        }

        // Body's local-forward is +Y (see MotionInterpreter.get_state_velocity
        // at line 605-616: velocity.Y = (Walk/Run)AnimSpeed × ForwardSpeed).
        // World forward = Transform((0,1,0), orientation). Yaw extracted
        // via atan2(-worldFwd.X, worldFwd.Y) so yaw = 0 ↔ orientation = Identity.
        var localForward = new Vector3(0f, 1f, 0f);
        var worldForward = Vector3.Transform(localForward, bodyOrientation);
        float currentYaw = MathF.Atan2(-worldForward.X, worldForward.Y);

        // Desired heading: face the target. (dx, dy) is the world-space
        // offset to the target. With local-forward=+Y we want yaw such
        // that Transform((0,1,0), R_Z(yaw)) = (dx, dy)/dist; that solves
        // to yaw = atan2(-dx, dy).
        float desiredYaw = MathF.Atan2(-dx, dy);
        float delta = WrapPi(desiredYaw - currentYaw);

        if (MathF.Abs(delta) <= HeadingSnapToleranceRad)
        {
            // ACE's set_heading(target, true) — sync to server-tic-rate.
            // We have the same sparse-UP problem ACE does, so the same
            // fudge applies.
            newOrientation = Quaternion.CreateFromAxisAngle(Vector3.UnitZ, desiredYaw);
        }
        else
        {
            // Retail BeginTurnToHeading / HandleMoveToPosition aux turn:
            // rotate at TurnRate clamped to dt, in the shorter direction.
            float maxStep = TurnRateRadPerSec * dt;
            float step = MathF.Sign(delta) * MathF.Min(MathF.Abs(delta), maxStep);
            // Apply incremental yaw around world +Z (preserving any
            // server-supplied pitch/roll from the latest UpdatePosition).
            var deltaQuat = Quaternion.CreateFromAxisAngle(Vector3.UnitZ, step);
            newOrientation = Quaternion.Normalize(deltaQuat * bodyOrientation);
        }

        return DriveResult.Steering;
    }

    /// <summary>
    /// Convert a landblock-local Origin from a MoveTo packet
    /// (<see cref="AcDream.Core.Net.Messages.CreateObject.MoveToPathData"/>)
    /// into acdream's render world space using the same arithmetic as
    /// <c>OnLivePositionUpdated</c>: shift by the landblock-grid offset
    /// from the live-mode center.
    /// </summary>
    public static Vector3 OriginToWorld(
        uint originCellId,
        float originX,
        float originY,
        float originZ,
        int liveCenterLandblockX,
        int liveCenterLandblockY)
    {
        int lbX = (int)((originCellId >> 24) & 0xFFu);
        int lbY = (int)((originCellId >> 16) & 0xFFu);
        return new Vector3(
            originX + (lbX - liveCenterLandblockX) * 192f,
            originY + (lbY - liveCenterLandblockY) * 192f,
            originZ);
    }

    /// <summary>
    /// Cap horizontal velocity so the body lands exactly at
    /// <paramref name="arrivalThreshold"/> rather than overshooting past
    /// it during the final tick of approach. Without this clamp, a body
    /// running at <c>RunAnimSpeed × speedMod ≈ 4 m/s</c> can overshoot
    /// the 0.6 m arrival window by up to one tick's advance (~6 cm at
    /// 60 fps) — visible as the creature "running slightly through" the
    /// player it's about to attack (user-reported 2026-04-28).
    ///
    /// <para>
    /// The clamp is a strict scale-down of the horizontal component
    /// (X/Y); the vertical component (Z) is left to gravity / terrain
    /// handling. <paramref name="moveTowards"/> false (flee branch) is a
    /// no-op since fleeing has no overshoot risk — the body wants to
    /// move AWAY from the destination.
    /// </para>
    /// </summary>
    public static Vector3 ClampApproachVelocity(
        Vector3 bodyPosition,
        Vector3 currentVelocity,
        Vector3 destinationWorld,
        float arrivalThreshold,
        float dt,
        bool moveTowards)
    {
        if (!moveTowards || dt <= 0f) return currentVelocity;

        float dx = destinationWorld.X - bodyPosition.X;
        float dy = destinationWorld.Y - bodyPosition.Y;
        float dist = MathF.Sqrt(dx * dx + dy * dy);
        float remaining = MathF.Max(0f, dist - arrivalThreshold);

        float vxy = MathF.Sqrt(currentVelocity.X * currentVelocity.X
                             + currentVelocity.Y * currentVelocity.Y);
        if (vxy < 1e-3f) return currentVelocity;

        float advance = vxy * dt;
        if (advance <= remaining) return currentVelocity;

        // Already inside or right at the threshold: zero horizontal
        // velocity, keep Z. (The arrival predicate in Drive() should
        // have fired this tick, but this is the belt-and-braces guard.)
        if (remaining < 1e-3f)
            return new Vector3(0f, 0f, currentVelocity.Z);

        float scale = remaining / advance;
        return new Vector3(
            currentVelocity.X * scale,
            currentVelocity.Y * scale,
            currentVelocity.Z);
    }

    /// <summary>Wrap an angle in radians to [-π, π].</summary>
    private static float WrapPi(float r)
    {
        const float TwoPi = MathF.PI * 2f;
        r %= TwoPi;
        if (r > MathF.PI) r -= TwoPi;
        if (r < -MathF.PI) r += TwoPi;
        return r;
    }
}