colmap 0.1.2

//! 重建结果和统计信息
//!
//! 这个模块定义了完整的重建结果数据结构和相关的统计信息。

use std::collections::HashMap;
use serde::{Deserialize, Serialize};
use crate::core::{
    Result, ColmapError, Point3dId, TrackId,
    Image, Point3d, Track, Camera
};
use crate::core::image::ImageId;

/// 重建统计信息
#[derive(Debug, Clone, Serialize, Deserialize)]
pub struct ReconstructionStats {
    /// 注册图像数量
    pub num_registered_images: usize,
    /// 总图像数量
    pub num_total_images: usize,
    /// 3D 点数量
    pub num_points3d: usize,
    /// 观测数量
    pub num_observations: usize,
    /// 平均轨迹长度
    pub mean_track_length: f64,
    /// 平均重投影误差
    pub mean_reprojection_error: f64,
    /// 重建覆盖率
    pub coverage_ratio: f64,
}

impl ReconstructionStats {
    /// 创建空的统计信息
    pub fn new() -> Self {
        Self {
            num_registered_images: 0,
            num_total_images: 0,
            num_points3d: 0,
            num_observations: 0,
            mean_track_length: 0.0,
            mean_reprojection_error: 0.0,
            coverage_ratio: 0.0,
        }
    }
}

impl Default for ReconstructionStats {
    fn default() -> Self {
        Self::new()
    }
}

/// 完整的重建结果
#[derive(Debug, Serialize, Deserialize)]
pub struct ReconstructionResult {
    /// 相机数据库
    pub cameras: HashMap<u32, Camera>,
    /// 图像数据库
    pub images: HashMap<ImageId, Image>,
    /// 3D 点云
    pub points3d: HashMap<Point3dId, Point3d>,
    /// 特征轨迹
    pub tracks: HashMap<TrackId, Track>,
    /// 重建统计信息
    pub stats: ReconstructionStats,
    /// 重建元数据
    pub metadata: ReconstructionMetadata,
}

/// 重建元数据
#[derive(Debug, Clone, Serialize, Deserialize)]
pub struct ReconstructionMetadata {
    /// 重建名称
    pub name: String,
    /// 创建时间
    pub created_at: String,
    /// 重建算法
    pub algorithm: String,
    /// 配置参数
    pub config: HashMap<String, String>,
    /// 处理时间（秒）
    pub processing_time: f64,
}

impl Default for ReconstructionMetadata {
    fn default() -> Self {
        Self {
            name: "Untitled Reconstruction".to_string(),
            created_at: chrono::Utc::now().to_rfc3339(),
            algorithm: "Incremental SfM".to_string(),
            config: HashMap::new(),
            processing_time: 0.0,
        }
    }
}

impl ReconstructionResult {
    /// 创建新的重建结果
    pub fn new() -> Self {
        Self {
            cameras: HashMap::new(),
            images: HashMap::new(),
            points3d: HashMap::new(),
            tracks: HashMap::new(),
            stats: ReconstructionStats::new(),
            metadata: ReconstructionMetadata::default(),
        }
    }
    
    /// 添加相机
    pub fn add_camera(&mut self, camera: Camera) {
        self.cameras.insert(camera.id, camera);
    }
    
    /// 添加图像
    pub fn add_image(&mut self, image: Image) {
        self.images.insert(image.id, image);
    }
    
    /// 添加 3D 点
    pub fn add_point3d(&mut self, point: Point3d) {
        self.points3d.insert(point.id, point);
    }
    
    /// 添加轨迹
    pub fn add_track(&mut self, track: Track) {
        self.tracks.insert(track.id, track);
    }
    
    /// 获取相机
    pub fn get_camera(&self, id: u32) -> Option<&Camera> {
        self.cameras.get(&id)
    }
    
    /// 获取图像
    pub fn get_image(&self, id: ImageId) -> Option<&Image> {
        self.images.get(&id)
    }
    
    /// 获取 3D 点
    pub fn get_point3d(&self, id: Point3dId) -> Option<&Point3d> {
        self.points3d.get(&id)
    }
    
