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 Vector3 FrictionForce;


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

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

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

	public float LateralSlip => sy;
	public float LateralSpeed => vy;
	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;


	// speed
	[Sync] private float vx { get; set; }
	[Sync] private float vy { get; set; }

	// force
	[Sync] private float fx { get; set; }
	[Sync] private float fy { get; set; }

	// slip
	[Sync] private float sx { get; set; }
	[Sync] private float sy { get; set; }

	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;

			vx = hitContactVelocity.Dot( hitForwardDirection ).InchToMeter();
			vy = hitContactVelocity.Dot( hitSidewaysDirection ).InchToMeter();
		}
		else
		{
			vx = 0;
			vy = 0;
		}
	}


	private Vector3 lowSpeedReferencePosition;
	private bool lowSpeedReferenceIsSet;
	private Vector3 currentPosition;
	private Vector3 referenceError;
	private Vector3 correctiveForce;
	public bool wheelIsBlocked;
	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;

		float mass = Vehicle.Body.Mass;
		float absForwardSpeed = Math.Abs( vx );
		float forwardForceClamp = mass * LoadContribution * absForwardSpeed * invDt;
		float absSideSpeed = Math.Abs( vy );
		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 );

		float peakForwardFrictionForce = 11000 * (1 - MathF.Exp( -0.00014f * forwardLoadFactor ));
		float absCombinedBrakeTorque = Math.Max( 0, brakeTorque + RollingResistanceTorque );

		float signedCombinedBrakeTorque = absCombinedBrakeTorque * (vx > 0 ? -1 : 1);
		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;
		fx = forwardInputForce > maxForwardForce ? maxForwardForce
			: forwardInputForce < -maxForwardForce ? -maxForwardForce : forwardInputForce;

		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 = vx * 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 += vx > 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 - fx) * mRadius, -maxCounterTorque, maxCounterTorque );

		sx = (vx - AngularVelocity * mRadius) / clampedAbsForwardSpeed;
		sx *= slipLoadModifier;

		sy = MathF.Atan2( vy, clampedAbsForwardSpeed );
		sy *= slipLoadModifier;

		float sideSlipSign = sy > 0 ? 1 : -1;
		float absSideSlip = Math.Abs( sy );
		float peakSideFrictionForce = 18000 * (1 - MathF.Exp( -0.0001f * sideLoadFactor ));

		float sideForce = -sideSlipSign * Tire.Evaluate( absSideSlip ) * peakSideFrictionForce;
		fy = Math.Clamp( sideForce, -sideForceClamp, sideForceClamp );

		// Calculate effect of camber on friction
		float camberFrictionCoeff = Math.Max( 0, Vehicle.WorldRotation.Up.Dot( ContactNormal ) );
		fy *= 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 )
				{
					fx += correctiveForce.Dot( hitForwardDirection );
				}
				fy += correctiveForce.Dot( hitSidewaysDirection );
			}
		}
		else
		{
			lowSpeedReferenceIsSet = false;
		}

		fx = Math.Clamp( fx, -peakForwardFrictionForce, peakForwardFrictionForce );
		fy = Math.Clamp( fy, -peakSideFrictionForce, peakSideFrictionForce );

		if ( absForwardSpeed > 0.01f || absAngularVelocity > 0.01f )
		{
			var f = MathF.Sqrt( fx * fx + fy * fy );
			var d = Math.Abs( new Vector2( sx, sy ).Normal.y );
			fy = f * d * Math.Sign( fy );

		}
		if ( IsOnGround )
		{
			FrictionForce.x = (hitSidewaysDirection.x * fy + hitForwardDirection.x * fx).MeterToInch();
			FrictionForce.y = (hitSidewaysDirection.y * fy + hitForwardDirection.y * fx).MeterToInch();
			FrictionForce.z = (hitSidewaysDirection.z * fy + hitForwardDirection.z * fx).MeterToInch();

			//DebugOverlay.Normal( WorldPosition, hitSidewaysDirection * 10, overlay: true, color: Color.Red );
			//DebugOverlay.Normal( WorldPosition, hitForwardDirection * 10, overlay: true, color: Color.Green );
			//DebugOverlay.Normal( WorldPosition, FrictionForce.ClampLength( 30 ), overlay: true, color: Color.Cyan );
			//DebugOverlay.ScreenText( Scene.Camera.PointToScreenPixels( WorldPosition ), $"{ForwardFriction}\nMotor:{(int)motorTorque}\nBrake:{(int)brakeTorque}", flags: TextFlag.LeftTop );
		}
		else
			FrictionForce = Vector3.Zero;
	}
}