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 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>
	/// <param name="LinearVelocity">Linear velocity on center of mass (World frame)</param>
	/// <param name="AngularVelocity">Angular velocity on center of mass (World frame)</param>
	public static void CalculateVelocityOffset(
		this Rigidbody physObj,
		Vector3 impulse,
		Vector3 position,
		out Vector3 LinearVelocity,
		out Vector3 AngularVelocity )
	{
		if ( !physObj.IsValid() || !physObj.MotionEnabled )
		{
			LinearVelocity = 0;
			AngularVelocity = 0;
			return;
		}

		Vector3 com = physObj.WorldTransform.PointToWorld( physObj.MassCenter );
		Rotation bodyRot = physObj.PhysicsBody.Rotation;

		Vector3 r = position - com;
		Vector3 torque = Vector3.Cross( r, impulse );
		Vector3 torqueLocal = bodyRot.Inverse * torque;
		Vector3 angularVelocityLocal = torqueLocal * physObj.PhysicsBody.Inertia.Inverse;
		AngularVelocity = bodyRot * angularVelocityLocal;
		LinearVelocity = impulse * (1 / physObj.Mass);
	}

	/// <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>
	/// <param name="LinearImpulse">Linear impulse on center of mass (World frame)</param>
	/// <param name="AngularImpulse">Angular impulse on center of mass (World frame)</param>
	public static void CalculateForceOffset(
		this Rigidbody physObj,
		Vector3 impulse,
		Vector3 position,
		out Vector3 LinearImpulse,
		out Vector3 AngularImpulse )
	{
		if ( !physObj.IsValid() || !physObj.MotionEnabled )
		{
			LinearImpulse = 0;
			AngularImpulse = 0;
			return;
		}

		Vector3 com = physObj.WorldTransform.PointToWorld( physObj.MassCenter );
		Rotation bodyRot = physObj.PhysicsBody.Rotation;

		Vector3 r = position - com;
		Vector3 torque = Vector3.Cross( r, impulse );
		Vector3 torqueLocal = bodyRot.Inverse * torque;
		Vector3 angularImpulseLocal = torqueLocal * physObj.PhysicsBody.Inertia.Inverse;
		AngularImpulse = bodyRot * angularImpulseLocal;
		LinearImpulse = impulse;
	}

}