using Sandbox;
using System;

namespace VeloX;

[Group( "VeloX" )]
[Title( "VeloX - Wheel" )]
public partial class VeloXWheel : Component
{
	[Property, Group( "Suspension" )] public float Radius { get; set; } = 0.35f;
	[Property, Group( "Suspension" )] public float Width { get; set; } = 0.1f;
	[Property] public float Mass { get; set; } = 5;
	[Property, Group( "Suspension" )] float RestLength { get; set; } = 0.22f;
	[Property, Group( "Suspension" )] public float SpringStiffness { get; set; } = 20000.0f;
	[Property, Group( "Suspension" )] float ReboundStiffness { get; set; } = 2200;
	[Property, Group( "Suspension" )] float CompressionStiffness { get; set; } = 2400;
	[Property, Group( "Suspension" )] float BumpStopStiffness { get; set; } = 5000;

	[Property, Group( "Traction" )] public TirePreset Tire { get; set; } = ResourceLibrary.Get<TirePreset>( "frictions/default.tire" );
	[Property, Group( "Traction" )] public float SurfaceGrip { get; set; } = 1f;
	[Property, Group( "Traction" )] public float SurfaceResistance { get; set; } = 0.05f;
	public bool AutoSimulate = true;


	public float BaseInertia => Mass * (Radius * Radius); // kg·m²
	[Property] public float Inertia { get; set; } = 1.5f; // kg·m²
	public float SideSlip => SlipAngle;
	public float ForwardSlip => DynamicSlipRatio;
	[Sync] public float Torque { get; set; }
	[Sync, Range( 0, 1 )] public float Brake { get; set; }
	[Property] public bool IsFront { get; protected set; }
	[Property] public float SteerMultiplier { get; set; }

	public float RPM { get => AngularVelocity * 60f / MathF.Tau; set => AngularVelocity = value / (60 / MathF.Tau); }
	[Sync] internal float DistributionFactor { get; set; }

	private Vector3 StartPos { get; set; }
	private static Rotation CylinderOffset = Rotation.FromRoll( 90 );

	[Sync] public bool IsOnGround { get; private set; }


	[Property] public float DriveTorque => Torque;
	[Property] public float BrakeTorque => Brake * 5000f;

	public float Compression { get; protected set; } // meters
	public float LastLength { get; protected set; } // meters
	public float Fz { get; protected set; }           // N
	public float AngularVelocity { get; protected set; } // rad/s
	public float Fx { get; protected set; }           // N
	public float Fy { get; protected set; }           // N
	public float RollAngle { get; protected set; }    // degrees


	private VeloXBase Vehicle;
	[Sync] public Vector3 ContactNormal { get; protected set; }
	[Sync] public Vector3 ContactPosition { get; protected set; }
	public float SlipRatio { get; protected set; }
	public float SlipAngle { get; protected set; }
	public float DynamicSlipRatio { get; protected set; }
	public float DynamicSlipAngle { get; protected set; }
	Rotation TransformRotationSteer => Vehicle.WorldTransform.RotationToWorld( Vehicle.SteerAngle * SteerMultiplier );

	protected override void OnAwake()
	{
		Vehicle = Components.Get<VeloXBase>( FindMode.EverythingInSelfAndAncestors );
		base.OnAwake();
		if ( StartPos.IsNearZeroLength )
			StartPos = LocalPosition;
		Inertia = BaseInertia;
	}

	internal void Update( VeloXBase vehicle, in float dt )
	{
		UpdateVisuals( vehicle, dt );
	}

	private void UpdateVisuals( VeloXBase vehicle, in float dt )
	{
		WorldRotation = vehicle.WorldTransform.RotationToWorld( vehicle.SteerAngle * SteerMultiplier ).RotateAroundAxis( Vector3.Right, -RollAngle );
		LocalPosition = StartPos + Vector3.Down * LastLength.MeterToInch();
	}

	private struct WheelTraceData
	{
		internal Vector3 ContactNormal;
		internal Vector3 ContactPosition;
		internal float Compression;
		internal float Force;
	}
	public void UpdateForce()
	{
		Vehicle.Body.ApplyForceAt( ContactPosition, FrictionForce + ContactNormal * Fz.MeterToInch() );
	}

