using System.Numerics;

namespace AcDream.Core.Physics;

/// <summary>
/// Retail frame-orientation helpers used by physics presentation.
/// </summary>
public static class RetailFrameMath
{
    /// <summary>
    /// Points the frame's local +Y axis along a world-space direction with
    /// zero roll. This is the observable contract of retail
    /// <c>Frame::set_vector_heading</c> at <c>0x00535DB0</c>: normalize the
    /// full three-dimensional vector, derive compass yaw plus elevation, and
    /// replace the frame rotation. A zero-length vector leaves the prior
    /// orientation unchanged.
    /// </summary>
    public static Quaternion SetVectorHeading(
        Quaternion currentOrientation,
        Vector3 direction)
    {
        float lengthSquared = direction.LengthSquared();
        if (lengthSquared < PhysicsGlobals.EpsilonSq || !float.IsFinite(lengthSquared))
            return currentOrientation;

        Vector3 forward = direction / MathF.Sqrt(lengthSquared);

        // Retail's zero-roll Euler construction is equivalent to choosing a
        // horizontal right axis from the compass heading, then deriving the
        // remaining up axis. At a perfectly vertical heading atan2(0, 0)
        // resolves to zero, so retain +X as the deterministic right axis.
        Vector3 right;
        if (forward.X == 0f && forward.Y == 0f)
        {
            right = Vector3.UnitX;
        }
        else
        {
            // Normalize the horizontal pair without squaring the original
            // components. That preserves every non-zero compass component,
            // including a nearly vertical vector below PhysicsGlobals.EPSILON,
            // just as retail's atan2-based construction does.
            float scale = MathF.Max(MathF.Abs(forward.X), MathF.Abs(forward.Y));
            float x = forward.X / scale;
            float y = forward.Y / scale;
            float horizontalLength = MathF.Sqrt((x * x) + (y * y));
            right = new Vector3(y / horizontalLength, -x / horizontalLength, 0f);
        }

        Vector3 up = Vector3.Normalize(Vector3.Cross(right, forward));

        // System.Numerics uses row-vector transforms. Each row below is the
        // world direction of one local basis axis: +X right, +Y heading,
        // +Z up.
        var rotation = new Matrix4x4(
            right.X,   right.Y,   right.Z,   0f,
            forward.X, forward.Y, forward.Z, 0f,
            up.X,      up.Y,      up.Z,      0f,
            0f,        0f,        0f,        1f);

        return Quaternion.Normalize(Quaternion.CreateFromRotationMatrix(rotation));
    }
}