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>
/// Bilateral symmetry detection: candidate planes are the three PCA axes of the vertex
/// cloud through the centroid; each is scored by mirroring sampled vertices and measuring
/// distance to the mesh <b>surface</b> (not nearest vertex — real meshes are shape-symmetric
/// but rarely tessellation-symmetric). Best candidate wins when its score ≥ 0.80.
/// </summary>
public static class SymmetryDetector
{
    const int MaxSamples = 2000;
    const float ScoreThreshold = 0.80f;

    public static SymmetryPlane? Detect( RigMesh mesh )
    {
        ArgumentNullException.ThrowIfNull( mesh );
        if ( mesh.Positions.Length == 0 || mesh.TriangleCount == 0 )
            return null;

        var bounds = mesh.ComputeBounds();
        var tolerance = 0.01f * bounds.Size.Length();
        if ( tolerance <= 0f )
            return null;

        // Work on distinct positions: triangulation duplicates vertices with uneven
        // multiplicity (a fan corner appears more often than its neighbors), which
        // injects phantom covariance cross-terms and tilts the PCA axes.
        var distinct = new HashSet<Vector3>( mesh.Positions ).ToArray();

        var centroid = Vector3.Zero;
        foreach ( var p in distinct ) centroid += p;
        centroid /= distinct.Length;

        // Candidates: PCA axes plus the cardinal axes (models are usually authored
        // axis-aligned, and a shape's exact symmetry plane can sit a few degrees off
        // the PCA eigenvectors when covariance cross-terms rotate them).
        var candidates = new List<Vector3>( PcaAxes( distinct, centroid ) );
        foreach ( var cardinal in new[] { Vector3.UnitX, Vector3.UnitY, Vector3.UnitZ } )
            if ( candidates.All( a => MathF.Abs( Vector3.Dot( a, cardinal ) ) < 0.9999f ) )
                candidates.Add( cardinal );

        // Deterministic vertex sample (stride, no RNG).
        var stride = Math.Max( 1, distinct.Length / MaxSamples );
        var samples = new List<Vector3>();
        for ( var i = 0; i < distinct.Length; i += stride )
            samples.Add( distinct[i] );

        var surface = new TriangleGrid( mesh, tolerance );

        // Score all candidates; a shape can have several true symmetry planes (any box
        // has diagonal ones), so among near-tied scores prefer the plane whose normal
        // carries the largest variance — the most "bilateral" left/right split.
        var scored = new List<(Vector3 Normal, float Score, float Variance)>();
        foreach ( var normal in candidates )
        {
            var hits = 0;
            foreach ( var p in samples )
            {
                var distance = Vector3.Dot( p - centroid, normal );
                var mirrored = p - normal * (2f * distance);
                if ( surface.IsWithin( mirrored, tolerance ) )
                    hits++;
            }
            var score = (float)hits / samples.Count;
            if ( score < ScoreThreshold )
                continue;
            float variance = 0;
            foreach ( var p in samples )
            {
                var d = Vector3.Dot( p - centroid, normal );
                variance += d * d;
            }
            scored.Add( (normal, score, variance / samples.Count) );
        }

        if ( scored.Count == 0 )
            return null;

        var bestScore = scored.Max( s => s.Score );
        var best = scored
            .Where( s => s.Score >= bestScore - 0.03f )
            .OrderByDescending( s => s.Variance )
            .First();
        return new SymmetryPlane( centroid, best.Normal, best.Score );
    }

    /// <summary>Three orthonormal PCA axes via power iteration with deflation.</summary>
    static Vector3[] PcaAxes( Vector3[] points, Vector3 centroid )
    {
        float xx = 0, xy = 0, xz = 0, yy = 0, yz = 0, zz = 0;
        foreach ( var p in points )
        {
            var d = p - centroid;
            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];
        // Plain array, not a Span collection expression: the compiler lowers span
        // literals through MemoryMarshal.CreateSpan, which the s&box whitelist bans.
        var c = new[] { xx, xy, xz, yy, yz, zz };
        for ( var k = 0; k < 3; k++ )
        {
            var v = k switch { 0 => new Vector3( 1, 0.1f, 0.01f ), 1 => new Vector3( 0.01f, 1, 0.1f ), _ => new Vector3( 0.1f, 0.01f, 1 ) };
            v = Vector3.Normalize( v );
            for ( var i = 0; i < 48; i++ )
            {
                var next = new Vector3(
                    c[0] * v.X + c[1] * v.Y + c[2] * v.Z,
                    c[1] * v.X + c[3] * v.Y + c[4] * v.Z,
                    c[2] * v.X + c[4] * v.Y + c[5] * v.Z );
                // Re-orthogonalize against found axes (deflation).
                for ( var j = 0; j < k; j++ )
                    next -= axes[j] * Vector3.Dot( next, axes[j] );
                var length = next.Length();
                if ( length < 1e-12f )
                    break;
                v = next / length;
            }
            // Keep v orthogonal even if iteration degenerated.
            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 : FallbackAxis( axes, k );
        }
        return axes;
    }

    static Vector3 FallbackAxis( 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] ) ),
    };

    /// <summary>Grid-bucketed triangles supporting "is this point within tolerance of the surface".</summary>
    sealed class TriangleGrid
    {
        readonly RigMesh _mesh;
        readonly Dictionary<(long, long, long), List<int>> _grid = new();
        readonly float _cell;

        public TriangleGrid( RigMesh mesh, float tolerance )
        {
            _mesh = mesh;
            _cell = MathF.Max( tolerance * 8f, 1e-6f );
            for ( var tri = 0; tri < mesh.TriangleCount; 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 ) ) - new Vector3( tolerance );
                var max = Vector3.Max( a, Vector3.Max( b, c ) ) + new Vector3( tolerance );
                for ( var x = Floor( min.X ); x <= Floor( max.X ); x++ )
                    for ( var y = Floor( min.Y ); y <= Floor( max.Y ); y++ )
                        for ( var z = Floor( min.Z ); z <= Floor( max.Z ); z++ )
                        {
                            if ( !_grid.TryGetValue( (x, y, z), out var bucket ) )
                                _grid[(x, y, z)] = bucket = new List<int>();
                            bucket.Add( tri );
                        }
            }
        }

        public bool IsWithin( Vector3 point, float tolerance )
        {
            if ( !_grid.TryGetValue( (Floor( point.X ), Floor( point.Y ), Floor( point.Z )), out var bucket ) )
                return false;
            var toleranceSquared = tolerance * tolerance;
            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 = PartContacts.ClosestPointOnTriangle( point, a, b, c );
                if ( Vector3.DistanceSquared( point, closest ) <= toleranceSquared )
                    return true;
            }
            return false;
        }

        long Floor( float v ) => (long)MathF.Floor( v / _cell );
    }
}