using System.Numerics;
using AutoRig.Mesh;

namespace AutoRig.Analyze;

// s&box compat: the engine defines Vector2/Vector3 in the GLOBAL namespace, which
// shadows using-directive imports - alias explicitly to System.Numerics.
using Vector2 = System.Numerics.Vector2;
using Vector3 = System.Numerics.Vector3;


/// <summary>
/// Finds contact regions between parts: for part pairs whose expanded bounds intersect,
/// surface samples of one part (vertices, edge midpoints, centroids) are tested against
/// the other part's triangles; ≥ 3 samples within tolerance form a contact.
/// </summary>
public static class PartContacts
{
    const int MaxPointsPerPair = 128;

    /// <summary>
    /// Detects contacts. Tolerance &lt;= 0 selects 0.5% of the whole-mesh bounds diagonal.
    /// Two tiers: a single global vertex-proximity pass (complete for finely tessellated
    /// meshes and O(V)), then per-pair surface sampling only for coarse-geometry pairs
    /// (long edges relative to tolerance) that vertex proximity can miss.
    /// </summary>
    public static IReadOnlyList<PartContact> Find(
        RigMesh mesh, IReadOnlyList<MeshPart> parts, float tolerance )
    {
        ArgumentNullException.ThrowIfNull( mesh );
        ArgumentNullException.ThrowIfNull( parts );

        if ( tolerance <= 0f )
            tolerance = 0.005f * mesh.ComputeBounds().Size.Length();
        if ( tolerance <= 0f || parts.Count < 2 )
            return [];

        // Triangle → part map and per-part longest edge.
        var triangleToPart = new int[mesh.TriangleCount];
        var maxEdge = new float[parts.Count];
        for ( var p = 0; p < parts.Count; p++ )
        {
            foreach ( var tri in parts[p].TriangleIndices )
            {
                triangleToPart[tri] = p;
                var a = mesh.Positions[mesh.Triangles[tri * 3]];
                var b = mesh.Positions[mesh.Triangles[tri * 3 + 1]];
                var c = mesh.Positions[mesh.Triangles[tri * 3 + 2]];
                maxEdge[p] = MathF.Max( maxEdge[p], Vector3.Distance( a, b ) );
                maxEdge[p] = MathF.Max( maxEdge[p], Vector3.Distance( b, c ) );
                maxEdge[p] = MathF.Max( maxEdge[p], Vector3.Distance( c, a ) );
            }
        }

        // Vertex → part (any triangle using it; parts don't share vertices post-segmentation).
        var vertexToPart = new int[mesh.Positions.Length];
        Array.Fill( vertexToPart, -1 );
        for ( var tri = 0; tri < mesh.TriangleCount; tri++ )
            for ( var k = 0; k < 3; k++ )
                vertexToPart[mesh.Triangles[tri * 3 + k]] = triangleToPart[tri];

        // ---- tier 1: global vertex-proximity pass ----
        var pairPoints = new Dictionary<(int A, int B), List<Vector3>>();
        var pairSeen = new Dictionary<(int A, int B), HashSet<(long, long, long)>>();
        var cell = tolerance;
        var vertexGrid = new Dictionary<(long, long, long), List<int>>();
        for ( var i = 0; i < mesh.Positions.Length; i++ )
        {
            if ( vertexToPart[i] < 0 )
                continue;
            var p = mesh.Positions[i];
            var key = (Floor( p.X, cell ), Floor( p.Y, cell ), Floor( p.Z, cell ));
            if ( !vertexGrid.TryGetValue( key, out var bucket ) )
                vertexGrid[key] = bucket = new List<int>();
            bucket.Add( i );
        }

        var toleranceSquared = tolerance * tolerance;
        for ( var i = 0; i < mesh.Positions.Length; i++ )
        {
            var partI = vertexToPart[i];
            if ( partI < 0 )
                continue;
            var p = mesh.Positions[i];
            var cx = Floor( p.X, cell );
            var cy = Floor( p.Y, cell );
            var cz = Floor( p.Z, cell );
            for ( var dx = -1L; dx <= 1; dx++ )
                for ( var dy = -1L; dy <= 1; dy++ )
                    for ( var dz = -1L; dz <= 1; dz++ )
                    {
                        if ( !vertexGrid.TryGetValue( (cx + dx, cy + dy, cz + dz), out var bucket ) )
                            continue;
                        foreach ( var j in bucket )
                        {
                            var partJ = vertexToPart[j];
                            if ( partJ <= partI ) // each unordered pair once; skips same-part too
                                continue;
                            if ( Vector3.DistanceSquared( p, mesh.Positions[j] ) > toleranceSquared )
                                continue;
                            AddPoint( pairPoints, pairSeen, (partI, partJ),
                                (p + mesh.Positions[j]) * 0.5f, tolerance );
                        }
                    }
        }

        // ---- tier 2: surface sampling for coarse pairs tier 1 could not certify ----
        for ( var a = 0; a < parts.Count; a++ )
        {
            for ( var b = a + 1; b < parts.Count; b++ )
            {
                if ( pairPoints.TryGetValue( (a, b), out var existing ) && existing.Count >= 3 )
                    continue; // tier 1 already confirmed
                if ( maxEdge[a] + maxEdge[b] <= 8f * tolerance )
                    continue; // tessellation finer than tolerance: tier 1 was conclusive
                if ( !parts[a].Bounds.Intersects( parts[b].Bounds, tolerance ) )
                    continue;

                var (sampled, target) = parts[a].SurfaceArea <= parts[b].SurfaceArea
                    ? (parts[a], parts[b])
                    : (parts[b], parts[a]);
                foreach ( var point in ContactPoints( mesh, sampled, target, tolerance ) )
                    AddPoint( pairPoints, pairSeen, (a, b), point, tolerance );
            }
        }

        // ---- assemble ----
        var contacts = new List<PartContact>();
        foreach ( var ((a, b), points) in pairPoints )
        {
            if ( points.Count < 3 )
                continue;

            var center = Vector3.Zero;
            foreach ( var p in points ) center += p;
            center /= points.Count;

            float extent = 0;
            foreach ( var p in points )
                extent = MathF.Max( extent, Vector3.Distance( p, center ) );

            var axes = PrincipalAxes( points, center );
            var planeNormal = axes[2];
            var smaller = parts[a].SurfaceArea <= parts[b].SurfaceArea ? parts[a] : parts[b];
            if ( Vector3.Dot( smaller.Bounds.Center - center, planeNormal ) < 0f )
                planeNormal = -planeNormal;

            contacts.Add( new PartContact
            {
                PartA = a,
                PartB = b,
                Center = center,
                Direction = axes[0],
                PlaneNormal = planeNormal,
                Extent = extent,
                SampleCount = points.Count,
            } );
        }
        contacts.Sort( ( x, y ) => (x.PartA, x.PartB).CompareTo( (y.PartA, y.PartB) ) );
        return contacts;
    }

