colmap 0.1.2

//! 网格重建模块
//!
//! 实现从点云生成三角网格的算法，包括泊松重建和行进立方体算法

use crate::core::{Point3, Result, ColmapError};
use crate::mvs::fusion::{FusedPoint, FusionResult};
use nalgebra::Vector3;

/// 网格重建器
#[derive(Debug)]
pub struct MeshReconstructor {
    /// 重建配置
    config: MeshReconstructionConfig,
}

/// 网格重建配置
#[derive(Debug, Clone)]
pub struct MeshReconstructionConfig {
    /// 重建算法类型
    pub algorithm: ReconstructionAlgorithm,
    /// 体素分辨率
    pub voxel_resolution: usize,
    /// 平滑迭代次数
    pub smoothing_iterations: usize,
    /// 简化目标三角形数
    pub target_triangle_count: Option<usize>,
    /// 最小置信度阈值
    pub min_confidence_threshold: f32,
    /// 法向量权重
    pub normal_weight: f32,
}

/// 重建算法类型
#[derive(Debug, Clone, PartialEq)]
pub enum ReconstructionAlgorithm {
    /// 泊松重建
    Poisson,
    /// 行进立方体
    MarchingCubes,
    /// Delaunay三角剖分
    Delaunay,
}

impl Default for MeshReconstructionConfig {
    fn default() -> Self {
        Self {
            algorithm: ReconstructionAlgorithm::Poisson,
            voxel_resolution: 256,
            smoothing_iterations: 5,
            target_triangle_count: None,
            min_confidence_threshold: 0.1,
            normal_weight: 1.0,
        }
    }
}

/// 三角形网格
#[derive(Debug, Clone)]
pub struct TriangleMesh {
    /// 顶点坐标
    pub vertices: Vec<Vertex>,
    /// 三角形面片 (顶点索引)
    pub triangles: Vec<Triangle>,
    /// 网格统计信息
    pub statistics: MeshStatistics,
}

/// 顶点
#[derive(Debug, Clone)]
pub struct Vertex {
    /// 3D坐标
    pub position: Point3,
    /// 法向量
    pub normal: Vector3<f32>,
    /// 颜色 (RGB)
    pub color: [u8; 3],
    /// 置信度
    pub confidence: f32,
}

/// 三角形
#[derive(Debug, Clone)]
pub struct Triangle {
    /// 顶点索引
    pub vertices: [usize; 3],
    /// 面法向量
    pub normal: Vector3<f32>,
    /// 面积
    pub area: f32,
}

/// 网格统计信息
#[derive(Debug, Clone)]
pub struct MeshStatistics {
    /// 顶点数
    pub num_vertices: usize,
    /// 三角形数
    pub num_triangles: usize,
    /// 边数
    pub num_edges: usize,
    /// 表面积
    pub surface_area: f32,
    /// 体积
    pub volume: f32,
}

/// 体素网格
#[derive(Debug)]
struct VoxelGrid3D {
    /// 分辨率
    resolution: usize,
    /// 边界框
    bounds: BoundingBox,
    /// 体素数据
    voxels: Vec<Vec<Vec<VoxelData>>>,
}

/// 体素数据
#[derive(Debug, Clone)]
struct VoxelData {
    /// 距离场值
    distance: f32,
    /// 梯度
    gradient: Vector3<f32>,
    /// 置信度
    confidence: f32,
    /// 是否包含点
    has_point: bool,
}

/// 边界框
#[derive(Debug, Clone)]
struct BoundingBox {
    min: Point3,
    max: Point3,
}

/// 行进立方体查找表
struct MarchingCubesLUT {
    /// 边表
    edge_table: [i32; 256],
    /// 三角形表
    triangle_table: [[i32; 16]; 256],
}

impl MeshReconstructor {
    /// 创建新的网格重建器
    pub fn new(config: MeshReconstructionConfig) -> Self {
        Self { config }
    }

