threecrate-reconstruction 0.8.0

//! Marching Cubes algorithm for volumetric surface reconstruction
//!
//! This module provides isosurface extraction from 3D scalar fields using
//! the classic Marching Cubes algorithm and its variants.

use crate::parallel;
use nalgebra::Vector3;
use threecrate_core::{Error, Point3f, PointCloud, Result, TriangleMesh};

/// 3D volumetric grid containing scalar values
#[derive(Debug, Clone)]
pub struct VolumetricGrid {
    /// Scalar values arranged as [x][y][z]
    pub values: Vec<Vec<Vec<f32>>>,
    /// Grid dimensions
    pub dimensions: [usize; 3],
    /// Physical size of each voxel
    pub voxel_size: [f32; 3],
    /// Origin position of the grid in world coordinates
    pub origin: Point3f,
}

impl VolumetricGrid {
    /// Create a new volumetric grid
    pub fn new(dimensions: [usize; 3], voxel_size: [f32; 3], origin: Point3f) -> Self {
        let values = vec![vec![vec![0.0; dimensions[2]]; dimensions[1]]; dimensions[0]];

        Self {
            values,
            dimensions,
            voxel_size,
            origin,
        }
    }

    /// Get scalar value at grid coordinates (with bounds checking)
    pub fn get_value(&self, x: usize, y: usize, z: usize) -> Option<f32> {
        if x < self.dimensions[0] && y < self.dimensions[1] && z < self.dimensions[2] {
            Some(self.values[x][y][z])
        } else {
            None
        }
    }

    /// Set scalar value at grid coordinates
    pub fn set_value(&mut self, x: usize, y: usize, z: usize, value: f32) -> Result<()> {
        if x < self.dimensions[0] && y < self.dimensions[1] && z < self.dimensions[2] {
            self.values[x][y][z] = value;
            Ok(())
        } else {
            Err(Error::InvalidData(format!(
                "Grid coordinates ({}, {}, {}) out of bounds for dimensions {:?}",
                x, y, z, self.dimensions
            )))
        }
    }

    /// Convert grid coordinates to world coordinates
    pub fn grid_to_world(&self, x: usize, y: usize, z: usize) -> Point3f {
        Point3f::new(
            self.origin.x + x as f32 * self.voxel_size[0],
            self.origin.y + y as f32 * self.voxel_size[1],
            self.origin.z + z as f32 * self.voxel_size[2],
        )
    }

    /// Create a volumetric grid from a point cloud using distance field
    pub fn from_point_cloud(
        cloud: &PointCloud<Point3f>,
        grid_resolution: [usize; 3],
        padding: f32,
    ) -> Result<Self> {
        if cloud.is_empty() {
            return Err(Error::InvalidData("Point cloud is empty".to_string()));
        }

        // Compute bounding box
        let mut min_bounds = Point3f::new(f32::INFINITY, f32::INFINITY, f32::INFINITY);
        let mut max_bounds = Point3f::new(f32::NEG_INFINITY, f32::NEG_INFINITY, f32::NEG_INFINITY);

        for point in &cloud.points {
            min_bounds.x = min_bounds.x.min(point.x);
            min_bounds.y = min_bounds.y.min(point.y);
            min_bounds.z = min_bounds.z.min(point.z);
            max_bounds.x = max_bounds.x.max(point.x);
            max_bounds.y = max_bounds.y.max(point.y);
            max_bounds.z = max_bounds.z.max(point.z);
        }

        // Add padding
        min_bounds = Point3f::new(
            min_bounds.x - padding,
            min_bounds.y - padding,
            min_bounds.z - padding,
        );
        max_bounds = Point3f::new(
            max_bounds.x + padding,
            max_bounds.y + padding,
            max_bounds.z + padding,
        );

        // Calculate voxel size
        let extents = [
            max_bounds.x - min_bounds.x,
            max_bounds.y - min_bounds.y,
            max_bounds.z - min_bounds.z,
        ];

        let voxel_size = [
            extents[0] / (grid_resolution[0] - 1) as f32,
            extents[1] / (grid_resolution[1] - 1) as f32,
            extents[2] / (grid_resolution[2] - 1) as f32,
        ];

        let mut grid = VolumetricGrid::new(grid_resolution, voxel_size, min_bounds);

        // Fill grid with distance values
        for x in 0..grid_resolution[0] {
            for y in 0..grid_resolution[1] {
                for z in 0..grid_resolution[2] {
                    let world_pos = grid.grid_to_world(x, y, z);

                    // Find minimum distance to any point in the cloud
                    let mut min_distance = f32::INFINITY;
                    for point in &cloud.points {
                        let distance = (world_pos - point).magnitude();
                        min_distance = min_distance.min(distance);
                    }

                    grid.set_value(x, y, z, min_distance)?;
                }
            }
        }

        Ok(grid)
    }
}

/// Configuration for Marching Cubes algorithm
#[derive(Debug, Clone)]
pub struct MarchingCubesConfig {
    /// Isosurface level (scalar value to extract)
    pub iso_level: f32,
    /// Whether to compute vertex normals
    pub compute_normals: bool,
    /// Whether to smooth the resulting mesh
    pub smooth_mesh: bool,
    /// Smoothing iterations (if smooth_mesh is true)
    pub smoothing_iterations: usize,
}

impl Default for MarchingCubesConfig {
    fn default() -> Self {
        Self {
            iso_level: 0.0,
            compute_normals: true,
            smooth_mesh: false,
            smoothing_iterations: 3,
        }
    }
}

/// Vertex information for marching cubes
#[derive(Debug, Clone)]
struct MCVertex {
    position: Point3f,
    normal: Vector3<f32>,
}