    static void AddPoint(
        Dictionary<(int A, int B), List<Vector3>> pairPoints,
        Dictionary<(int A, int B), HashSet<(long, long, long)>> pairSeen,
        (int A, int B) pair, Vector3 point, float tolerance )
    {
        if ( !pairPoints.TryGetValue( pair, out var points ) )
        {
            pairPoints[pair] = points = new List<Vector3>();
            pairSeen[pair] = new HashSet<(long, long, long)>();
        }
        if ( points.Count >= MaxPointsPerPair )
            return;
        var q = (Floor( point.X, tolerance ), Floor( point.Y, tolerance ), Floor( point.Z, tolerance ));
        if ( pairSeen[pair].Add( q ) )
            points.Add( point );
    }

    /// <summary>Surface samples of <paramref name="sampled"/> within tolerance of <paramref name="target"/>'s triangles.</summary>
    static List<Vector3> ContactPoints( RigMesh mesh, MeshPart sampled, MeshPart target, float tolerance )
    {
        // Contacts can only exist inside the AABB overlap of the two parts — restrict
        // both the target grid and the surface samples to it, so per-pair cost scales
        // with the interface size, not the part size (a 100k-triangle chassis touched
        // by 50 small parts would otherwise be re-gridded whole 50 times).
        var overlapMin = Vector3.Max( sampled.Bounds.Min, target.Bounds.Min ) - new Vector3( tolerance * 2f );
        var overlapMax = Vector3.Min( sampled.Bounds.Max, target.Bounds.Max ) + new Vector3( tolerance * 2f );

        bool TriangleOverlaps( int tri )
        {
            var a = mesh.Positions[mesh.Triangles[tri * 3]];
            var b = mesh.Positions[mesh.Triangles[tri * 3 + 1]];
            var c = mesh.Positions[mesh.Triangles[tri * 3 + 2]];
            var min = Vector3.Min( a, Vector3.Min( b, c ) );
            var max = Vector3.Max( a, Vector3.Max( b, c ) );
            return min.X <= overlapMax.X && max.X >= overlapMin.X
                && min.Y <= overlapMax.Y && max.Y >= overlapMin.Y
                && min.Z <= overlapMax.Z && max.Z >= overlapMin.Z;
        }

        // Target triangles near the overlap, bucketed on a coarse grid for pruning.
        var cell = MathF.Max( tolerance * 8f, 1e-6f );
        var grid = new Dictionary<(long, long, long), List<int>>();
        foreach ( var tri in target.TriangleIndices )
        {
            if ( !TriangleOverlaps( tri ) )
                continue;
            var a = mesh.Positions[mesh.Triangles[tri * 3]];
            var b = mesh.Positions[mesh.Triangles[tri * 3 + 1]];
            var c = mesh.Positions[mesh.Triangles[tri * 3 + 2]];
            var min = Vector3.Min( a, Vector3.Min( b, c ) ) - new Vector3( tolerance );
            var max = Vector3.Max( a, Vector3.Max( b, c ) ) + new Vector3( tolerance );
            for ( var x = Floor( min.X, cell ); x <= Floor( max.X, cell ); x++ )
                for ( var y = Floor( min.Y, cell ); y <= Floor( max.Y, cell ); y++ )
                    for ( var z = Floor( min.Z, cell ); z <= Floor( max.Z, cell ); z++ )
                    {
                        if ( !grid.TryGetValue( (x, y, z), out var bucket ) )
                            grid[(x, y, z)] = bucket = new List<int>();
                        bucket.Add( tri );
                    }
        }
        if ( grid.Count == 0 )
            return [];

        // Enough points to estimate center/direction/extent; exhaustive coverage of a
        // large interface adds nothing. Stride keeps per-pair samples bounded.
        const int maxPoints = 128;
        const int maxSampledTriangles = 4000;

        var overlapTriangles = 0;
        foreach ( var tri in sampled.TriangleIndices )
            if ( TriangleOverlaps( tri ) )
                overlapTriangles++;
        var strideStep = Math.Max( 1, overlapTriangles / maxSampledTriangles );

        var points = new List<Vector3>();
        var seen = new HashSet<(long, long, long)>();
        var toleranceSquared = tolerance * tolerance;

        foreach ( var sample in SurfaceSamples( mesh, sampled, TriangleOverlaps, strideStep ) )
        {
            if ( points.Count >= maxPoints )
                break;
            var key = (Floor( sample.X, cell ), Floor( sample.Y, cell ), Floor( sample.Z, cell ));
            if ( !grid.TryGetValue( key, out var bucket ) )
                continue;
            foreach ( var tri in bucket )
            {
                var a = mesh.Positions[mesh.Triangles[tri * 3]];
                var b = mesh.Positions[mesh.Triangles[tri * 3 + 1]];
                var c = mesh.Positions[mesh.Triangles[tri * 3 + 2]];
                var closest = ClosestPointOnTriangle( sample, a, b, c );
                if ( Vector3.DistanceSquared( sample, closest ) <= toleranceSquared )
                {
                    // Deduplicate near-identical contact points (quantized to tolerance cells).
                    var q = (Floor( sample.X, tolerance ), Floor( sample.Y, tolerance ), Floor( sample.Z, tolerance ));
                    if ( seen.Add( q ) )
                        points.Add( (sample + closest) * 0.5f );
                    break;
                }
            }
        }
        return points;
    }

