using Sandbox.Navigation;

namespace Machines.Race;

/// <summary>
/// Bridges unnavigable gaps and bakes the navmesh+checkpoint optimal line.
/// </summary>
public sealed class GapBridger
{
	private readonly RacingPath _path;
	private Scene Scene => _path.Scene;

	public GapBridger( RacingPath path )
	{
		_path = path;
	}

	/// <summary>
	/// Bridge a gap via navmesh with Hermite-smoothed ends; falls back to pure Hermite if no navmesh.
	/// When <paramref name="forceAerial"/> is set (an authored jump link), always arc through the air -
	/// never route along the ground via navmesh, which would dip onto whatever path is underneath.
	/// </summary>
	public List<Vector3> BridgeGap( Vector3 start, Vector3 end, Vector3 startTangent, Vector3 endTangent, bool forceAerial = false )
	{
		var gapLength = (end - start).Length;
		var tangentScale = gapLength * 0.4f;
		var zDiff = MathF.Abs( end.z - start.z );

		// Authored jump or significant Z diff: navmesh is 2D and ground-hugging, use a pure aerial Hermite arc.
		if ( forceAerial || zDiff > 40f )
		{
			return GenerateHermiteCurve( start, end, startTangent * tangentScale, endTangent * tangentScale, 10 );
		}

		List<Vector3> guidePoints = null;

		// CalculatePath requires an initialized navmesh; guard before calling.
		if ( Scene.NavMesh?.IsEnabled == true )
		{
			var path = Scene.NavMesh.CalculatePath( new CalculatePathRequest()
			{
				Start = start,
				Target = end
			} );

			if ( path.IsValid() && path.Points.Count >= 2 )
			{
				guidePoints = path.Points.Skip( 1 ).Select( p => p.Position ).ToList();
			}
		}

		// No navmesh guide: pure Hermite fallback.
		if ( guidePoints == null || guidePoints.Count < 2 )
		{
			return GenerateHermiteCurve( start, end, startTangent * tangentScale, endTangent * tangentScale, 10 );
		}

		// Blend navmesh guide with Hermite-smoothed ends matching spline tangents.
		var result = new List<Vector3>();
		var totalGuidePoints = guidePoints.Count;

		if ( totalGuidePoints <= 3 )
		{
			// Short gap: full Hermite (navmesh adds little value).
			return GenerateHermiteCurve( start, end, startTangent * tangentScale, endTangent * tangentScale, 10 );
		}

		// Ease-in: Hermite from start to ~30% of guide.
		var easeInTarget = guidePoints[totalGuidePoints / 3];
		var easeInTangentEnd = (guidePoints[totalGuidePoints / 3 + 1] - guidePoints[Math.Max( 0, totalGuidePoints / 3 - 1 )]).Normal;
		var easeInScale = (easeInTarget - start).Length * 0.5f;
		result.AddRange( GenerateHermiteCurve( start, easeInTarget, startTangent * easeInScale, easeInTangentEnd * easeInScale, 5 ) );

		// Middle: raw navmesh guide points.
		for ( int i = totalGuidePoints / 3 + 1; i < totalGuidePoints - totalGuidePoints / 3; i++ )
			result.Add( guidePoints[i] );

		// Ease-out: Hermite from ~70% of guide to end.
		var easeOutStart = guidePoints[totalGuidePoints - totalGuidePoints / 3];
		var easeOutTangentStart = (guidePoints[Math.Min( totalGuidePoints - 1, totalGuidePoints - totalGuidePoints / 3 + 1 )] -
								   guidePoints[Math.Max( 0, totalGuidePoints - totalGuidePoints / 3 - 1 )]).Normal;
		var easeOutScale = (end - easeOutStart).Length * 0.5f;
		result.AddRange( GenerateHermiteCurve( easeOutStart, end, easeOutTangentStart * easeOutScale, endTangent * easeOutScale, 5 ) );

		return result;
	}

	/// <summary>
	/// Generate <paramref name="segments"/> points along a cubic Hermite curve (excludes start, includes end).
	/// </summary>
	public static List<Vector3> GenerateHermiteCurve( Vector3 p0, Vector3 p1, Vector3 m0, Vector3 m1, int segments )
	{
		var points = new List<Vector3>( segments );
		for ( int i = 1; i <= segments; i++ )
		{
			var t = i / (float)segments;
			var t2 = t * t;
			var t3 = t2 * t;

			// Hermite basis
			var h00 = 2f * t3 - 3f * t2 + 1f;
			var h10 = t3 - 2f * t2 + t;
			var h01 = -2f * t3 + 3f * t2;
			var h11 = t3 - t2;

			points.Add( h00 * p0 + h10 * m0 + h01 * p1 + h11 * m1 );
		}
		return points;
	}

