acdream/src/AcDream.Core.Net/Messages/UpdateMotion.cs

using System.Buffers.Binary;
using System.Collections.Generic;

namespace AcDream.Core.Net.Messages;

/// <summary>
/// Inbound <c>UpdateMotion</c> GameMessage (opcode <c>0xF74C</c>). The server
/// sends this whenever an already-spawned entity changes its motion state —
/// NPCs starting a walk cycle, creatures switching to attack stance, doors
/// opening, a player waving, etc. acdream's animation system needs to
/// consume these so the motion tick can switch the entity's cycle to the
/// new (stance, forward-command) pair instead of sitting on whatever the
/// initial CreateObject said.
///
/// <para>
/// Wire layout (see
/// <c>references/ACE/Source/ACE.Server/Network/GameMessages/Messages/GameMessageUpdateMotion.cs</c>
/// and <c>references/ACE/Source/ACE.Server/Network/Motion/MovementData.cs::Write</c>
/// with <c>header = true</c>):
/// </para>
/// <list type="bullet">
///   <item><b>u32 opcode</b> — 0xF74C</item>
///   <item><b>u32 objectGuid</b> — which entity this update is for</item>
///   <item><b>u16 instanceSequence</b> — Sequences.ObjectInstance, tracked but not used for pose</item>
///   <item><b>MovementData with header</b>:
///     <list type="bullet">
///       <item>u16 movementSequence</item>
///       <item>u16 serverControlSequence</item>
///       <item>u8 isAutonomous, then align to 4 bytes</item>
///       <item>u8 movementType</item>
///       <item>u8 motionFlags</item>
///       <item>u16 currentStyle (MotionStance)</item>
///       <item>InterpretedMotionState when movementType == Invalid (0):
///         u32 flagsAndCommandCount, then each present field in flag order
///         (CurrentStyle u16, ForwardCommand u16, SidestepCommand u16,
///         TurnCommand u16, forward speed f32, sidestep speed f32,
///         turn speed f32), commands list, align.</item>
///     </list>
///   </item>
/// </list>
///
/// <para>
/// We only extract the two fields the animation system actually consumes:
/// the current <c>Stance</c> and the <c>ForwardCommand</c>. Everything else
/// is skipped. The outer message doesn't carry a length for MovementData,
/// so our parser reads exactly as far as it needs and leaves subsequent
/// bytes untouched.
/// </para>
/// </summary>
public static class UpdateMotion
{
    public const uint Opcode = 0xF74Cu;

    /// <summary>
    /// Extracted payload: the guid of the entity whose motion changed and
    /// the (stance, forward-command) pair describing its new pose. The
    /// command is nullable because the <c>ForwardCommand</c> flag may be
    /// unset in the InterpretedMotionState; the stance is always present
    /// (even if 0, meaning "no specific stance").
    ///
    /// <para>L.2g S1 (DEV-6): the three staleness stamps + autonomy flag are
    /// exposed for the retail gate (<see cref="AcDream.Core.Physics"/>
    /// <c>MotionSequenceGate</c>) — retail compares them against
    /// <c>update_times[INSTANCE_TS/MOVEMENT_TS/SERVER_CONTROLLED_MOVE_TS]</c>
    /// and stores <c>last_move_was_autonomous</c>
    /// (<c>CPhysics::SetObjectMovement</c> 0x00509690).</para>
    /// </summary>
    public readonly record struct Parsed(
        uint Guid,
        CreateObject.ServerMotionState MotionState,
        ushort InstanceSequence,
        ushort MovementSequence,
        ushort ServerControlSequence,
        bool IsAutonomous);

