using Sandbox;
using System;
using System.Text.RegularExpressions;

namespace VeloX;

[Group( "VeloX" )]
[Title( "VeloX - Wheel" )]
public partial class VeloXWheel : Component
{

	[Property] public float Radius { get; set; } = 15;
	[Property] public float Mass { get; set; } = 20;
	[Property] public float RollingResistance { get; set; } = 20;
	[Property] public float SlipCircleShape { get; set; } = 1.05f;

	public FrictionPreset LongitudinalFrictionPreset => WheelFriction.Longitudinal;
	public FrictionPreset LateralFrictionPreset => WheelFriction.Lateral;
	public FrictionPreset AligningFrictionPreset => WheelFriction.Aligning;

	[Property] public TirePreset WheelFriction { get; set; }
	[Property] public float Width { get; set; } = 6;
	[Sync] public float SideSlip { get; private set; }
	[Sync] public float ForwardSlip { get; private set; }
	[Sync] public float Torque { get; set; }
	[Sync, Range( 0, 1 )] public float Brake { get; set; }
	[Property] float BrakePowerMax { get; set; } = 3000;
	[Property] public bool IsFront { get; protected set; }
	[Property] public float SteerMultiplier { get; set; }
	[Property] public float CasterAngle { get; set; } = 7; // todo
	[Property] public float CamberAngle { get; set; } = -3;
	[Property] public float ToeAngle { get; set; } = 0.5f;

	[Property, Group( "Suspension" )] float SuspensionLength { get; set; } = 10;
	[Property, Group( "Suspension" )] float SpringStrength { get; set; } = 800;
	[Property, Group( "Suspension" )] float SpringDamper { get; set; } = 3000;

	[Property] public bool AutoPhysics { get; set; } = true;

	public float Spin { get; private set; }

	public float RPM { get => angularVelocity * 60f / MathF.Tau; set => angularVelocity = value / (60 / MathF.Tau); }
	public float AngularVelocity => angularVelocity;

	internal float DistributionFactor { get; set; }

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

	public SceneTraceResult Trace { get; private set; }
	public bool IsOnGround => Trace.Hit;

	private float lastSpringOffset;
	private float angularVelocity;
	private float load;
	private float lastFraction;
	private RealTimeUntil expandSoundCD;
	private RealTimeUntil contractSoundCD;

	private Vector3 contactPos;
	private Vector3 forward;
	private Vector3 right;
	private Vector3 up;

	private Friction forwardFriction;
	private Friction sideFriction;
	private Vector3 force;
	public float CounterTorque { get; private set; }

	private float BaseInertia => 0.5f * Mass * MathF.Pow( Radius.InchToMeter(), 2 );
	public float Inertia => BaseInertia;


	protected override void OnAwake()
	{
		base.OnAwake();
		if ( StartPos.IsNearZeroLength )
			StartPos = LocalPosition;
	}

	private void DoSuspensionSounds( VeloXBase vehicle, float change )
	{
		if ( change > 0.1f && expandSoundCD )
		{
			expandSoundCD = 0.3f;
			var sound = Sound.Play( vehicle.SuspensionUpSound, WorldPosition );
			sound.Volume = Math.Clamp( Math.Abs( change ) * 5f, 0, 0.5f );
		}

		if ( change < -0.1f && contractSoundCD )
		{
			contractSoundCD = 0.3f;
			change = MathF.Abs( change );
			var sound = Sound.Play( change > 0.3f ? vehicle.SuspensionHeavySound : vehicle.SuspensionDownSound, WorldPosition );
			sound.Volume = Math.Clamp( change * 5f, 0, 1 );
		}
	}

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

	private void UpdateVisuals( VeloXBase vehicle, in float dt )
	{
		var entityAngles = vehicle.WorldRotation;

		Spin -= angularVelocity.MeterToInch() * dt;


		var steerRotated = entityAngles.RotateAroundAxis( Vector3.Up, vehicle.SteerAngle.yaw * SteerMultiplier + ToeAngle );
		var camberRotated = steerRotated.RotateAroundAxis( Vector3.Forward, -CamberAngle );
		var angularVelocityRotated = camberRotated.RotateAroundAxis( Vector3.Right, Spin );

		WorldRotation = angularVelocityRotated;

	}

