namespace Machines.Race;

/// <summary>
/// Baked racing line: ordered 3D waypoints with distance-based lookup and closest-point queries.
/// </summary>
[Serializable]
public sealed class RacingLine
{
	/// <summary>
	/// Ordered waypoints forming this racing line (loops: last connects back to first).
	/// </summary>
	public List<Vector3> Points { get; set; } = new();

	/// <summary>
	/// Cumulative arc distances per waypoint index (index 0 = 0, index i = distance from start to i).
	/// </summary>
	public List<float> CumulativeDistances { get; set; } = new();

	/// <summary>
	/// Total length of the racing line (full loop).
	/// </summary>
	public float TotalLength { get; set; }

	/// <summary>
	/// Curvature (1/radius) sampled at <see cref="CurvatureStep"/> intervals; index = (int)(distance / CurvatureStep).
	/// </summary>
	public List<float> Curvatures { get; set; } = new();

	/// <summary>
	/// Arc-length spacing (units) between entries in <see cref="Curvatures"/>.
	/// </summary>
	public const float CurvatureStep = 50f;

	/// <summary>
	/// Arc-length offset on each side of a sample for curvature estimation (large enough to ignore micro-segment noise).
	/// </summary>
	private const float CurvatureSampleSpan = 80f;

	/// <summary>
	/// Section metadata per contiguous segment (source spline, gap flag, surface type). Empty for checkpoint-only paths.
	/// </summary>
	public List<PathSegmentInfo> Segments { get; set; } = new();

	/// <summary>
	/// Surface hints (friction, flags) at the same intervals as <see cref="Curvatures"/>; built by <see cref="RebuildHints"/>.
	/// </summary>
	public List<PathHint> Hints { get; set; } = new();

	/// <summary>
	/// Friction threshold below which <see cref="PathHintFlags.LowGrip"/> is flagged.
	/// </summary>
	public const float LowGripThreshold = 0.7f;

	/// <summary>
	/// Curvature threshold above which <see cref="PathHintFlags.Corner"/> is flagged (~700 unit radius).
	/// </summary>
	public const float CornerCurvatureThreshold = 0.004f;

	/// <summary>
	/// Whether this racing line has valid data.
	/// </summary>
	public bool IsValid => Points.Count >= 2 && CumulativeDistances.Count == Points.Count;

	/// <summary>
	/// Rebuild cumulative distances from the current points list.
	/// </summary>
	public void RebuildDistances()
	{
		CumulativeDistances.Clear();

		if ( Points.Count == 0 )
		{
			TotalLength = 0f;
			return;
		}

		float cumulative = 0f;
		CumulativeDistances.Add( 0f );

		for ( int i = 1; i < Points.Count; i++ )
		{
			cumulative += (Points[i] - Points[i - 1]).Length;
			CumulativeDistances.Add( cumulative );
		}

		// Closing segment (last -> first)
		TotalLength = cumulative + (Points[0] - Points[^1]).Length;

		RebuildCurvatures();
	}

	/// <summary>
	/// Recompute the <see cref="Curvatures"/> table using Menger curvature at fixed arc-length intervals.
	/// </summary>
	public void RebuildCurvatures()
	{
		Curvatures.Clear();

		if ( Points.Count < 2 || TotalLength < 0.001f )
			return;

		var count = (int)(TotalLength / CurvatureStep) + 1;
		for ( int i = 0; i < count; i++ )
		{
			var d = i * CurvatureStep;
			var a = GetPointAtDistance( d - CurvatureSampleSpan ).WithZ( 0f );
			var b = GetPointAtDistance( d ).WithZ( 0f );
			var c = GetPointAtDistance( d + CurvatureSampleSpan ).WithZ( 0f );

			Curvatures.Add( MengerCurvature( a, b, c ) );
		}
	}

	private static float MengerCurvature( Vector3 a, Vector3 b, Vector3 c )
	{
		var ab = (b - a).Length;
		var bc = (c - b).Length;
		var ca = (a - c).Length;

		var denom = ab * bc * ca;
		if ( denom < 0.001f )
			return 0f;

		// 4*Area = 2*|cross(b-a, c-a)|
		var cross = Vector3.Cross( b - a, c - a ).Length;
		return (2f * cross) / denom;
	}