    /// 从点云重建网格
    pub fn reconstruct_mesh(
        &self,
        fusion_result: &FusionResult,
    ) -> Result<TriangleMesh> {
        // 过滤低置信度点
        let filtered_points = self.filter_points_by_confidence(&fusion_result.points)?;
        
        if filtered_points.is_empty() {
            return Err(ColmapError::MvsReconstruction(
                "No points left after confidence filtering".to_string(),
            ));
        }

        println!("Reconstructing mesh from {} points using {:?} algorithm", 
                filtered_points.len(), self.config.algorithm);

        let mesh = match self.config.algorithm {
            ReconstructionAlgorithm::Poisson => {
                self.poisson_reconstruction(&filtered_points)?
            },
            ReconstructionAlgorithm::MarchingCubes => {
                self.marching_cubes_reconstruction(&filtered_points)?
            },
            ReconstructionAlgorithm::Delaunay => {
                self.delaunay_reconstruction(&filtered_points)?
            },
        };

        // 后处理
        let processed_mesh = self.post_process_mesh(mesh)?;

        Ok(processed_mesh)
    }

    /// 按置信度过滤点
    fn filter_points_by_confidence(
        &self,
        points: &[FusedPoint],
    ) -> Result<Vec<FusedPoint>> {
        let filtered: Vec<FusedPoint> = points
            .iter()
            .filter(|p| p.confidence >= self.config.min_confidence_threshold)
            .cloned()
            .collect();

        Ok(filtered)
    }

    /// 泊松重建
    fn poisson_reconstruction(
        &self,
        points: &[FusedPoint],
    ) -> Result<TriangleMesh> {
        // 计算边界框
        let bounds = self.compute_bounding_box(points)?;
        
        // 创建体素网格
        let mut voxel_grid = VoxelGrid3D::new(self.config.voxel_resolution, bounds);
        
        // 构建距离场
        self.build_distance_field(&mut voxel_grid, points)?;
        
        // 求解泊松方程
        self.solve_poisson_equation(&mut voxel_grid)?;
        
        // 提取等值面
        let mesh = self.extract_isosurface(&voxel_grid)?;
        
        Ok(mesh)
    }

    /// 行进立方体重建
    fn marching_cubes_reconstruction(
        &self,
        points: &[FusedPoint],
    ) -> Result<TriangleMesh> {
        // 计算边界框
        let bounds = self.compute_bounding_box(points)?;
        
        // 创建体素网格
        let mut voxel_grid = VoxelGrid3D::new(self.config.voxel_resolution, bounds);
        
        // 构建距离场
        self.build_distance_field(&mut voxel_grid, points)?;
        
        // 行进立方体算法
        let mesh = self.marching_cubes(&voxel_grid)?;
        
        Ok(mesh)
    }

    /// Delaunay三角剖分重建
    fn delaunay_reconstruction(
        &self,
        points: &[FusedPoint],
    ) -> Result<TriangleMesh> {
        // 简化实现：2D Delaunay三角剖分投影到3D
        let vertices: Vec<Vertex> = points
            .iter()
            .map(|p| Vertex {
                position: p.position,
                normal: p.normal,
                color: p.color,
                confidence: p.confidence,
            })
            .collect();

        // 简化的三角剖分（实际实现需要复杂的3D Delaunay算法）
        let triangles = self.simple_triangulation(&vertices)?;
        
        let statistics = self.compute_mesh_statistics(&vertices, &triangles);

        Ok(TriangleMesh {
            vertices,
            triangles,
            statistics,
        })
    }

    /// 计算边界框
    fn compute_bounding_box(&self, points: &[FusedPoint]) -> Result<BoundingBox> {
        if points.is_empty() {
            return Err(ColmapError::MvsReconstruction(
                "Cannot compute bounding box for empty point set".to_string(),
            ));
        }

        let first_point = &points[0].position;
        let mut min_point = *first_point;
        let mut max_point = *first_point;

        for point in points.iter().skip(1) {
            let pos = &point.position;
            min_point.x = min_point.x.min(pos.x);
            min_point.y = min_point.y.min(pos.y);
            min_point.z = min_point.z.min(pos.z);
            max_point.x = max_point.x.max(pos.x);
            max_point.y = max_point.y.max(pos.y);
            max_point.z = max_point.z.max(pos.z);
        }

        // 扩展边界框
        let margin = 0.1;
        let size = Point3::new(
            max_point.x - min_point.x,
            max_point.y - min_point.y,
            max_point.z - min_point.z,
        );
        
        min_point.x -= size.x * margin;
        min_point.y -= size.y * margin;
        min_point.z -= size.z * margin;
        max_point.x += size.x * margin;
        max_point.y += size.y * margin;
        max_point.z += size.z * margin;

        Ok(BoundingBox {
            min: min_point,
            max: max_point,
        })
    }

