using Sandbox;
using Sandbox.Rendering;
using Sandbox.Services;
using Sandbox.UI;
using System;
using System.Collections.Specialized;
using System.Diagnostics.Metrics;
using System.Numerics;
using System.Text.RegularExpressions;
using System.Threading;
using static Sandbox.CameraComponent;
using static Sandbox.Package;
using static Sandbox.SkinnedModelRenderer;

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;
	[Property] public TirePreset TirePreset { get; set; }
	[Property] public float Width { get; set; } = 6;
	public float SideSlip { get; private set; }
	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] public float Ackermann { get; set; } = 0;

	[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 * 30f / MathF.PI; set => angularVelocity = value / (30f / MathF.PI); }
	public float AngularVelocity { get => angularVelocity; set => angularVelocity = value; }

	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 Vector3 contactPos;
	private Vector3 forward;
	private Vector3 right;
	private Vector3 up;

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

	internal float BaseInertia => 0.5f * Mass * MathF.Pow( Radius.InchToMeter(), 2 );
	public float Inertia
	{
		get => BaseInertia + inertia;
		set => inertia = value;
	}


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

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

	private void UpdateVisuals( VeloXBase vehicle, in float dt )
	{
		Spin -= angularVelocity.RadianToDegree() * dt;
		WorldRotation = vehicle.WorldTransform.RotationToWorld( GetSteer( vehicle.SteerAngle.yaw ) ) * Rotation.FromAxis( Vector3.Right, Spin );
	}

	private Rotation GetSteer( float steer )
	{

		float angle = (-steer * SteerMultiplier).DegreeToRadian();

		float t = MathF.Tan( (MathF.PI / 2) - angle ) - Ackermann;
		float steering_angle = MathF.CopySign( float.Pi / 2, t ) - MathF.Atan( t );
		var steering_axis = Vector3.Up * MathF.Cos( -CasterAngle.DegreeToRadian() ) +
			Vector3.Right * MathF.Sin( -CasterAngle.DegreeToRadian() );

		return Rotation.FromAxis( Vector3.Forward, -CamberAngle ) * Rotation.FromAxis( steering_axis, steering_angle.RadianToDegree() );
	}


	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 ));
	private float inertia;


	private (float, float, float, float) StepLongitudinal( float Tm, float Tb, float Vx, float W, float Lc, float R, float I )
	{
		float wInit = W;
		float vxAbs = Math.Abs( Vx );
		float Sx;
		if ( Lc < 0.01f )
		{
			Sx = 0;
		}
		else if ( vxAbs >= 0.01f )
		{
			Sx = (W * R - Vx) / vxAbs;
		}
		else
		{
			Sx = (W * R - Vx) * 0.6f;
		}

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

		W += Tm / I * Time.Delta;

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

		float tbCap = Math.Abs( W ) * I / Time.Delta;
		float tbr = Math.Abs( Tb ) - Math.Abs( tbCap );
		tbr = Math.Max( tbr, 0 );

		Tb = Math.Clamp( Tb, -tbCap, tbCap );

		W += Tb / I * Time.Delta;

		float maxTorque = TirePreset.Pacejka.PacejkaFx( Math.Abs( Sx ) ) * Lc * R;

		float errorTorque = (W - Vx / R) * I / Time.Delta;
		float surfaceTorque = Math.Clamp( errorTorque, -maxTorque, maxTorque );

		W -= surfaceTorque / I * Time.Delta;
		float Fx = surfaceTorque / R;

		tbr *= (W > 0 ? -1 : 1);
		float TbCap2 = Math.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;

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

	private void StepLateral( float Vx, float Vy, float Lc, out float Sy, out float Fy )
	{
		float VxAbs = Math.Abs( Vx );

		if ( Lc < 0.01f )
		{
			Sy = 0;
		}
		else if ( VxAbs > 0.1f )
		{

			Sy = MathX.RadianToDegree( MathF.Atan( Vy / VxAbs ) ) / 50;
		}
		else
		{

			Sy = Vy * (0.003f / Time.Delta);
		}

		Sy = Math.Clamp( Sy, -1, 1 );
		float slipSign = Sy < 0 ? -1 : 1;
		Fy = -slipSign * TirePreset.Pacejka.PacejkaFy( Math.Abs( Sy ) ) * Lc;
	}

	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 * 1.05f, SyClamped );
			Vector2 slipDir = combinedSlip.Normal;

			float F = MathF.Sqrt( Fx * Fx + Fy * Fy );
			float absSlipDirY = Math.Abs( slipDir.y );

			Fy = F * absSlipDirY * (Fy < 0 ? -1 : 1);
		}
	}

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

		var ang = vehicle.WorldTransform.RotationToWorld( GetSteer( vehicle.SteerAngle.yaw ) );

		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( true )
					.WithoutTags( vehicle.WheelIgnoredTags )
					.Run();

		forward = Vector3.VectorPlaneProject( ang.Forward, Trace.Normal );
		right = Vector3.VectorPlaneProject( ang.Right, Trace.Normal );

		var fraction = Trace.Fraction;

		contactPos = pos - maxLen * fraction * ang.Up;

		LocalPosition = vehicle.WorldTransform.PointToLocal( contactPos );

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

		if ( !IsOnGround )
			return;

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

		var offset = maxLen - (fraction * maxLen);
		var springForce = offset * SpringStrength;
		var damperForce = (lastSpringOffset - offset) * SpringDamper;

		lastSpringOffset = offset;


		var velU = Trace.Normal.Dot( vel );

		if ( velU < 0 && offset + Math.Abs( velU * Time.Delta ) > SuspensionLength )
		{
			vehicle.Body.CalculateVelocityOffset( -velU / Time.Delta * Trace.Normal, pos, out var linearImp, out var angularImp );

			vehicle.Body.Velocity += linearImp;
			vehicle.Body.AngularVelocity += angularImp;
			vehicle.Body.CalculateVelocityOffset( Trace.HitPosition - (contactPos + Trace.Normal * velU * Time.Delta), pos, out var lin, out _ );

			vehicle.WorldPosition += lin / Time.Delta;
			damperForce = 0;

		}

		force = (springForce - damperForce) * Trace.Normal;

		load = Math.Max( springForce - damperForce, 0 );
		float R = Radius.InchToMeter();

		float forwardSpeed = vel.Dot( forward ).InchToMeter();
		float sideSpeed = vel.Dot( right ).InchToMeter();

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

		float F_roll = TirePreset.GetRollingResistance( angularVelocity * R, 1.0f ) * 10000;
	
		( float W, float Sx, float Fx, float counterTq) = StepLongitudinal(
			Torque,
			Brake * BrakePowerMax + F_roll,
			forwardSpeed,
			angularVelocity,
			longitudinalLoadCoefficient,
			R,
			Inertia
		);

		StepLateral( forwardSpeed, sideSpeed, lateralLoadCoefficient, out float Sy, out float Fy );

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

		CounterTorque = counterTq;
		angularVelocity = W;

		force += forward * Fx;
		force += right * Fy * Math.Clamp( vehicle.TotalSpeed * 0.005f, 0, 1 );
		ForwardSlip = Sx;
		SideSlip = Sy * Math.Clamp( vehicle.TotalSpeed * 0.005f, 0, 1 );

		vehicle.Body.ApplyForceAt( pos, force / Time.Delta );

	}

#if DEBUG
	protected override void OnUpdate()
	{
		DebugOverlay.Normal( contactPos, forward * forwardFriction, Color.Red, overlay: true );
		DebugOverlay.Normal( contactPos, right * sideFriction, Color.Green, overlay: true );
		DebugOverlay.Normal( contactPos, up * force / 1000f, Color.Blue, overlay: true );
	}
#endif

}