using System;

namespace VeloX;

internal readonly struct Hinge( Vector3 hinge_anchor, Vector3 hinge_arm )
{
	[Description( "the point that the wheels are rotated around as the suspension compresses" )]
	public readonly Vector3 Anchor = hinge_anchor;

	[Description( "anchor to wheel vector" )]
	public readonly Vector3 Arm = hinge_arm;

	[Description( "arm length squared" )]
	public readonly float LengthSquared = hinge_arm.Dot( hinge_arm );

	[Description( "1 / arm length in hinge axis normal plane" )]
	public readonly float NormXY = 1 / MathF.Sqrt( hinge_arm.x * hinge_arm.x + hinge_arm.y * hinge_arm.y );

	public readonly Vector3 Rotate( float travel )
	{
		float z = Arm.z + travel;
		float lengthSq = MathF.Max( LengthSquared - z * z, 0.0f );
		float nxy = NormXY * MathF.Sqrt( lengthSq );
		return Anchor + new Vector3( Arm.x * nxy, Arm.y * nxy, z );
	}
}