acdream/src/AcDream.Core/Physics/Motion/MoveToMath.cs

using System;
using System.Numerics;

namespace AcDream.Core.Physics.Motion;

/// <summary>
/// R4-V1 — pure-math free functions consumed by the (future) MoveToManager
/// port: <c>heading_diff</c>, <c>heading_greater</c>, <c>Position::heading</c>
/// / <c>Frame::get_heading</c> / <c>Frame::set_heading</c>, and
/// <c>Position::cylinder_distance</c>. No GL/App dependency — Core-only,
/// per the Code Structure Rules. NAME WATCH: this file (not
/// <c>MoveToManager.cs</c>, per r4-port-plan.md §3 "New code target") is the
/// R4-V1 deliverable; the manager itself is R4-V2.
/// </summary>
public static class MoveToMath
{
    /// <summary>
    /// Universal heading/distance epsilon (same literal as R3's A5/A6 —
    /// r4-moveto-decomp.md §12 constants inventory).
    /// </summary>
    public const float Epsilon = 0.000199999995f;

    /// <summary>
    /// Retail <c>heading_diff</c> (<c>0x00528fb0</c>, free function, raw
    /// 306327-306347), PINNED by direct disassembly of the PDB-matched
    /// retail binary (ghidra-confirmations.md §P3 — the strongest evidence
    /// tier in the R4 pin set):
    /// <code>
    /// d = h1 - h2;
    /// if (fabs(h1 - h2) &lt; F_EPSILON) d = 0;
    /// if (d &lt; -F_EPSILON) d += 360;
    /// if (F_EPSILON &lt; d &amp;&amp; turnCmd != TurnRight) d = 360 - d;   // the mirror
    /// return d;
    /// </code>
    /// The mirror gates on the turn command NOT being TurnRight
    /// (0x6500000d) — TurnLeft (and any other command) measures the
    /// COMPLEMENTARY angle. This CONTRADICTS r4-moveto-decomp.md §5g's
    /// "arg3 UNUSED" claim, which the Ghidra disassembly pin overrides
    /// (V0-pins.md §P3 adjudication). Call sites: <c>BeginTurnToHeading</c>
    /// passes the CONSTANT TurnRight (mirror explicitly disabled — the
    /// direction pick stays the ≤180 test elsewhere); <c>HandleTurnToHeading</c>
    /// passes the LIVE <c>current_command</c> (can be TurnLeft).
    /// </summary>
    /// <param name="h1">First heading, degrees.</param>
    /// <param name="h2">Second heading, degrees.</param>
    /// <param name="turnCmd">The active turn command id — gates the mirror.</param>
    /// <returns>Normalized heading difference, degrees.</returns>
    public static float HeadingDiff(float h1, float h2, uint turnCmd)
    {
        float d = h1 - h2;
        if (MathF.Abs(h1 - h2) < Epsilon)
        {
            d = 0f;
        }
        if (d < -Epsilon)
        {
            d += 360f;
        }
        if (Epsilon < d && turnCmd != MotionCommand.TurnRight)
        {
            d = 360f - d;
        }
        return d;
    }

    /// <summary>
    /// Retail <c>heading_greater</c> (<c>00528f60</c>, free function, raw
    /// 306281-306323), verbatim per r4-moveto-decomp.md §5f:
    /// <code>
    /// if (fabs(a - b) &gt; 180) greater = (b &gt; a);   // wrapped case: compare flipped
    /// else                     greater = (a &gt; b);
    /// if (turnCmd == TurnRight) return greater;
    /// return !greater;                           // TurnLeft (and any other cmd): inverted
    /// </code>
    /// "Has the turn passed the target heading" — direction-aware,
    /// 360°-wrap-aware. The visible TurnRight-arg idiom: the gate is
    /// <c>== TurnRight</c> (not <c>!= TurnRight</c> as in
    /// <see cref="HeadingDiff"/>'s mirror) — every OTHER command inverts,
    /// not just TurnLeft specifically.
    /// </summary>
    public static bool HeadingGreater(float a, float b, uint turnCmd)
    {
        bool greater = MathF.Abs(a - b) > 180f
            ? b > a
            : a > b;

        return turnCmd == MotionCommand.TurnRight ? greater : !greater;
    }

    /// <summary>
    /// Retail <c>Position::heading(from, to)</c> (<c>0x005a9520</c>, raw
    /// 438288-438290), PINNED per V0-pins.md §P5: compass degrees, 0 =
    /// North (+Y), 90 = East (+X), CLOCKWISE, range [0,360).
    /// <code>
    /// heading(from, to) = (450 - atan2Deg(dy, dx)) % 360
    /// </code>
    /// Golden cardinals: N(0,+1)→0, E(+1,0)→90, S(0,-1)→180, W(-1,0)→270.
    /// Horizontal (X/Y) only — Z (height) does not participate, matching
    /// retail's compass-heading semantics. An in-tree twin of this formula
    /// already exists at <c>SceneryHelpers.cs:75</c> (render-side,
    /// independently verified — not reused directly to keep this file
    /// GL-free per the Code Structure Rules, but the formula is identical).
    /// </summary>
    public static float PositionHeading(Vector3 from, Vector3 to)
    {
        float dx = to.X - from.X;
        float dy = to.Y - from.Y;
        float headingDeg = 450f - MathF.Atan2(dy, dx) * (180f / MathF.PI);
        headingDeg %= 360f;
        if (headingDeg < 0f) headingDeg += 360f;
        return headingDeg;
    }