	private (float, float, float, float) StepLongitudinal( float Vx, float Lc, float kFx, float kSx)
	{
		float Tm = Torque;
		float Tb = Brake * BrakePowerMax + RollingResistance;
		float R = Radius.InchToMeter();
		float I = Inertia;

		float Winit = angularVelocity;
		float W = angularVelocity;

		float VxAbs = MathF.Abs( Vx );
		float Sx;
		if ( VxAbs >= 0.1f )
			Sx = (Vx - W * R) / VxAbs;

		else
			Sx = (Vx - W * R) * 0.6f;

		Sx = Math.Clamp( Sx * kSx, -1, 1 );

		W += Tm / I * Time.Delta;

		Tb *= W > 0 ? -1 : 1;

		float TbCap = MathF.Abs( W ) * I / Time.Delta;
		float Tbr = MathF.Abs( Tb ) - MathF.Abs( TbCap );
		Tbr = MathF.Max( Tbr, 0 );
		Tb = Math.Clamp( Tb, -TbCap, TbCap );
		W += Tb / I * Time.Delta;

		float maxTorque = LongitudinalFrictionPreset.Evaluate( Sx ) * Lc * kFx;

		float errorTorque = (W - Vx / R) * I / Time.Delta;

		float surfaceTorque = MathX.Clamp( errorTorque, -maxTorque, maxTorque );

		W -= surfaceTorque / I * Time.Delta;

		float Fx = surfaceTorque / R;


		Tbr *= W > 0 ? -1 : 1;
		float TbCap2 = MathF.Abs( W ) * I / Time.Delta;

		float Tb2 = Math.Clamp( Tbr, -TbCap2, TbCap2 );

		W += Tb2 / I * Time.Delta;

		float deltaOmegaTorque = (W - Winit) * I / Time.Delta;

		float Tcnt = -surfaceTorque + Tb + Tb2 - deltaOmegaTorque;
		if ( Lc < 0.001f )
			Sx = 0;

		return (W, Sx, Fx, Tcnt);
	}

	private (float, float) StepLateral( float Vx, float Vy, float Lc, float kFy, float kSy)
	{
		float VxAbs = MathF.Abs( Vx );
		float Sy;

		if ( VxAbs > 0.1f )
			Sy = MathF.Atan( Vy / VxAbs ).RadianToDegree() * 0.01111f;
		else
			Sy = Vy * (0.003f / Time.Delta);


		Sy *= kSy * 0.95f;
		Sy = Math.Clamp( Sy * kSy, -1, 1 );
		float Fy = -MathF.Sign( Sy ) * LateralFrictionPreset.Evaluate( Sy ) * Lc * kFy;
		if ( Lc < 0.0001f )
			Sy = 0;

		return (Sy, Fy);
	}

	private void SlipCircle( float Sx, float Sy, float Fx, ref float Fy )
	{
		float SxAbs = Math.Abs( Sx );
		if ( SxAbs > 0.01f )
		{

			float SxClamped = Math.Clamp( Sx, -1, 1 );

			float SyClamped = Math.Clamp( Sy, -1, 1 );

			Vector2 combinedSlip = new(
				SxClamped * SlipCircleShape,
				SyClamped
			);

			Vector2 slipDir = combinedSlip.Normal;

			float F = MathF.Sqrt( Fx * Fx + Fy * Fy );

			float absSlipDirY = MathF.Abs( slipDir.y );

			Fy = F * absSlipDirY * MathF.Sign( Fy );
		}
	}


	private static float GetLongitudinalLoadCoefficient( float load ) => 11000 * (1 - MathF.Exp( -0.00014f * load ));
	private static float GetLateralLoadCoefficient( float load ) => 18000 * (1 - MathF.Exp( -0.0001f * load ));