    /// 获取轨迹
    pub fn get_track(&self, id: TrackId) -> Option<&Track> {
        self.tracks.get(&id)
    }
    
    /// 获取已注册的图像
    pub fn registered_images(&self) -> Vec<&Image> {
        self.images.values().filter(|img| img.is_registered()).collect()
    }
    
    /// 获取未注册的图像
    pub fn unregistered_images(&self) -> Vec<&Image> {
        self.images.values().filter(|img| !img.is_registered()).collect()
    }
    
    /// 计算重建的边界框
    pub fn bounding_box(&self) -> Option<(nalgebra::Point3<f64>, nalgebra::Point3<f64>)> {
        if self.points3d.is_empty() {
            return None;
        }
        
        let mut min_point = nalgebra::Point3::new(f64::INFINITY, f64::INFINITY, f64::INFINITY);
        let mut max_point = nalgebra::Point3::new(f64::NEG_INFINITY, f64::NEG_INFINITY, f64::NEG_INFINITY);
        
        for point in self.points3d.values() {
            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);
        }
        
        Some((min_point, max_point))
    }
    
    /// 计算重建的中心点
    pub fn center(&self) -> Option<nalgebra::Point3<f64>> {
        if let Some((min_point, max_point)) = self.bounding_box() {
            Some(nalgebra::Point3::from((min_point.coords + max_point.coords) / 2.0))
        } else {
            None
        }
    }
    
    /// 计算重建的尺度（边界框对角线长度）
    pub fn scale(&self) -> Option<f64> {
        if let Some((min_point, max_point)) = self.bounding_box() {
            Some((max_point - min_point).norm())
        } else {
            None
        }
    }
    
    /// 获取已注册图像数量
    pub fn num_reg_images(&self) -> usize {
        self.registered_images().len()
    }
    
    /// 获取 3D 点数量
    pub fn num_points3d(&self) -> usize {
        self.points3d.len()
    }
    
    /// 获取观测数量
    pub fn num_observations(&self) -> usize {
        self.points3d.values().map(|p| p.num_observations()).sum()
    }
    
    /// 获取轨迹数量
    pub fn num_tracks(&self) -> usize {
        self.tracks.len()
    }
    
    /// 获取平均轨迹长度
    pub fn mean_track_length(&self) -> f64 {
        if self.tracks.is_empty() {
            0.0
        } else {
            let total_length: usize = self.tracks.values().map(|t| t.length).sum();
            total_length as f64 / self.tracks.len() as f64
        }
    }
    
    /// 获取平均重投影误差
    pub fn mean_reprojection_error(&self) -> f64 {
        if self.points3d.is_empty() {
            0.0
        } else {
            let total_error: f64 = self.points3d.values().map(|p| p.error).sum();
            total_error / self.points3d.len() as f64
        }
    }
    
    /// 更新统计信息
    pub fn update_stats(&mut self) {
        let registered_images: Vec<_> = self.registered_images();
        
        self.stats.num_registered_images = registered_images.len();
        self.stats.num_total_images = self.images.len();
        self.stats.num_points3d = self.points3d.len();
        
        // 计算观测数量
        self.stats.num_observations = self.points3d.values()
            .map(|p| p.num_observations())
            .sum();
        
        // 计算平均轨迹长度
        if !self.tracks.is_empty() {
            let total_length: usize = self.tracks.values().map(|t| t.length).sum();
            self.stats.mean_track_length = total_length as f64 / self.tracks.len() as f64;
        }
        
        // 计算平均重投影误差
        if !self.points3d.is_empty() {
            let total_error: f64 = self.points3d.values().map(|p| p.error).sum();
            self.stats.mean_reprojection_error = total_error / self.points3d.len() as f64;
        }
        
        // 计算覆盖率
        if !self.images.is_empty() {
            self.stats.coverage_ratio = self.stats.num_registered_images as f64 / self.stats.num_total_images as f64;
        }
    }
    
    /// 验证重建结果的一致性
    pub fn validate(&self) -> Result<()> {
        // 检查相机-图像关联
        for image in self.images.values() {
            if !self.cameras.contains_key(&image.camera_id) {
                return Err(ColmapError::Database(
                    format!("图像 {} 引用了不存在的相机 {}", image.id, image.camera_id)
                ));
            }
        }
        
        // 检查 3D 点-轨迹关联
        for point in self.points3d.values() {
            if let Some(track_id) = point.track_id
                && !self.tracks.contains_key(&track_id) {
                    return Err(ColmapError::Database(
                        format!("3D 点 {} 引用了不存在的轨迹 {}", point.id, track_id)
                    ));
                }
        }
        
        // 检查轨迹-3D 点关联
        for track in self.tracks.values() {
            if let Some(point3d_id) = track.point3d_id
                && !self.points3d.contains_key(&point3d_id) {
                    return Err(ColmapError::Database(
                        format!("轨迹 {} 引用了不存在的 3D 点 {}", track.id, point3d_id)
                    ));
                }
        }
        
        Ok(())
    }
    
    /// 清空重建结果
    pub fn clear(&mut self) {
        self.cameras.clear();
        self.images.clear();
        self.points3d.clear();
        self.tracks.clear();
        self.stats = ReconstructionStats::new();
        self.metadata = ReconstructionMetadata::default();
    }
    
    /// 获取重建摘要信息
    pub fn summary(&self) -> String {
        format!(
            "重建摘要:\n\
             - 相机数量: {}\n\
             - 图像数量: {} (已注册: {})\n\
             - 3D 点数量: {}\n\
             - 观测数量: {}\n\
             - 平均轨迹长度: {:.2}\n\
             - 平均重投影误差: {:.3} 像素\n\
             - 覆盖率: {:.1}%",
            self.cameras.len(),
            self.stats.num_total_images,
            self.stats.num_registered_images,
            self.stats.num_points3d,
            self.stats.num_observations,
            self.stats.mean_track_length,
            self.stats.mean_reprojection_error,
            self.stats.coverage_ratio * 100.0
        )
    }
}