	/// <summary>
	/// Curvature (1/radius) at the given arc distance, wrapped. Larger = tighter corner.
	/// </summary>
	public float GetCurvatureAtDistance( float distance )
	{
		if ( Curvatures.Count == 0 )
			return 0f;

		distance %= TotalLength;
		if ( distance < 0f )
			distance += TotalLength;

		var idx = (int)(distance / CurvatureStep);
		idx = idx.Clamp( 0, Curvatures.Count - 1 );
		return Curvatures[idx];
	}

	/// <summary>
	/// Max safe cornering speed at distance for a given lateral acceleration budget (v = sqrt(a/k)).
	/// </summary>
	public float GetSafeSpeedAtDistance( float distance, float latAccel )
	{
		var k = GetCurvatureAtDistance( distance );
		if ( k < 0.00001f )
			return float.MaxValue;

		return MathF.Sqrt( latAccel / k );
	}

	/// <summary>
	/// Flat tangent direction at the given arc distance (wrapped).
	/// </summary>
	public Vector3 GetTangentAtDistance( float distance )
	{
		var ahead = GetPointAtDistance( distance + 10f );
		var behind = GetPointAtDistance( distance - 10f );
		var dir = (ahead - behind).WithZ( 0f );
		return dir.LengthSquared > 0.0001f ? dir.Normal : Vector3.Forward;
	}

	/// <summary>
	/// Position on the racing line at the given arc distance (wrapped).
	/// </summary>
	public Vector3 GetPointAtDistance( float distance )
	{
		if ( Points.Count < 2 )
			return Points.Count > 0 ? Points[0] : Vector3.Zero;

		// Wrap
		distance %= TotalLength;
		if ( distance < 0f )
			distance += TotalLength;

		// Find enclosing segment
		for ( int i = 0; i < Points.Count - 1; i++ )
		{
			if ( distance <= CumulativeDistances[i + 1] )
			{
				var segStart = CumulativeDistances[i];
				var segLength = CumulativeDistances[i + 1] - segStart;
				var t = segLength > 0.001f ? (distance - segStart) / segLength : 0f;
				return Vector3.Lerp( Points[i], Points[i + 1], t );
			}
		}

		// Closing segment (last -> first)
		var lastDist = CumulativeDistances[^1];
		var closingLength = TotalLength - lastDist;
		var closingT = closingLength > 0.001f ? (distance - lastDist) / closingLength : 0f;
		return Vector3.Lerp( Points[^1], Points[0], closingT );
	}

	/// <summary>
	/// Point on the Catmull-Rom spline at the given arc distance (smoother than <see cref="GetPointAtDistance"/>).
	/// </summary>
	public Vector3 GetSplinePointAtDistance( float distance )
	{
		if ( Points.Count < 2 )
			return Points.Count > 0 ? Points[0] : Vector3.Zero;

		distance %= TotalLength;
		if ( distance < 0f )
			distance += TotalLength;

		for ( int i = 0; i < Points.Count - 1; i++ )
		{
			if ( distance <= CumulativeDistances[i + 1] )
			{
				var segStart = CumulativeDistances[i];
				var segLength = CumulativeDistances[i + 1] - segStart;
				var t = segLength > 0.001f ? (distance - segStart) / segLength : 0f;
				return TrackSpline.GetPoint( Points, i, t );
			}
		}

		// Closing segment (last -> first)
		var lastDist = CumulativeDistances[^1];
		var closingLength = TotalLength - lastDist;
		var closingT = closingLength > 0.001f ? (distance - lastDist) / closingLength : 0f;
		return TrackSpline.GetPoint( Points, Points.Count - 1, closingT );
	}

	/// <summary>
	/// Approximate arc distance for a world position projected onto the nearest segment.
	/// </summary>
	public float GetDistanceAtPosition( Vector3 position )
	{
		return GetDistanceAtPosition( position, -1f, 0f );
	}

