using Sandbox;
using System;

namespace VeloX;


public partial class VeloXWheel
{
	/// <summary>
	/// Constant torque acting similar to brake torque.
	/// Imitates rolling resistance.
	/// </summary>
	[Property, Range( 0, 500 ), Sync] public float RollingResistanceTorque { get; set; } = 30f;

	/// <summary>
	/// The percentage this wheel is contributing to the total vehicle load bearing.
	/// </summary>
	public float LoadContribution { get; set; } = 0.25f;

	/// <summary>
	/// Maximum load the tire is rated for in [N]. 
	/// Used to calculate friction.Default value is adequate for most cars but
	/// larger and heavier vehicles such as semi trucks will use higher values.
	/// A good rule of the thumb is that this value should be 2x the Load
	/// while vehicle is stationary.
	/// </summary>
	[Property, Sync] public float LoadRating { get; set; } = 5400;


	/// <summary>
	/// The amount of torque returned by the wheel.
	/// Under no-slip conditions this will be equal to the torque that was input.
	/// When there is wheel spin, the value will be less than the input torque.
	/// </summary>
	public float CounterTorque { get; private set; }

	//[Property, Range( 0, 2 )] public float BrakeMult { get; set; } = 1f;
	public Friction ForwardFriction = new();
	public Friction SidewayFriction = new();
	public Vector3 FrictionForce;


	private Vector3 hitContactVelocity;
	private Vector3 hitForwardDirection;
	private Vector3 hitSidewaysDirection;

	public Vector3 ContactRight => hitSidewaysDirection;
	public Vector3 ContactForward => hitForwardDirection;

	[Sync]
	public float LongitudinalSlip
	{
		get => ForwardFriction.Slip;
		private set => ForwardFriction.Slip = value;
	}

	public float LongitudinalSpeed => ForwardFriction.Speed;
	public bool IsSkiddingLongitudinally => NormalizedLongitudinalSlip > 0.35f;
	public float NormalizedLongitudinalSlip => Math.Clamp( Math.Abs( LongitudinalSlip ), 0, 1 );


	[Sync]
	public float LateralSlip
	{
		get => SidewayFriction.Slip;
		private set => SidewayFriction.Slip = value;
	}

	public float LateralSpeed => SidewayFriction.Speed;
	public bool IsSkiddingLaterally => NormalizedLateralSlip > 0.35f;
	public float NormalizedLateralSlip => Math.Clamp( Math.Abs( LateralSlip ), 0, 1 );
	public bool IsSkidding => IsSkiddingLaterally || IsSkiddingLongitudinally;
	public float NormalizedSlip => (NormalizedLateralSlip + NormalizedLongitudinalSlip) / 2f;

	private void UpdateHitVariables()
	{
		if ( IsOnGround )
		{
			hitContactVelocity = Vehicle.Body.GetVelocityAtPoint( ContactPosition + Vehicle.Body.MassCenter );

			hitForwardDirection = ContactNormal.Cross( TransformRotationSteer.Right ).Normal;
			hitSidewaysDirection = Rotation.FromAxis( ContactNormal, 90f ) * hitForwardDirection;

			ForwardFriction.Speed = hitContactVelocity.Dot( hitForwardDirection ).InchToMeter();
			SidewayFriction.Speed = hitContactVelocity.Dot( hitSidewaysDirection ).InchToMeter();
		}
		else
		{
			ForwardFriction.Speed = 0f;
			SidewayFriction.Speed = 0f;
		}
	}


