using Sandbox;

namespace VeloX;

public abstract partial class VeloXBase
{

	private Vector3 linForce;
	private Vector3 angForce;
	protected const float BrakeForce = 4500f;
	protected const float HandbrakeForce = 35000f;
	protected void PhysicsSimulate()
	{
		if ( Body.Sleeping && Input.AnalogMove.x == 0 )
			return;

		var drag = AngularDrag;
		var mass = Body.Mass;
		var angVel = Body.AngularVelocity;

		linForce.x = 0;
		linForce.y = 0;
		linForce.z = 0;

		angForce = angForce.WithX( angVel.x * drag.x * mass * 1000 );
		angForce = angForce.WithY( angVel.y * drag.y * mass * 1000 );
		angForce = angForce.WithZ( angVel.z * drag.z * mass * 1000 );
		if ( Wheels.Count > 0 )
		{
			Vector3 vehVel = Body.Velocity;
			Vector3 vehAngVel = Body.AngularVelocity;

			var dt = Time.Delta;
			CombinedLoad = 0;
			foreach ( var v in Wheels )
				CombinedLoad += v.Fz;
			foreach ( var v in Wheels )
			{
				if ( v.IsFront )
				{
					v.BrakeTorque = SwappedBrakes * BrakeForce * 1.3f;
				}
				else
				{
					v.BrakeTorque = SwappedBrakes * BrakeForce * 0.7f;
					v.BrakeTorque += Handbrake * HandbrakeForce;

				}

				if ( TotalSpeed < 1 && !AnyInput )
				{
					v.BrakeTorque = HandbrakeForce;
				}

				v.Update( this, in dt );
				v.DoPhysics( in dt );
			}



			Body.Velocity = vehVel;
			Body.AngularVelocity = vehAngVel;
		}

		Body.ApplyForce( linForce );
		Body.ApplyTorque( angForce );


	}
}