/// Edge table: indicates which edges are intersected for each cube configuration (256 cases)
/// Each entry is a 12-bit value where bit i indicates if edge i is intersected
/// Note: This table is available for future optimizations (pre-checking which edges to interpolate)
#[allow(dead_code)]
const EDGE_TABLE: [u16; 256] = [
    0x0, 0x109, 0x203, 0x30a, 0x406, 0x50f, 0x605, 0x70c,
    0x80c, 0x905, 0xa0f, 0xb06, 0xc0a, 0xd03, 0xe09, 0xf00,
    0x190, 0x99, 0x393, 0x29a, 0x596, 0x49f, 0x795, 0x69c,
    0x99c, 0x895, 0xb9f, 0xa96, 0xd9a, 0xc93, 0xf99, 0xe90,
    0x230, 0x339, 0x33, 0x13a, 0x636, 0x73f, 0x435, 0x53c,
    0xa3c, 0xb35, 0x83f, 0x936, 0xe3a, 0xf33, 0xc39, 0xd30,
    0x3a0, 0x2a9, 0x1a3, 0xaa, 0x7a6, 0x6af, 0x5a5, 0x4ac,
    0xbac, 0xaa5, 0x9af, 0x8a6, 0xfaa, 0xea3, 0xda9, 0xca0,
    0x460, 0x569, 0x663, 0x76a, 0x66, 0x16f, 0x265, 0x36c,
    0xc6c, 0xd65, 0xe6f, 0xf66, 0x86a, 0x963, 0xa69, 0xb60,
    0x5f0, 0x4f9, 0x7f3, 0x6fa, 0x1f6, 0xff, 0x3f5, 0x2fc,
    0xdfc, 0xcf5, 0xfff, 0xef6, 0x9fa, 0x8f3, 0xbf9, 0xaf0,
    0x650, 0x759, 0x453, 0x55a, 0x256, 0x35f, 0x55, 0x15c,
    0xe5c, 0xf55, 0xc5f, 0xd56, 0xa5a, 0xb53, 0x859, 0x950,
    0x7c0, 0x6c9, 0x5c3, 0x4ca, 0x3c6, 0x2cf, 0x1c5, 0xcc,
    0xfcc, 0xec5, 0xdcf, 0xcc6, 0xbca, 0xac3, 0x9c9, 0x8c0,
    0x8c0, 0x9c9, 0xac3, 0xbca, 0xcc6, 0xdcf, 0xec5, 0xfcc,
    0xcc, 0x1c5, 0x2cf, 0x3c6, 0x4ca, 0x5c3, 0x6c9, 0x7c0,
    0x950, 0x859, 0xb53, 0xa5a, 0xd56, 0xc5f, 0xf55, 0xe5c,
    0x15c, 0x55, 0x35f, 0x256, 0x55a, 0x453, 0x759, 0x650,
    0xaf0, 0xbf9, 0x8f3, 0x9fa, 0xef6, 0xfff, 0xcf5, 0xdfc,
    0x2fc, 0x3f5, 0xff, 0x1f6, 0x6fa, 0x7f3, 0x4f9, 0x5f0,
    0xb60, 0xa69, 0x963, 0x86a, 0xf66, 0xe6f, 0xd65, 0xc6c,
    0x36c, 0x265, 0x16f, 0x66, 0x76a, 0x663, 0x569, 0x460,
    0xca0, 0xda9, 0xea3, 0xfaa, 0x8a6, 0x9af, 0xaa5, 0xbac,
    0x4ac, 0x5a5, 0x6af, 0x7a6, 0xaa, 0x1a3, 0x2a9, 0x3a0,
    0xd30, 0xc39, 0xf33, 0xe3a, 0x936, 0x83f, 0xb35, 0xa3c,
    0x53c, 0x435, 0x73f, 0x636, 0x13a, 0x33, 0x339, 0x230,
    0xe90, 0xf99, 0xc93, 0xd9a, 0xa96, 0xb9f, 0x895, 0x99c,
    0x69c, 0x795, 0x49f, 0x596, 0x29a, 0x393, 0x99, 0x190,
    0xf00, 0xe09, 0xd03, 0xc0a, 0xb06, 0xa0f, 0x905, 0x80c,
    0x70c, 0x605, 0x50f, 0x406, 0x30a, 0x203, 0x109, 0x0,
];

