using System;
using System.Buffers;

namespace Skateboard.Physics;

public struct SkateHelper
{
	public Vector3 Position;
	public Vector3 Velocity;

	public bool HitWall;
	public bool HitFloor;
	public bool HitPhysics;

	public Vector3 HitNormal;

	public float GroundBounce;
	public float WallBounce;
	public float MaxStandableAngle;

	public SceneTrace Trace;
	public GameObject Myself;

	public SkateHelper( SceneTrace trace, GameObject myself, Vector3 position, Vector3 velocity, params string[] solidTags )
	{
		this = default;

		Myself = myself;
		Position = position;
		Velocity = velocity;

		GroundBounce = 0f;
		WallBounce = 0f;
		MaxStandableAngle = 75f;

		Trace = trace
			.WithAnyTags( solidTags )
			.IgnoreGameObjectHierarchy( myself );
	}

	public SkateHelper( SceneTrace trace, GameObject myself, Vector3 position, Vector3 velocity )
		: this(
			trace,
			myself,
			position,
			velocity,
			"solid", "playerclip", "player",
			"passbullets", "unskateable",
			"skateable", "vert"
		)
	{
	}

	public SceneTraceResult TraceFromTo( Vector3 start, Vector3 end )
		=> Trace.FromTo( start, end ).Run();

	public SceneTraceResult TraceMove( Vector3 delta )
	{
		var tr = TraceFromTo( Position, Position + delta );
		Position = tr.EndPosition;
		return tr;
	}

	public float TryMove( float timestep, bool inAir )
	{
		var timeLeft = timestep;
		var travelFraction = 0f;

		HitWall = HitFloor = HitPhysics = false;

		using var moveplanes = new VelocityClipPlanes( Velocity );

		for ( int bump = 0; bump < moveplanes.Max; bump++ )
		{
			if ( Velocity.Length.AlmostEqual( 0 ) )
				break;

			var tr = TraceFromTo( Position, Position + Velocity * timeLeft );

			if ( bump == 0 && tr.Hit )
			{
				HitPhysics = tr.GameObject?.Tags.Has( "dynamicprop" ) ?? false;

				if ( tr.Normal.Angle( Vector3.Up ) >= MaxStandableAngle )
				{
					HitWall = true;

					if ( !inAir && (HasTag( tr, "skateable" ) || HasTag( tr, "vert" )) )
					{
						HitWall = false;
						HitFloor = true;
					}
				}
				else
				{
					HitFloor = true;
				}
			}

			travelFraction += tr.Fraction;

			if ( tr.Fraction > 1f / 32f )
			{
				Position = tr.EndPosition + tr.Normal * 0.001f;
				if ( tr.Fraction == 1f )
					break;

				moveplanes.StartBump( Velocity );
			}

			if ( travelFraction == 0f )
			{
				Position += tr.Normal * Time.Delta;
			}

			if ( bump == 0 && tr.Hit )
				HitNormal = tr.Normal;

			if ( bump == 0 && tr.Hit && HasTag( tr, "unskateable" ) )
			{
				HitWall = true;
			}

			timeLeft -= timeLeft * tr.Fraction;

			if ( !moveplanes.TryAdd(
				tr.Normal,
				ref Velocity,
				IsFloor( tr ) ? GroundBounce : WallBounce
			) )
				break;
		}

		if ( travelFraction == 0 )
			Velocity = 0;

		return travelFraction;
	}

	public float TryMoveWithStep( float timeDelta, float stepSize, bool inAir )
	{
		var startPosition = Position;
		var stepMove = this;

		var fraction = TryMove( timeDelta, inAir );

		stepMove.TraceMove( Vector3.Up * stepSize );
		var stepFraction = stepMove.TryMove( timeDelta, inAir );
		var tr = stepMove.TraceMove( Vector3.Down * stepSize );

		if ( !tr.Hit )
			return fraction;

		if ( tr.Normal.Angle( Vector3.Up ) > MaxStandableAngle )
			return fraction;

		if ( startPosition.Distance( Position.WithZ( startPosition.z ) ) > startPosition.Distance( stepMove.Position.WithZ( startPosition.z ) ) )
			return fraction;

		Position = stepMove.Position;
		Velocity = stepMove.Velocity;
		HitWall = stepMove.HitWall;

		return stepFraction;
	}

