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

namespace Diamonds;

public sealed partial class DiamondDebris
{
	void ConstrainParticles()
	{
#if STANDALONE
#endif
		foreach ( var chip in particles )
		{
			var body = chip.Body;
			if ( body.Sleeping ) continue;
			var velocity = body.Velocity;
			// Fast keyframed gems and crowded seams can impart huge solver impulses.
			// Bound those kicks as well as normal flight, before and after each step.
			float maxSpeed = chip.IsSpark ? MaxSparkSpeed : 1000;
			if ( velocity.LengthSquared > maxSpeed * maxSpeed )
			{
				velocity = velocity.Normal * maxSpeed;
				body.Velocity = velocity;
			}
			if ( chip.IsSpark ) continue;

			// Solid walls handle ordinary bounces. Correct any solver penetration
			// using the rotated triangle, so even its tips stay inside the canvas.
			float minX = float.PositiveInfinity, maxX = float.NegativeInfinity, minY = float.PositiveInfinity;
			float floorPenetration = 0;
			var position = body.Position;
			var rotation = body.Rotation;
			for ( int i = 0; i < chip.Vertices.Length; i++ )
			{
				var vertex = chip.Vertices[i];
				var point = position + rotation * new Vector3( vertex.x, vertex.y, 0 );
				Accumulate( point );
				AccumulateFloor( point );
				minY = MathF.Min( minY, point.y );
				var next = chip.Vertices[(i + 1) % chip.Vertices.Length];
				var end = position + rotation * new Vector3( next.x, next.y, 0 );
				// A triangle edge can cross a tooth tip even when both vertices clear it.
				for ( float y = BoardHeight; y + Height * 0.5f > 0; y -= Height )
				{
					if ( y <= MathF.Min( point.y, end.y ) || y >= MathF.Max( point.y, end.y ) ) continue;
					Accumulate( point + (end - point) * ((y - point.y) / (end.y - point.y)) );
				}
				float cornerY = BoardHeight - Height * 0.5f;
				if ( cornerY > MathF.Min( point.y, end.y ) && cornerY < MathF.Max( point.y, end.y ) )
					Accumulate( point + (end - point) * ((cornerY - point.y) / (end.y - point.y)) );
				// Check floor peaks crossed by an edge as well as its endpoints.
				for ( float x = MathF.Ceiling( MathF.Min( point.x, end.x ) / Width ) * Width;
					x < MathF.Max( point.x, end.x ); x += Width )
				{
					if ( x <= MathF.Min( point.x, end.x ) ) continue;
					AccumulateFloor( point + (end - point) * ((x - point.x) / (end.x - point.x)) );
				}
			}
			void AccumulateFloor( Vector3 point )
			{
				float x = Math.Clamp( point.x, 0, BoardWidth );
				float floor = BoardHeight - Height * 0.5f * MathF.Abs( 1 - x / (Width * 0.5f) % 2 );
				floorPenetration = MathF.Max( floorPenetration, point.y - floor );
			}
			void Accumulate( Vector3 point )
			{
				float inset = SideWallInset( point.y, FillCornerPockets );
				minX = MathF.Min( minX, point.x - inset );
				maxX = MathF.Max( maxX, point.x + inset );
			}
			float dx = minX < 0 ? -minX : maxX > BoardWidth ? BoardWidth - maxX : 0;
			// Leave ordinary contact slop to the solver so resting piles can sleep.
			float dy = minY < 0 ? -minY : floorPenetration > 0.5f ? -floorPenetration : 0;
			if ( dx == 0 && dy == 0 ) continue;
			body.Position += new Vector3( dx, dy, 0 );
			if ( dx * velocity.x < 0 ) velocity.x *= -0.24f;
			if ( dy * velocity.y < 0 ) velocity.y *= -0.24f;
			body.Velocity = velocity;
		}
	}
}