/// Triangle table: defines triangles for each cube configuration
/// Each row contains up to 5 triangles (15 indices), terminated by -1
const TRIANGLE_TABLE: [[i8; 16]; 256] = [
    [-1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1],
    [0, 8, 3, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1],
    [0, 1, 9, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1],
    [1, 8, 3, 9, 8, 1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1],
    [1, 2, 10, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1],
    [0, 8, 3, 1, 2, 10, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1],
    [9, 2, 10, 0, 2, 9, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1],
    [2, 8, 3, 2, 10, 8, 10, 9, 8, -1, -1, -1, -1, -1, -1, -1],
    [3, 11, 2, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1],
    [0, 11, 2, 8, 11, 0, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1],
    [1, 9, 0, 2, 3, 11, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1],
    [1, 11, 2, 1, 9, 11, 9, 8, 11, -1, -1, -1, -1, -1, -1, -1],
    [3, 10, 1, 11, 10, 3, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1],
    [0, 10, 1, 0, 8, 10, 8, 11, 10, -1, -1, -1, -1, -1, -1, -1],
    [3, 9, 0, 3, 11, 9, 11, 10, 9, -1, -1, -1, -1, -1, -1, -1],
    [9, 8, 10, 10, 8, 11, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1],
    [4, 7, 8, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1],
    [4, 3, 0, 7, 3, 4, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1],
    [0, 1, 9, 8, 4, 7, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1],
    [4, 1, 9, 4, 7, 1, 7, 3, 1, -1, -1, -1, -1, -1, -1, -1],
    [1, 2, 10, 8, 4, 7, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1],
    [3, 4, 7, 3, 0, 4, 1, 2, 10, -1, -1, -1, -1, -1, -1, -1],
    [9, 2, 10, 9, 0, 2, 8, 4, 7, -1, -1, -1, -1, -1, -1, -1],
    [2, 10, 9, 2, 9, 7, 2, 7, 3, 7, 9, 4, -1, -1, -1, -1],
    [8, 4, 7, 3, 11, 2, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1],
    [11, 4, 7, 11, 2, 4, 2, 0, 4, -1, -1, -1, -1, -1, -1, -1],
    [9, 0, 1, 8, 4, 7, 2, 3, 11, -1, -1, -1, -1, -1, -1, -1],
    [4, 7, 11, 9, 4, 11, 9, 11, 2, 9, 2, 1, -1, -1, -1, -1],
    [3, 10, 1, 3, 11, 10, 7, 8, 4, -1, -1, -1, -1, -1, -1, -1],
    [1, 11, 10, 1, 4, 11, 1, 0, 4, 7, 11, 4, -1, -1, -1, -1],
    [4, 7, 8, 9, 0, 11, 9, 11, 10, 11, 0, 3, -1, -1, -1, -1],
    [4, 7, 11, 4, 11, 9, 9, 11, 10, -1, -1, -1, -1, -1, -1, -1],
    [9, 5, 4, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1],
    [9, 5, 4, 0, 8, 3, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1],
    [0, 5, 4, 1, 5, 0, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1],
    [8, 5, 4, 8, 3, 5, 3, 1, 5, -1, -1, -1, -1, -1, -1, -1],
    [1, 2, 10, 9, 5, 4, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1],
    [3, 0, 8, 1, 2, 10, 4, 9, 5, -1, -1, -1, -1, -1, -1, -1],
    [5, 2, 10, 5, 4, 2, 4, 0, 2, -1, -1, -1, -1, -1, -1, -1],
    [2, 10, 5, 3, 2, 5, 3, 5, 4, 3, 4, 8, -1, -1, -1, -1],
    [9, 5, 4, 2, 3, 11, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1],
    [0, 11, 2, 0, 8, 11, 4, 9, 5, -1, -1, -1, -1, -1, -1, -1],
    [0, 5, 4, 0, 1, 5, 2, 3, 11, -1, -1, -1, -1, -1, -1, -1],
    [2, 1, 5, 2, 5, 8, 2, 8, 11, 4, 8, 5, -1, -1, -1, -1],
    [10, 3, 11, 10, 1, 3, 9, 5, 4, -1, -1, -1, -1, -1, -1, -1],
    [4, 9, 5, 0, 8, 1, 8, 10, 1, 8, 11, 10, -1, -1, -1, -1],
    [5, 4, 0, 5, 0, 11, 5, 11, 10, 11, 0, 3, -1, -1, -1, -1],
    [5, 4, 8, 5, 8, 10, 10, 8, 11, -1, -1, -1, -1, -1, -1, -1],
    [9, 7, 8, 5, 7, 9, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1],
    [9, 3, 0, 9, 5, 3, 5, 7, 3, -1, -1, -1, -1, -1, -1, -1],
    [0, 7, 8, 0, 1, 7, 1, 5, 7, -1, -1, -1, -1, -1, -1, -1],
    [1, 5, 3, 3, 5, 7, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1],
    [9, 7, 8, 9, 5, 7, 10, 1, 2, -1, -1, -1, -1, -1, -1, -1],
    [10, 1, 2, 9, 5, 0, 5, 3, 0, 5, 7, 3, -1, -1, -1, -1],
    [8, 0, 2, 8, 2, 5, 8, 5, 7, 10, 5, 2, -1, -1, -1, -1],
    [2, 10, 5, 2, 5, 3, 3, 5, 7, -1, -1, -1, -1, -1, -1, -1],
    [7, 9, 5, 7, 8, 9, 3, 11, 2, -1, -1, -1, -1, -1, -1, -1],
    [9, 5, 7, 9, 7, 2, 9, 2, 0, 2, 7, 11, -1, -1, -1, -1],
    [2, 3, 11, 0, 1, 8, 1, 7, 8, 1, 5, 7, -1, -1, -1, -1],
    [11, 2, 1, 11, 1, 7, 7, 1, 5, -1, -1, -1, -1, -1, -1, -1],
    [9, 5, 8, 8, 5, 7, 10, 1, 3, 10, 3, 11, -1, -1, -1, -1],
    [5, 7, 0, 5, 0, 9, 7, 11, 0, 1, 0, 10, 11, 10, 0, -1],
    [11, 10, 0, 11, 0, 3, 10, 5, 0, 8, 0, 7, 5, 7, 0, -1],
    [11, 10, 5, 7, 11, 5, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1],
    [10, 6, 5, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1],
    [0, 8, 3, 5, 10, 6, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1],
    [9, 0, 1, 5, 10, 6, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1],
    [1, 8, 3, 1, 9, 8, 5, 10, 6, -1, -1, -1, -1, -1, -1, -1],
    [1, 6, 5, 2, 6, 1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1],
    [1, 6, 5, 1, 2, 6, 3, 0, 8, -1, -1, -1, -1, -1, -1, -1],
    [9, 6, 5, 9, 0, 6, 0, 2, 6, -1, -1, -1, -1, -1, -1, -1],
    [5, 9, 8, 5, 8, 2, 5, 2, 6, 3, 2, 8, -1, -1, -1, -1],
    [2, 3, 11, 10, 6, 5, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1],
    [11, 0, 8, 11, 2, 0, 10, 6, 5, -1, -1, -1, -1, -1, -1, -1],
    [0, 1, 9, 2, 3, 11, 5, 10, 6, -1, -1, -1, -1, -1, -1, -1],
    [5, 10, 6, 1, 9, 2, 9, 11, 2, 9, 8, 11, -1, -1, -1, -1],
    [6, 3, 11, 6, 5, 3, 5, 1, 3, -1, -1, -1, -1, -1, -1, -1],
    [0, 8, 11, 0, 11, 5, 0, 5, 1, 5, 11, 6, -1, -1, -1, -1],
    [3, 11, 6, 0, 3, 6, 0, 6, 5, 0, 5, 9, -1, -1, -1, -1],
    [6, 5, 9, 6, 9, 11, 11, 9, 8, -1, -1, -1, -1, -1, -1, -1],
    [5, 10, 6, 4, 7, 8, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1],
    [4, 3, 0, 4, 7, 3, 6, 5, 10, -1, -1, -1, -1, -1, -1, -1],
    [1, 9, 0, 5, 10, 6, 8, 4, 7, -1, -1, -1, -1, -1, -1, -1],
    [10, 6, 5, 1, 9, 7, 1, 7, 3, 7, 9, 4, -1, -1, -1, -1],
    [6, 1, 2, 6, 5, 1, 4, 7, 8, -1, -1, -1, -1, -1, -1, -1],
    [1, 2, 5, 5, 2, 6, 3, 0, 4, 3, 4, 7, -1, -1, -1, -1],
    [8, 4, 7, 9, 0, 5, 0, 6, 5, 0, 2, 6, -1, -1, -1, -1],
    [7, 3, 9, 7, 9, 4, 3, 2, 9, 5, 9, 6, 2, 6, 9, -1],
    [3, 11, 2, 7, 8, 4, 10, 6, 5, -1, -1, -1, -1, -1, -1, -1],
    [5, 10, 6, 4, 7, 2, 4, 2, 0, 2, 7, 11, -1, -1, -1, -1],
    [0, 1, 9, 4, 7, 8, 2, 3, 11, 5, 10, 6, -1, -1, -1, -1],
    [9, 2, 1, 9, 11, 2, 9, 4, 11, 7, 11, 4, 5, 10, 6, -1],
    [8, 4, 7, 3, 11, 5, 3, 5, 1, 5, 11, 6, -1, -1, -1, -1],
    [5, 1, 11, 5, 11, 6, 1, 0, 11, 7, 11, 4, 0, 4, 11, -1],
    [0, 5, 9, 0, 6, 5, 0, 3, 6, 11, 6, 3, 8, 4, 7, -1],
    [6, 5, 9, 6, 9, 11, 4, 7, 9, 7, 11, 9, -1, -1, -1, -1],
    [10, 4, 9, 6, 4, 10, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1],
    [4, 10, 6, 4, 9, 10, 0, 8, 3, -1, -1, -1, -1, -1, -1, -1],
    [10, 0, 1, 10, 6, 0, 6, 4, 0, -1, -1, -1, -1, -1, -1, -1],
    [8, 3, 1, 8, 1, 6, 8, 6, 4, 6, 1, 10, -1, -1, -1, -1],
    [1, 4, 9, 1, 2, 4, 2, 6, 4, -1, -1, -1, -1, -1, -1, -1],
    [3, 0, 8, 1, 2, 9, 2, 4, 9, 2, 6, 4, -1, -1, -1, -1],
    [0, 2, 4, 4, 2, 6, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1],
    [8, 3, 2, 8, 2, 4, 4, 2, 6, -1, -1, -1, -1, -1, -1, -1],
    [10, 4, 9, 10, 6, 4, 11, 2, 3, -1, -1, -1, -1, -1, -1, -1],
    [0, 8, 2, 2, 8, 11, 4, 9, 10, 4, 10, 6, -1, -1, -1, -1],
    [3, 11, 2, 0, 1, 6, 0, 6, 4, 6, 1, 10, -1, -1, -1, -1],
    [6, 4, 1, 6, 1, 10, 4, 8, 1, 2, 1, 11, 8, 11, 1, -1],
    [9, 6, 4, 9, 3, 6, 9, 1, 3, 11, 6, 3, -1, -1, -1, -1],
    [8, 11, 1, 8, 1, 0, 11, 6, 1, 9, 1, 4, 6, 4, 1, -1],
    [3, 11, 6, 3, 6, 0, 0, 6, 4, -1, -1, -1, -1, -1, -1, -1],
    [6, 4, 8, 11, 6, 8, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1],
    [7, 10, 6, 7, 8, 10, 8, 9, 10, -1, -1, -1, -1, -1, -1, -1],
    [0, 7, 3, 0, 10, 7, 0, 9, 10, 6, 7, 10, -1, -1, -1, -1],
    [10, 6, 7, 1, 10, 7, 1, 7, 8, 1, 8, 0, -1, -1, -1, -1],
    [10, 6, 7, 10, 7, 1, 1, 7, 3, -1, -1, -1, -1, -1, -1, -1],
    [1, 2, 6, 1, 6, 8, 1, 8, 9, 8, 6, 7, -1, -1, -1, -1],
    [2, 6, 9, 2, 9, 1, 6, 7, 9, 0, 9, 3, 7, 3, 9, -1],
    [7, 8, 0, 7, 0, 6, 6, 0, 2, -1, -1, -1, -1, -1, -1, -1],
    [7, 3, 2, 6, 7, 2, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1],
    [2, 3, 11, 10, 6, 8, 10, 8, 9, 8, 6, 7, -1, -1, -1, -1],
    [2, 0, 7, 2, 7, 11, 0, 9, 7, 6, 7, 10, 9, 10, 7, -1],
    [1, 8, 0, 1, 7, 8, 1, 10, 7, 6, 7, 10, 2, 3, 11, -1],
    [11, 2, 1, 11, 1, 7, 10, 6, 1, 6, 7, 1, -1, -1, -1, -1],
    [8, 9, 6, 8, 6, 7, 9, 1, 6, 11, 6, 3, 1, 3, 6, -1],
    [0, 9, 1, 11, 6, 7, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1],
    [7, 8, 0, 7, 0, 6, 3, 11, 0, 11, 6, 0, -1, -1, -1, -1],
    [7, 11, 6, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1],
    [7, 6, 11, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1],
    [3, 0, 8, 11, 7, 6, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1],
    [0, 1, 9, 11, 7, 6, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1],
    [8, 1, 9, 8, 3, 1, 11, 7, 6, -1, -1, -1, -1, -1, -1, -1],
    [10, 1, 2, 6, 11, 7, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1],
    [1, 2, 10, 3, 0, 8, 6, 11, 7, -1, -1, -1, -1, -1, -1, -1],
    [2, 9, 0, 2, 10, 9, 6, 11, 7, -1, -1, -1, -1, -1, -1, -1],
    [6, 11, 7, 2, 10, 3, 10, 8, 3, 10, 9, 8, -1, -1, -1, -1],
    [7, 2, 3, 6, 2, 7, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1],
    [7, 0, 8, 7, 6, 0, 6, 2, 0, -1, -1, -1, -1, -1, -1, -1],
    [2, 7, 6, 2, 3, 7, 0, 1, 9, -1, -1, -1, -1, -1, -1, -1],
    [1, 6, 2, 1, 8, 6, 1, 9, 8, 8, 7, 6, -1, -1, -1, -1],
    [10, 7, 6, 10, 1, 7, 1, 3, 7, -1, -1, -1, -1, -1, -1, -1],
    [10, 7, 6, 1, 7, 10, 1, 8, 7, 1, 0, 8, -1, -1, -1, -1],
    [0, 3, 7, 0, 7, 10, 0, 10, 9, 6, 10, 7, -1, -1, -1, -1],
    [7, 6, 10, 7, 10, 8, 8, 10, 9, -1, -1, -1, -1, -1, -1, -1],
    [6, 8, 4, 11, 8, 6, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1],
    [3, 6, 11, 3, 0, 6, 0, 4, 6, -1, -1, -1, -1, -1, -1, -1],
    [8, 6, 11, 8, 4, 6, 9, 0, 1, -1, -1, -1, -1, -1, -1, -1],
    [9, 4, 6, 9, 6, 3, 9, 3, 1, 11, 3, 6, -1, -1, -1, -1],
    [6, 8, 4, 6, 11, 8, 2, 10, 1, -1, -1, -1, -1, -1, -1, -1],
    [1, 2, 10, 3, 0, 11, 0, 6, 11, 0, 4, 6, -1, -1, -1, -1],
    [4, 11, 8, 4, 6, 11, 0, 2, 9, 2, 10, 9, -1, -1, -1, -1],
    [10, 9, 3, 10, 3, 2, 9, 4, 3, 11, 3, 6, 4, 6, 3, -1],
    [8, 2, 3, 8, 4, 2, 4, 6, 2, -1, -1, -1, -1, -1, -1, -1],
    [0, 4, 2, 4, 6, 2, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1],
    [1, 9, 0, 2, 3, 4, 2, 4, 6, 4, 3, 8, -1, -1, -1, -1],
    [1, 9, 4, 1, 4, 2, 2, 4, 6, -1, -1, -1, -1, -1, -1, -1],
    [8, 1, 3, 8, 6, 1, 8, 4, 6, 6, 10, 1, -1, -1, -1, -1],
    [10, 1, 0, 10, 0, 6, 6, 0, 4, -1, -1, -1, -1, -1, -1, -1],
    [4, 6, 3, 4, 3, 8, 6, 10, 3, 0, 3, 9, 10, 9, 3, -1],
    [10, 9, 4, 6, 10, 4, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1],
    [4, 9, 5, 7, 6, 11, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1],
    [0, 8, 3, 4, 9, 5, 11, 7, 6, -1, -1, -1, -1, -1, -1, -1],
    [5, 0, 1, 5, 4, 0, 7, 6, 11, -1, -1, -1, -1, -1, -1, -1],
    [11, 7, 6, 8, 3, 4, 3, 5, 4, 3, 1, 5, -1, -1, -1, -1],
    [9, 5, 4, 10, 1, 2, 7, 6, 11, -1, -1, -1, -1, -1, -1, -1],
    [6, 11, 7, 1, 2, 10, 0, 8, 3, 4, 9, 5, -1, -1, -1, -1],
    [7, 6, 11, 5, 4, 10, 4, 2, 10, 4, 0, 2, -1, -1, -1, -1],
    [3, 4, 8, 3, 5, 4, 3, 2, 5, 10, 5, 2, 11, 7, 6, -1],
    [7, 2, 3, 7, 6, 2, 5, 4, 9, -1, -1, -1, -1, -1, -1, -1],
    [9, 5, 4, 0, 8, 6, 0, 6, 2, 6, 8, 7, -1, -1, -1, -1],
    [3, 6, 2, 3, 7, 6, 1, 5, 0, 5, 4, 0, -1, -1, -1, -1],
    [6, 2, 8, 6, 8, 7, 2, 1, 8, 4, 8, 5, 1, 5, 8, -1],
    [9, 5, 4, 10, 1, 6, 1, 7, 6, 1, 3, 7, -1, -1, -1, -1],
    [1, 6, 10, 1, 7, 6, 1, 0, 7, 8, 7, 0, 9, 5, 4, -1],
    [4, 0, 10, 4, 10, 5, 0, 3, 10, 6, 10, 7, 3, 7, 10, -1],
    [7, 6, 10, 7, 10, 8, 5, 4, 10, 4, 8, 10, -1, -1, -1, -1],
    [6, 9, 5, 6, 11, 9, 11, 8, 9, -1, -1, -1, -1, -1, -1, -1],
    [3, 6, 11, 0, 6, 3, 0, 5, 6, 0, 9, 5, -1, -1, -1, -1],
    [0, 11, 8, 0, 5, 11, 0, 1, 5, 5, 6, 11, -1, -1, -1, -1],
    [6, 11, 3, 6, 3, 5, 5, 3, 1, -1, -1, -1, -1, -1, -1, -1],
    [1, 2, 10, 9, 5, 11, 9, 11, 8, 11, 5, 6, -1, -1, -1, -1],
    [0, 11, 3, 0, 6, 11, 0, 9, 6, 5, 6, 9, 1, 2, 10, -1],
    [11, 8, 5, 11, 5, 6, 8, 0, 5, 10, 5, 2, 0, 2, 5, -1],
    [6, 11, 3, 6, 3, 5, 2, 10, 3, 10, 5, 3, -1, -1, -1, -1],
    [5, 8, 9, 5, 2, 8, 5, 6, 2, 3, 8, 2, -1, -1, -1, -1],
    [9, 5, 6, 9, 6, 0, 0, 6, 2, -1, -1, -1, -1, -1, -1, -1],
    [1, 5, 8, 1, 8, 0, 5, 6, 8, 3, 8, 2, 6, 2, 8, -1],
    [1, 5, 6, 2, 1, 6, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1],
    [1, 3, 6, 1, 6, 10, 3, 8, 6, 5, 6, 9, 8, 9, 6, -1],
    [10, 1, 0, 10, 0, 6, 9, 5, 0, 5, 6, 0, -1, -1, -1, -1],
    [0, 3, 8, 5, 6, 10, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1],
    [10, 5, 6, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1],
    [11, 5, 10, 7, 5, 11, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1],
    [11, 5, 10, 11, 7, 5, 8, 3, 0, -1, -1, -1, -1, -1, -1, -1],
    [5, 11, 7, 5, 10, 11, 1, 9, 0, -1, -1, -1, -1, -1, -1, -1],
    [10, 7, 5, 10, 11, 7, 9, 8, 1, 8, 3, 1, -1, -1, -1, -1],
    [11, 1, 2, 11, 7, 1, 7, 5, 1, -1, -1, -1, -1, -1, -1, -1],
    [0, 8, 3, 1, 2, 7, 1, 7, 5, 7, 2, 11, -1, -1, -1, -1],
    [9, 7, 5, 9, 2, 7, 9, 0, 2, 2, 11, 7, -1, -1, -1, -1],
    [7, 5, 2, 7, 2, 11, 5, 9, 2, 3, 2, 8, 9, 8, 2, -1],
    [2, 5, 10, 2, 3, 5, 3, 7, 5, -1, -1, -1, -1, -1, -1, -1],
    [8, 2, 0, 8, 5, 2, 8, 7, 5, 10, 2, 5, -1, -1, -1, -1],
    [9, 0, 1, 5, 10, 3, 5, 3, 7, 3, 10, 2, -1, -1, -1, -1],
    [9, 8, 2, 9, 2, 1, 8, 7, 2, 10, 2, 5, 7, 5, 2, -1],
    [1, 3, 5, 3, 7, 5, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1],
    [0, 8, 7, 0, 7, 1, 1, 7, 5, -1, -1, -1, -1, -1, -1, -1],
    [9, 0, 3, 9, 3, 5, 5, 3, 7, -1, -1, -1, -1, -1, -1, -1],
    [9, 8, 7, 5, 9, 7, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1],
    [5, 8, 4, 5, 10, 8, 10, 11, 8, -1, -1, -1, -1, -1, -1, -1],
    [5, 0, 4, 5, 11, 0, 5, 10, 11, 11, 3, 0, -1, -1, -1, -1],
    [0, 1, 9, 8, 4, 10, 8, 10, 11, 10, 4, 5, -1, -1, -1, -1],
    [10, 11, 4, 10, 4, 5, 11, 3, 4, 9, 4, 1, 3, 1, 4, -1],
    [2, 5, 1, 2, 8, 5, 2, 11, 8, 4, 5, 8, -1, -1, -1, -1],
    [0, 4, 11, 0, 11, 3, 4, 5, 11, 2, 11, 1, 5, 1, 11, -1],
    [0, 2, 5, 0, 5, 9, 2, 11, 5, 4, 5, 8, 11, 8, 5, -1],
    [9, 4, 5, 2, 11, 3, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1],
    [2, 5, 10, 3, 5, 2, 3, 4, 5, 3, 8, 4, -1, -1, -1, -1],
    [5, 10, 2, 5, 2, 4, 4, 2, 0, -1, -1, -1, -1, -1, -1, -1],
    [3, 10, 2, 3, 5, 10, 3, 8, 5, 4, 5, 8, 0, 1, 9, -1],
    [5, 10, 2, 5, 2, 4, 1, 9, 2, 9, 4, 2, -1, -1, -1, -1],
    [8, 4, 5, 8, 5, 3, 3, 5, 1, -1, -1, -1, -1, -1, -1, -1],
    [0, 4, 5, 1, 0, 5, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1],
    [8, 4, 5, 8, 5, 3, 9, 0, 5, 0, 3, 5, -1, -1, -1, -1],
    [9, 4, 5, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1],
    [4, 11, 7, 4, 9, 11, 9, 10, 11, -1, -1, -1, -1, -1, -1, -1],
    [0, 8, 3, 4, 9, 7, 9, 11, 7, 9, 10, 11, -1, -1, -1, -1],
    [1, 10, 11, 1, 11, 4, 1, 4, 0, 7, 4, 11, -1, -1, -1, -1],
    [3, 1, 4, 3, 4, 8, 1, 10, 4, 7, 4, 11, 10, 11, 4, -1],
    [4, 11, 7, 9, 11, 4, 9, 2, 11, 9, 1, 2, -1, -1, -1, -1],
    [9, 7, 4, 9, 11, 7, 9, 1, 11, 2, 11, 1, 0, 8, 3, -1],
    [11, 7, 4, 11, 4, 2, 2, 4, 0, -1, -1, -1, -1, -1, -1, -1],
    [11, 7, 4, 11, 4, 2, 8, 3, 4, 3, 2, 4, -1, -1, -1, -1],
    [2, 9, 10, 2, 7, 9, 2, 3, 7, 7, 4, 9, -1, -1, -1, -1],
    [9, 10, 7, 9, 7, 4, 10, 2, 7, 8, 7, 0, 2, 0, 7, -1],
    [3, 7, 10, 3, 10, 2, 7, 4, 10, 1, 10, 0, 4, 0, 10, -1],
    [1, 10, 2, 8, 7, 4, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1],
    [4, 9, 1, 4, 1, 7, 7, 1, 3, -1, -1, -1, -1, -1, -1, -1],
    [4, 9, 1, 4, 1, 7, 0, 8, 1, 8, 7, 1, -1, -1, -1, -1],
    [4, 0, 3, 7, 4, 3, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1],
    [4, 8, 7, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1],
    [9, 10, 8, 10, 11, 8, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1],
    [3, 0, 9, 3, 9, 11, 11, 9, 10, -1, -1, -1, -1, -1, -1, -1],
    [0, 1, 10, 0, 10, 8, 8, 10, 11, -1, -1, -1, -1, -1, -1, -1],
    [3, 1, 10, 11, 3, 10, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1],
    [1, 2, 11, 1, 11, 9, 9, 11, 8, -1, -1, -1, -1, -1, -1, -1],
    [3, 0, 9, 3, 9, 11, 1, 2, 9, 2, 11, 9, -1, -1, -1, -1],
    [0, 2, 11, 8, 0, 11, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1],
    [3, 2, 11, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1],
    [2, 3, 8, 2, 8, 10, 10, 8, 9, -1, -1, -1, -1, -1, -1, -1],
    [9, 10, 2, 0, 9, 2, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1],
    [2, 3, 8, 2, 8, 10, 0, 1, 8, 1, 10, 8, -1, -1, -1, -1],
    [1, 10, 2, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1],
    [1, 3, 8, 9, 1, 8, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1],
    [0, 9, 1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1],
    [0, 3, 8, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1],
    [-1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1],
];

