namespace AdvancedMappingTools;
/// <summary>Read-only contact graph of selected mesh surfaces. Corrections require a separate undoable action.</summary>
public static class ConnectionInspector
{
	public sealed record Contact( int A, int B, float Gap, Vector3 PointA, Vector3 PointB );
	public sealed class Report
	{
		public List<MeshComponent> Meshes { get; } = [];
		public List<List<int>> Groups { get; } = [];
		public List<Contact> Near { get; } = [];
		public bool Complete { get; set; } = true;
		internal List<(Transform World, int Geometry)> Snapshots { get; } = [];

		public bool Matches() => Meshes.Count == Snapshots.Count && Meshes.Select( ( m, i ) => m.IsValid() && m.Mesh != null && m.WorldTransform.Equals( Snapshots[i].World ) && PartHash.Geometry( m.Mesh ) == Snapshots[i].Geometry ).All( x => x );
		public string Summary => $"{Meshes.Count} mesh parts; {Groups.Count} contact group(s)." + (Complete ? "" : " Limit reached: partial result; correction disabled.");
	}

	readonly record struct Tri( Vector3 A, Vector3 B, Vector3 C );
	static Vector3 Closest( Vector3 p, Tri t )
	{
		var ab = t.B - t.A;
		var ac = t.C - t.A;
		var ap = p - t.A;
		var d1 = ab.Dot( ap );
		var d2 = ac.Dot( ap );
		if ( d1 <= 0 && d2 <= 0 )
			return t.A;
		var bp = p - t.B;
		var d3 = ab.Dot( bp );
		var d4 = ac.Dot( bp );
		if ( d3 >= 0 && d4 <= d3 )
			return t.B;
		var vc = d1 * d4 - d3 * d2;
		if ( vc <= 0 && d1 >= 0 && d3 <= 0 )
			return t.A + ab * (d1 / (d1 - d3));
		var cp = p - t.C;
		var d5 = ab.Dot( cp );
		var d6 = ac.Dot( cp );
		if ( d6 >= 0 && d5 <= d6 )
			return t.C;
		var vb = d5 * d2 - d1 * d6;
		if ( vb <= 0 && d2 >= 0 && d6 <= 0 )
			return t.A + ac * (d2 / (d2 - d6));
		var va = d3 * d6 - d5 * d4;
		if ( va <= 0 && (d4 - d3) >= 0 && (d5 - d6) >= 0 )
			return t.B + (t.C - t.B) * ((d4 - d3) / ((d4 - d3) + (d5 - d6)));
		var denominator = va + vb + vc;
		if ( MathF.Abs( denominator ) < 1e-12f )
			return t.A;
		return t.A + ab * (vb / denominator) + ac * (vc / denominator);
	}

	static bool Pierce( Vector3 a, Vector3 b, Tri t, out Vector3 point )
	{
		point = default;
		var dir = b - a;
		var edge = t.B - t.A;
		var cross = Vector3.Cross( dir, t.C - t.A );
		var det = edge.Dot( cross );
		if ( MathF.Abs( det ) < 1e-7f )
			return false;
		var inv = 1 / det;
		var p = a - t.A;
		var u = p.Dot( cross ) * inv;
		if ( u < 0 || u > 1 )
			return false;
		var q = Vector3.Cross( p, edge );
		var v = dir.Dot( q ) * inv;
		if ( v < 0 || u + v > 1 )
			return false;
		var d = (t.C - t.A).Dot( q ) * inv;
		if ( d < 0 || d > 1 )
			return false;
		point = a + dir * d;
		return true;
	}