    static IEnumerable<Vector3> SurfaceSamples( RigMesh mesh, MeshPart part, Func<int, bool> filter, int stride )
    {
        var kept = 0;
        foreach ( var tri in part.TriangleIndices )
        {
            if ( !filter( tri ) )
                continue;
            if ( kept++ % stride != 0 )
                continue;
            var a = mesh.Positions[mesh.Triangles[tri * 3]];
            var b = mesh.Positions[mesh.Triangles[tri * 3 + 1]];
            var c = mesh.Positions[mesh.Triangles[tri * 3 + 2]];
            yield return a;
            yield return b;
            yield return c;
            yield return (a + b) * 0.5f;
            yield return (b + c) * 0.5f;
            yield return (c + a) * 0.5f;
            yield return (a + b + c) / 3f;
        }
    }

    static long Floor( float v, float cell ) => (long)MathF.Floor( v / cell );

    /// <summary>Closest point on triangle abc to point p (Ericson, Real-Time Collision Detection).</summary>
    internal static Vector3 ClosestPointOnTriangle( Vector3 p, Vector3 a, Vector3 b, Vector3 c )
    {
        var ab = b - a;
        var ac = c - a;
        var ap = p - a;

        var d1 = Vector3.Dot( ab, ap );
        var d2 = Vector3.Dot( ac, ap );
        if ( d1 <= 0f && d2 <= 0f ) return a;

        var bp = p - b;
        var d3 = Vector3.Dot( ab, bp );
        var d4 = Vector3.Dot( ac, bp );
        if ( d3 >= 0f && d4 <= d3 ) return b;

        var vc = d1 * d4 - d3 * d2;
        if ( vc <= 0f && d1 >= 0f && d3 <= 0f )
            return a + ab * (d1 / (d1 - d3));

        var cp = p - c;
        var d5 = Vector3.Dot( ab, cp );
        var d6 = Vector3.Dot( ac, cp );
        if ( d6 >= 0f && d5 <= d6 ) return c;

        var vb = d5 * d2 - d1 * d6;
        if ( vb <= 0f && d2 >= 0f && d6 <= 0f )
            return a + ac * (d2 / (d2 - d6));

        var va = d3 * d6 - d5 * d4;
        if ( va <= 0f && d4 - d3 >= 0f && d5 - d6 >= 0f )
            return b + (c - b) * ((d4 - d3) / ((d4 - d3) + (d5 - d6)));

        var denominator = 1f / (va + vb + vc);
        var v = vb * denominator;
        var w = vc * denominator;
        return a + ab * v + ac * w;
    }