	private Vector3 lowSpeedReferencePosition;
	private bool lowSpeedReferenceIsSet;
	private Vector3 currentPosition;
	private Vector3 referenceError;
	private Vector3 correctiveForce;
	private void UpdateFriction( float dt )
	{
		var motorTorque = DriveTorque;
		var brakeTorque = BrakeTorque;

		float allWheelLoadSum = Vehicle.CombinedLoad;

		LoadContribution = allWheelLoadSum == 0 ? 1f : Fz / allWheelLoadSum;

		float mRadius = Radius;

		float invDt = 1f / dt;
		float invRadius = 1f / mRadius;
		float inertia = Inertia;
		float invInertia = 1f / Inertia;

		float loadClamped = Math.Clamp( Fz, 0, LoadRating );

		float forwardLoadFactor = loadClamped * 1.35f;
		float sideLoadFactor = loadClamped * 1.9f;

		float loadPercent = Math.Clamp( Fz / LoadRating, 0f, 1f );
		float slipLoadModifier = 1f - loadPercent * 0.4f;
		//DebugOverlay.Text( WorldPosition, SidewayFriction.Speed.ToString(), overlay: true );


		float mass = Vehicle.Body.Mass;
		float absForwardSpeed = Math.Abs( ForwardFriction.Speed );
		float forwardForceClamp = mass * LoadContribution * absForwardSpeed * invDt;
		float absSideSpeed = Math.Abs( SidewayFriction.Speed );
		float sideForceClamp = mass * LoadContribution * absSideSpeed * invDt;

		float forwardSpeedClamp = 1.5f * (dt / 0.005f);
		forwardSpeedClamp = Math.Clamp( forwardSpeedClamp, 1.5f, 10f );
		float clampedAbsForwardSpeed = Math.Max( absForwardSpeed, forwardSpeedClamp );

		// Calculate effect of camber on friction
		float camberFrictionCoeff = Math.Max( 0, Vehicle.WorldRotation.Up.Dot( ContactNormal ) );

		float peakForwardFrictionForce = forwardLoadFactor;
		float absCombinedBrakeTorque = Math.Max( 0, brakeTorque + RollingResistanceTorque );

		float signedCombinedBrakeTorque = absCombinedBrakeTorque * -Math.Sign( ForwardFriction.Speed );
		float signedCombinedBrakeForce = signedCombinedBrakeTorque * invRadius;
		float motorForce = motorTorque * invRadius;
		float forwardInputForce = motorForce + signedCombinedBrakeForce;
		float absMotorTorque = Math.Abs( motorTorque );
		float absBrakeTorque = Math.Abs( brakeTorque );

		float maxForwardForce = Math.Min( peakForwardFrictionForce, forwardForceClamp );

		maxForwardForce = absMotorTorque < absBrakeTorque ? maxForwardForce : peakForwardFrictionForce;
		ForwardFriction.Force = forwardInputForce > maxForwardForce ? maxForwardForce
			: forwardInputForce < -maxForwardForce ? -maxForwardForce : forwardInputForce;

		bool wheelIsBlocked = false;
		if ( IsOnGround )
		{
			float combinedWheelForce = motorForce + absCombinedBrakeTorque * invRadius * -Math.Sign( AngularVelocity );
			float wheelForceClampOverflow = 0;
			if ( (combinedWheelForce >= 0 && AngularVelocity < 0) || (combinedWheelForce < 0 && AngularVelocity > 0) )
			{
				float absWheelForceClamp = Math.Abs( AngularVelocity ) * inertia * invRadius * invDt;
				float absCombinedWheelForce = combinedWheelForce < 0 ? -combinedWheelForce : combinedWheelForce;
				float wheelForceDiff = absCombinedWheelForce - absWheelForceClamp;
				wheelForceClampOverflow = Math.Max( 0, wheelForceDiff ) * Math.Sign( combinedWheelForce );
				combinedWheelForce = Math.Clamp( combinedWheelForce, -absWheelForceClamp, absWheelForceClamp );
			}
			AngularVelocity += combinedWheelForce * mRadius * invInertia * dt;

			// Surface (corrective) force
			float noSlipAngularVelocity = ForwardFriction.Speed * invRadius;
			float angularVelocityError = AngularVelocity - noSlipAngularVelocity;

			float angularVelocityCorrectionForce = Math.Clamp( -angularVelocityError * inertia * invRadius * invDt, -maxForwardForce, maxForwardForce );
			if ( absMotorTorque < absBrakeTorque && Math.Abs( wheelForceClampOverflow ) > Math.Abs( angularVelocityCorrectionForce ) )
			{
				wheelIsBlocked = true;
				AngularVelocity += ForwardFriction.Speed > 0 ? 1e-10f : -1e-10f;
			}
			else
			{
				AngularVelocity += angularVelocityCorrectionForce * mRadius * invInertia * dt;
			}
		}
		else
		{
			float maxBrakeTorque = AngularVelocity * inertia * invDt + motorTorque;
			maxBrakeTorque = maxBrakeTorque < 0 ? -maxBrakeTorque : maxBrakeTorque;
			float brakeTorqueSign = AngularVelocity < 0f ? -1f : 1f;
			float clampedBrakeTorque = Math.Clamp( absCombinedBrakeTorque, -maxBrakeTorque, maxBrakeTorque );
			AngularVelocity += (motorTorque - brakeTorqueSign * clampedBrakeTorque) * invInertia * dt;
		}

		float absAngularVelocity = AngularVelocity < 0 ? -AngularVelocity : AngularVelocity;
		float maxCounterTorque = inertia * absAngularVelocity;
		CounterTorque = Math.Clamp( (signedCombinedBrakeForce - ForwardFriction.Force) * mRadius, -maxCounterTorque, maxCounterTorque );


		ForwardFriction.Slip = (ForwardFriction.Speed - AngularVelocity * mRadius) / clampedAbsForwardSpeed;
		ForwardFriction.Slip *= slipLoadModifier;

		SidewayFriction.Slip = MathF.Atan2( SidewayFriction.Speed, clampedAbsForwardSpeed );
		SidewayFriction.Slip *= slipLoadModifier;

		float sideSlipSign = SidewayFriction.Slip > 0 ? 1 : -1;
		float absSideSlip = Math.Abs( SidewayFriction.Slip );
		float peakSideFrictionForce = sideLoadFactor;

		float sideForce = -sideSlipSign * Tire.Evaluate( absSideSlip ) * sideLoadFactor;
		SidewayFriction.Force = Math.Clamp( sideForce, -sideForceClamp, sideForceClamp );
		SidewayFriction.Force *= camberFrictionCoeff;

		if ( IsOnGround && absForwardSpeed < 0.12f && absSideSpeed < 0.12f )
		{
			float verticalOffset = RestLength + mRadius;
			var transformPosition = WorldPosition;

			var transformUp = TransformRotationSteer.Up;

			currentPosition.x = transformPosition.x - transformUp.x * verticalOffset;
			currentPosition.y = transformPosition.y - transformUp.y * verticalOffset;
			currentPosition.z = transformPosition.z - transformUp.z * verticalOffset;

			if ( !lowSpeedReferenceIsSet )
			{
				lowSpeedReferenceIsSet = true;
				lowSpeedReferencePosition = currentPosition;
			}
			else
			{
				referenceError.x = lowSpeedReferencePosition.x - currentPosition.x;
				referenceError.y = lowSpeedReferencePosition.y - currentPosition.y;
				referenceError.z = lowSpeedReferencePosition.z - currentPosition.z;

				correctiveForce.x = invDt * LoadContribution * mass * referenceError.x;
				correctiveForce.y = invDt * LoadContribution * mass * referenceError.y;
				correctiveForce.z = invDt * LoadContribution * mass * referenceError.z;

				if ( wheelIsBlocked && absAngularVelocity < 0.5f )
				{
					ForwardFriction.Force += correctiveForce.Dot( hitForwardDirection );
				}
				SidewayFriction.Force += correctiveForce.Dot( hitSidewaysDirection );

			}

		}
		else
		{
			lowSpeedReferenceIsSet = false;
		}
		ForwardFriction.Force = Math.Clamp( ForwardFriction.Force, -peakForwardFrictionForce, peakForwardFrictionForce );

		SidewayFriction.Force = Math.Clamp( SidewayFriction.Force, -peakSideFrictionForce, peakSideFrictionForce );

		if ( absForwardSpeed > 2f || absAngularVelocity > 4f )
		{

			float forwardSlipPercent = ForwardFriction.Slip / Tire.GetPeakSlip();
			float sideSlipPercent = SidewayFriction.Slip / Tire.GetPeakSlip();
			float slipCircleLimit = MathF.Sqrt( forwardSlipPercent * forwardSlipPercent + sideSlipPercent * sideSlipPercent );
			if ( slipCircleLimit > 1f )
			{
				float beta = MathF.Atan2( sideSlipPercent, forwardSlipPercent * 0.9f );
				float sinBeta = MathF.Sin( beta );
				float cosBeta = MathF.Cos( beta );

				float absForwardForce = ForwardFriction.Force < 0 ? -ForwardFriction.Force : ForwardFriction.Force;

				float absSideForce = SidewayFriction.Force < 0 ? -SidewayFriction.Force : SidewayFriction.Force;
				float f = absForwardForce * cosBeta * cosBeta + absSideForce * sinBeta * sinBeta;

				ForwardFriction.Force = 0.5f * ForwardFriction.Force - f * cosBeta;
				SidewayFriction.Force = 0.5f * SidewayFriction.Force - f * sinBeta;
			}
		}

		if ( IsOnGround )
		{
			FrictionForce.x = (hitSidewaysDirection.x * SidewayFriction.Force + hitForwardDirection.x * ForwardFriction.Force).MeterToInch();
			FrictionForce.y = (hitSidewaysDirection.y * SidewayFriction.Force + hitForwardDirection.y * ForwardFriction.Force).MeterToInch();
			FrictionForce.z = (hitSidewaysDirection.z * SidewayFriction.Force + hitForwardDirection.z * ForwardFriction.Force).MeterToInch();
			//DebugOverlay.Normal( WorldPosition, hitSidewaysDirection * 10, overlay: true );
			//DebugOverlay.Normal( WorldPosition, hitForwardDirection * 10, overlay: true );
			//DebugOverlay.Normal( WorldPosition, FrictionForce / 100, overlay: true );
			//DebugOverlay.Normal( ContactPosition, Vector3.Up * AngularVelocity, overlay: true );

			//DebugOverlay.Sphere( new( ContactPosition, 4 ), overlay: true );
			//Vehicle.Body.ApplyForceAt( ContactPosition, FrictionForce );

		}
		else
			FrictionForce = Vector3.Zero;
	}
}