	/// <summary>
	/// Build the optimal line via navmesh between checkpoints, replacing gap-detours with parabolic arcs.
	/// </summary>
	public List<Vector3> BakeOptimalPath( List<Checkpoint> checkpoints, out List<bool> bridgeFlags )
	{
		var allPoints = new List<Vector3>();
		bridgeFlags = new List<bool>();

		for ( int i = 0; i < checkpoints.Count; i++ )
		{
			var start = checkpoints[i].WorldPosition;
			var end = checkpoints[(i + 1) % checkpoints.Count].WorldPosition;

			var hasNav = Scene.NavMesh?.IsEnabled == true;
			var path = hasNav
				? Scene.NavMesh.CalculatePath( new CalculatePathRequest()
				{
					Start = start,
					Target = end
				} )
				: default;

			if ( !hasNav || !path.IsValid() )
			{
				// No path: bridge with arc.
				var arc = GenerateArc( start, end );
				allPoints.AddRange( arc );
				for ( int j = 0; j < arc.Count; j++ )
					bridgeFlags.Add( true );
				continue;
			}

			var pathPoints = path.Points.Select( p => p.Position ).ToList();
			var straightDist = (end - start).WithZ( 0f ).Length;

			bool isGap = false;

			// Gap check 1: path/straight ratio (skip very short distances to avoid false positives).
			if ( straightDist > 50f )
			{
				var pathLength = ComputePathLength( pathPoints );
				if ( pathLength > straightDist * _path.GapDetectionRatio )
				{
					isGap = true;
					Log.Info( $"RacingPath: Gap detected (ratio) between checkpoints {i}→{(i + 1) % checkpoints.Count} " +
							  $"(path={pathLength:F0}, straight={straightDist:F0}, ratio={pathLength / straightDist:F2})" );
				}
			}

			// Gap check 2: backtrack detection.
			if ( !isGap && DetectsBacktrack( pathPoints, start, end ) )
			{
				isGap = true;
				Log.Info( $"RacingPath: Gap detected (backtrack) between checkpoints {i}→{(i + 1) % checkpoints.Count}" );
			}

			if ( isGap )
			{
				var arc = GenerateArc( start, end );
				allPoints.AddRange( arc );
				for ( int j = 0; j < arc.Count; j++ )
					bridgeFlags.Add( true );
			}
			else
			{
				// Normal navmesh path (skip last point, next segment starts there).
				for ( int p = 0; p < pathPoints.Count - 1; p++ )
				{
					allPoints.Add( pathPoints[p] );
					bridgeFlags.Add( false );
				}
			}
		}

		return allPoints;
	}

	/// <summary>
	/// True if the path backtracks relative to the start->end vector (horizontal projection only).
	/// </summary>
	private bool DetectsBacktrack( List<Vector3> points, Vector3 start, Vector3 end )
	{
		var delta = (end - start).WithZ( 0f );
		if ( delta.LengthSquared < 0.001f )
			return false;

		var dir = delta.Normal;
		float maxProjection = 0f;

		for ( int i = 1; i < points.Count; i++ )
		{
			var projection = Vector3.Dot( (points[i] - start).WithZ( 0f ), dir );

			if ( projection < maxProjection - _path.BacktrackTolerance )
				return true;

			maxProjection = MathF.Max( maxProjection, projection );
		}

		return false;
	}

	/// <summary>
	/// Parabolic arc between two points, height scaling with horizontal distance; excludes endpoint.
	/// </summary>
	public List<Vector3> GenerateArc( Vector3 start, Vector3 end )
	{
		var horizontalDist = (end - start).WithZ( 0f ).Length;
		var arcHeight = MathF.Max( _path.MinGapArcHeight, MathF.Min( horizontalDist * _path.GapArcHeightRatio, _path.MaxGapArcHeight ) );

		var points = new List<Vector3>( _path.GapArcSegments );
		for ( int i = 0; i < _path.GapArcSegments; i++ )
		{
			float t = i / (float)_path.GapArcSegments;
			var pos = Vector3.Lerp( start, end, t );
			// Parabola: 4*h*t*(1-t), peak h at t=0.5
			pos += Vector3.Up * (arcHeight * 4f * t * (1f - t));
			points.Add( pos );
		}

		return points;
	}

	private static float ComputePathLength( List<Vector3> points )
	{
		float length = 0f;
		for ( int i = 1; i < points.Count; i++ )
			length += (points[i] - points[i - 1]).Length;
		return length;
	}

	/// <summary>
	/// Get the outgoing tangent direction at the tail of the chain (last few points).
	/// </summary>
	public static Vector3 GetChainTailTangent( List<Vector3> chain )
	{
		if ( chain.Count < 2 )
			return Vector3.Forward;
		var dir = (chain[^1] - chain[^2]).Normal;
		return dir.LengthSquared > 0.001f ? dir : Vector3.Forward;
	}

	/// <summary>
	/// Get the incoming tangent direction at the head of a segment (first few points).
	/// </summary>
	public static Vector3 GetSegmentHeadTangent( List<Vector3> segment )
	{
		if ( segment.Count < 2 )
			return Vector3.Forward;
		var dir = (segment[1] - segment[0]).Normal;
		return dir.LengthSquared > 0.001f ? dir : Vector3.Forward;
	}
}