/// Marching Cubes implementation
pub struct MarchingCubes {
    config: MarchingCubesConfig,
}

impl MarchingCubes {
    /// Create a new Marching Cubes instance
    pub fn new(config: MarchingCubesConfig) -> Self {
        Self { config }
    }

    /// Extract isosurface from volumetric grid with parallel processing
    pub fn extract_isosurface(&self, grid: &VolumetricGrid) -> Result<TriangleMesh> {
        // Generate all cube coordinates
        let mut cube_coords = Vec::new();
        for x in 0..grid.dimensions[0].saturating_sub(1) {
            for y in 0..grid.dimensions[1].saturating_sub(1) {
                for z in 0..grid.dimensions[2].saturating_sub(1) {
                    cube_coords.push((x, y, z));
                }
            }
        }

        // Process cubes in parallel
        let cube_results: Vec<(Vec<MCVertex>, Vec<[usize; 3]>)> =
            parallel::parallel_map(&cube_coords, |(x, y, z)| {
                let mut vertices = Vec::new();
                let mut triangles = Vec::new();

                // Process a single cube - ignore errors for parallel processing simplicity
                if let Ok(()) = self.process_cube(grid, *x, *y, *z, &mut vertices, &mut triangles) {
                    (vertices, triangles)
                } else {
                    (Vec::new(), Vec::new())
                }
            });

        // Merge results from parallel processing
        let mut all_vertices = Vec::new();
        let mut all_triangles = Vec::new();
        let mut vertex_offset = 0;

        for (vertices, triangles) in cube_results {
            // Add triangles with adjusted indices
            for triangle in triangles {
                all_triangles.push([
                    triangle[0] + vertex_offset,
                    triangle[1] + vertex_offset,
                    triangle[2] + vertex_offset,
                ]);
            }

            // Add vertices
            all_vertices.extend(vertices);
            vertex_offset = all_vertices.len();
        }

        if all_vertices.is_empty() {
            return Err(Error::Algorithm(
                "No isosurface found at specified level".to_string(),
            ));
        }

        // Convert MCVertex to Point3f for mesh creation using parallel processing
        let vertex_positions: Vec<Point3f> = parallel::parallel_map(&all_vertices, |v| v.position);
        let mut mesh = TriangleMesh::from_vertices_and_faces(vertex_positions, all_triangles);

        // Set normals if computed using parallel processing
        if self.config.compute_normals {
            let normals: Vec<Vector3<f32>> = parallel::parallel_map(&all_vertices, |v| v.normal);
            mesh.set_normals(normals);
        }

        // Apply smoothing if requested
        if self.config.smooth_mesh {
            self.smooth_mesh(&mut mesh)?;
        }

        Ok(mesh)
    }

