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);
	}

	// The horizontal direction a head faces, robust past vertical. The visor (-Z) projected flat
	// flips 180° once you look down (or up) beyond vertical — chin to chest while crouching, or
	// looking at your own feet — and everything built on it (torso yaw, stick direction) turned
	// around. Looking down, the top of the head (+Y) points the way you face; looking up it points
	// back. So: when the flat visor disagrees with the flat crown, the head is past vertical and the
	// visor is reversed; near vertical the crown alone carries the direction. Zero if undefined.
	public static Vector3 HeadFacing(Basis head)
	{
		Vector3 forward = -head.Z, up = head.Y;
		Vector3 visor = new(forward.X, 0f, forward.Z);
		Vector3 crown = new Vector3(up.X, 0f, up.Z) * -forward.Y;
		if (visor.Dot(crown) < 0f)
			visor = -visor;
		Vector3 f = visor + crown * 0.5f;
		return f.LengthSquared() > 1e-6f ? f.Normalized() : Vector3.Zero;
	}

	// Yaw (about +Y, Godot convention: 0 = facing -Z) of a horizontal direction.
	public static float YawOf(Vector3 flat) => Mathf.Atan2(-flat.X, -flat.Z);

	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;
	}
}