namespace AutoRig.Voxel;

/// <summary>
/// BFS distance fields over a voxel grid: boundary distance (steps from the nearest
/// empty voxel) and geodesic distance through solid voxels from a seed.
/// </summary>
public sealed class DistanceField
{
    /// <summary>Steps from the nearest non-solid voxel; 0 for non-solid voxels.</summary>
    public int[] BoundaryDistance { get; }

    readonly VoxelGrid _grid;

    DistanceField( VoxelGrid grid, int[] boundaryDistance )
    {
        _grid = grid;
        BoundaryDistance = boundaryDistance;
    }

    public int At( int x, int y, int z ) => BoundaryDistance[_grid.Index( x, y, z )];

    /// <summary>Multi-source BFS from all empty voxels inward (6-connectivity).</summary>
    public static DistanceField FromGrid( VoxelGrid grid )
    {
        ArgumentNullException.ThrowIfNull( grid );
        var total = grid.SizeX * grid.SizeY * grid.SizeZ;
        var distance = new int[total];
        Array.Fill( distance, -1 );
        var queue = new Queue<(int X, int Y, int Z)>();

        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 ) )
                    {
                        distance[grid.Index( x, y, z )] = 0;
                        queue.Enqueue( (x, y, z) );
                    }
                }

        Bfs( grid, distance, queue, solidOnly: false );
        return new DistanceField( grid, distance );
    }

    /// <summary>
    /// BFS steps through solid voxels from a seed; unreachable/empty = int.MaxValue.
    /// 6-connectivity by default; <paramref name="diagonal"/> uses 26-connectivity
    /// (chebyshev-like distances — useful when manhattan inflation would distort
    /// separation comparisons).
    /// </summary>
    public static int[] GeodesicFrom( VoxelGrid grid, (int X, int Y, int Z) seed, bool diagonal = false )
    {
        ArgumentNullException.ThrowIfNull( grid );
        var total = grid.SizeX * grid.SizeY * grid.SizeZ;
        var distance = new int[total];
        Array.Fill( distance, int.MaxValue );

        if ( !grid.IsSolid( seed.X, seed.Y, seed.Z ) )
            return distance;

        var queue = new Queue<(int X, int Y, int Z)>();
        distance[grid.Index( seed.X, seed.Y, seed.Z )] = 0;
        queue.Enqueue( seed );

        while ( queue.Count > 0 )
        {
            var (x, y, z) = queue.Dequeue();
            var next = distance[grid.Index( x, y, z )] + 1;
            if ( diagonal )
            {
                for ( var dz = -1; dz <= 1; dz++ )
                    for ( var dy = -1; dy <= 1; dy++ )
                        for ( var dx = -1; dx <= 1; dx++ )
                        {
                            if ( dx == 0 && dy == 0 && dz == 0 )
                                continue;
                            Visit( x + dx, y + dy, z + dz );
                        }
            }
            else
            {
                Visit( x - 1, y, z );
                Visit( x + 1, y, z );
                Visit( x, y - 1, z );
                Visit( x, y + 1, z );
                Visit( x, y, z - 1 );
                Visit( x, y, z + 1 );
            }

            void Visit( int nx, int ny, int nz )
            {
                if ( nx < 0 || nx >= grid.SizeX || ny < 0 || ny >= grid.SizeY || nz < 0 || nz >= grid.SizeZ )
                    return;
                if ( !grid.IsSolid( nx, ny, nz ) )
                    return;
                var i = grid.Index( nx, ny, nz );
                if ( distance[i] <= next )
                    return;
                distance[i] = next;
                queue.Enqueue( (nx, ny, nz) );
            }
        }
        return distance;
    }

    static void Bfs( VoxelGrid grid, int[] distance, Queue<(int X, int Y, int Z)> queue, bool solidOnly )
    {
        while ( queue.Count > 0 )
        {
            var (x, y, z) = queue.Dequeue();
            var next = distance[grid.Index( x, y, z )] + 1;
            Visit( x - 1, y, z );
            Visit( x + 1, y, z );
            Visit( x, y - 1, z );
            Visit( x, y + 1, z );
            Visit( x, y, z - 1 );
            Visit( x, y, z + 1 );

            void Visit( int nx, int ny, int nz )
            {
                if ( nx < 0 || nx >= grid.SizeX || ny < 0 || ny >= grid.SizeY || nz < 0 || nz >= grid.SizeZ )
                    return;
                if ( solidOnly && !grid.IsSolid( nx, ny, nz ) )
                    return;
                var i = grid.Index( nx, ny, nz );
                if ( distance[i] >= 0 && distance[i] <= next )
                    return;
                if ( distance[i] < 0 )
                {
                    distance[i] = next;
                    queue.Enqueue( (nx, ny, nz) );
                }
            }
        }
    }
}