using Sandbox;
using System;

namespace VeloX;
public class MacPhersonSuspension
{
	public readonly Vector3 WheelOffset;
	public readonly Vector3 UprightTop;
	public readonly Vector3 UprightAxis;
	private readonly Hinge Hinge;

	public MacPhersonSuspension( Vector3 wheel, Vector3 strutBody, Vector3 strutWheel, Vector3 hingeBody )
	{
		WheelOffset = wheel - strutWheel;
		UprightTop = strutBody;
		UprightAxis = (strutBody - strutWheel).Normal;
		Hinge = new( hingeBody, strutWheel - hingeBody );
	}

	public void GetWheelTransform( float travel, out Rotation rotation, out Vector3 position )
	{
		Vector3 hingeEnd = Hinge.Rotate( travel );
		Vector3 uprightAxisNew = (UprightTop - hingeEnd).Normal;

		rotation = Rotation.FromAxis(
			Vector3.Cross( UprightAxis, uprightAxisNew ),
			MathF.Acos( Vector3.Dot( UprightAxis, uprightAxisNew ) ).RadianToDegree()
		);

		position = hingeEnd + WheelOffset.Transform( rotation );
	}
}