    /// <summary>
    /// All three principal axes of the point cloud, descending by variance (power
    /// iteration with deflation). [0] is the largest-spread direction, [2] the plane
    /// normal of a flat cloud. Sign of each axis: toward the dominant positive axis.
    /// </summary>
    internal static Vector3[] PrincipalAxes( List<Vector3> points, Vector3 center )
    {
        float xx = 0, xy = 0, xz = 0, yy = 0, yz = 0, zz = 0;
        foreach ( var p in points )
        {
            var d = p - center;
            xx += d.X * d.X; xy += d.X * d.Y; xz += d.X * d.Z;
            yy += d.Y * d.Y; yz += d.Y * d.Z; zz += d.Z * d.Z;
        }

        var axes = new Vector3[3];
        for ( var k = 0; k < 3; k++ )
        {
            var v = k switch
            {
                0 => new Vector3( 1f, 0.9f, 0.8f ),
                1 => new Vector3( -0.8f, 1f, 0.9f ),
                _ => new Vector3( 0.9f, -0.8f, 1f ),
            };
            v = Vector3.Normalize( v );
            for ( var i = 0; i < 48; i++ )
            {
                var next = new Vector3(
                    xx * v.X + xy * v.Y + xz * v.Z,
                    xy * v.X + yy * v.Y + yz * v.Z,
                    xz * v.X + yz * v.Y + zz * v.Z );
                for ( var j = 0; j < k; j++ )
                    next -= axes[j] * Vector3.Dot( next, axes[j] );
                var length = next.Length();
                if ( length < 1e-12f )
                    break; // degenerate in this direction (flat/linear cloud)
                v = next / length;
            }
            for ( var j = 0; j < k; j++ )
                v -= axes[j] * Vector3.Dot( v, axes[j] );
            var final = v.Length();
            axes[k] = final > 1e-6f ? v / final : Fallback( axes, k );

            // Deterministic sign: toward the dominant positive component.
            var a = axes[k];
            var dominant = MathF.Abs( a.X ) >= MathF.Abs( a.Y ) && MathF.Abs( a.X ) >= MathF.Abs( a.Z )
                ? a.X
                : MathF.Abs( a.Y ) >= MathF.Abs( a.Z ) ? a.Y : a.Z;
            if ( dominant < 0 )
                axes[k] = -a;
        }
        return axes;

        static Vector3 Fallback( Vector3[] axes, int k ) => k switch
        {
            0 => Vector3.UnitX,
            1 => Vector3.Normalize( Vector3.Cross( axes[0],
                MathF.Abs( axes[0].Z ) < 0.9f ? Vector3.UnitZ : Vector3.UnitX ) ),
            _ => Vector3.Normalize( Vector3.Cross( axes[0], axes[1] ) ),
        };
    }
}