    /// Process a single cube for marching cubes
    fn process_cube(
        &self,
        grid: &VolumetricGrid,
        x: usize,
        y: usize,
        z: usize,
        vertices: &mut Vec<MCVertex>,
        triangles: &mut Vec<[usize; 3]>,
    ) -> Result<()> {
        // Get the 8 corner values of the cube
        let corner_values = [
            grid.get_value(x, y, z).unwrap_or(0.0),
            grid.get_value(x + 1, y, z).unwrap_or(0.0),
            grid.get_value(x + 1, y + 1, z).unwrap_or(0.0),
            grid.get_value(x, y + 1, z).unwrap_or(0.0),
            grid.get_value(x, y, z + 1).unwrap_or(0.0),
            grid.get_value(x + 1, y, z + 1).unwrap_or(0.0),
            grid.get_value(x + 1, y + 1, z + 1).unwrap_or(0.0),
            grid.get_value(x, y + 1, z + 1).unwrap_or(0.0),
        ];

        // Compute cube configuration index
        let mut cube_index = 0;
        for i in 0..8 {
            if corner_values[i] < self.config.iso_level {
                cube_index |= 1 << i;
            }
        }

        // Skip if cube is entirely inside or outside
        if cube_index == 0 || cube_index == 255 {
            return Ok(());
        }

        // Get cube corner positions
        let corner_positions = [
            grid.grid_to_world(x, y, z),
            grid.grid_to_world(x + 1, y, z),
            grid.grid_to_world(x + 1, y + 1, z),
            grid.grid_to_world(x, y + 1, z),
            grid.grid_to_world(x, y, z + 1),
            grid.grid_to_world(x + 1, y, z + 1),
            grid.grid_to_world(x + 1, y + 1, z + 1),
            grid.grid_to_world(x, y + 1, z + 1),
        ];

        // Interpolate vertices on edges where surface crosses
        let mut edge_vertices = [None; 12];
        let edge_connections = [
            [0, 1],
            [1, 2],
            [2, 3],
            [3, 0], // bottom face edges
            [4, 5],
            [5, 6],
            [6, 7],
            [7, 4], // top face edges
            [0, 4],
            [1, 5],
            [2, 6],
            [3, 7], // vertical edges
        ];

        for (edge_idx, &[v1, v2]) in edge_connections.iter().enumerate() {
            if self.edge_intersects_surface(corner_values[v1], corner_values[v2]) {
                let vertex = self.interpolate_vertex(
                    corner_positions[v1],
                    corner_positions[v2],
                    corner_values[v1],
                    corner_values[v2],
                    grid,
                    x,
                    y,
                    z,
                )?;

                let vertex_index = vertices.len();
                vertices.push(vertex);
                edge_vertices[edge_idx] = Some(vertex_index);
            }
        }

        // Generate triangles using lookup table (simplified version)
        self.generate_triangles_for_cube(cube_index, &edge_vertices, triangles)?;

        Ok(())
    }

