using Sandbox;
using System;

namespace VeloX.Powertrain;

[Category( "VeloX/Powertrain/Differential" )]
public abstract class BaseDifferential : PowertrainComponent
{
	[Property] public float FinalDrive { get; set; } = 3.392f;
	[Property] public override float Inertia { get; set; } = 0.01f;
	//[Property] public float CoastRamp { get; set; } = 1f;
	//[Property] public float PowerRamp { get; set; } = 1f;
	//[Property] public float Stiffness { get; set; } = 0.1f;
	//[Property] public float SlipTorque { get; set; } = 0f;
	//[Property] public float SteerLock { get; set; } = 45f;


	/// <summary>
	///     The PowertrainComponent this component will output to.
	/// </summary>
	[Property]
	public PowertrainComponent OutputB
	{
		get => _outputb;
		set
		{
			if ( value == this )
			{
				_outputb = null;
				return;
			}

			_outputb = value;
			if ( _outputb != null )
				_outputb.Input = this;
		}

	}
	private PowertrainComponent _outputb;

	public override bool HasOutput => Output.IsValid() && OutputB.IsValid();

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

		if ( !HasOutput )
			return angularVelocity;

		float aW = Output.QueryAngularVelocity( angularVelocity );
		float bW = OutputB.QueryAngularVelocity( angularVelocity );

		return (aW + bW) * FinalDrive * 0.5f;
	}

	public abstract void SplitTorque( float aW, float bW, float aI, float bI, out float tqA, out float tqB );

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

		float aW = Output.QueryAngularVelocity( angularVelocity );
		float bW = OutputB.QueryAngularVelocity( angularVelocity );

		float aI = Output.QueryInertia();
		float bI = OutputB.QueryInertia();

		Torque = torque * FinalDrive;

		SplitTorque( aW, bW, aI, bI, out float tqA, out float tqB );

		tqA = Output.ForwardStep( tqA, inertia * 0.5f * MathF.Pow( FinalDrive, 2 ) + aI );
		tqB = OutputB.ForwardStep( tqB, inertia * 0.5f * MathF.Pow( FinalDrive, 2 ) + bI );

		return tqA + tqB;
	}

}