using Sandbox;
using Sandbox.Services;
using System;

namespace VeloX;
public class Pacejka
{
	public struct LateralForce()
	{
		[Description( "Shape factor" )]
		[Range( 1, 3 )] public float a0 = 1.4f;        // 0

		[Description( "Load infl on lat friction coeff (*1000) (1/kN)" )]
		[Range( -100, 100 )] public float a1 = -0f;    // 1

		[Description( "Lateral friction coefficient at load = 0 (*1000)" )]
		[Range( 1, 2500 )] public float a2 = 1688f;    // 2

		[Description( "Maximum stiffness (N/deg)" )]
		[Range( 1, 5000 )] public float a3 = 2400f;    // 3

		[Description( "Load at maximum stiffness (kN)" )]
		[Range( -100, 100 )] public float a4 = 6.026f; // 4

		[Description( "Camber infiuence on stiffness (%/deg/100)" )]
		[Range( -10, 10 )] public float a5 = 0f;       // 5

		[Description( "Curvature change with load" )]
		[Range( -10, 10 )] public float a6 = -0.359f;  // 6

		[Description( "Curvature at load = 0" )]
		[Range( -10, 10 )] public float a7 = 1.0f;     // 7

		[Description( "Horizontal shift because of camber (deg/deg)" )]
		[Range( -10, 10 )] public float a8 = 0f;       // 8

		[Description( "Load influence on horizontal shift (deg/kN)" )]
		[Range( -10, 10 )] public float a9 = -0.00611f;// 9

		[Description( "Horizontal shift at load = 0 (deg)" )]
		[Range( -10, 10 )] public float a10 = -0.0322f;// 10

		[Description( "Camber influence on vertical shift (N/deg/kN)" )]
		[Range( -10, 100 )] public float a111 = 0f;    // 11

		[Description( "Camber influence on vertical shift (N/deg/kN**2" )]
		[Range( -10, 10 )] public float a112 = 0f;     // 12

		[Description( "Load influence on vertical shift (N/kN)" )]
		[Range( -100, 100 )] public float a12 = 0f;    // 13

		[Description( "Vertical shift at load = 0 (N)" )]
		[Range( -10, 10 )] public float a13 = 0f;      // 14
	}

	public struct LongitudinalForce()
	{
		[Description( "Shape factor" )]
		[Range( 1, 3 )] public float b0 = 1.65f;       // 0 

		[Description( "Load infl on long friction coeff (*1000) (1/kN)" )]
		[Range( -300, 300 )] public float b1 = 0f;     // 1 

		[Description( "Longitudinal friction coefficient at load = 0 (*1000)" )]
		[Range( 0, 10000 )] public float b2 = 1690f;    // 2 

		[Description( "Curvature factor of stiffness (N/%/kN**2)" )]
		[Range( -100, 100 )] public float b3 = 0f;     // 3 

		[Description( "Change of stiffness with load at load = 0 (N/%/kN)" )]
		[Range( -1000, 1000 )] public float b4 = 229f; // 4 

		[Description( "Change of progressivity of stiffness/load (1/kN)" )]
		[Range( -10, 10 )] public float b5 = 0f;       // 5 

		[Description( "Curvature change with load" )]
		[Range( -10, 10 )] public float b6 = 0f;       // 6 

		[Description( "Curvature change with load" )]
		[Range( -10, 10 )] public float b7 = 0f;       // 7 

		[Description( "Curvature at load = 0" )]
		[Range( -10, 10 )] public float b8 = -10f;     // 7 

		[Description( "Load influence on horizontal shift (%/kN)" )]
		[Range( -10, 10 )] public float b9 = 0f;       // 9 

		[Description( "Horizontal shift at load = 0 (%)" )]
		[Range( -10, 10 )] public float b10 = 0f;      // 10 
	}


	public struct AligningMoment()
	{
		[Description( "Shape factor" )]
		[Range( 1, 7 )] public float c0 = 2.0f;        // 0

		[Description( "Load influence of peak value (Nm/kN**2)" )]
		[Range( -10, 10 )] public float c1 = -3.8f;    // 1

		[Description( "Load influence of peak value (Nm/kN)" )]
		[Range( -10, 10 )] public float c2 = -3.14f;   // 2

		[Description( "Curvature factor of stiffness (Nm/deg/kN**2" )]
		[Range( -10, 10 )] public float c3 = -1.16f;   // 3

		[Description( "Change of stiffness with load at load = 0 (Nm/deg/kN)" )]
		[Range( -100, 100 )] public float c4 = -7.2f;  // 4

		[Description( "Change of progressivity of stiffness/load (1/kN)" )]
		[Range( -10, 10 )] public float c5 = 0.0f;     // 5

		[Description( "Camber influence on stiffness (%/deg/100)" )]
		[Range( -10, 10 )] public float c6 = 0.0f;     // 6

		[Description( "Curvature change with load" )]
		[Range( -10, 10 )] public float c7 = 0.044f;   // 7

		[Description( "Curvature change with load" )]
		[Range( -10, 10 )] public float c8 = -0.58f;   // 8

		[Description( "Curvature at load = 0" )]
		[Range( -10, 10 )] public float c9 = 0.18f;    // 9

		[Description( "Camber influence of stiffness" )]
		[Range( -10, 10 )] public float c10 = 0.0f;    // 10

		[Description( "Camber influence on horizontal shift (deg/deg)" )]
		[Range( -10, 10 )] public float c11 = 0.0f;    // 11