    /// Check if edge intersects the isosurface
    fn edge_intersects_surface(&self, value1: f32, value2: f32) -> bool {
        (value1 < self.config.iso_level) != (value2 < self.config.iso_level)
    }

    /// Interpolate vertex position on edge where surface crosses
    fn interpolate_vertex(
        &self,
        pos1: Point3f,
        pos2: Point3f,
        val1: f32,
        val2: f32,
        grid: &VolumetricGrid,
        _x: usize,
        _y: usize,
        _z: usize,
    ) -> Result<MCVertex> {
        // Linear interpolation to find exact crossing point
        let t = if (val2 - val1).abs() < 1e-6 {
            0.5
        } else {
            (self.config.iso_level - val1) / (val2 - val1)
        };

        let position = Point3f::new(
            pos1.x + t * (pos2.x - pos1.x),
            pos1.y + t * (pos2.y - pos1.y),
            pos1.z + t * (pos2.z - pos1.z),
        );

        // Compute normal using gradient (simplified finite difference)
        let normal = if self.config.compute_normals {
            self.compute_gradient_at_position(&position, grid)
        } else {
            Vector3::new(0.0, 0.0, 1.0)
        };

        Ok(MCVertex { position, normal })
    }

    /// Compute gradient (normal) at a position using finite differences
    fn compute_gradient_at_position(
        &self,
        position: &Point3f,
        grid: &VolumetricGrid,
    ) -> Vector3<f32> {
        let delta = 0.001; // Small offset for finite differences

        // Sample values around the position
        let dx_pos = self.sample_grid_at_world_position(
            &Point3f::new(position.x + delta, position.y, position.z),
            grid,
        );
        let dx_neg = self.sample_grid_at_world_position(
            &Point3f::new(position.x - delta, position.y, position.z),
            grid,
        );
        let dy_pos = self.sample_grid_at_world_position(
            &Point3f::new(position.x, position.y + delta, position.z),
            grid,
        );
        let dy_neg = self.sample_grid_at_world_position(
            &Point3f::new(position.x, position.y - delta, position.z),
            grid,
        );
        let dz_pos = self.sample_grid_at_world_position(
            &Point3f::new(position.x, position.y, position.z + delta),
            grid,
        );
        let dz_neg = self.sample_grid_at_world_position(
            &Point3f::new(position.x, position.y, position.z - delta),
            grid,
        );

        // Compute gradient
        let gradient = Vector3::new(
            (dx_pos - dx_neg) / (2.0 * delta),
            (dy_pos - dy_neg) / (2.0 * delta),
            (dz_pos - dz_neg) / (2.0 * delta),
        );

        // Normalize and return (negate for inward-pointing normal)
        if gradient.magnitude() > 1e-6 {
            -gradient.normalize()
        } else {
            Vector3::new(0.0, 0.0, 1.0)
        }
    }