impl Default for ReconstructionResult {
    fn default() -> Self {
        Self::new()
    }
}

#[cfg(test)]
mod tests {
    use super::*;
    use crate::core::{CameraIntrinsics, DistortionModel};
    use nalgebra::Point3;
    
    #[test]
    fn test_reconstruction_result() {
        let mut result = ReconstructionResult::new();
        
        // 添加相机
        let intrinsics = CameraIntrinsics::new(
            800.0, 800.0, 320.0, 240.0,
            DistortionModel::None
        );
        let camera = Camera::new(1, intrinsics, (640, 480), "TestCamera".to_string());
        result.add_camera(camera);
        
        // 添加图像
        let image = Image::new(1, "test.jpg".to_string(), 1, (640, 480));
        result.add_image(image);
        
        // 添加 3D 点
        let point = Point3d::new(1, Point3::new(1.0, 2.0, 3.0));
        result.add_point3d(point);
        
        assert_eq!(result.cameras.len(), 1);
        assert_eq!(result.images.len(), 1);
        assert_eq!(result.points3d.len(), 1);
        
        // 验证一致性
        assert!(result.validate().is_ok());
        
        // 更新统计信息
        result.update_stats();
        assert_eq!(result.stats.num_total_images, 1);
        assert_eq!(result.stats.num_points3d, 1);
    }
    
    #[test]
    fn test_bounding_box() {
        let mut result = ReconstructionResult::new();
        
        let point1 = Point3d::new(1, Point3::new(0.0, 0.0, 0.0));
        let point2 = Point3d::new(2, Point3::new(1.0, 1.0, 1.0));
        
        result.add_point3d(point1);
        result.add_point3d(point2);
        
        let (min_point, max_point) = result.bounding_box().unwrap();
        assert_eq!(min_point, Point3::new(0.0, 0.0, 0.0));
        assert_eq!(max_point, Point3::new(1.0, 1.0, 1.0));
        
        let center = result.center().unwrap();
        assert_eq!(center, Point3::new(0.5, 0.5, 0.5));
        
        let scale = result.scale().unwrap();
        assert!((scale - 3.0_f64.sqrt()).abs() < 1e-10);
    }
}