	static (Vector3 A, Vector3 B) Edges( Vector3 p, Vector3 q, Vector3 a, Vector3 b )
	{
		var u = q - p;
		var v = b - a;
		var w = p - a;
		var aa = u.Dot( u );
		var bb = u.Dot( v );
		var cc = v.Dot( v );
		var dd = u.Dot( w );
		var ee = v.Dot( w );
		if ( aa < 1e-10f || cc < 1e-10f )
			return (p, a);
		var denominator = aa * cc - bb * bb;
		var s = denominator > 1e-10f ? Math.Clamp( (bb * ee - cc * dd) / denominator, 0, 1 ) : 0;
		var t = (bb * s + ee) / cc;
		if ( t < 0 )
		{
			t = 0;
			s = Math.Clamp( -dd / aa, 0, 1 );
		}
		else if ( t > 1 )
		{
			t = 1;
			s = Math.Clamp( (bb - dd) / aa, 0, 1 );
		}

		return (p + u * s, a + v * t);
	}

	static Tri[] Triangles( MeshComponent mesh ) => mesh.Mesh.FaceHandles.SelectMany( face =>
	{
		var vertices = mesh.Mesh.CreateFace( face, mesh.WorldTransform, Color.White );
		if ( vertices is null )
			return Array.Empty<Tri>();
		return Enumerable.Range( 0, vertices.Length / 3 ).Select( i => new Tri( vertices[i * 3].Position, vertices[i * 3 + 1].Position, vertices[i * 3 + 2].Position ) ).ToArray();
	} ).ToArray();
	static float BoxGap( BBox a, BBox b )
	{
		var d = Vector3.Max( Vector3.Max( a.Mins - b.Maxs, b.Mins - a.Maxs ), Vector3.Zero );
		return d.Length;
	}

	public static Report Scan( IEnumerable<GameObject> selection, float tolerance = .25f, float maxGap = 32 )
	{
		var report = new Report();
		var all = selection.Where( o => o.IsValid() ).SelectMany( o => o.Components.GetAll<MeshComponent>( FindMode.EverythingInSelfAndDescendants ) ).Where( m => m.IsValid() && m.Active && m.Mesh != null ).Distinct().ToList();
		if ( all.Count > 256 )
			report.Complete = false;
		report.Meshes.AddRange( all.Take( 256 ) );
		report.Snapshots.AddRange( report.Meshes.Select( m => (m.WorldTransform, PartHash.Geometry( m.Mesh )) ) );
		var n = report.Meshes.Count;
		var parents = Enumerable.Range( 0, n ).ToArray();
		int Root( int i )
		{
			while ( parents[i] != i )
			{
				parents[i] = parents[parents[i]];
				i = parents[i];
			}

			return i;
		}

		var triangles = report.Meshes.Select( Triangles ).ToArray();
		var bounds = report.Meshes.Select( m => m.Mesh.CalculateBounds( m.WorldTransform ) ).ToArray();
		var budget = 2000000;
		for ( var i = 0; i < n; i++ )
			for ( var j = i + 1; j < n; j++ )
			{
				if ( BoxGap( bounds[i], bounds[j] ) > MathF.Max( tolerance, maxGap ) )
					continue;
				var gap = float.PositiveInfinity;
				var pa = Vector3.Zero;
				var pb = Vector3.Zero;
				void Keep( Vector3 a, Vector3 b )
				{
					var d = a.Distance( b );
					if ( d < gap )
					{
						gap = d;
						pa = a;
						pb = b;
					}
				}

				foreach ( var a in triangles[i] )
				{
					foreach ( var b in triangles[j] )
					{
						if ( --budget < 0 )
						{
							report.Complete = false;
							break;
						}

						var av = new[]
						{
							a.A,
							a.B,
							a.C
						};
						var bv = new[]
						{
							b.A,
							b.B,
							b.C
						};
						foreach ( var p in av )
							Keep( p, Closest( p, b ) );
						foreach ( var p in bv )
							Keep( Closest( p, a ), p );
						for ( var k = 0; k < 3; k++ )
						{
							if ( Pierce( av[k], av[(k + 1) % 3], b, out var hit ) || Pierce( bv[k], bv[(k + 1) % 3], a, out hit ) )
							{
								Keep( hit, hit );
								break;
							}

							for ( var l = 0; l < 3; l++ )
							{
								var pair = Edges( av[k], av[(k + 1) % 3], bv[l], bv[(l + 1) % 3] );
								Keep( pair.A, pair.B );
							}
						}

						if ( gap <= .0001f )
							break;
					}

					if ( budget < 0 || gap <= .0001f )
						break;
				}

				if ( budget < 0 )
					continue;
				if ( gap <= tolerance )
					parents[Root( j )] = Root( i );
				if ( gap <= maxGap )
					report.Near.Add( new( i, j, gap, pa, pb ) );
			}

		report.Groups.AddRange( Enumerable.Range( 0, n ).GroupBy( Root ).Select( g => g.ToList() ).OrderByDescending( g => g.Count ) );
		return report;
	}