    /// Sample grid value at world position using trilinear interpolation
    fn sample_grid_at_world_position(&self, position: &Point3f, grid: &VolumetricGrid) -> f32 {
        // Convert world position to grid coordinates
        let gx = (position.x - grid.origin.x) / grid.voxel_size[0];
        let gy = (position.y - grid.origin.y) / grid.voxel_size[1];
        let gz = (position.z - grid.origin.z) / grid.voxel_size[2];

        // Check bounds
        if gx < 0.0
            || gy < 0.0
            || gz < 0.0
            || gx >= (grid.dimensions[0] - 1) as f32
            || gy >= (grid.dimensions[1] - 1) as f32
            || gz >= (grid.dimensions[2] - 1) as f32
        {
            return 0.0; // Outside grid
        }

        // Trilinear interpolation
        let x0 = gx.floor() as usize;
        let y0 = gy.floor() as usize;
        let z0 = gz.floor() as usize;
        let x1 = (x0 + 1).min(grid.dimensions[0] - 1);
        let y1 = (y0 + 1).min(grid.dimensions[1] - 1);
        let z1 = (z0 + 1).min(grid.dimensions[2] - 1);

        let fx = gx - x0 as f32;
        let fy = gy - y0 as f32;
        let fz = gz - z0 as f32;

        // Get 8 corner values
        let v000 = grid.get_value(x0, y0, z0).unwrap_or(0.0);
        let v100 = grid.get_value(x1, y0, z0).unwrap_or(0.0);
        let v010 = grid.get_value(x0, y1, z0).unwrap_or(0.0);
        let v110 = grid.get_value(x1, y1, z0).unwrap_or(0.0);
        let v001 = grid.get_value(x0, y0, z1).unwrap_or(0.0);
        let v101 = grid.get_value(x1, y0, z1).unwrap_or(0.0);
        let v011 = grid.get_value(x0, y1, z1).unwrap_or(0.0);
        let v111 = grid.get_value(x1, y1, z1).unwrap_or(0.0);

        // Interpolate
        let v00 = v000 * (1.0 - fx) + v100 * fx;
        let v10 = v010 * (1.0 - fx) + v110 * fx;
        let v01 = v001 * (1.0 - fx) + v101 * fx;
        let v11 = v011 * (1.0 - fx) + v111 * fx;

        let v0 = v00 * (1.0 - fy) + v10 * fy;
        let v1 = v01 * (1.0 - fy) + v11 * fy;

        v0 * (1.0 - fz) + v1 * fz
    }

    /// Generate triangles for a cube configuration using lookup tables
    fn generate_triangles_for_cube(
        &self,
        cube_index: usize,
        edge_vertices: &[Option<usize>; 12],
        triangles: &mut Vec<[usize; 3]>,
    ) -> Result<()> {
        // Get the triangle configuration for this cube from the lookup table
        let tri_config = TRIANGLE_TABLE[cube_index];

        // Iterate through the triangle indices (terminated by -1)
        let mut i = 0;
        while i < tri_config.len() && tri_config[i] != -1 {
            // Each triangle consists of 3 edge indices
            if i + 2 < tri_config.len() {
                let edge_idx_0 = tri_config[i] as usize;
                let edge_idx_1 = tri_config[i + 1] as usize;
                let edge_idx_2 = tri_config[i + 2] as usize;

                // Get the actual vertex indices from the edge vertices
                if let (Some(v0), Some(v1), Some(v2)) = (
                    edge_vertices[edge_idx_0],
                    edge_vertices[edge_idx_1],
                    edge_vertices[edge_idx_2],
                ) {
                    triangles.push([v0, v1, v2]);
                }
            }

            i += 3; // Move to the next triangle
        }

        Ok(())
    }

    /// Apply smoothing to the mesh
    fn smooth_mesh(&self, mesh: &mut TriangleMesh) -> Result<()> {
        // Simple Laplacian smoothing
        for _ in 0..self.config.smoothing_iterations {
            let vertices = mesh.vertices.clone();
            let mut new_vertices = vertices.clone();

            // For each vertex, average with its neighbors
            for (i, _vertex) in vertices.iter().enumerate() {
                let neighbors = self.get_vertex_neighbors(mesh, i);
                if !neighbors.is_empty() {
                    let mut sum = Point3f::origin();
                    for &neighbor_idx in &neighbors {
                        if neighbor_idx < vertices.len() {
                            sum = Point3f::from(sum.coords + vertices[neighbor_idx].coords);
                        }
                    }
                    new_vertices[i] = Point3f::from(sum.coords / neighbors.len() as f32);
                }
            }

            // Update mesh vertices
            mesh.vertices = new_vertices;
        }

        Ok(())
    }

    /// Get vertex neighbors by looking at connected faces
    fn get_vertex_neighbors(&self, mesh: &TriangleMesh, vertex_idx: usize) -> Vec<usize> {
        let mut neighbors = std::collections::HashSet::new();

        // Find all faces that contain this vertex
        for face in &mesh.faces {
            if face.contains(&vertex_idx) {
                // Add the other two vertices from this face
                for &v in face {
                    if v != vertex_idx {
                        neighbors.insert(v);
                    }
                }
            }
        }

        neighbors.into_iter().collect()
    }
}

