using System;
using System.Numerics;

namespace AcDream.Core.Physics;

/// <summary>
/// Chooses the visible locomotion cycle for server-controlled remotes whose
/// UpdateMotion packet is a MoveToObject/MoveToPosition union rather than an
/// InterpretedMotionState.
///
/// Retail references:
/// <list type="bullet">
/// <item><description>
/// <c>MovementManager::PerformMovement</c> (0x00524440) dispatches movement
/// types 6/7 into <c>MoveToManager::MoveToObject/MoveToPosition</c> instead
/// of unpacking an InterpretedMotionState.
/// </description></item>
/// <item><description>
/// <c>MovementParameters::UnPackNet</c> (0x0052AC50) shows MoveTo packets
/// carry movement params + run rate, not a ForwardCommand field.
/// </description></item>
/// <item><description>
/// ACE <c>MovementData.Write</c> uses the same movement type union; holtburger
/// documents the matching <c>MovementType::MoveToPosition = 7</c>.
/// </description></item>
/// </list>
/// </summary>
public static class ServerControlledLocomotion
{
    public const float StopSpeed = 0.20f;
    public const float RunThreshold = 1.25f;
    public const float MinSpeedMod = 0.25f;
    public const float MaxSpeedMod = 3.00f;

    // R4-V4: PlanMoveToStart DELETED - MoveTo UMs route to the verbatim
    // MoveToManager (BeginMoveForward -> get_command -> _DoMotion produces
    // the cycle through the same sink every other motion uses).
    // PlanFromVelocity below survives (M16 register row, retires in R6).

    public static LocomotionCycle PlanFromVelocity(Vector3 worldVelocity)
    {
        float horizontalSpeed = MathF.Sqrt(
            worldVelocity.X * worldVelocity.X +
            worldVelocity.Y * worldVelocity.Y);

        if (horizontalSpeed < StopSpeed)
            return new LocomotionCycle(MotionCommand.Ready, 1f, false);

        if (horizontalSpeed < RunThreshold)
        {
            float speedMod = Math.Clamp(
                horizontalSpeed / MotionInterpreter.WalkAnimSpeed,
                MinSpeedMod,
                MaxSpeedMod);
            return new LocomotionCycle(MotionCommand.WalkForward, speedMod, true);
        }

        return new LocomotionCycle(
            MotionCommand.RunForward,
            Math.Clamp(horizontalSpeed / MotionInterpreter.RunAnimSpeed, MinSpeedMod, MaxSpeedMod),
            true);
    }

    /// <summary>
    /// Limits the AP-80 position-velocity adaptation to the locomotion state
    /// family it exists to supply. Retail UpdatePosition never selects a
    /// motion at all; therefore an authoritative action/substate such as Dead
    /// must never be replaced by this adaptation.
    /// </summary>
    public static bool CanApplyVelocityCycle(uint currentMotion)
        => currentMotion == MotionCommand.Ready || IsLocomotion(currentMotion);

    public static bool IsLocomotion(uint motion)
    {
        uint low = motion & 0xFFu;
        return low is 0x05 or 0x06 or 0x07 or 0x0F or 0x10;
    }

    public readonly record struct LocomotionCycle(
        uint Motion,
        float SpeedMod,
        bool IsMoving);

    private static float SanitizePositive(float value)
    {
        return float.IsFinite(value) && value > 0f ? value : 1f;
    }
}