using Sandbox;
using System;
using System.Threading;

namespace VeloX;

public partial class VeloXCar
{
	public static float ExpDecay( float a, float b, float decay, float dt ) => b + (a - b) * MathF.Exp( -decay * dt );
	[Property, Feature( "Steer" )] public float SteerConeMaxSpeed { get; set; } = 1800;
	[Property, Feature( "Steer" )] public float SteerConeMaxAngle { get; set; } = 0.25f;
	[Property, Feature( "Steer" )] public float SteerConeChangeRate { get; set; } = 8;
	[Property, Feature( "Steer" )] public float CounterSteer { get; set; } = 0.1f;
	[Property, Feature( "Steer" )] public float MaxSteerAngle { get; set; } = 35f;

	[Sync] public float Steering { get; private set; }

	private float inputSteer;

	public static float SignedAngle( Vector3 from, Vector3 to, Vector3 axis )
	{
		float unsignedAngle = Vector3.GetAngle( from, to );

		float cross_x = from.y * to.z - from.z * to.y;
		float cross_y = from.z * to.x - from.x * to.z;
		float cross_z = from.x * to.y - from.y * to.x;
		float sign = MathF.Sign( axis.x * cross_x + axis.y * cross_y + axis.z * cross_z );
		return unsignedAngle * sign;
	}
	public float VelocityAngle { get; private set; }
	public int CarDirection { get { return ForwardSpeed.InchToMeter() < 5 ? 0 : (VelocityAngle < 90 && VelocityAngle > -90 ? 1 : -1); } }
	private void UpdateSteering( float dt )
	{
		var inputSteer = Input.AnalogMove.y;


		VelocityAngle = 0;// -SignedAngle( Body.Velocity, WorldRotation.Forward, WorldRotation.Up );

		//var steerConeFactor = Math.Clamp( TotalSpeed / SteerConeMaxSpeed, 0, 1 );
		//var steerCone = 1 - steerConeFactor * (1 - SteerConeMaxAngle);

		inputSteer = ExpDecay( this.inputSteer, inputSteer, SteerConeChangeRate, dt );
		this.inputSteer = inputSteer;

		float target = -inputSteer * MaxSteerAngle;
		if ( CarDirection > 0 )
			target -= VelocityAngle * CounterSteer;

		inputSteer = Math.Clamp( inputSteer, -1, 1 );
		Steering = inputSteer;
		SteerAngle = new( 0, target, 0 );
	}
}