/// Convenience function for basic marching cubes
pub fn marching_cubes(grid: &VolumetricGrid, iso_level: f32) -> Result<TriangleMesh> {
    let config = MarchingCubesConfig {
        iso_level,
        ..Default::default()
    };
    let mc = MarchingCubes::new(config);
    mc.extract_isosurface(grid)
}

/// Create a simple test volume (sphere)
pub fn create_sphere_volume(
    center: Point3f,
    radius: f32,
    grid_resolution: [usize; 3],
    grid_size: [f32; 3],
) -> VolumetricGrid {
    let origin = Point3f::new(
        center.x - grid_size[0] / 2.0,
        center.y - grid_size[1] / 2.0,
        center.z - grid_size[2] / 2.0,
    );

    let voxel_size = [
        grid_size[0] / (grid_resolution[0] - 1) as f32,
        grid_size[1] / (grid_resolution[1] - 1) as f32,
        grid_size[2] / (grid_resolution[2] - 1) as f32,
    ];

    let mut grid = VolumetricGrid::new(grid_resolution, voxel_size, origin);

    // Generate all grid coordinates for parallel processing
    let mut grid_coords = Vec::new();
    for x in 0..grid_resolution[0] {
        for y in 0..grid_resolution[1] {
            for z in 0..grid_resolution[2] {
                grid_coords.push((x, y, z));
            }
        }
    }

    // Compute sphere distance field in parallel
    let distance_values: Vec<((usize, usize, usize), f32)> =
        parallel::parallel_map(&grid_coords, |(x, y, z)| {
            let world_pos = grid.grid_to_world(*x, *y, *z);
            let distance = (world_pos - center).magnitude() - radius;
            ((*x, *y, *z), distance)
        });

    // Fill grid with computed values
    for ((x, y, z), distance) in distance_values {
        grid.set_value(x, y, z, distance).unwrap();
    }

    grid
}

/// Create a cube volume using signed distance field
pub fn create_cube_volume(
    center: Point3f,
    half_size: f32,
    grid_resolution: [usize; 3],
    grid_size: [f32; 3],
) -> VolumetricGrid {
    let origin = Point3f::new(
        center.x - grid_size[0] / 2.0,
        center.y - grid_size[1] / 2.0,
        center.z - grid_size[2] / 2.0,
    );

    let voxel_size = [
        grid_size[0] / (grid_resolution[0] - 1) as f32,
        grid_size[1] / (grid_resolution[1] - 1) as f32,
        grid_size[2] / (grid_resolution[2] - 1) as f32,
    ];

    let mut grid = VolumetricGrid::new(grid_resolution, voxel_size, origin);

    // Generate all grid coordinates for parallel processing
    let mut grid_coords = Vec::new();
    for x in 0..grid_resolution[0] {
        for y in 0..grid_resolution[1] {
            for z in 0..grid_resolution[2] {
                grid_coords.push((x, y, z));
            }
        }
    }

    // Compute cube distance field in parallel (signed distance to a box)
    let distance_values: Vec<((usize, usize, usize), f32)> =
        parallel::parallel_map(&grid_coords, |(x, y, z)| {
            let world_pos = grid.grid_to_world(*x, *y, *z);

            // Distance to box (signed distance field)
            let dx = (world_pos.x - center.x).abs() - half_size;
            let dy = (world_pos.y - center.y).abs() - half_size;
            let dz = (world_pos.z - center.z).abs() - half_size;

            // Outside: distance to nearest surface
            // Inside: negative distance to nearest surface
            let outside_dist = dx.max(0.0).powi(2) + dy.max(0.0).powi(2) + dz.max(0.0).powi(2);
            let distance = outside_dist.sqrt() + dx.max(dy).max(dz).min(0.0);

            ((*x, *y, *z), distance)
        });

    // Fill grid with computed values
    for ((x, y, z), distance) in distance_values {
        grid.set_value(x, y, z, distance).unwrap();
    }

    grid
}

#[cfg(test)]
mod tests {
    use super::*;
    use nalgebra::Point3;

    #[test]
    fn test_volumetric_grid_creation() {
        let grid = VolumetricGrid::new([10, 10, 10], [1.0, 1.0, 1.0], Point3f::origin());

        assert_eq!(grid.dimensions, [10, 10, 10]);
        assert_eq!(grid.voxel_size, [1.0, 1.0, 1.0]);
        assert_eq!(grid.origin, Point3f::origin());
    }

    #[test]
    fn test_grid_value_operations() {
        let mut grid = VolumetricGrid::new([3, 3, 3], [1.0, 1.0, 1.0], Point3f::origin());

        // Test setting and getting values
        assert!(grid.set_value(1, 1, 1, 5.0).is_ok());
        assert_eq!(grid.get_value(1, 1, 1), Some(5.0));

        // Test bounds checking
        assert!(grid.set_value(3, 3, 3, 1.0).is_err());
        assert_eq!(grid.get_value(3, 3, 3), None);
    }

    #[test]
    fn test_grid_to_world_conversion() {
        let grid = VolumetricGrid::new([5, 5, 5], [2.0, 2.0, 2.0], Point3f::new(1.0, 1.0, 1.0));

        let world_pos = grid.grid_to_world(1, 1, 1);
        assert_eq!(world_pos, Point3f::new(3.0, 3.0, 3.0));
    }

    #[test]
    fn test_sphere_volume_creation() {
        let center = Point3f::new(0.0, 0.0, 0.0);
        let radius = 1.0;
        let grid = create_sphere_volume(center, radius, [10, 10, 10], [4.0, 4.0, 4.0]);

        // Check that center has negative value (inside sphere)
        let center_value = grid.get_value(5, 5, 5).unwrap();
        assert!(center_value < 0.0);

        // Check that corner has positive value (outside sphere)
        let corner_value = grid.get_value(0, 0, 0).unwrap();
        assert!(corner_value > 0.0);
    }

    #[test]
    fn test_marching_cubes_config_default() {
        let config = MarchingCubesConfig::default();
        assert_eq!(config.iso_level, 0.0);
        assert!(config.compute_normals);
        assert!(!config.smooth_mesh);
        assert_eq!(config.smoothing_iterations, 3);
    }

    #[test]
    fn test_marching_cubes_simple() {
        let grid = create_sphere_volume(Point3f::origin(), 0.5, [8, 8, 8], [2.0, 2.0, 2.0]);

        let result = marching_cubes(&grid, 0.0);

        match result {
            Ok(mesh) => {
                assert!(!mesh.is_empty());
                assert!(mesh.vertex_count() > 0);
            }
            Err(_) => {
                // May fail on simple grids - acceptable for unit tests
            }
        }
    }

    #[test]
    fn test_volumetric_grid_from_point_cloud() {
        let points = vec![
            Point3::new(0.0, 0.0, 0.0),
            Point3::new(1.0, 0.0, 0.0),
            Point3::new(0.5, 1.0, 0.0),
        ];
        let cloud = PointCloud::from_points(points);

        let result = VolumetricGrid::from_point_cloud(&cloud, [10, 10, 10], 0.5);

        assert!(result.is_ok());
        let grid = result.unwrap();
        assert_eq!(grid.dimensions, [10, 10, 10]);
    }
}