using Godot;

// Shared math for the chase-driven player bodies (hands, torso, arm segments).
public static class BodyMath
{
    // Shortest-arc rotation taking `current` to `target`, as axis * angle (radians), world space.
    // Never use Quaternion.GetAngle()/GetAxis() for servo errors: GetAngle() is 2*acos(W) and
    // returns NaN when float rounding puts W at 1.0000001 (the Stage-1 chest collapse), and
    // GetAxis() returns the raw un-normalized xyz once W > 1 - 1e-6 (servo gain collapses
    // ~1000x inside 0.16 deg). atan2 is exact near zero and can't leave its domain.
    public static Vector3 RotationError(Quaternion target, Quaternion current)
    {
        Quaternion q = target * current.Inverse();
        if (q.W < 0f)
            q = new Quaternion(-q.X, -q.Y, -q.Z, -q.W);
        Vector3 v = new(q.X, q.Y, q.Z);
        float s = v.Length();
        if (s < 1e-9f)
            return Vector3.Zero;
        return v * (2f * Mathf.Atan2(s, q.W) / s);
    }

    public static bool IsFinite(RigidBody3D body) =>
        body.GlobalTransform.IsFinite() && body.LinearVelocity.IsFinite() && body.AngularVelocity.IsFinite();

    // Escape hatch: a non-finite body is unrecoverable for the solver and floods the renderer
    // with !v.is_finite() errors — put it back on its target, motionless. Returns true if reset.
    public static bool ResetIfNonFinite(RigidBody3D body, Transform3D target, string who)
    {
        if (IsFinite(body))
            return false;
        body.GlobalTransform = target;
        body.LinearVelocity = Vector3.Zero;
        body.AngularVelocity = Vector3.Zero;
        GD.PushWarning($"{who}: non-finite body state, reset to target");
        return true;
    }
}