	public static Contact NearestGap( Report report, int group, float tolerance = .25f )
	{
		if ( !report.Complete || group < 0 || group >= report.Groups.Count )
			return null;
		var ids = report.Groups[group].ToHashSet();
		return report.Near.Where( c => ids.Contains( c.A ) != ids.Contains( c.B ) && c.Gap > tolerance ).OrderBy( c => c.Gap ).FirstOrDefault();
	}

	static bool Descends( GameObject child, GameObject ancestor )
	{
		for ( var p = child.Parent; p != null; p = p.Parent )
			if ( p == ancestor )
				return true;
		return false;
	}

	public static List<GameObject> Movers( Report report, int group )
	{
		var objects = report.Groups[group].Select( i => report.Meshes[i].GameObject ).Distinct().ToList();
		return objects.Where( o => !objects.Any( parent => parent != o && Descends( o, parent ) ) ).ToList();
	}

	public static Vector3 Correction( Report report, int group, Contact contact ) => report.Groups[group].Contains( contact.A ) ? contact.PointB - contact.PointA : contact.PointA - contact.PointB;
}

[ToolCategory( "Editing" ), Title( "Connection Inspector" ), Icon( "hub" )]
public sealed class ConnectionInspectorEditor( PrimitiveTool tool ) : BuilderEditor( tool )
{
	[Property, Group( "Inspection" ), Range( .01f, 4, slider: false ), WideMode]
	public float Tolerance { get; set; } = .25f;

	[Property, Group( "Inspection" ), Range( 1, 128, slider: false ), WideMode]
	public float MaximumGap { get; set; } = 32;

	[Property, Group( "Inspection" ), Range( 0, 255, slider: false ), WideMode]
	public int Group { get; set; } = 1;

	[Property, Group( "Inspection" ), WideMode]
	public bool ShowMarkers { get; set; } = true;

	[Property, Group( "Ground Reference" ), WideMode]
	public bool CheckGround { get; set; }

	[Property, Group( "Ground Reference" ), Range( -8192, 8192, slider: false ), WideMode]
	public float GroundHeight { get; set; }
	public override IReadOnlySet<string> UnusedOptions => EditorTypeLibrary.GetType( typeof(BuilderEditor) ).Properties.Where( p => p.HasAttribute<PropertyAttribute>() ).Select( p => p.Name ).ToHashSet();

	ConnectionInspector.Report report;
	protected override int MinPoints => 0;
	public override bool CanBuild => false;

	public override PolygonMesh Build() => null;
	protected override void BuildMesh( MeshKit kit )
	{
	}

	public override void OnUpdate( SceneTrace trace )
	{
		if ( !ShowMarkers || report is null )
			return;
		for ( var g = 0; g < report.Groups.Count; g++ )
		{
			Gizmo.Draw.Color = g == 0 ? Color.Green : Color.Yellow;
			foreach ( var i in report.Groups[g] )
			{
				var m = report.Meshes[i];
				if ( m.IsValid() && m.Mesh != null )
					DrawLabel( m.Mesh.CalculateBounds( m.WorldTransform ).Center, $"Contact group {g}" + (CheckGround && m.Mesh.CalculateBounds( m.WorldTransform ).Mins.z > GroundHeight + Tolerance ? " (above ground reference)" : ""), g == 0 ? Color.Green : Color.Yellow );
			}
		}
	}