	public void DoPhysics( VeloXBase vehicle )
	{
		var pos = vehicle.WorldTransform.PointToWorld( StartPos );

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

		forward = ang.Forward;
		right = ang.Right;
		up = ang.Up;

		var maxLen = SuspensionLength;

		var endPos = pos + ang.Down * maxLen;
		Trace = Scene.Trace
					.IgnoreGameObjectHierarchy( vehicle.GameObject )
					.Cylinder( Width, Radius, pos, endPos )
					.Rotated( vehicle.WorldTransform.Rotation * CylinderOffset )
					.UseRenderMeshes( false )
					.UseHitPosition( false )
					.WithoutTags( vehicle.WheelIgnoredTags )
					.Run();

		var fraction = Trace.Fraction;

		contactPos = pos - maxLen * fraction * up;

		LocalPosition = vehicle.WorldTransform.PointToLocal( contactPos );

		DoSuspensionSounds( vehicle, fraction - lastFraction );
		lastFraction = fraction;

		var normal = Trace.Normal;

		var vel = vehicle.Body.GetVelocityAtPoint( pos );

		vel.x = vel.x.InchToMeter();
		vel.y = vel.y.InchToMeter();
		vel.z = vel.z.InchToMeter();

		if ( !IsOnGround )
		{
			SideSlip = 0;
			ForwardSlip = 0;
			return;
		}

		var offset = maxLen - (fraction * maxLen);
		var springForce = (offset * SpringStrength);
		var damperForce = (lastSpringOffset - offset) * SpringDamper;
		lastSpringOffset = offset;
		force = (springForce - damperForce) * MathF.Max( 0, up.Dot( normal ) ) * normal / Time.Delta;

		// Vector3.CalculateVelocityOffset is broken (need fix)
		//var velU = normal.Dot( vel ).MeterToInch();
		//if ( velU < 0 && offset + Math.Abs( velU * dt ) > SuspensionLength )
		//{
		//	var (linearVel, angularVel) = vehicle.Body.PhysicsBody.CalculateVelocityOffset( (-velU.InchToMeter() / dt) * normal, pos );
		//	vehicle.Body.Velocity += linearVel;
		//	vehicle.Body.AngularVelocity += angularVel;
		//}


		load = springForce - damperForce;
		load = Math.Max( load, 0 );

		var longitudinalLoadCoefficient = GetLongitudinalLoadCoefficient( load );
		var lateralLoadCoefficient = GetLateralLoadCoefficient( load );

		float forwardSpeed = 0;
		float sideSpeed = 0;


		if ( IsOnGround )
		{
			forwardSpeed = vel.Dot( forward );
			sideSpeed = vel.Dot( right );
		}
		(float W, float Sx, float Fx, float Tcnt) = StepLongitudinal(
		   forwardSpeed,
		   longitudinalLoadCoefficient,
		   0.95f,
		   0.9f
		);

		(float Sy, float Fy) = StepLateral(
			forwardSpeed,
			sideSpeed,
			lateralLoadCoefficient,
			0.95f,
			0.9f
		);

		SlipCircle( Sx, Sy, Fx, ref Fy );

		angularVelocity = W;
		CounterTorque = Tcnt;

		forwardFriction = new Friction()
		{
			Slip = Sx,
			Force = Fx.MeterToInch(),
			Speed = forwardSpeed
		};

		sideFriction = new Friction()
		{
			Slip = Sy,
			Force = Fy.MeterToInch(),
			Speed = sideSpeed
		};


		var frictionforce = right * sideFriction.Force + forward * forwardFriction.Force;

		vehicle.Body.ApplyForceAt( contactPos, force + frictionforce );

	}


	// debug
	protected override void OnUpdate()
	{
		DebugOverlay.Normal( contactPos, forward * forwardFriction.Force / 10000f, Color.Red, overlay: true );
		DebugOverlay.Normal( contactPos, right * sideFriction.Force / 10000f, Color.Green, overlay: true );
		DebugOverlay.Normal( contactPos, up * force / 50000f, Color.Blue, overlay: true );
	}
}