namespace Machines.Player;

/// <summary>
/// Shared steering math for bot brains
/// </summary>
public static class BotSteering
{
	/// <summary>
	/// Raw steer (-1..1) to rotate <paramref name="forward"/> toward <paramref name="toTarget"/>. Ignores height.
	/// </summary>
	public static float RawSteer( Vector3 forward, Vector3 toTarget, float sharpness )
	{
		var cross = Vector3.Cross( forward, toTarget );
		return (cross.z * sharpness).Clamp( -1f, 1f );
	}

	/// <summary>
	/// Feedforward steer (-1..1) to hold a track curvature at speed without reactive correction
	/// </summary>
	public static float CurvatureFeedforward( float curvature, float speed, float turnRate, float maxSpeed )
	{
		if ( curvature < 0.00001f || speed < 1f || turnRate < 1f )
			return 0f;

		// Yaw rate to maintain curvature at this speed
		var neededYawRate = speed * curvature * (180f / MathF.PI);

		// Mirror CarMovement's turn modifiers
		var speedFactor = MathF.Min( 1f, speed / 100f );
		var turnFalloff = MathF.Max( 0.5f, 1f - (speed / maxSpeed) * 0.8f );

		var effectiveTurnRate = turnRate * turnFalloff * speedFactor;
		if ( effectiveTurnRate < 1f )
			return 0f;

		return (neededYawRate / effectiveTurnRate).Clamp( -1f, 1f );
	}
}