	public override Widget CreateWidget() => new Sheet( this );
	sealed class Sheet : ToolSidebarWidget
	{
		readonly SerializedObject _serialized;
		readonly ConnectionInspectorEditor editor;
		readonly Label status;
		public Sheet( ConnectionInspectorEditor e )
		{
			editor = e;
			var group = AddGroup( "Selected Mesh Connections" );
			group.Add( new Label( "Select one assembly, then scan. Separate contact groups are candidates, not automatically defects. Scan never changes objects." ) { WordWrap = true } );
			group.Add( new ListButton( "Scan Selection", "search" ) { Clicked = Scan } );
			group.Add( new ListButton( "Select Contact Group", "select_all" ) { Clicked = Select } );
			group.Add( new ListButton( "Place Group On Ground Reference", "vertical_align_bottom" ) { Clicked = Ground } );
			group.Add( new ListButton( "Snap Group To Nearest Surface", "link" ) { Clicked = Snap } );
			status = group.Add( new Label( "No scan yet." ) { WordWrap = true } );
			_serialized = e.GetSerialized();
			PropertySheet.Fill( this, _serialized, "Inspection Settings", e.HiddenOptions );
		}

		void Scan()
		{
			editor.report = ConnectionInspector.Scan( SceneEditorSession.Active.Selection.OfType<GameObject>(), editor.Tolerance, editor.MaximumGap );
			status.Text = editor.report.Summary;
		}

		void Select()
		{
			var r = editor.report;
			if ( r is null || editor.Group < 0 || editor.Group >= r.Groups.Count )
				return;
			var selection = SceneEditorSession.Active.Selection;
			selection.Clear();
			foreach ( var i in r.Groups[editor.Group] )
				if ( r.Meshes[i].IsValid() )
					selection.Add( r.Meshes[i].GameObject );
		}

		void Ground()
		{
			var r = editor.report;
			if ( !editor.CheckGround || r is null || !r.Complete || editor.Group < 0 || editor.Group >= r.Groups.Count || !r.Matches() )
			{
				status.Text = "Enable ground reference and scan a valid group first.";
				return;
			}

			var bottom = r.Groups[editor.Group].Min( i => r.Meshes[i].Mesh.CalculateBounds( r.Meshes[i].WorldTransform ).Mins.z );
			var delta = Vector3.Up * (editor.GroundHeight - bottom);
			var objects = ConnectionInspector.Movers( r, editor.Group );
			using ( SceneEditorSession.Scope() )
			using ( SceneEditorSession.Active.UndoScope( "Place Contact Group On Ground" ).WithGameObjectChanges( objects, GameObjectUndoFlags.All ).Push() )
				foreach ( var go in objects )
					go.WorldPosition += delta;
			editor.report = ConnectionInspector.Scan( r.Meshes.Select( m => m.GameObject ), editor.Tolerance, editor.MaximumGap );
			status.Text = editor.report.Summary;
		}

		void Snap()
		{
			var r = editor.report;
			if ( r is null || !r.Matches() )
			{
				status.Text = "Scan again: objects changed.";
				return;
			}

			r = ConnectionInspector.Scan( r.Meshes.Select( m => m.GameObject ), editor.Tolerance, editor.MaximumGap );
			editor.report = r;
			var contact = ConnectionInspector.NearestGap( r, editor.Group, editor.Tolerance );
			if ( contact is null )
			{
				status.Text = "No safe nearby correction. Check group number or scan limits.";
				return;
			}

			var movers = ConnectionInspector.Movers( r, editor.Group );
			var delta = ConnectionInspector.Correction( r, editor.Group, contact );
			using ( SceneEditorSession.Scope() )
			using ( SceneEditorSession.Active.UndoScope( "Snap Contact Group" ).WithGameObjectChanges( movers, GameObjectUndoFlags.All ).Push() )
				foreach ( var go in movers )
					go.WorldPosition += delta;
			editor.report = ConnectionInspector.Scan( r.Meshes.Select( m => m.GameObject ), editor.Tolerance, editor.MaximumGap );
			status.Text = editor.report.Summary;
		}
	}
}