using System;
using Sandbox;
namespace Meteor.VehicleTool;

public static class MathM
{
	public static float ExpDecay( float a, float b, float decay, float dt ) => b + (a - b) * MathF.Exp( -decay * dt );
	public static float AngleDifference( float a, float b ) => ((((b - a) % 360) + 540) % 360) - 180;
	public static float ExpDecayAngle( float a, float b, float decay, float dt ) => ExpDecay( a, a + AngleDifference( a, b ), decay, dt );

	/// <summary>
	///     Converts angular velocity (rad/s) to rotations per minute.
	/// </summary>
	public static float AngularVelocityToRPM( this float angularVelocity ) => angularVelocity * 9.5492965855137f;

	/// <summary>
	///     Converts rotations per minute to angular velocity (rad/s).
	/// </summary>
	public static float RPMToAngularVelocity( this float RPM ) => RPM * 0.10471975511966f;


	public static float Map( float x, float a, float b, float c, float d ) => (x - a) / (b - a) * (d - c) + c;

	public static float Fade( float n, float min, float mid, float max )
	{
		if ( n < min || n > max )
			return 0;

		if ( n > mid )
			min = mid - (max - mid);

		return MathF.Cos( (1 - ((n - min) / (mid - min))) * (MathF.PI / 2) );
	}

	public static Vector3 MeterToInch( this Vector3 v ) => new( v.x.MeterToInch(), v.y.MeterToInch(), v.z.MeterToInch() );
	public static Vector3 InchToMeter( this Vector3 v ) => new( v.x.InchToMeter(), v.y.InchToMeter(), v.z.InchToMeter() );
	public static Vector3 InchToMillimeter( this Vector3 v ) => new( v.x.InchToMillimeter(), v.y.InchToMillimeter(), v.z.InchToMillimeter() );
	public static Vector3 MillimeterToInch( this Vector3 v ) => new( v.x.MillimeterToInch(), v.y.MillimeterToInch(), v.z.MillimeterToInch() );

	public static float SignedAngle( this 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 struct LinearSmoothFloat( float inRate = 1, float outRate = 1, float inValue = 0 )
	{
		public float value = inValue;
		public float Get( float sample, float dt )
		{
			var dif = sample - value;
			if ( dif * value >= 0 )
			{
				value += dif * Math.Min( outRate * dt / Math.Abs( dif ), 1 );
			}
			else
				value += dif * Math.Min( inRate * dt / Math.Abs( dif ), 1 );

			return Math.Clamp( value, -1, 1 );
		}
	}
	public struct NonLinearSmoothFloat( float inRate = 1, float outRate = 1, float inValue = 0 )
	{
		public float value = inValue;
		public float Get( float sample, float dt )
		{
			var dif = sample - value;
			float ratedt;
			if ( dif * value >= 0 )
				ratedt = outRate * dt;
			else
				ratedt = inRate * dt;

			value += dif * ratedt / (1 + ratedt);
			return value;
		}
	}

	public struct SpringFloat( float spring = 10, float damp = 2, float inValue = 0 )
	{
		public float value = inValue;
		public float vel;
		public float Get( float sample, float dt )
		{
			vel = vel * Math.Max( 1 - damp * dt, 0 ) + (sample - value) * Math.Min( spring * dt, 1 / dt );
			value += vel * dt;

			return value;
		}
	}

}