using Sandbox;

namespace VeloX;
public static class PhysicsExtensions
{
	/// <summary>
	/// Calculates the linear and angular velocities on the center of mass for an offset impulse.
	/// </summary>
	/// <param name="physObj">The physics object</param>
	/// <param name="impulse">The impulse acting on the object in kg*units/s (World frame)</param>
	/// <param name="position">The location of the impulse in world coordinates</param>
	/// <returns>
	/// Vector1: Linear velocity from the impulse (World frame)
	/// Vector2: Angular velocity from the impulse (Local frame)
	/// </returns>
	public static (Vector3 LinearVelocity, Vector3 AngularVelocity) CalculateVelocityOffset( this PhysicsBody physObj, Vector3 impulse, Vector3 position )
	{
		if ( !physObj.IsValid() || !physObj.MotionEnabled )
			return (Vector3.Zero, Vector3.Zero);


		Vector3 linearVelocity = impulse / physObj.Mass;

		Vector3 centerOfMass = physObj.MassCenter;
		Vector3 relativePosition = position - centerOfMass;
		Vector3 torque = relativePosition.Cross( impulse );

		Rotation bodyRotation = physObj.Transform.Rotation;
		Vector3 localTorque = bodyRotation.Inverse * torque;

		Vector3 localInverseInertia = physObj.Inertia.Inverse;

		Vector3 localAngularVelocity = new(
			localTorque.x * localInverseInertia.x,
			localTorque.y * localInverseInertia.y,
			localTorque.z * localInverseInertia.z );

		return (linearVelocity, localAngularVelocity);
	}

	/// <summary>
	/// Calculates the linear and angular impulses on the object's center of mass for an offset impulse.
	/// </summary>
	/// <param name="physObj">The physics object</param>
	/// <param name="impulse">The impulse acting on the object in kg*units/s (World frame)</param>
	/// <param name="position">The location of the impulse in world coordinates</param>
	/// <returns>
	/// Vector1: Linear impulse on center of mass (World frame)
	/// Vector2: Angular impulse on center of mass (Local frame)
	/// </returns>
	public static (Vector3 LinearImpulse, Vector3 AngularImpulse) CalculateForceOffset(
		this PhysicsBody physObj,
		Vector3 impulse,
		Vector3 position )
	{
		if ( !physObj.IsValid() || !physObj.MotionEnabled )
		{
			return (Vector3.Zero, Vector3.Zero);
		}

		// 1. Linear impulse is the same as the input impulse (conservation of momentum)
		Vector3 linearImpulse = impulse;

		// 2. Calculate angular impulse (torque) from the offset force
		// τ = r × F (cross product of position relative to COM and force)
		Vector3 centerOfMass = physObj.MassCenter;
		Vector3 relativePosition = position - centerOfMass;
		Vector3 worldAngularImpulse = relativePosition.Cross( impulse );

		// Convert angular impulse to local space (since we'll use it with LocalInertia)
		Rotation bodyRotation = physObj.Transform.Rotation;
		Vector3 localAngularImpulse = bodyRotation.Inverse * worldAngularImpulse;

		return (linearImpulse, localAngularImpulse);
	}

}