using Sandbox.Utility;
using System;

namespace HC3.Rides;

#nullable enable

/// <summary>
/// Helper struct wrapping a <see cref="TrackElement"/> with information about its place in a <see cref="ITrackSection"/>.
/// </summary>
/// <param name="Index">Index of the element in <see cref="ITrackSection.Elements"/></param>
/// <param name="Element">The element in question.</param>
/// <param name="StartDistance">Distance along the track at the start of the element.</param>
/// <param name="EndDistance">Distance along the track at the end of the element.</param>
public readonly record struct PlacedTrackElement(
	int Index,
	TrackElement Element,
	float StartDistance,
	float EndDistance )
{
	public TrackFeature Feature => Element.Feature;
}

/// <summary>
/// Interface for a continuous section of track, made out of a sequence of <see cref="TrackElement"/>s.
/// </summary>
public interface ITrackSection
{
	/// <summary>
	/// Position and orientation of the track between each element.
	/// </summary>
	IReadOnlyList<TrackNode> Nodes { get; }

	/// <summary>
	/// Describes the shape of the track, and what special features it has.
	/// </summary>
	IReadOnlyList<TrackElement> Elements { get; }

	/// <summary>
	/// Total length of track in game units.
	/// </summary>
	float Length { get; }

	/// <summary>
	/// Gets the total length of the track from the first node to the given <paramref name="nodeIndex"/>.
	/// </summary>
	float GetDistanceAtNode( int nodeIndex );

	/// <summary>
	/// Does track obstruct the given <paramref name="trackGridPos"/>, optionally ignoring elements that neighbour <paramref name="ignoreNodes"/>.
	/// </summary>
	public bool HasObstruction( Vector3Int trackGridPos, IReadOnlySet<TrackNode>? ignoreNodes = null ) => true;

	Transform SampleAtDistance( float distance );
}

public static class TrackSectionExtensions
{
	/// <summary>
	/// Is this track a closed loop?
	/// </summary>
	public static bool IsCycle( this ITrackSection track ) =>
		track.Nodes.Count > 1 && track.Nodes[0] == track.Nodes[^1];

	public static int GetElementIndex( this ITrackSection track, float distance )
	{
		if ( track.Elements.Count < 1 ) return -1;

		if ( distance < 0f ) return 0;
		if ( distance >= track.Length ) return track.Elements.Count - 1;

		var minIndex = 0;
		var maxIndex = track.Elements.Count;

		while ( maxIndex - minIndex > 1 )
		{
			var midIndex = (maxIndex + minIndex) / 2;
			var midDistance = track.GetDistanceAtNode( midIndex );

			if ( midDistance.AlmostEqual( distance ) )
			{
				return midIndex;
			}

			if ( midDistance > distance )
			{
				maxIndex = midIndex;
			}
			else
			{
				minIndex = midIndex;
			}
		}

		return Math.Clamp( minIndex, 0, track.Elements.Count - 1 );
	}

	public static Transform SampleAtNode( this ITrackSection track, int nodeIndex ) =>
		track.SampleAtDistance( track.GetDistanceAtNode( nodeIndex ) );

	public static Transform SampleAtDistance( this ITrackSection track, float distance, float sampleOffset )
	{
		var frontSample = track.SampleAtDistance( distance + sampleOffset );
		var backSample = track.SampleAtDistance( distance - sampleOffset );

		var position = (frontSample.Position + backSample.Position) * 0.5f;
		var rotation = Rotation.Slerp( frontSample.Rotation, backSample.Rotation, 0.5f );

		return new Transform( position, rotation );
	}

	public static float NormalizeDistance( this ITrackSection track, float distance )
	{
		var trackLength = track.Length;

		return track.IsCycle()
			? distance - MathF.Floor( distance / trackLength ) * trackLength
			: Math.Clamp( distance, 0f, trackLength );
	}

	public static float NormalizeDelta( this ITrackSection track, float delta )
	{
		var trackLength = track.Length;

		return track.IsCycle()
			? delta - MathF.Floor( delta / trackLength + 0.5f ) * trackLength
			: delta;
	}

	public static Transform SampleAtDistance( this ITrackSection track, TrackDefinition? trackDef, Spline spline, float distance )
	{
		if ( spline.PointCount < 2 ) return Transform.Zero;

		distance = track.NormalizeDistance( distance );

		var elementIndex = track.GetElementIndex( distance );
		var prevDist = track.GetDistanceAtNode( elementIndex );
		var nextDist = track.GetDistanceAtNode( elementIndex + 1 );
		var element = track.Elements[elementIndex];
		var prevNode = track.Nodes[elementIndex];
		var nextNode = track.Nodes[elementIndex + 1];

		var invertedRoll = element.Direction is TurnDirection.Right ? 180f : -180f;

		return SampleTransform( spline, trackDef,
			prevNode.Orientation.Banking.Roll() + (prevNode.Orientation.Inverted ? invertedRoll : 0f),
			nextNode.Orientation.Banking.Roll() + (nextNode.Orientation.Inverted ? invertedRoll : 0f),
			distance, prevDist, nextDist );
	}

	private static Transform SampleTransform( Spline spline, TrackDefinition? trackDef,
		float prevRoll, float nextRoll,
		float distance, float prevDist, float nextDist )
	{
		var sample = spline.SampleAtDistance( distance );
		var pivotOffset = (trackDef?.RailHeight ?? 0f) + 8f;

		if ( nextRoll > prevRoll + 270f )
		{
			nextRoll -= 360f;
		}
		else if ( nextRoll < prevRoll - 270f )
		{
			nextRoll += 360f;
		}

		// Make roll super smooth instead of linearly interpolating

		var t = (distance - prevDist) / (nextDist - prevDist);

		if ( prevRoll is < -95f or > 95f )
		{
			t = Easing.SineEaseOut( t );
		}
		else if ( nextRoll is < -95f or > 95f )
		{
			t = Easing.SineEaseIn( t );
		}
		else
		{
			t = Easing.SineEaseInOut( t );
		}

		var roll = MathX.Lerp( prevRoll, nextRoll, t );
		var rotation = Rotation.LookAt( sample.Tangent, sample.Up ) * Rotation.FromRoll( roll );

		return new Transform( Vector3.Up * pivotOffset ).ToWorld( new Transform( sample.Position - rotation.Up * pivotOffset, rotation ) );
	}

	public static PlacedTrackElement? GetElements( this ITrackSection track, float headPosition, float length, List<PlacedTrackElement> elements )
	{
		if ( track.Elements.Count == 0 ) return null;

		var headIndex = track.GetElementIndex( track.NormalizeDistance( headPosition ) );
		var tailIndex = track.GetElementIndex( track.NormalizeDistance( headPosition - length ) );

		if ( tailIndex > headIndex )
		{
			headIndex += track.Elements.Count;
		}

		for ( var i = headIndex; i >= tailIndex; --i )
		{
			var index = i % track.Elements.Count;

			elements.Add( new PlacedTrackElement( index, track.Elements[index],
				track.GetDistanceAtNode( index ),
				track.GetDistanceAtNode( index + 1 ) ) );
		}

		if ( !track.IsCycle() && headIndex >= elements.Count - 1 )
		{
			return null;
		}

		var nextIndex = (headIndex + 1) % track.Elements.Count;

		return new PlacedTrackElement( nextIndex, track.Elements[nextIndex],
			track.GetDistanceAtNode( nextIndex ),
			track.GetDistanceAtNode( nextIndex + 1 ) );
	}
}