using System;
using System.Collections.Generic;
using Sandbox;

namespace BrickJam;

/// <summary>
/// Collide-and-slide character mover for the Scene System — a re-implementation of the legacy
/// Entity-System <c>Sandbox.MoveHelper</c> (which has no Scene-System equivalent). No bounce; slides
/// along surfaces, steps over small ledges, and can unstick. The owner supplies a <paramref name="sweep"/>
/// closure that runs a capsule trace (with the right tags/ignores) between two foot positions.
/// </summary>
public struct MoveHelper
{
	public Vector3 Position;
	public Vector3 Velocity;
	public float MaxStandableAngle;
	public bool HitWall;

	private readonly Func<Vector3, Vector3, SceneTraceResult> sweep;

	public MoveHelper( Vector3 position, Vector3 velocity, Func<Vector3, Vector3, SceneTraceResult> sweep )
	{
		Position = position;
		Velocity = velocity;
		MaxStandableAngle = 70f;
		HitWall = false;
		this.sweep = sweep;
	}

	public readonly SceneTraceResult TraceDirection( Vector3 direction ) => sweep( Position, Position + direction );

	private readonly bool IsFloor( Vector3 normal ) => Vector3.GetAngle( Vector3.Up, normal ) <= MaxStandableAngle;

	private static Vector3 ClipVelocity( Vector3 velocity, Vector3 normal )
	{
		var backoff = Vector3.Dot( velocity, normal );
		return velocity - normal * backoff;
	}

	/// <summary>Slide the body by <c>Velocity * timestep</c>, clipping against everything it hits.</summary>
	public float TryMove( float timestep )
	{
		var planes = new List<Vector3>( 5 );
		var primalVelocity = Velocity;
		var timeLeft = timestep;
		var traveled = 0f;
		HitWall = false;

		for ( var bump = 0; bump < 4; bump++ )
		{
			if ( Velocity.IsNearlyZero( 0.001f ) )
				break;

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

			if ( tr.Fraction > 0f )
			{
				Position = tr.EndPosition;
				traveled += tr.Fraction;
				planes.Clear();
			}

			if ( tr.Fraction >= 1f )
				break;

			timeLeft -= timeLeft * tr.Fraction;

			if ( !IsFloor( tr.Normal ) )
				HitWall = true;

			if ( planes.Count >= 5 )
			{
				Velocity = Vector3.Zero;
				break;
			}

			planes.Add( tr.Normal );

			// Clip against every plane we've touched this move so we slide along creases.
			var clipped = Velocity;
			foreach ( var plane in planes )
				clipped = ClipVelocity( clipped, plane );

			Velocity = clipped;

			// Reversing direction means we're wedged - stop to avoid jitter.
			if ( Vector3.Dot( Velocity, primalVelocity ) <= 0f )
			{
				Velocity = Vector3.Zero;
				break;
			}
		}

		return traveled;
	}

	/// <summary>Move, but also try stepping up <paramref name="stepSize"/> and keep whichever goes further.</summary>
	public float TryMoveWithStep( float timestep, float stepSize )
	{
		var startPos = Position;
		var startVel = Velocity;

		// Plain move.
		var fraction = TryMove( timestep );
		var flatPos = Position;
		var flatVel = Velocity;
		var flatDist = (flatPos - startPos).WithZ( 0 ).Length;

		// Stepped move: up -> across -> down onto a floor.
		Position = startPos;
		Velocity = startVel;

		var up = sweep( startPos, startPos + Vector3.Up * stepSize );
		Position = up.EndPosition;
		TryMove( timestep );

		var down = sweep( Position, Position + Vector3.Down * stepSize );
		var stepValid = down.Hit && IsFloor( down.Normal );
		if ( stepValid )
			Position = down.EndPosition;

		var stepDist = stepValid ? (Position - startPos).WithZ( 0 ).Length : -1f;

		// Keep the stepped result only if it made more horizontal progress.
		if ( !stepValid || flatDist >= stepDist )
		{
			Position = flatPos;
			Velocity = flatVel;
		}
		else
		{
			Velocity = flatVel; // preserve horizontal speed when stepping up
		}

		return fraction;
	}

	/// <summary>Nudge out of geometry if we started embedded. Returns true if free.</summary>
	public bool TryUnstuck()
	{
		if ( !sweep( Position, Position ).StartedSolid )
			return true;

		for ( var i = 1; i <= 20; i++ )
		{
			var offset = (Vector3.Random.WithZ( Vector3.Random.z.Clamp( 0f, 1f ) )).Normal * i;
			if ( !sweep( Position + offset, Position + offset ).StartedSolid )
			{
				Position += offset;
				return true;
			}
		}

		return false;
	}
}