using Sandbox;
using Sandbox.Services;
using System;
using System.Collections.Specialized;
using System.Numerics;
using System.Text.RegularExpressions;
using static Sandbox.CameraComponent;

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 => sideFriction.Slip.MeterToInch();
	public float ForwardSlip => forwardFriction.Slip.MeterToInch();
	[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 => 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 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 => Mass * MathF.Pow( Radius.InchToMeter(), 2 );
	public float Inertia => BaseInertia;


	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 )
	{
		var entityAngles = vehicle.WorldRotation;

		Spin -= angularVelocity.MeterToInch() * 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 ));
	float lastload;
	public void DoPhysics( VeloXBase vehicle )
	{
		var pos = vehicle.WorldTransform.PointToWorld( StartPos );

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

		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 );

		if ( !IsOnGround )
		{
			forwardFriction = new Friction();
			sideFriction = new Friction();
			return;
		}

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

		// Vector3.CalculateVelocityOffset is broken (need fix)
		var velU = normal.Dot( vel );
		if ( velU < 0 && offset + Math.Abs( velU * Time.Delta ) > SuspensionLength )
		{
			var impulse = (-velU / Time.Delta) * normal;
			var body = vehicle.Body.PhysicsBody;
			Vector3 com = body.MassCenter;
			Rotation bodyRot = body.Rotation;

			Vector3 r = pos - com;

			Vector3 torque = Vector3.Cross( r, impulse );

			Vector3 torqueLocal = bodyRot.Inverse * torque;
			Vector3 angularVelocityLocal = torqueLocal * body.Inertia.Inverse;

			var centerAngularVelocity = bodyRot * angularVelocityLocal;

			var centerVelocity = impulse * (1 / body.Mass);

			vehicle.Body.Velocity += centerVelocity * 10;
			vehicle.Body.AngularVelocity += centerAngularVelocity;
			damperForce = 0;
		}

		force = (springForce - damperForce) * MathF.Max( 0, up.Dot( normal ) ) * normal;

		load = Math.Max( force.z, 0 ).InchToMeter();

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

			float camber_rad = CamberAngle.DegreeToRadian();
			float R = Radius.InchToMeter();
			TirePreset.ComputeState(
				2500,
				angularVelocity * R,
				forwardSpeed,
				sideSpeed,
				camber_rad,
				out var tireState
			);

			float linearSpeed = angularVelocity * Radius.InchToMeter();
			float F_roll = TirePreset.GetRollingResistance( linearSpeed, 1.0f );
			F_roll = -MathF.Sign( forwardSpeed ) * F_roll;

			float Fx_total = tireState.fx + F_roll;

			float T_brake = Brake * BrakePowerMax;

			if ( angularVelocity > 0 ) T_brake = -T_brake;
			else T_brake = angularVelocity < 0 ? T_brake : -MathF.Sign( Torque ) * T_brake;
			float totalTorque = Torque + T_brake - Fx_total * R;

			// not work
			Vector3 c = pos.Cross( forward );
			Vector3 v = (c * vehicle.Body.PhysicsBody.Inertia).Cross( pos );
			var m = 1 / (1 / vehicle.Body.Mass + forward.Dot( v ));
			m *= Inertia / (m * R * R + Inertia);
			float ve = forward.Dot( vel ).InchToMeter() - angularVelocity;

			angularVelocity += ve * m * Radius * (1 / Inertia);// totalTorque * (1 / Inertia) * Time.Delta;

			forwardFriction = new Friction()
			{
				Slip = tireState.slip,
				Force = Fx_total,
				Speed = forwardSpeed
			};

			sideFriction = new Friction()
			{
				Slip = tireState.slip_angle,
				Force = tireState.fy,
				Speed = sideSpeed
			};
			Vector3 frictionForce = forward * forwardFriction.Force + right * sideFriction.Force;
			vehicle.Body.ApplyForceAt( contactPos, (force + frictionForce) / Time.Delta );

		}
		else
		{
			// Колесо в воздухе: сбрасываем силы
			forwardFriction = new Friction();
			sideFriction = new Friction();

			// Обновление угловой скорости только от мотора/тормозов
			float T_brake = Brake * BrakePowerMax;

			if ( angularVelocity > 0 ) T_brake = -T_brake;
			else T_brake = angularVelocity < 0 ? T_brake : -MathF.Sign( Torque ) * T_brake;
			angularVelocity += (Torque + T_brake) / Inertia * Time.Delta;
		}


	}


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