using System;
using Sandbox;
using static Diamonds.DiamondBoard;

namespace Diamonds;

public sealed partial class DiamondDebris
{
	static bool NearDiamond( Vector2 position, float radius, Vector2 center ) =>
		MathF.Abs( position.x - center.x ) < Width / 2 + radius &&
		MathF.Abs( position.y - center.y ) < Height / 2 + radius;

	static void TransformShard( Particle shard, Span<Vector2> vertices )
		=> TransformShard( shard, shard.Body.Transform, vertices );

	static void TransformShard( Particle shard, Transform transform, Span<Vector2> vertices )
	{
		var position = transform.Position;
		var rotation = transform.Rotation;
		for ( int i = 0; i < vertices.Length; i++ )
		{
			var vertex = shard.Vertices[i];
			var point = position + rotation * new Vector3( vertex.x, vertex.y, 0 );
			vertices[i] = new( point.x, point.y );
		}
	}

	bool ClearOfDiamonds( ReadOnlySpan<Vector2> vertices, Vector2 position, float radius )
	{
		foreach ( var diamond in crushDiamonds )
		{
			if ( !NearDiamond( position, radius, diamond.Center ) ) continue;
			if ( Separation( vertices, diamond.Center ).Length > 0.55f ) return false;
		}
		return true;
	}

	bool RestoreClearShardPose( Particle shard, Span<Vector2> vertices )
	{
		if ( !shard.HasClearTransform ) return false;
		var position = shard.LastClearTransform.Position;
		TransformShard( shard, shard.LastClearTransform, vertices );
		if ( !ClearOfDiamonds( vertices, new Vector2( position.x, position.y ), shard.Radius ) ) return false;
		shard.Body.Transform = shard.LastClearTransform;
		// Discard the solver's squeeze impulse, otherwise it repeats next step.
		// Diamond containment takes precedence over transient shard overlap.
		shard.Body.Velocity = Vector3.Zero;
		shard.Body.AngularVelocity = Vector3.Zero;
		shard.Body.Sleeping = false;
		return true;
	}

	static float PolygonArea( ReadOnlySpan<Vector2> vertices )
	{
		float area = 0;
		for ( int i = 1; i + 1 < vertices.Length; i++ )
		{
			var a = vertices[i] - vertices[0];
			var b = vertices[i + 1] - vertices[0];
			area += a.x * b.y - a.y * b.x;
		}
		return MathF.Abs( area ) * 0.5f;
	}

	static float CoveredArea( ReadOnlySpan<Vector2> vertices, Vector2 center )
	{
		// Clip the actual rotated polygon against all four diamond half-planes.
		// Fixed scratch buffers avoid allocations in this per-contact path.
		Span<Vector2> input = stackalloc Vector2[12];
		Span<Vector2> output = stackalloc Vector2[12];
		vertices.CopyTo( input );
		int count = vertices.Length;
		for ( int face = 0; face < 4 && count > 0; face++ )
		{
			var normal = new Vector2( (face & 1) == 0 ? Height : -Height, (face & 2) == 0 ? Width : -Width );
			int written = 0;
			var previous = input[count - 1];
			float previousDistance = Vector2.Dot( previous - center, normal ) - Width * Height / 2;
			for ( int i = 0; i < count; i++ )
			{
				var current = input[i];
				float distance = Vector2.Dot( current - center, normal ) - Width * Height / 2;
				if ( (distance <= 0) != (previousDistance <= 0) )
					output[written++] = previous + (current - previous) * (previousDistance / (previousDistance - distance));
				if ( distance <= 0 ) output[written++] = current;
				previous = current;
				previousDistance = distance;
			}
			count = written;
			var swap = input; input = output; output = swap;
		}
		return PolygonArea( input[..count] );
	}

	static Vector2 Separation( ReadOnlySpan<Vector2> vertices, Vector2 center )
	{
		float depth = float.PositiveInfinity;
		Vector2 correction = default;
		for ( int axis = 0; axis < vertices.Length + 2; axis++ )
		{
			Vector2 normal;
			if ( axis < 2 ) normal = new Vector2( Height, axis == 0 ? Width : -Width ).Normal;
			else
			{
				int edgeIndex = axis - 2;
				var edge = vertices[(edgeIndex + 1) % vertices.Length] - vertices[edgeIndex];
				normal = new Vector2( edge.y, -edge.x ).Normal;
			}
			float extent = MathF.Max( MathF.Abs( normal.x ) * Width / 2, MathF.Abs( normal.y ) * Height / 2 );
			float min = float.PositiveInfinity, max = float.NegativeInfinity;
			foreach ( var vertex in vertices )
			{
				float projection = Vector2.Dot( vertex - center, normal );
				min = MathF.Min( min, projection ); max = MathF.Max( max, projection );
			}
			float forward = extent - min, backward = max + extent;
			if ( forward <= 0 || backward <= 0 ) return Vector2.Zero;
			float overlap = MathF.Min( forward, backward );
			if ( overlap >= depth ) continue;
			depth = overlap;
			correction = normal * (forward < backward ? overlap + 0.05f : -overlap - 0.05f);
		}
		return correction;
	}

	void ResolveShardOverlaps()
	{
#if STANDALONE
#endif
		Span<Vector2> polygon = stackalloc Vector2[8];
		Span<Vector2> candidate = stackalloc Vector2[8];
		foreach ( var shard in particles )
		{
			if ( shard.IsSpark ) continue;
			var position = shard.Position;
			var vertices = polygon[..shard.Vertices.Length];
			bool transformed = false;
			foreach ( var diamond in crushDiamonds )
			{
				if ( !NearDiamond( position, shard.Radius, diamond.Center ) ) continue;
				if ( !transformed ) { TransformShard( shard, vertices ); transformed = true; }
				var correction = Separation( vertices, diamond.Center );
				if ( correction.Length <= 0.55f ) continue; // Ordinary Box2D contact slop.
				var moved = candidate[..vertices.Length];
				bool clear = true;
				for ( int i = 0; i < vertices.Length; i++ )
				{
					moved[i] = vertices[i] + correction;
					if ( !HasRoom( moved[i], 0 ) ) clear = false;
				}
				if ( !clear ) continue;
				foreach ( var other in crushDiamonds )
				{
					if ( !NearDiamond( position + correction, shard.Radius, other.Center ) ) continue;
					if ( Separation( moved, other.Center ).Length > 0.55f ) { clear = false; break; }
				}
				if ( !clear ) continue; // Never resolve one face by pushing into another gem.
				shard.Body.Position += new Vector3( correction.x, correction.y, 0 );
				shard.Body.Sleeping = false;
				var normal = new Vector3( correction.x, correction.y, 0 ).Normal;
				var velocity = shard.Body.Velocity;
				float closing = Vector3.Dot( velocity - new Vector3( diamond.Velocity.x, diamond.Velocity.y, 0 ), normal );
				if ( closing < 0 ) shard.Body.Velocity = velocity - normal * closing;
				shard.LastClearTransform = shard.Body.Transform;
				shard.HasClearTransform = true;
				break;
			}
		}
	}
}