	internal void StepPhys( VeloXBase vehicle, in float dt )
	{

		var _rigidbody = vehicle.Body;
		const int numSamples = 3;
		float halfWidth = Width.MeterToInch() * 0.5f;

		var ang = vehicle.WorldTransform.RotationToWorld( vehicle.SteerAngle * SteerMultiplier );

		Vector3 right = Vector3.VectorPlaneProject( ang.Right, Vector3.Up ).Normal;


		int hitCount = 0;
		WheelTraceData wheelTraceData = new();
		for ( int i = 0; i < numSamples; i++ )
		{
			float t = (float)i / (numSamples - 1);
			float offset = MathX.Lerp( -halfWidth, halfWidth, t );
			Vector3 start = vehicle.WorldTransform.PointToWorld( StartPos + Vector3.Right * offset );
			Vector3 end = start + vehicle.WorldRotation.Down * RestLength.MeterToInch();
			if ( TraceWheel( vehicle, ref wheelTraceData, start, end, Width.MeterToInch() / numSamples, dt ) )
				hitCount++;
		}


		if ( hitCount > 0 )
		{

			IsOnGround = true;

			//// Average all contacts
			Fz = Math.Max( wheelTraceData.Force / hitCount, 0 );
			Compression = wheelTraceData.Compression / hitCount;
			ContactNormal = (wheelTraceData.ContactNormal / hitCount).Normal;
			ContactPosition = wheelTraceData.ContactPosition / hitCount;
			//DoSuspensionSounds( vehicle, (RestLength - Compression) * 0.8f);
			LastLength = RestLength - Compression;

			//DebugOverlay.Normal( ContactPosition, ContactNormal * Fz / 1000 );
			// Apply suspension force
			//_rigidbody.ApplyForceAt( ContactPosition, ContactNormal * Fz.MeterToInch() );

			UpdateHitVariables();
			// Friction

			Vector3 forward = ContactNormal.Cross( right ).Normal;
			right = Rotation.FromAxis( ContactNormal, 90f ) * forward;

			var velAtContact = _rigidbody.GetVelocityAtPoint( ContactPosition + vehicle.Body.MassCenter ) * 0.0254f;
			float vx = Vector3.Dot( velAtContact, forward );
			float vy = Vector3.Dot( velAtContact, right );
			float wheelLinearVel = AngularVelocity * Radius;

			float creepVel = 0.5f;
			SlipRatio = (wheelLinearVel - vx) / (MathF.Abs( vx ) + creepVel);
			SlipAngle = MathX.RadianToDegree( MathF.Atan2( vy, MathF.Abs( vx ) + creepVel ) );

			float latCoeff = 1.0f - MathF.Exp( -MathF.Max( MathF.Abs( vx ), 1f ) * dt / 0.01f );
			float longCoeff = 1.0f - MathF.Exp( -MathF.Max( MathF.Abs( vx ), 1f ) * dt / 0.01f );

			DynamicSlipAngle += (SlipAngle - DynamicSlipAngle) * latCoeff;
			DynamicSlipRatio += (SlipRatio - DynamicSlipRatio) * longCoeff;


			UpdateFriction( dt );
		}
		else
		{
			IsOnGround = false;
			// Wheel is off the ground
			Compression = 0f;
			Fz = 0f;
			Fx = 0f;
			Fy = 0f;
			ContactNormal = Vector3.Up;
			ContactPosition = WorldPosition;
		}

	}

	const string playerTag = "player";

	private bool TraceWheel( VeloXBase vehicle, ref WheelTraceData wheelTraceData, Vector3 start, Vector3 end, float width, in float dt )
	{

		var trace = Scene.Trace
						.FromTo( start, end )
						.Cylinder( width, Radius.MeterToInch() )
						.Rotated( vehicle.WorldRotation * CylinderOffset )
						.UseHitPosition( false )
						.IgnoreGameObjectHierarchy( Vehicle.GameObject )
						.WithoutTags( playerTag )
						.Run();

		//DebugOverlay.Trace( trace, overlay: true );
		if ( trace.Hit )
		{

			Vector3 contactPos = trace.EndPosition;
			Vector3 contactNormal = trace.Normal;
			float currentLength = trace.Distance.InchToMeter();
			float compression = (RestLength - currentLength).Clamp( -RestLength, RestLength );

			// Nonlinear spring
			float springForce = SpringStiffness * compression * (1f + 2f * compression / RestLength);

			// Damping
			float damperVelocity = (LastLength - currentLength) / dt;
			float damperForce = damperVelocity > 0 ? damperVelocity * ReboundStiffness : damperVelocity * CompressionStiffness;

			float FzPoint = springForce + damperForce;

			// Bump stop
			float minCompression = -0.05f;
			if ( compression <= minCompression ) FzPoint += (minCompression - compression) * BumpStopStiffness;

			wheelTraceData.ContactNormal += contactNormal;
			wheelTraceData.ContactPosition += contactPos;
			wheelTraceData.Compression += compression;
			wheelTraceData.Force += Math.Max( 0, FzPoint );

			return true;
		}

		return false;
	}


	public void DoPhysics( in float dt )
	{
		if ( IsProxy )
			return;
		StepRotation( Vehicle, dt );
		if ( AutoSimulate )
			StepPhys( Vehicle, dt );
	}

	private void StepRotation( VeloXBase vehicle, in float dt )
	{
		float inertia = MathF.Max( 1f, Inertia );

		float roadTorque = Fx * Radius;
		float externalTorque = DriveTorque - roadTorque;
		float rollingResistanceTorque = Fz * Radius * SurfaceResistance;

		const float HubCoulombNm = 20f;
		const float HubViscous = 0.1f;

		float coulombTorque = BrakeTorque + rollingResistanceTorque + HubCoulombNm;

		float omega = AngularVelocity;

		if ( MathF.Abs( omega ) < 1e-6f && MathF.Abs( externalTorque ) <= coulombTorque )
		{
			AngularVelocity = 0f;
		}
		else
		{

			// viscous decay
			if ( HubViscous > 0f ) omega *= MathF.Exp( -(HubViscous / inertia) * dt );

			// Coulomb drag
			if ( coulombTorque > 0f && omega != 0f )
			{
				float dir = MathF.Sign( omega );
				float deltaOmega = (coulombTorque / inertia) * dt;
				if ( deltaOmega >= MathF.Abs( omega ) ) omega = 0f;
				else omega -= dir * deltaOmega;
			}

			if ( MathF.Abs( omega ) < 0.01f ) omega = 0f;
		}
		AngularVelocity = omega; // wider sanity range

		RollAngle += MathX.RadianToDegree( AngularVelocity ) * dt;
		RollAngle = (RollAngle % 360f + 360f) % 360f;
	}
}