namespace Machines.Race;

/// <summary>
/// Static utility for Catmull-Rom spline evaluation along a track defined by checkpoints.
/// </summary>
public static class TrackSpline
{
	/// <summary>
	/// Evaluate one Catmull-Rom segment from 4 control points at parameter t (0..1).
	/// </summary>
	public static Vector3 CatmullRom( Vector3 p0, Vector3 p1, Vector3 p2, Vector3 p3, float t )
	{
		var t2 = t * t;
		var t3 = t2 * t;
		return 0.5f * (
			(2f * p1) +
			(-p0 + p2) * t +
			(2f * p0 - 5f * p1 + 4f * p2 - p3) * t2 +
			(-p0 + 3f * p1 - 3f * p2 + p3) * t3
		);
	}

	/// <summary>
	/// Get a point on a looping track spline at segment <paramref name="segmentIndex"/> and local t (0..1).
	/// </summary>
	public static Vector3 GetPoint( IReadOnlyList<Vector3> points, int segmentIndex, float t )
	{
		var count = points.Count;
		if ( count < 2 )
			return count > 0 ? points[0] : Vector3.Zero;

		// Wrap indices for the loop
		var i0 = ((segmentIndex - 1) % count + count) % count;
		var i1 = ((segmentIndex) % count + count) % count;
		var i2 = ((segmentIndex + 1) % count + count) % count;
		var i3 = ((segmentIndex + 2) % count + count) % count;

		return CatmullRom( points[i0], points[i1], points[i2], points[i3], t );
	}

	/// <summary>
	/// Get a point at a normalized track position (0..1 across all segments).
	/// </summary>
	public static Vector3 GetPointNormalized( IReadOnlyList<Vector3> points, float normalizedT )
	{
		var count = points.Count;
		if ( count < 2 )
			return count > 0 ? points[0] : Vector3.Zero;

		var segments = count;
		var scaled = normalizedT * segments;
		var segmentIndex = (int)MathF.Floor( scaled );
		segmentIndex = ((segmentIndex) % count + count) % count;
		var t = scaled - MathF.Floor( scaled );

		return GetPoint( points, segmentIndex, t );
	}
}