    /// <summary>
    /// Retail <c>Frame::get_heading</c> (<c>0x00535760</c>, raw 319781) —
    /// extracts the compass heading (P5 convention) from a body orientation
    /// quaternion. This method uses the scalar bridge derived from acdream's
    /// body convention
    /// (<c>PlayerMovementController.cs:1022-1025</c>: <c>Orientation =
    /// AxisAngle(Z, Yaw - PI/2)</c>, local-forward = +Y, Yaw=0 faces +X):
    /// world-forward = <c>(cos Yaw, sin Yaw)</c>, so
    /// <c>YawDeg = atan2Deg(forward.Y, forward.X)</c> and
    /// <c>heading = (90 - YawDeg) mod 360</c> — the exact inverse of
    /// <see cref="SetHeading"/>. Identity quaternion (Yaw=PI/2, i.e. facing
    /// +Y/North) → heading 0, matching P5's "identity quaternion faces
    /// heading 0" pin.
    /// </summary>
    public static float GetHeading(Quaternion orientation)
    {
        var forward = Vector3.Transform(new Vector3(0f, 1f, 0f), orientation);
        float yawDeg = MathF.Atan2(forward.Y, forward.X) * (180f / MathF.PI);
        float headingDeg = 90f - yawDeg;
        headingDeg %= 360f;
        if (headingDeg < 0f) headingDeg += 360f;
        return headingDeg;
    }

    /// <summary>
    /// Retail <c>Frame::set_heading</c> (<c>0x00535e40</c>, raw
    /// 320055-320066) — builds a body orientation quaternion facing
    /// <paramref name="headingDeg"/> (P5 compass convention), preserving
    /// acdream's body-orientation convention (rotation about world Z only;
    /// <paramref name="baseOrientation"/>'s pitch/roll, if any, is
    /// discarded — matching retail's <c>set_heading</c>, which is a pure
    /// yaw-about-Z setter). Exact inverse of <see cref="GetHeading"/>:
    /// <c>YawDeg = 90 - headingDeg</c>, then <c>Orientation =
    /// AxisAngle(Z, Yaw - PI/2)</c> per
    /// <c>PlayerMovementController.cs:1025</c>'s convention.
    /// </summary>
    /// <param name="baseOrientation">Unused beyond signature parity with
    /// the render-side <c>SceneryHelpers.SetHeading</c> twin — retail's
    /// <c>set_heading</c> is a pure yaw-about-Z setter with no dependency
    /// on the prior orientation's roll/pitch component in the body-frame
    /// convention this port uses.</param>
    /// <param name="headingDeg">Desired compass heading, degrees.</param>
    public static Quaternion SetHeading(Quaternion baseOrientation, float headingDeg)
    {
        _ = baseOrientation;
        float yawDeg = 90f - headingDeg;
        float yaw = yawDeg * (MathF.PI / 180f);
        return Quaternion.CreateFromAxisAngle(Vector3.UnitZ, yaw - MathF.PI / 2f);
    }

    /// <summary>
    /// R4-V5: scalar projection of <see cref="GetHeading"/>. The local
    /// player's <c>Yaw</c> property exposes this horizontal projection while
    /// its complete body quaternion remains authoritative. Same P5 bridge:
    /// <c>heading = (90 - yawDeg) mod 360</c>.
    /// </summary>
    public static float HeadingFromYaw(float yawRad)
    {
        float headingDeg = 90f - yawRad * (180f / MathF.PI);
        headingDeg %= 360f;
        if (headingDeg < 0f) headingDeg += 360f;
        return headingDeg;
    }

    /// <summary>
    /// R4-V5: exact inverse of <see cref="HeadingFromYaw"/> — the scalar
    /// bridge used by an explicit <c>Frame::set_heading</c> operation. This
    /// operation deliberately replaces pitch/roll; ordinary frame composition
    /// does not pass through this projection.
    /// Returns radians wrapped to [-π, π] matching the controller's own
    /// wrap discipline.
    /// </summary>
    public static float YawFromHeading(float headingDeg)
    {
        float yaw = (90f - headingDeg) * (MathF.PI / 180f);
        while (yaw > MathF.PI) yaw -= 2f * MathF.PI;
        while (yaw < -MathF.PI) yaw += 2f * MathF.PI;
        return yaw;
    }