    /// <summary>
    /// Parse a reassembled UpdateMotion body. <paramref name="body"/> must
    /// start with the 4-byte opcode. Returns null on malformed input
    /// (truncated fields, wrong opcode, malformed InterpretedMotionState).
    /// </summary>
    public static Parsed? TryParse(ReadOnlySpan<byte> body)
    {
        try
        {
            int pos = 0;

            if (body.Length - pos < 4) return null;
            uint opcode = BinaryPrimitives.ReadUInt32LittleEndian(body.Slice(pos));
            pos += 4;
            if (opcode != Opcode) return null;

            if (body.Length - pos < 4) return null;
            uint guid = BinaryPrimitives.ReadUInt32LittleEndian(body.Slice(pos));
            pos += 4;

            // ObjectInstance sequence (u16) — retail's dispatch-level
            // INSTANCE_TS staleness gate compares this against the object's
            // known incarnation (DispatchSmartBoxEvent case 0xF74C).
            if (body.Length - pos < 2) return null;
            ushort instanceSequence = BinaryPrimitives.ReadUInt16LittleEndian(body.Slice(pos));
            pos += 2;

            // MovementData header: u16 movementSequence, u16 serverControlSequence,
            // u8 isAutonomous, then Align().
            //
            // ACE's Align() (Network/Extensions.cs:55) uses
            // CalculatePadMultiple(BaseStream.Length, 4) — i.e. it pads based on
            // the ABSOLUTE stream length, not a relative offset within the
            // MovementData block.
            //
            // At this point the absolute stream has: opcode (4) + guid (4) +
            // objectInstance (2) + movSeq (2) + srvSeq (2) + isAut (1) = 15.
            // Align(4) rounds 15 → 16, so ONE pad byte is written.
            // MovementData header = 2+2+1+1 = 6 bytes.
            //
            // Previous version mistakenly reserved 8 bytes here, which shifted
            // every subsequent field by 2 and made every remote-char UpdateMotion
            // decode as garbage (stance read from the packed-flags dword).
            //
            // L.2g S1 (DEV-6): the header fields feed retail's
            // CPhysics::SetObjectMovement staleness gates + the
            // last_move_was_autonomous store, so parse them instead of
            // skipping.
            if (body.Length - pos < 6) return null;
            ushort movementSequence = BinaryPrimitives.ReadUInt16LittleEndian(body.Slice(pos));
            pos += 2;
            ushort serverControlSequence = BinaryPrimitives.ReadUInt16LittleEndian(body.Slice(pos));
            pos += 2;
            bool isAutonomous = body[pos] != 0;
            pos += 2; // u8 isAutonomous + Align(4) pad byte

            // movementType u8, motionFlags u8, currentStyle u16
            if (body.Length - pos < 4) return null;
            byte movementType = body[pos]; pos += 1;
            byte _motionFlags = body[pos]; pos += 1;
            ushort currentStyle = BinaryPrimitives.ReadUInt16LittleEndian(body.Slice(pos));
            pos += 2;

            if (Environment.GetEnvironmentVariable("ACDREAM_DUMP_MOTION") == "1")
            {
                int preHex = Math.Min(body.Length, 32);
                var hex = new System.Text.StringBuilder();
                for (int i = 0; i < preHex; i++) hex.Append($"{body[i]:X2} ");
                System.Console.WriteLine(
                    $"  UM raw: mt=0x{movementType:X2} mf=0x{_motionFlags:X2} cs=0x{currentStyle:X4}  | {hex}");
            }

            ushort? forwardCommand = null;
            float? forwardSpeed = null;
            ushort? sidestepCommand = null;
            float? sidestepSpeed = null;
            ushort? turnCommand = null;
            float? turnSpeed = null;
            uint? moveToParameters = null;
            float? moveToSpeed = null;
            float? moveToRunRate = null;
            CreateObject.MoveToPathData? moveToPath = null;
            List<CreateObject.MotionItem>? commands = null;

            if (movementType == 0)
            {
                // InterpretedMotionState — same layout as in CreateObject's
                // MovementInvalid branch, just reached via the header'd path.
                // Includes the Commands list (MotionItem[]) that carries
                // Actions, emotes, and other one-shots not in ForwardCommand.
                if (body.Length - pos < 4) return new Parsed(guid, new CreateObject.ServerMotionState(currentStyle, null, MovementType: movementType), instanceSequence, movementSequence, serverControlSequence, isAutonomous);
                uint packed = BinaryPrimitives.ReadUInt32LittleEndian(body.Slice(pos));
                pos += 4;
                uint flags = packed & 0x7Fu;
                uint numCommands = packed >> 7;

                // Flag-bit layout + write order (ACE
                // InterpretedMotionState.Write @ line 127 + MovementStateFlag
                // enum — note the bit values are NOT in write order):
                //   CurrentStyle    = 0x01   written first    (ushort)
                //   ForwardCommand  = 0x02   written second   (ushort)
                //   SideStepCommand = 0x08   written third    (ushort)
                //   TurnCommand     = 0x20   written fourth   (ushort)
                //   ForwardSpeed    = 0x04   written fifth    (float)
                //   SideStepSpeed   = 0x10   written sixth    (float)
                //   TurnSpeed       = 0x40   written seventh  (float)
                // Our earlier version had the bit-to-field mapping wrong
                // (treated Side/Turn commands as floats and ForwardSpeed as
                // the wrong bit) — that's why every remote's ForwardSpeed
                // was reading as "absent" (HasValue=False).

                if ((flags & 0x1u) != 0)
                {
                    if (body.Length - pos < 2) return new Parsed(guid, new CreateObject.ServerMotionState(currentStyle, null, MovementType: movementType), instanceSequence, movementSequence, serverControlSequence, isAutonomous);
                    currentStyle = BinaryPrimitives.ReadUInt16LittleEndian(body.Slice(pos));
                    pos += 2;
                }
                if ((flags & 0x2u) != 0)
                {
                    if (body.Length - pos < 2) return new Parsed(guid, new CreateObject.ServerMotionState(currentStyle, null, MovementType: movementType), instanceSequence, movementSequence, serverControlSequence, isAutonomous);
                    forwardCommand = BinaryPrimitives.ReadUInt16LittleEndian(body.Slice(pos));
                    pos += 2;
                }
                // SideStepCommand — ushort, bit 0x8
                if ((flags & 0x8u) != 0)
                {
                    if (body.Length - pos < 2) goto done;
                    sidestepCommand = BinaryPrimitives.ReadUInt16LittleEndian(body.Slice(pos));
                    pos += 2;
                }
                // TurnCommand — ushort, bit 0x20
                if ((flags & 0x20u) != 0)
                {
                    if (body.Length - pos < 2) goto done;
                    turnCommand = BinaryPrimitives.ReadUInt16LittleEndian(body.Slice(pos));
                    pos += 2;
                }
                // ForwardSpeed — float, bit 0x4
                if ((flags & 0x4u) != 0)
                {
                    if (body.Length - pos < 4) goto done;
                    forwardSpeed = BinaryPrimitives.ReadSingleLittleEndian(body.Slice(pos));
                    pos += 4;
                }
                // SideStepSpeed — float, bit 0x10
                if ((flags & 0x10u) != 0)
                {
                    if (body.Length - pos < 4) goto done;
                    sidestepSpeed = BinaryPrimitives.ReadSingleLittleEndian(body.Slice(pos));
                    pos += 4;
                }
                // TurnSpeed — float, bit 0x40
                if ((flags & 0x40u) != 0)
                {
                    if (body.Length - pos < 4) goto done;
                    turnSpeed = BinaryPrimitives.ReadSingleLittleEndian(body.Slice(pos));
                    pos += 4;
                }

                // Commands list: actions/emotes/attacks. Guard against a
                // malformed numCommands by capping at a sane max.
                if (numCommands > 0 && numCommands < 1024)
                {
                    commands = new List<CreateObject.MotionItem>((int)numCommands);
                    for (int i = 0; i < numCommands; i++)
                    {
                        if (body.Length - pos < 8) break;
                        ushort cmd = BinaryPrimitives.ReadUInt16LittleEndian(body.Slice(pos));
                        ushort seq = BinaryPrimitives.ReadUInt16LittleEndian(body.Slice(pos + 2));
                        float speed = BinaryPrimitives.ReadSingleLittleEndian(body.Slice(pos + 4));
                        pos += 8;
                        commands.Add(new CreateObject.MotionItem(cmd, seq, speed));
                    }
                }
                done:;
            }
            else if (movementType is 6 or 7)
            {
                TryParseMoveToPayload(
                    body,
                    pos,
                    movementType,
                    out moveToParameters,
                    out moveToSpeed,
                    out moveToRunRate,
                    out moveToPath);
            }

            return new Parsed(guid, new CreateObject.ServerMotionState(
                currentStyle, forwardCommand, forwardSpeed, commands,
                sidestepCommand, sidestepSpeed, turnCommand, turnSpeed,
                movementType,
                moveToParameters,
                moveToSpeed,
                moveToRunRate,
                moveToPath),
                instanceSequence, movementSequence, serverControlSequence, isAutonomous);
        }
        catch
        {
            return null;
        }
    }

