using AutoRig.Voxel;

namespace AutoRig.Solve.Organic;

/// <summary>
/// Finds the extremity tips of a solid volume: local maxima of geodesic distance from
/// the "core" (the deepest-interior voxel).
/// </summary>
public static class Extremities
{
    /// <summary>
    /// The geodesic center of the volume: double-sweep BFS (deepest voxel → farthest A
    /// → farthest B), then the voxel minimizing max(distance-to-A, distance-to-B); ties
    /// break toward deeper voxels. Picking the plain deepest voxel fails when a round
    /// appendage (a character's head) is thicker than the body — the core would sit AT
    /// an extremity and hide it from detection.
    /// </summary>
    public static (int X, int Y, int Z) Core( VoxelGrid grid, DistanceField field )
    {
        // Seed: deepest voxel (any tie).
        var seed = (X: 0, Y: 0, Z: 0);
        var seedDepth = -1;
        for ( var z = 0; z < grid.SizeZ; z++ )
            for ( var y = 0; y < grid.SizeY; y++ )
                for ( var x = 0; x < grid.SizeX; x++ )
                {
                    var d = field.At( x, y, z );
                    if ( d > seedDepth )
                    {
                        seedDepth = d;
                        seed = (x, y, z);
                    }
                }
        if ( seedDepth <= 0 )
            return seed;

        var a = Farthest( grid, DistanceField.GeodesicFrom( grid, seed ) ) ?? seed;
        var fromA = DistanceField.GeodesicFrom( grid, a );
        var b = Farthest( grid, fromA ) ?? seed;
        var fromB = DistanceField.GeodesicFrom( grid, b );

        var best = seed;
        var bestEccentricity = int.MaxValue;
        var bestDepth = -1;
        for ( var z = 0; z < grid.SizeZ; z++ )
            for ( var y = 0; y < grid.SizeY; y++ )
                for ( var x = 0; x < grid.SizeX; x++ )
                {
                    if ( !grid.IsSolid( x, y, z ) )
                        continue;
                    var i = grid.Index( x, y, z );
                    if ( fromA[i] == int.MaxValue || fromB[i] == int.MaxValue )
                        continue;
                    var eccentricity = Math.Max( fromA[i], fromB[i] );
                    var depth = field.At( x, y, z );
                    if ( eccentricity < bestEccentricity
                        || (eccentricity == bestEccentricity && depth > bestDepth) )
                    {
                        bestEccentricity = eccentricity;
                        bestDepth = depth;
                        best = (x, y, z);
                    }
                }
        return best;
    }

    static (int X, int Y, int Z)? Farthest( VoxelGrid grid, int[] field )
    {
        (int X, int Y, int Z)? best = null;
        var bestDistance = -1;
        for ( var z = 0; z < grid.SizeZ; z++ )
            for ( var y = 0; y < grid.SizeY; y++ )
                for ( var x = 0; x < grid.SizeX; x++ )
                {
                    var d = field[grid.Index( x, y, z )];
                    if ( d != int.MaxValue && d > bestDistance )
                    {
                        bestDistance = d;
                        best = (x, y, z);
                    }
                }
        return best;
    }

    public static List<(int X, int Y, int Z)> Find( VoxelGrid grid, DistanceField field )
    {
        ArgumentNullException.ThrowIfNull( grid );
        ArgumentNullException.ThrowIfNull( field );

        var core = Core( grid, field );
        var geodesic = DistanceField.GeodesicFrom( grid, core );

        var globalMax = 0;
        foreach ( var d in geodesic )
            if ( d != int.MaxValue && d > globalMax )
                globalMax = d;
        if ( globalMax == 0 )
            return [];

        // The distance floor for candidates uses the 26-connectivity field: 6-conn
        // manhattan inflation lets a chunky torso's own corners (diagonal from the
        // core) masquerade as limb-length distances.
        var core26 = DistanceField.GeodesicFrom( grid, core, diagonal: true );
        var globalMax26 = 0;
        foreach ( var d in core26 )
            if ( d != int.MaxValue && d > globalMax26 )
                globalMax26 = d;
        var threshold26 = globalMax26 * 0.25;

        // Local maxima of core-geodesic distance within a 5^3 neighborhood.
        var candidates = new List<(int X, int Y, int Z, int D)>();
        for ( var z = 0; z < grid.SizeZ; z++ )
            for ( var y = 0; y < grid.SizeY; y++ )
                for ( var x = 0; x < grid.SizeX; x++ )
                {
                    var d = geodesic[grid.Index( x, y, z )];
                    if ( d == int.MaxValue || core26[grid.Index( x, y, z )] < threshold26 )
                        continue;
                    var isMax = true;
                    for ( var dz = -2; dz <= 2 && isMax; dz++ )
                        for ( var dy = -2; dy <= 2 && isMax; dy++ )
                            for ( var dx = -2; dx <= 2 && isMax; dx++ )
                            {
                                var nx = x + dx; var ny = y + dy; var nz = z + dz;
                                if ( nx < 0 || nx >= grid.SizeX || ny < 0 || ny >= grid.SizeY
                                    || nz < 0 || nz >= grid.SizeZ )
                                    continue;
                                var nd = geodesic[grid.Index( nx, ny, nz )];
                                if ( nd != int.MaxValue && (nd > d
                                    || (nd == d && (nz, ny, nx).CompareTo( (z, y, x) ) < 0)) )
                                    isMax = false; // strictly farther neighbor, or equal with lower order (dedup plateaus)
                            }
                    if ( isMax )
                        candidates.Add( (x, y, z, d) );
                }

        // Deduplicate by 26-connectivity GEODESIC separation: the corner voxels of one
        // blocky limb end are a short chebyshev walk apart (~ the cross-section size),
        // while genuinely distinct limbs are far apart through the volume even when
        // close in space (a hand resting near a hip). 6-connectivity would inflate
        // diagonal-face distances into the limb range. Keep the farthest candidate of
        // each geodesic cluster.
        var minSeparation = Math.Max( 4, (int)(globalMax26 * 0.4f) );

        candidates.Sort( ( a, b ) => b.D != a.D ? b.D.CompareTo( a.D )
            : (a.Z, a.Y, a.X).CompareTo( (b.Z, b.Y, b.X) ) );

        var tips = new List<(int X, int Y, int Z)>();
        var tipFields = new List<int[]>();
        foreach ( var (x, y, z, _) in candidates )
        {
            var index = grid.Index( x, y, z );
            var tooClose = tipFields.Any( f => f[index] < minSeparation );
            if ( tooClose )
                continue;
            tips.Add( (x, y, z) );
            tipFields.Add( DistanceField.GeodesicFrom( grid, (x, y, z), diagonal: true ) );
        }
        return tips;
    }
}