	public bool IsFloor( SceneTraceResult tr )
		=> tr.Hit && tr.Normal.Angle( Vector3.Up ) < MaxStandableAngle;

	public void ApplyFriction( float friction, float delta )
	{
		const float StopSpeed = 100f;

		var speed = Velocity.Length;
		if ( speed < 0.1f )
			return;

		var drop = (speed < StopSpeed ? StopSpeed : speed) * delta * friction;
		var newSpeed = MathF.Max( speed - drop, 0f );

		if ( newSpeed != speed )
			Velocity *= newSpeed / speed;
	}

	public bool TryUnstuck()
	{
		if ( !TraceFromTo( Position, Position ).StartedSolid )
			return true;

		for ( int i = 1; i < 20; i++ )
			if ( TryUnstuckAt( Position + Vector3.Up * i ) )
				return true;

		for ( int i = 1; i < 100; i++ )
			if ( TryUnstuckAt( Position + Vector3.Random * i ) )
				return true;

		return false;
	}

	private bool TryUnstuckAt( Vector3 tryPos )
	{
		var tr = TraceFromTo( tryPos, Position );
		if ( tr.StartedSolid )
			return false;

		Position = tryPos + tr.Direction.Normal * (tr.Distance - 0.5f);
		Velocity = 0;
		return true;
	}

	private static bool HasTag( SceneTraceResult tr, string tag )
	{
		if ( tr.Tags is null )
			return false;

		return tr.Tags.Contains( tag, StringComparer.OrdinalIgnoreCase );
	}

	private struct VelocityClipPlanes : IDisposable
	{
		private Vector3 originalVelocity;
		private Vector3 bumpVelocity;
		private Vector3[] planes;

		public int Max { get; private set; }
		public int Count { get; private set; }

		public VelocityClipPlanes( Vector3 originalVelocity, int max = 5 )
		{
			Max = max;
			this.originalVelocity = originalVelocity;
			bumpVelocity = originalVelocity;
			planes = ArrayPool<Vector3>.Shared.Rent( max );
			Count = 0;
		}

		public bool TryAdd( Vector3 normal, ref Vector3 velocity, float bounce )
		{
			if ( Count == Max )
			{
				velocity = 0f;
				return false;
			}

			planes[Count++] = normal;

			if ( Count == 1 )
			{
				bumpVelocity = ClipVelocity( bumpVelocity, normal, 1f + bounce );
				velocity = bumpVelocity;
				return true;
			}

			velocity = bumpVelocity;

			if ( TryClip( ref velocity ) )
			{
				if ( Count != 2 )
				{
					velocity = Vector3.Zero;
					return true;
				}

				var dir = Vector3.Cross( planes[0], planes[1] );
				velocity = dir.Normal * dir.Dot( velocity );
			}

			if ( velocity.Dot( originalVelocity ) < 0f )
				velocity = 0f;

			return true;
		}

		private bool TryClip( ref Vector3 velocity )
		{
			for ( int i = 0; i < Count; i++ )
			{
				velocity = ClipVelocity( bumpVelocity, planes[i] );
				if ( MovingTowardsAnyPlane( velocity, i ) )
					return false;
			}

			return true;
		}

		private bool MovingTowardsAnyPlane( Vector3 velocity, int iSkip )
		{
			for ( int j = 0; j < Count; j++ )
			{
				if ( j == iSkip )
					continue;
				if ( velocity.Dot( planes[j] ) < 0f )
					return false;
			}

			return true;
		}

		public void StartBump( Vector3 velocity )
		{
			bumpVelocity = velocity;
			Count = 0;
		}

		private static Vector3 ClipVelocity( Vector3 vel, Vector3 norm, float overbounce = 1f )
		{
			var backoff = Vector3.Dot( vel, norm ) * overbounce;
			var o = vel - norm * backoff;
			var adjust = Vector3.Dot( o, norm );

			if ( adjust >= 1f )
				return o;

			adjust = MathF.Min( adjust, -1f );
			return o - norm * adjust;
		}

		public void Dispose()
		{
			ArrayPool<Vector3>.Shared.Return( planes );
		}
	}
}