    private static bool TryParseMoveToPayload(
        ReadOnlySpan<byte> body,
        int pos,
        byte movementType,
        out uint? movementParameters,
        out float? speed,
        out float? runRate,
        out CreateObject.MoveToPathData? path)
    {
        movementParameters = null;
        speed = null;
        runRate = null;
        path = null;

        // Retail MovementManager::PerformMovement (0x00524440) consumes
        // MoveToObject/MoveToPosition as:
        //   [object guid, for MoveToObject only]
        //   Origin(cell + xyz)
        //   MovementParameters::UnPackNet (0x0052AC50): flags, distance,
        //     min, fail, speed, walk/run threshold, desired heading
        //   f32 runRate copied into CMotionInterp::my_run_rate.
        //
        // Phase L.1c (2026-04-28): the full path payload is now retained on
        // <see cref="CreateObject.MoveToPathData"/> so the per-tick remote
        // body driver can steer toward Origin instead of holding velocity at
        // zero between sparse UpdatePosition snaps. The 882a07c stabilizer
        // was deliberately conservative because we only had speed+runRate;
        // with the rest of the packet captured, the body solver has full
        // path data and can run faithfully.
        uint? targetGuid = null;
        if (movementType == 6)
        {
            if (body.Length - pos < 4) return false;
            targetGuid = BinaryPrimitives.ReadUInt32LittleEndian(body.Slice(pos));
            pos += 4;
        }

        if (body.Length - pos < 16 + 28 + 4) return false;

        uint originCellId = BinaryPrimitives.ReadUInt32LittleEndian(body.Slice(pos));
        pos += 4;
        float originX = BinaryPrimitives.ReadSingleLittleEndian(body.Slice(pos));
        pos += 4;
        float originY = BinaryPrimitives.ReadSingleLittleEndian(body.Slice(pos));
        pos += 4;
        float originZ = BinaryPrimitives.ReadSingleLittleEndian(body.Slice(pos));
        pos += 4;

        movementParameters = BinaryPrimitives.ReadUInt32LittleEndian(body.Slice(pos));
        pos += 4;
        float distanceToObject = BinaryPrimitives.ReadSingleLittleEndian(body.Slice(pos));
        pos += 4;
        float minDistance = BinaryPrimitives.ReadSingleLittleEndian(body.Slice(pos));
        pos += 4;
        float failDistance = BinaryPrimitives.ReadSingleLittleEndian(body.Slice(pos));
        pos += 4;
        speed = BinaryPrimitives.ReadSingleLittleEndian(body.Slice(pos));
        pos += 4;
        float walkRunThreshold = BinaryPrimitives.ReadSingleLittleEndian(body.Slice(pos));
        pos += 4;
        float desiredHeading = BinaryPrimitives.ReadSingleLittleEndian(body.Slice(pos));
        pos += 4;
        runRate = BinaryPrimitives.ReadSingleLittleEndian(body.Slice(pos));

        path = new CreateObject.MoveToPathData(
            targetGuid,
            originCellId,
            originX,
            originY,
            originZ,
            distanceToObject,
            minDistance,
            failDistance,
            walkRunThreshold,
            desiredHeading);
        return true;
    }
}