    /// <summary>
    /// Retail <c>Position::cylinder_distance</c>, the pure-math shape
    /// consumed by <c>MoveToManager::GetCurrentDistance</c>
    /// (<c>005291b0</c>, r4-moveto-decomp.md §5a) when <c>use_spheres</c>
    /// (wire bit 0x400) is set — object moves use edge-to-edge cylinder
    /// distance; position moves use plain center (Euclidean) distance
    /// instead (not ported here — <c>Vector3.Distance</c> already covers
    /// it). Binary Ninja garbles the x87 result plumbing, but the complete
    /// branch structure is visible in
    /// <c>Position::cylinder_distance @ 0x005A97F0</c> and is independently
    /// preserved by ACE <c>Physics.Common.Position.CylinderDistance</c>:
    /// subtract both radii from the full 3D center distance, compute the
    /// signed vertical gap between the two cylinders, then combine gaps only
    /// when their signs agree. Overlap is therefore a negative distance
    /// rather than a value clamped to zero.
    /// </summary>
    public static float CylinderDistance(
        float ownRadius, float ownHeight, Vector3 ownPos,
        float targetRadius, float targetHeight, Vector3 targetPos)
    {
        float radialGap = Vector3.Distance(ownPos, targetPos)
            - (ownRadius + targetRadius);
        float verticalGap = ownPos.Z <= targetPos.Z
            ? targetPos.Z - (ownPos.Z + ownHeight)
            : ownPos.Z - (targetPos.Z + targetHeight);

        if (verticalGap > 0f && radialGap > 0f)
            return MathF.Sqrt(verticalGap * verticalGap + radialGap * radialGap);
        if (verticalGap < 0f && radialGap < 0f)
            return -MathF.Sqrt(verticalGap * verticalGap + radialGap * radialGap);
        return radialGap;
    }

    /// <summary>
    /// Retail <c>Position::cylinder_distance_no_z</c> — the <b>signed</b>
    /// horizontal (X/Y) edge-to-edge distance between two cylinders:
    /// <c>centerDist − ownRadius − targetRadius</c>. Unlike
    /// <see cref="CylinderDistance"/> (the arrival-gate variant, which CLAMPS at
    /// 0), this variant is NOT clamped — overlapping cylinders report a NEGATIVE
    /// value. <c>StickyManager::adjust_offset</c> (0x00555430) relies on the
    /// sign: when the follower is inside the desired 0.3 m stick gap the
    /// distance goes negative, the per-tick delta inverts, and the mover backs
    /// off to restore the gap (ACE StickyManager.cs:156).
    /// </summary>
    public static float CylinderDistanceNoZ(
        float ownRadius, Vector3 ownPos, float targetRadius, Vector3 targetPos)
    {
        float dx = targetPos.X - ownPos.X;
        float dy = targetPos.Y - ownPos.Y;
        float centerDist = MathF.Sqrt(dx * dx + dy * dy);
        return centerDist - ownRadius - targetRadius;
    }

    /// <summary>
    /// Retail <c>AC1Legacy::Vector3::normalize_check_small</c> — normalize
    /// <paramref name="v"/> in place, returning <c>true</c> if the vector was
    /// too small to normalize (near-zero) and leaving it unchanged in that
    /// case. Consumed by <c>StickyManager::adjust_offset</c> (don't chase
    /// jitter when already at the target) and
    /// <see cref="TargetManager.ReceiveUpdate"/> (interpolated-heading
    /// fallback). A public shared twin of the private helper in
    /// <c>ParticleSystem</c>; same 1e-8 near-zero length guard.
    /// </summary>
    /// <returns><c>true</c> = too small (left unchanged); <c>false</c> =
    /// normalized.</returns>
    public static bool NormalizeCheckSmall(ref Vector3 v)
    {
        float length = v.Length();
        if (length < 1e-8f)
            return true;
        v /= length;
        return false;
    }

    /// <summary>
    /// Retail <c>Position::globaltolocalvec</c> — rotate a WORLD-space vector
    /// into a frame's LOCAL coordinates by the inverse of the frame's
    /// orientation. Consumed by <c>StickyManager::adjust_offset</c>
    /// (0x00555430) to express the self→target offset in the mover's own frame
    /// before flattening Z and steering. Pure rotation (no translation) —
    /// <paramref name="worldVec"/> is a direction/offset, not a point.
    /// </summary>
    public static Vector3 GlobalToLocalVec(Quaternion frameOrientation, Vector3 worldVec)
        => Vector3.Transform(worldVec, Quaternion.Conjugate(frameOrientation));

    /// <summary>
    /// Landblock-local wire origin → world space (verbatim relocation from
    /// the deleted <c>RemoteMoveToDriver.OriginToWorld</c>, R4-V4): MoveTo /
    /// TurnTo packets carry positions block-local; acdream's streaming world
    /// re-centers on a live-center landblock grid.
    /// </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);
    }
}