using Sandbox;
using System;
using System.Numerics;

namespace VeloX;
public static class PhysicsExtensions
{
	public static Vector3 Transform( this Vector3 value, Quaternion rotation )
	{
		float x2 = rotation.X + rotation.X;
		float y2 = rotation.Y + rotation.Y;
		float z2 = rotation.Z + rotation.Z;

		float wx2 = rotation.W * x2;
		float wy2 = rotation.W * y2;
		float wz2 = rotation.W * z2;
		float xx2 = rotation.X * x2;
		float xy2 = rotation.X * y2;
		float xz2 = rotation.X * z2;
		float yy2 = rotation.Y * y2;
		float yz2 = rotation.Y * z2;
		float zz2 = rotation.Z * z2;

		return new Vector3(
			value.x * (1.0f - yy2 - zz2) + value.y * (xy2 - wz2) + value.z * (xz2 + wy2),
			value.x * (xy2 + wz2) + value.y * (1.0f - xx2 - zz2) + value.z * (yz2 - wx2),
			value.x * (xz2 - wy2) + value.y * (yz2 + wx2) + value.z * (1.0f - xx2 - yy2)
		);
	}
	/// <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 r = position - physObj.MassCenter;


		// Calculate torque impulse in world frame: τ = r × impulse
		Vector3 torqueImpulseWorld = r.Cross( impulse );
		Rotation worldToLocal = physObj.Rotation.Inverse;
		Vector3 torqueImpulseLocal = torqueImpulseWorld.Transform( worldToLocal );

		var InverseInertiaDiagLocal = physObj.Inertia.Inverse;

		// Compute angular velocity change in rad/s (local frame)
		Vector3 angularVelocityRadLocal = new(
			InverseInertiaDiagLocal.x * torqueImpulseLocal.x,
			InverseInertiaDiagLocal.y * torqueImpulseLocal.y,
			InverseInertiaDiagLocal.z * torqueImpulseLocal.z
		);
		const float radToDeg = 180f / MathF.PI;
		Vector3 angularVelocityDegLocal = angularVelocityRadLocal * radToDeg;

		return (linearVelocity, angularVelocityDegLocal);
	}

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

}