    /// 构建距离场
    fn build_distance_field(
        &self,
        voxel_grid: &mut VoxelGrid3D,
        points: &[FusedPoint],
    ) -> Result<()> {
        let resolution = voxel_grid.resolution;
        
        for i in 0..resolution {
            for j in 0..resolution {
                for k in 0..resolution {
                    let voxel_pos = voxel_grid.get_voxel_position(i, j, k);
                    
                    // 计算到最近点的距离
                    let mut min_distance = f32::MAX;
                    let mut closest_normal = Vector3::new(0.0, 0.0, 1.0);
                    let mut total_confidence = 0.0;
                    
                    for point in points {
                        let distance = self.point_distance(&voxel_pos, &point.position);
                        if distance < min_distance {
                            min_distance = distance;
                            closest_normal = point.normal;
                        }
                        
                        // 基于距离的权重
                        let weight = (-distance * distance / (2.0 * 0.1 * 0.1)).exp();
                        total_confidence += point.confidence * weight;
                    }
                    
                    // 计算梯度（简化为最近点的法向量）
                    let gradient = closest_normal * self.config.normal_weight;
                    
                    voxel_grid.voxels[i][j][k] = VoxelData {
                        distance: min_distance,
                        gradient,
                        confidence: total_confidence,
                        has_point: min_distance < 0.05, // 阈值
                    };
                }
            }
        }
        
        Ok(())
    }

    /// 求解泊松方程
    fn solve_poisson_equation(
        &self,
        voxel_grid: &mut VoxelGrid3D,
    ) -> Result<()> {
        // 简化实现：使用雅可比迭代
        let resolution = voxel_grid.resolution;
        let mut new_distances = vec![vec![vec![0.0; resolution]; resolution]; resolution];
        
        for _iteration in 0..10 {
            for i in 1..(resolution - 1) {
                for j in 1..(resolution - 1) {
                    for k in 1..(resolution - 1) {
                        // 拉普拉斯算子
                        let laplacian = 
                            voxel_grid.voxels[i-1][j][k].distance +
                            voxel_grid.voxels[i+1][j][k].distance +
                            voxel_grid.voxels[i][j-1][k].distance +
                            voxel_grid.voxels[i][j+1][k].distance +
                            voxel_grid.voxels[i][j][k-1].distance +
                            voxel_grid.voxels[i][j][k+1].distance -
                            6.0 * voxel_grid.voxels[i][j][k].distance;
                        
                        // 散度
                        let divergence = voxel_grid.voxels[i][j][k].gradient.norm();
                        
                        new_distances[i][j][k] = voxel_grid.voxels[i][j][k].distance + 
                            0.1 * (laplacian - divergence);
                    }
                }
            }
            
            // 更新距离值
            for i in 1..(resolution - 1) {
                for j in 1..(resolution - 1) {
                    for k in 1..(resolution - 1) {
                        voxel_grid.voxels[i][j][k].distance = new_distances[i][j][k];
                    }
                }
            }
        }
        
        Ok(())
    }

    /// 提取等值面
    fn extract_isosurface(
        &self,
        voxel_grid: &VoxelGrid3D,
    ) -> Result<TriangleMesh> {
        // 使用行进立方体算法提取等值面
        self.marching_cubes(voxel_grid)
    }

