using Sandbox;
using System;

namespace VeloX.Powertrain;

public class Clutch : PowertrainComponent
{
	[Property] public override float Inertia { get; set; } = 0.002f;
	[Property] public float SlipTorque { get; set; } = 1000f;

	public float Pressing { get; set; } = 1; // todo

	public override float QueryInertia()
	{
		if ( !HasOutput )
			return Inertia;

		return Inertia + Output.QueryInertia() * Pressing;
	}

	public override float QueryAngularVelocity( float angularVelocity )
	{
		this.angularVelocity = angularVelocity;

		if ( !HasOutput )
			return angularVelocity;


		float outputW = Output.QueryAngularVelocity( angularVelocity ) * Pressing;
		float inputW = angularVelocity * (1 - Pressing);

		return outputW + inputW;
	}


	public override float ForwardStep( float torque, float inertia )
	{
		if ( !HasOutput )
			return torque;

		Torque = Math.Clamp( torque, -SlipTorque, SlipTorque );

		Torque = torque * (1 - (1 - MathF.Pow( Pressing, 0.3f )));

		float returnTorque = Output.ForwardStep( Torque, inertia * Pressing + Inertia ) * Pressing;

		return Math.Clamp( returnTorque, -SlipTorque, SlipTorque );
	}
}