		[Description( "Load influence on horizontal shift (deg/kN)" )]
		[Range( -10, 10 )] public float c12 = 0.0f;    // 12

		[Description( "Horizontal shift at load = 0 (deg)" )]
		[Range( -10, 10 )] public float c13 = 0.0f;    // 13

		[Description( "Camber influence on vertical shift (Nm/deg/kN**2" )]
		[Range( -10, 10 )] public float c14 = 0.14f;   // 14

		[Description( "Camber influence on vertical shift (Nm/deg/kN)" )]
		[Range( -10, 10 )] public float c15 = -1.029f; // 15

		[Description( "Load influence on vertical shift (Nm/kN)" )]
		[Range( -10, 10 )] public float c16 = 0.0f;    // 16

		[Description( "Vertical shift at load = 0 (Nm)" )]
		[Range( -10, 10 )] public float c17 = 0.0f;    // 17
	}
	public struct CombiningForce
	{
		public float gy1 = 1;     // 0
		public float gy2 = 1;    // 1
		public float gx1 = 1;   // 2
		public float gx2 = 1f;   // 3

		public CombiningForce()
		{
		}
	}


	public LateralForce Lateral = new();
	public LongitudinalForce Longitudinal = new();
	public AligningMoment Aligning = new();
	public CombiningForce Combining = new();



	/// pacejka magic formula for longitudinal force
	public float PacejkaFx( float sigma, float Fz, float friction_coeff )
	{
		var b = Longitudinal;

		// shape factor
		float C = b.b0;

		// peak factor
		float D = (b.b1 * Fz + b.b2) * Fz;

		// stiffness at sigma = 0
		float BCD = (b.b3 * Fz + b.b4) * Fz * MathF.Exp( -b.b5 * Fz );

		// stiffness factor
		float B = BCD / (C * D);

		// curvature factor
		float E = (b.b6 * Fz + b.b7) * Fz + b.b8;

		// horizontal shift
		float Sh = b.b9 * Fz + b.b10;

		// composite
		float S = 100 * sigma + Sh;

		// longitudinal force
		float BS = B * S;
		float Fx = D * MathF.Sin( C * MathF.Atan( BS - E * (BS - MathF.Atan( BS )) ) );

		// scale by surface friction
		Fx *= friction_coeff;

		return Fx;
	}

	/// pacejka magic formula for lateral force
	public float PacejkaFy( float alpha, float Fz, float gamma, float friction_coeff, out float camber_alpha )
	{
		var a = Lateral;

		// shape factor
		float C = a.a0;

		// peak factor
		float D = (a.a1 * Fz + a.a2) * Fz;

		// stiffness at alpha = 0
		float BCD = a.a3 * MathF.Atan2( Fz, a.a4 ) * (1 - a.a5 * MathF.Abs( gamma ));
		// stiffness factor
		float B = BCD / (C * D);

		// curvature factor
		float E = a.a6 * Fz + a.a7;

		// horizontal shift
		float Sh = a.a8 * gamma + a.a9 * Fz + a.a10;

		// vertical shift
		float Sv = ((a.a111 * Fz + a.a112) * gamma + a.a12) * Fz + a.a13;

		// composite slip angle
		float S = alpha + Sh;

		// lateral force
		float BS = B * S;
		float Fy = D * MathF.Sin( C * MathF.Atan( BS - E * (BS - MathF.Atan( BS )) ) ) + Sv;

		// scale by surface friction
		Fy *= friction_coeff;
		camber_alpha = Sh + Sv / BCD * friction_coeff;

		return Fy;
	}

	/// pacejka magic formula for aligning torque
	public float PacejkaMz( float alpha, float Fz, float gamma, float friction_coeff )
	{
		var c = Aligning;

		// shape factor
		float C = c.c0;

		// peak factor
		float D = (c.c1 * Fz + c.c2) * Fz;

		// stiffness at alpha = 0
		float BCD = (c.c3 * Fz + c.c4) * Fz * (1 - c.c6 * MathF.Abs( gamma )) * MathF.Exp( -c.c5 * Fz );

		// stiffness factor
		float B = BCD / (C * D);

		// curvature factor
		float E = (c.c7 * Fz * Fz + c.c8 * Fz + c.c9) * (1 - c.c10 * MathF.Abs( gamma ));

		// horizontal shift
		float Sh = c.c11 * gamma + c.c12 * Fz + c.c13;

		// composite slip angle
		float S = alpha + Sh;

		// vertical shift
		float Sv = (c.c14 * Fz * Fz + c.c15 * Fz) * gamma + c.c16 * Fz + c.c17;

		// self-aligning torque
		float BS = B * S;
		float Mz = D * MathF.Sin( C * MathF.Atan( BS - E * (BS - MathF.Atan( BS )) ) ) + Sv;

		// scale by surface friction
		Mz *= friction_coeff;

		return Mz;
	}

	/// pacejka magic formula for the longitudinal combining factor
	public float PacejkaGx( float sigma, float alpha )
	{
		var p = Combining;
		float a = p.gx2 * sigma;
		float b = p.gx1 * alpha;
		float c = a * a + 1;
		return MathF.Sqrt( c / (c + b * b) );
	}

	/// pacejka magic formula for the lateral combining factor
	public float PacejkaGy( float sigma, float alpha )
	{
		var p = Combining;
		float a = p.gy2 * alpha;
		float b = p.gy1 * sigma;
		float c = a * a + 1;
		return MathF.Sqrt( c / (c + b * b) );
	}
}