    /// 行进立方体算法
    fn marching_cubes(
        &self,
        voxel_grid: &VoxelGrid3D,
    ) -> Result<TriangleMesh> {
        let mut vertices = Vec::new();
        let mut triangles = Vec::new();
        let lut = MarchingCubesLUT::new();
        
        let resolution = voxel_grid.resolution;
        let iso_value = 0.0; // 等值面值
        
        for i in 0..(resolution - 1) {
            for j in 0..(resolution - 1) {
                for k in 0..(resolution - 1) {
                    // 获取立方体8个顶点的值
                    let cube_values = [
                        voxel_grid.voxels[i][j][k].distance,
                        voxel_grid.voxels[i+1][j][k].distance,
                        voxel_grid.voxels[i+1][j+1][k].distance,
                        voxel_grid.voxels[i][j+1][k].distance,
                        voxel_grid.voxels[i][j][k+1].distance,
                        voxel_grid.voxels[i+1][j][k+1].distance,
                        voxel_grid.voxels[i+1][j+1][k+1].distance,
                        voxel_grid.voxels[i][j+1][k+1].distance,
                    ];
                    
                    // 计算立方体配置索引
                    let mut cube_index = 0;
                    for (idx, &value) in cube_values.iter().enumerate() {
                        if value < iso_value {
                            cube_index |= 1 << idx;
                        }
                    }
                    
                    // 检查是否有三角形
                    if lut.edge_table[cube_index] == 0 {
                        continue;
                    }
                    
                    // 计算边上的交点
                    let mut edge_vertices = [Point3::new(0.0, 0.0, 0.0); 12];
                    self.compute_edge_intersections(
                        voxel_grid, i, j, k, 
                        &cube_values, iso_value,
                        &mut edge_vertices
                    );
                    
                    // 生成三角形
                    let triangle_config = &lut.triangle_table[cube_index];
                    let mut tri_idx = 0;
                    
                    while triangle_config[tri_idx] != -1 {
                        let v1_idx = vertices.len();
                        let v2_idx = vertices.len() + 1;
                        let v3_idx = vertices.len() + 2;
                        
                        // 添加顶点
                        for offset in 0..3 {
                            let edge_idx = triangle_config[tri_idx + offset] as usize;
                            let position = edge_vertices[edge_idx];
                            
                            vertices.push(Vertex {
                                position,
                                normal: Vector3::new(0.0, 0.0, 1.0), // 稍后计算
                                color: [128, 128, 128],
                                confidence: 1.0,
                            });
                        }
                        
                        // 添加三角形
                        let triangle = Triangle {
                            vertices: [v1_idx, v2_idx, v3_idx],
                            normal: Vector3::new(0.0, 0.0, 1.0), // 稍后计算
                            area: 0.0, // 稍后计算
                        };
                        
                        triangles.push(triangle);
                        tri_idx += 3;
                    }
                }
            }
        }
        
        // 计算法向量和面积
        self.compute_normals_and_areas(&mut vertices, &mut triangles)?;
        
        let statistics = self.compute_mesh_statistics(&vertices, &triangles);
        
        Ok(TriangleMesh {
            vertices,
            triangles,
            statistics,
        })
    }

    /// 计算边交点
    fn compute_edge_intersections(
        &self,
        voxel_grid: &VoxelGrid3D,
        i: usize, j: usize, k: usize,
        cube_values: &[f32; 8],
        iso_value: f32,
        edge_vertices: &mut [Point3; 12],
    ) {
        // 立方体边的定义
        let edges = [
            (0, 1), (1, 2), (2, 3), (3, 0), // 底面
            (4, 5), (5, 6), (6, 7), (7, 4), // 顶面
            (0, 4), (1, 5), (2, 6), (3, 7), // 垂直边
        ];
        
        // 立方体顶点位置
        let cube_positions = [
            voxel_grid.get_voxel_position(i, j, k),
            voxel_grid.get_voxel_position(i+1, j, k),
            voxel_grid.get_voxel_position(i+1, j+1, k),
            voxel_grid.get_voxel_position(i, j+1, k),
            voxel_grid.get_voxel_position(i, j, k+1),
            voxel_grid.get_voxel_position(i+1, j, k+1),
            voxel_grid.get_voxel_position(i+1, j+1, k+1),
            voxel_grid.get_voxel_position(i, j+1, k+1),
        ];
        
        for (edge_idx, &(v1, v2)) in edges.iter().enumerate() {
            let val1 = cube_values[v1];
            let val2 = cube_values[v2];
            
            if (val1 < iso_value) != (val2 < iso_value) {
                // 线性插值计算交点
                let t = (iso_value - val1) / (val2 - val1);
                let pos1 = &cube_positions[v1];
                let pos2 = &cube_positions[v2];
                
                edge_vertices[edge_idx] = Point3::new(
                    pos1.x + t as f64 * (pos2.x - pos1.x),
                    pos1.y + t as f64 * (pos2.y - pos1.y),
                    pos1.z + t as f64 * (pos2.z - pos1.z),
                );
            }
        }
    }

