using Sandbox;
using Sandbox.Services;
using System;
using System.Threading;

namespace VeloX;
public class Pacejka
{
	public class PacejkaPreset
	{
		[KeyProperty] public float B { get; set; } = 10.86f;
		[KeyProperty] public float C { get; set; } = 2.15f;
		[KeyProperty] public float D { get; set; } = 0.933f;
		[KeyProperty] public float E { get; set; } = 0.992f;

		public float Evaluate( float slip ) => D * MathF.Sin( C * MathF.Atan( B * slip - E * (B * slip - MathF.Atan( B * slip )) ) );

		public float GetPeakSlip()
		{
			float peakSlip = -1;
			float yMax = 0;

			for ( float i = 0; i < 1f; i += 0.01f )
			{
				float y = Evaluate( i );
				if ( y > yMax )
				{
					yMax = y;
					peakSlip = i;
				}
			}

			return peakSlip;
		}

	}

	public PacejkaPreset Lateral { get; set; } = new();
	public PacejkaPreset Longitudinal { get; set; } = new();

	public float PacejkaFx( float slip ) => Longitudinal.Evaluate( slip );
	public float PacejkaFy( float slip ) => Lateral.Evaluate( slip );
}