	/// <summary>
	/// Arc distance for a position, constrained to a forward window to avoid snapping to parallel sections.
	/// </summary>
	public float GetDistanceAtPosition( Vector3 position, float lastKnownDistance, float searchWindow = 500f )
	{
		if ( Points.Count < 2 )
			return 0f;

		var bestDist = float.MaxValue;
		var bestLineDistance = 0f;

		for ( int i = 0; i < Points.Count; i++ )
		{
			// Skip segments outside the forward search window
			if ( lastKnownDistance >= 0f )
			{
				var segDist = i < CumulativeDistances.Count ? CumulativeDistances[i] : 0f;
				var ahead = segDist - lastKnownDistance;
				if ( ahead < 0f ) ahead += TotalLength; // wrapping
				if ( ahead > searchWindow )
					continue;
			}

			var next = (i + 1) % Points.Count;
			var segStart = Points[i];
			var segEnd = Points[next];
			var segDir = segEnd - segStart;
			var segLen = segDir.Length;

			if ( segLen < 0.001f )
				continue;

			var toPos = position - segStart;
			var t = MathX.Clamp( Vector3.Dot( toPos, segDir ) / (segLen * segLen), 0f, 1f );
			var projected = segStart + segDir * t;
			var dist = (position - projected).LengthSquared;

			if ( dist < bestDist )
			{
				bestDist = dist;
				var segStartDist = i < CumulativeDistances.Count ? CumulativeDistances[i] : 0f;
				bestLineDistance = segStartDist + t * segLen;
			}
		}

		return bestLineDistance;
	}

	/// <summary>
	/// <see cref="PathSegmentInfo"/> at the given arc distance, or null if no metadata was baked.
	/// </summary>
	public PathSegmentInfo GetSegmentAtDistance( float distance )
	{
		if ( Segments.Count == 0 )
			return null;

		distance %= TotalLength;
		if ( distance < 0f )
			distance += TotalLength;

		foreach ( var seg in Segments )
		{
			if ( seg.Contains( distance ) )
				return seg;
		}

		// Fallback: last segment handles floating-point edge at TotalLength
		return Segments[^1];
	}

	/// <summary>
	/// <see cref="PathHint"/> at the given arc distance, or <see cref="PathHint.Default"/> if unbaked.
	/// </summary>
	public PathHint GetHintAtDistance( float distance )
	{
		if ( Hints.Count == 0 )
			return PathHint.Default;

		distance %= TotalLength;
		if ( distance < 0f )
			distance += TotalLength;

		var idx = (int)(distance / CurvatureStep);
		idx = idx.Clamp( 0, Hints.Count - 1 );
		return Hints[idx];
	}

	/// <summary>
	/// Rebuild the <see cref="Hints"/> table; call after <see cref="RebuildDistances"/>.
	/// </summary>
	public void RebuildHints( Scene scene )
	{
		Hints.Clear();

		if ( Points.Count < 2 || TotalLength < 0.001f || scene == null )
			return;

		var count = (int)(TotalLength / CurvatureStep) + 1;
		for ( int i = 0; i < count; i++ )
		{
			var d = i * CurvatureStep;
			var pos = GetPointAtDistance( d );

			// Downward trace to find surface
			var start = pos + Vector3.Up * 200f;
			var end = pos - Vector3.Up * 200f;

			var tr = scene.Trace
				.Ray( start, end )
				.Run();

			var hint = new PathHint { Friction = 1f, Flags = PathHintFlags.None };

			if ( !tr.Hit )
			{
				hint.Flags |= PathHintFlags.Airborne;
				hint.Friction = 0f;
			}
			else
			{
				// Collider override > surface friction > default (matches CarMovement.ProbeGround)
				var collider = tr.GameObject?.GetComponent<Collider>();
				var friction = collider != null && collider.Friction.HasValue
					? collider.Friction.Value
					: (tr.Surface?.Friction ?? 1f);
				hint.Friction = friction;

				if ( hint.Friction < LowGripThreshold )
					hint.Flags |= PathHintFlags.LowGrip;
			}

			// Flag corners via curvature
			if ( i < Curvatures.Count && Curvatures[i] >= CornerCurvatureThreshold )
				hint.Flags |= PathHintFlags.Corner;

			Hints.Add( hint );
		}
	}
}