    /// 简单三角剖分
    fn simple_triangulation(
        &self,
        vertices: &[Vertex],
    ) -> Result<Vec<Triangle>> {
        let mut triangles = Vec::new();
        
        // 简化实现：每3个点形成一个三角形
        for i in (0..vertices.len()).step_by(3) {
            if i + 2 < vertices.len() {
                let triangle = Triangle {
                    vertices: [i, i + 1, i + 2],
                    normal: Vector3::new(0.0, 0.0, 1.0),
                    area: 0.0,
                };
                triangles.push(triangle);
            }
        }
        
        Ok(triangles)
    }

    /// 计算法向量和面积
    fn compute_normals_and_areas(
        &self,
        vertices: &mut [Vertex],
        triangles: &mut [Triangle],
    ) -> Result<()> {
        // 重置顶点法向量
        for vertex in vertices.iter_mut() {
            vertex.normal = Vector3::new(0.0, 0.0, 0.0);
        }
        
        // 计算面法向量和面积
        for triangle in triangles.iter_mut() {
            let v1 = &vertices[triangle.vertices[0]].position;
            let v2 = &vertices[triangle.vertices[1]].position;
            let v3 = &vertices[triangle.vertices[2]].position;
            
            let edge1 = Vector3::new(
                (v2.x - v1.x) as f32,
                (v2.y - v1.y) as f32,
                (v2.z - v1.z) as f32,
            );
            let edge2 = Vector3::new(
                (v3.x - v1.x) as f32,
                (v3.y - v1.y) as f32,
                (v3.z - v1.z) as f32,
            );
            
            let face_normal = edge1.cross(&edge2);
            let area = face_normal.norm() * 0.5;
            
            triangle.normal = if area > 0.0 {
                face_normal.normalize()
            } else {
                Vector3::new(0.0, 0.0, 1.0)
            };
            triangle.area = area;
            
            // 累加到顶点法向量
            for &vertex_idx in &triangle.vertices {
                vertices[vertex_idx].normal += triangle.normal * area;
            }
        }
        
        // 归一化顶点法向量
        for vertex in vertices.iter_mut() {
            if vertex.normal.norm() > 0.0 {
                vertex.normal = vertex.normal.normalize();
            } else {
                vertex.normal = Vector3::new(0.0, 0.0, 1.0);
            }
        }
        
        Ok(())
    }

    /// 后处理网格
    fn post_process_mesh(
        &self,
        mut mesh: TriangleMesh,
    ) -> Result<TriangleMesh> {
        // 平滑处理
        for _ in 0..self.config.smoothing_iterations {
            self.smooth_mesh(&mut mesh)?;
        }
        
        // 网格简化
        if let Some(target_count) = self.config.target_triangle_count
            && mesh.triangles.len() > target_count {
                mesh = self.simplify_mesh(mesh, target_count)?;
            }
        
        // 重新计算统计信息
        mesh.statistics = self.compute_mesh_statistics(&mesh.vertices, &mesh.triangles);
        
        Ok(mesh)
    }

    /// 平滑网格
    fn smooth_mesh(&self, mesh: &mut TriangleMesh) -> Result<()> {
        // 拉普拉斯平滑
        let mut new_positions = vec![Point3::new(0.0, 0.0, 0.0); mesh.vertices.len()];
        let mut neighbor_counts = vec![0; mesh.vertices.len()];
        
        // 计算邻居平均位置
        for triangle in &mesh.triangles {
            for i in 0..3 {
                let v1 = triangle.vertices[i];
                let v2 = triangle.vertices[(i + 1) % 3];
                
                let pos1 = &mesh.vertices[v1].position;
                let pos2 = &mesh.vertices[v2].position;
                
                new_positions[v1].x += pos2.x;
                new_positions[v1].y += pos2.y;
                new_positions[v1].z += pos2.z;
                neighbor_counts[v1] += 1;
                
                new_positions[v2].x += pos1.x;
                new_positions[v2].y += pos1.y;
                new_positions[v2].z += pos1.z;
                neighbor_counts[v2] += 1;
            }
        }
        
        // 更新顶点位置
        for i in 0..mesh.vertices.len() {
            if neighbor_counts[i] > 0 {
                let count = neighbor_counts[i] as f64;
                let old_pos = &mesh.vertices[i].position;
                let avg_pos = Point3::new(
                    new_positions[i].x / count,
                    new_positions[i].y / count,
                    new_positions[i].z / count,
                );
                
                // 混合原位置和平均位置
                let lambda = 0.1; // 平滑因子
                mesh.vertices[i].position = Point3::new(
                    old_pos.x * (1.0 - lambda) + avg_pos.x * lambda,
                    old_pos.y * (1.0 - lambda) + avg_pos.y * lambda,
                    old_pos.z * (1.0 - lambda) + avg_pos.z * lambda,
                );
            }
        }
        
        Ok(())
    }

    /// 简化网格
    fn simplify_mesh(
        &self,
        mesh: TriangleMesh,
        target_triangle_count: usize,
    ) -> Result<TriangleMesh> {
        // 简化实现：随机移除三角形
        let mut triangles = mesh.triangles;
        
        while triangles.len() > target_triangle_count {
            if triangles.len() <= 1 {
                break;
            }
            triangles.pop();
        }
        
        Ok(TriangleMesh {
            vertices: mesh.vertices,
            triangles,
            statistics: mesh.statistics,
        })
    }

    /// 计算网格统计信息
    fn compute_mesh_statistics(
        &self,
        vertices: &[Vertex],
        triangles: &[Triangle],
    ) -> MeshStatistics {
        let surface_area: f32 = triangles.iter().map(|t| t.area).sum();
        
        // 简化的体积计算
        let volume = surface_area * 0.1; // 占位符
        
        // 估算边数（欧拉公式：V - E + F = 2）
        let num_edges = if vertices.len() >= 2 && !triangles.is_empty() {
            vertices.len() + triangles.len() - 2
        } else {
            0
        };
        
        MeshStatistics {
            num_vertices: vertices.len(),
            num_triangles: triangles.len(),
            num_edges,
            surface_area,
            volume,
        }
    }

    /// 计算点距离
    fn point_distance(&self, p1: &Point3, p2: &Point3) -> f32 {
        let dx = (p1.x - p2.x) as f32;
        let dy = (p1.y - p2.y) as f32;
        let dz = (p1.z - p2.z) as f32;
        (dx * dx + dy * dy + dz * dz).sqrt()
    }
}

impl VoxelGrid3D {
    fn new(resolution: usize, bounds: BoundingBox) -> Self {
        let voxels = vec![
            vec![
                vec![
                    VoxelData {
                        distance: 0.0,
                        gradient: Vector3::new(0.0, 0.0, 0.0),
                        confidence: 0.0,
                        has_point: false,
                    };
                    resolution
                ];
                resolution
            ];
            resolution
        ];
        
        Self {
            resolution,
            bounds,
            voxels,
        }
    }
    
    fn get_voxel_position(&self, i: usize, j: usize, k: usize) -> Point3 {
        let size_x = self.bounds.max.x - self.bounds.min.x;
        let size_y = self.bounds.max.y - self.bounds.min.y;
        let size_z = self.bounds.max.z - self.bounds.min.z;
        
        Point3::new(
            self.bounds.min.x + (i as f64 / (self.resolution - 1) as f64) * size_x,
            self.bounds.min.y + (j as f64 / (self.resolution - 1) as f64) * size_y,
            self.bounds.min.z + (k as f64 / (self.resolution - 1) as f64) * size_z,
        )
    }
}

impl MarchingCubesLUT {
    fn new() -> Self {
        // 简化的查找表（实际实现需要完整的256项表）
        let edge_table = [0; 256];
        let triangle_table = [[-1; 16]; 256];
        
        Self {
            edge_table,
            triangle_table,
        }
    }
}