"""
统一投影管理器（无旧兼容代码）
- 统一去畸变、投影、渲染、颜色管理入口
- 不再保留旧缓存、旧接口，所有数据源来自 ColorManager 映射表
"""

import time
from typing import Dict, List, Optional, Tuple, Union

import numpy as np

from lidar_manager.color_utils.color_mapper import ColorMapper
from lidar_manager.logger import get_module_logger
from lidar_manager.pointcloud.color_manager import ColorManager
from lidar_manager.pointcloud.projector import LidarToCameraProjector, apply_pinhole_distortion, apply_fisheye_distortion
from lidar_manager.pointcloud.renderer import PointCloudRenderer
from lidar_manager.projection.projection_manager_cache import ProjectionCacheManager

logger = get_module_logger(__name__)


class ProjectionManager:
    """
    统一投影管理器

    核心职责：
    - process_image：唯一去畸变入口
    - project_points：唯一投影入口，构建 ColorManager 映射表
    - render_to_image：唯一渲染入口
    - 查询接口：nearest、visible、统计等均基于 ColorManager
    """

    def __init__(
        self,
        image_processor=None,
        projector: Optional[LidarToCameraProjector] = None,
        renderer: Optional[PointCloudRenderer] = None,
        color_manager: Optional[ColorManager] = None,
        initial_distance_range: Optional[Tuple[float, float]] = None,
        initial_intensity_range: Optional[Tuple[float, float]] = None,
    ):
        self._image_processor = image_processor
        self._renderer = renderer or PointCloudRenderer()
        self._color_mgr = color_manager or ColorManager(
            initial_distance_range=initial_distance_range,
            initial_intensity_range=initial_intensity_range
        )
        self._projector = projector or LidarToCameraProjector(
            camera_intrinsic=np.eye(3), extrinsic_lidar2cam=np.eye(4)
        )

        self._K: Optional[np.ndarray] = None
        self._T: Optional[np.ndarray] = None
        self._image_size: Optional[Tuple[int, int]] = None
        self._remove_backward_points: bool = True
        self._current_color_type: str = "distance"

        # 缓存管理器
        self._cache_mgr = ProjectionCacheManager(max_cache_size_mb=500.0)

        # 🆕 畸变模式管理
        self._distortion_mode: str = 'image_undistort'  # 'image_undistort' | 'pointcloud_distort'
        self._original_K: Optional[np.ndarray] = None  # 原始内参（用于点云加畸变模式）
        self._distortion_coeffs: Optional[np.ndarray] = None  # 畸变系数
        self._camera_model: str = 'pinhole'  # 相机模型类型 ('pinhole' | 'fisheye')

        # 背景模式
        self._background_mode: str = 'image'  # 'image' 或 'color'

    # ========== 配置与依赖 ==========
    def update_intrinsic(self, K: np.ndarray) -> None:
        self._K = K.copy()
        # 🔧 修复：在点云加畸变模式下，_projector应该使用原始内参_original_K
        # 但在图像去畸变模式下，应该使用去畸变后的内参K
        if self._distortion_mode == 'pointcloud_distort' and self._original_K is not None:
            # 点云加畸变模式：使用原始内参
            self._projector.update_intrinsic(self._original_K)
        else:
            # 图像去畸变模式：使用去畸变后的内参
            self._projector.update_intrinsic(K)
        self.clear_undistort_cache()
        # 内参变化会影响投影，清除点云投影缓存
        self.clear_pointcloud_cache()

    def update_extrinsic(self, T: np.ndarray) -> None:
        self._T = T.copy()
        self._projector.update_extrinsic(T)
        # 外参变化时，不直接清除缓存，而是通过角度阈值机制判断
        # 在下次project_points()调用时，会根据角度差决定是否更新缓存

    def update_image_size(self, width: int, height: int) -> None:
        self._image_size = (width, height)
        # 图像尺寸变化会影响投影范围，必须清除点云投影缓存
        self.clear_pointcloud_cache()

    def set_remove_backward_points(self, remove: bool) -> None:
        self._remove_backward_points = remove
        self._projector.set_remove_backward_points(remove)

    def set_image_processor(self, image_processor) -> None:
        self._image_processor = image_processor

    def update_color_scheme(self, color_list: List, recompute_normalized: bool = False) -> None:
        """更新配色方案（唯一入口）"""
        self._color_mgr.update_color_scheme(color_list, recompute_normalized=recompute_normalized)

    def set_background_mode(self, mode: str, color: Optional[List[int]] = None) -> None:
        """
        设置背景模式
        
        参数:
            mode: 'image' 或 'color'
            color: 背景色（BGR格式），mode='color'时必需
        """
        if mode not in ['image', 'color']:
            logger.warning(f"[ProjectionManager] 无效的背景模式: {mode}，使用默认'image'")
            mode = 'image'
        
        old_mode = getattr(self, '_background_mode', None)
        self._background_mode = mode
        
        if mode == 'color' and color is not None:
            self._cache_mgr.set_background_color(list(color))
        elif mode == 'image':
            self._cache_mgr.set_background_color(None)

    # ========== 🆕 畸变模式管理 ==========
    def set_distortion_mode(self, mode: str) -> None:
        """
        设置畸变模式
        
        参数:
            mode: 'image_undistort' - 图像去畸变模式（默认）
                  'pointcloud_distort' - 点云加畸变模式
        """
        if mode not in ['image_undistort', 'pointcloud_distort']:
            logger.warning(f"[ProjectionManager] 无效的畸变模式: {mode}，使用默认'image_undistort'")
            mode = 'image_undistort'
        
        if self._distortion_mode != mode:
            self._distortion_mode = mode
            # 🔧 修复：切换模式时，更新_projector的内参
            if mode == 'pointcloud_distort' and self._original_K is not None:
                # 点云加畸变模式：使用原始内参
                self._projector.update_intrinsic(self._original_K)
            elif mode == 'image_undistort' and self._K is not None:
                # 图像去畸变模式：使用去畸变后的内参
                self._projector.update_intrinsic(self._K)
            # 切换模式时清除所有缓存
            self.clear_undistort_cache()
            self.clear_pointcloud_cache()
            logger.info(f"[ProjectionManager] 畸变模式已切换为: {mode}")

    def get_distortion_mode(self) -> str:
        """获取当前畸变模式"""
        return self._distortion_mode

    def set_distortion_params(self, K: np.ndarray, D: Optional[np.ndarray] = None, camera_model: str = 'pinhole') -> None:
        """
        设置原始畸变参数（用于点云加畸变模式）
        
        参数:
            K: 原始内参矩阵 (3x3)
            D: 畸变系数（可选，如果为None则从image_processor获取）
            camera_model: 相机模型 ('pinhole' | 'fisheye')
        """
        self._original_K = K.copy()
        self._camera_model = camera_model
        
        # 如果未提供畸变系数，尝试从image_processor获取
        if D is None and self._image_processor is not None:
            D = self._image_processor.get_distortion_coeffs()
        
        if D is not None and len(D) > 0:
            self._distortion_coeffs = D.copy() if isinstance(D, np.ndarray) else np.array(D)
        else:
            self._distortion_coeffs = None
            logger.warning("[ProjectionManager] 未设置畸变系数，点云加畸变模式可能无法正常工作")
        
        # 🔧 修复：如果当前是点云加畸变模式，立即更新_projector的内参
        if self._distortion_mode == 'pointcloud_distort' and self._original_K is not None:
            self._projector.update_intrinsic(self._original_K)

    def apply_distortion_to_2d(self, points_2d_normalized: np.ndarray) -> np.ndarray:
        """
        对2D投影点应用畸变（点云加畸变模式使用）
        
        参数:
            points_2d_normalized: (N, 2) 归一化相机坐标 (x/z, y/z)
        
        返回:
            (N, 2) 畸变后的像素坐标
        
        注意：输入应该是归一化坐标，不是像素坐标
        """
        if self._distortion_coeffs is None or self._original_K is None:
            logger.warning("[ProjectionManager] 畸变参数未设置，无法应用畸变")
            # 如果没有畸变参数，直接投影到像素坐标
            if self._original_K is not None:
                fx, fy = self._original_K[0, 0], self._original_K[1, 1]
                cx, cy = self._original_K[0, 2], self._original_K[1, 2]
                u = fx * points_2d_normalized[:, 0] + cx
                v = fy * points_2d_normalized[:, 1] + cy
                return np.column_stack([u, v])
            else:
                return points_2d_normalized
        
        # 🔧 调试：输出畸变参数信息（仅在第一次调用时）
        if not hasattr(self, '_distortion_debug_logged'):
            logger.info(f"[ProjectionManager] 应用畸变 - 相机模型: {self._camera_model}")
            logger.info(f"[ProjectionManager] 应用畸变 - 原始内参K:\n{self._original_K}")
            logger.info(f"[ProjectionManager] 应用畸变 - 畸变系数: {self._distortion_coeffs}")
            if self._camera_model == 'pinhole' and len(self._distortion_coeffs) >= 5:
                logger.info(f"[ProjectionManager] 应用畸变 - k1={self._distortion_coeffs[0]:.6f}, "
                           f"k2={self._distortion_coeffs[1]:.6f}, p1={self._distortion_coeffs[2]:.6f}, "
                           f"p2={self._distortion_coeffs[3]:.6f}, k3={self._distortion_coeffs[4]:.6f}")
            self._distortion_debug_logged = True
        
        # 🔧 调试：记录调用信息
        logger.debug(f"[ProjectionManager] apply_distortion_to_2d调用: 输入点数={len(points_2d_normalized)}, "
                   f"相机模型={self._camera_model}, 畸变系数长度={len(self._distortion_coeffs) if self._distortion_coeffs is not None else 0}")
        
        if self._camera_model == 'fisheye':
            result = apply_fisheye_distortion(points_2d_normalized, self._distortion_coeffs, self._original_K)
        else:
            result = apply_pinhole_distortion(points_2d_normalized, self._distortion_coeffs, self._original_K)
        
        # 🔧 调试：记录输出信息
        if len(result) > 0:
            logger.debug(f"[ProjectionManager] apply_distortion_to_2d返回: 输出点数={len(result)}, "
                       f"像素坐标范围: u=({np.min(result[:, 0]):.2f}, {np.max(result[:, 0]):.2f}), "
                       f"v=({np.min(result[:, 1]):.2f}, {np.max(result[:, 1]):.2f})")
        
        return result

    def undistort_point(self, pixel_coord: Tuple[int, int]) -> Optional[Tuple[float, float]]:
        """
        将像素坐标去畸变（用于点云加畸变模式下的2D点选择）
        
        参数:
            pixel_coord: (px, py) 畸变状态的像素坐标
        
        返回:
            (x, y) 去畸变后的归一化坐标，或None（如果失败）
        
        注意：此方法用于PnP选点时，将用户点击的畸变像素坐标转换为去畸变坐标
        """
        if self._distortion_coeffs is None or self._original_K is None:
            logger.warning("[ProjectionManager] 畸变参数未设置，无法去畸变")
            return None
        
        try:
            import cv2
            px, py = pixel_coord
            
            # 使用OpenCV的去畸变函数
            if self._camera_model == 'fisheye':
                # 鱼眼相机去畸变：OpenCV要求 (N,1,2) 且 dtype 为 float32/float64
                point_2d = np.array([[[px, py]]], dtype=np.float32)
                undistorted = cv2.fisheye.undistortPoints(
                    point_2d, self._original_K, self._distortion_coeffs, None, None
                )
            else:
                # 针孔相机去畸变
                point_2d = np.array([[[px, py]]], dtype=np.float32)
                undistorted = cv2.undistortPoints(
                    point_2d, self._original_K, self._distortion_coeffs, None, None
                )
            
            if len(undistorted) > 0 and len(undistorted[0]) > 0:
                return (float(undistorted[0][0][0]), float(undistorted[0][0][1]))
            else:
                return None
        except Exception as e:
            logger.error(f"[ProjectionManager] 去畸变失败: {e}", exc_info=True)
            return None

    # ========== 统一去畸变 ==========
    def process_image(self, image: np.ndarray, force_recompute: bool = False) -> Tuple[np.ndarray, Optional[np.ndarray]]:
        """
        根据畸变模式处理图像
        
        参数:
            image: 原始图像
            force_recompute: 是否强制重新计算
        
        返回:
            (processed_image, K): 处理后的图像和对应的内参
        
        行为:
            - 'image_undistort': 去畸变图像，返回去畸变后的图像和新内参K'
            - 'pointcloud_distort': 直接返回原始图像和原始内参K
        """
        # 🆕 点云加畸变模式：直接返回原始图像和原始内参
        if self._distortion_mode == 'pointcloud_distort':
            if self._original_K is not None:
                # 使用原始内参
                h, w = image.shape[:2]
                # 🔧 修复：检查图像尺寸是否与配置一致
                if self._image_size is not None and self._image_size != (w, h):
                    logger.warning(
                        f"[ProjectionManager] ⚠️ 图像尺寸不匹配！"
                        f"实际图像尺寸: {w}x{h}, 配置中的image_size: {self._image_size}"
                        f"这可能导致投影错误。建议检查配置文件中的image_size设置。"
                    )
                self.update_image_size(w, h)
                logger.debug(f"[ProjectionManager] 点云加畸变模式 - 使用原始图像尺寸: {w}x{h}, 原始内参K:\n{self._original_K}")
                return image.copy(), self._original_K.copy()
            else:
                # 如果没有原始内参，使用当前内参
                logger.warning("[ProjectionManager] 点云加畸变模式但未设置原始内参，使用当前内参")
                return image.copy(), self._K
        
        # 图像去畸变模式（原有逻辑）
        if self._image_processor is None:
            logger.error("[ProjectionManager] 图像处理器未设置，无法去畸变")
            return image.copy(), self._K

        image_hash = hash(image.tobytes())
        cached_image = self._cache_mgr.get_undistorted_image(image_hash)
        if not force_recompute and cached_image is not None:
            return cached_image, self._K

        undistorted_image, new_K = self._image_processor.undistort_image(image)
        self._cache_mgr.set_undistorted_image(undistorted_image, image_hash)

        if new_K is not None:
            self.update_intrinsic(new_K)
        h, w = undistorted_image.shape[:2]
        self.update_image_size(w, h)

        return undistorted_image, new_K

    def clear_undistort_cache(self) -> None:
        self._cache_mgr.clear_undistort_cache()

    # ========== 角度阈值计算 ==========
    def _compute_rotation_angle_diff(self, R1: np.ndarray, R2: np.ndarray) -> float:
        """
        计算两个旋转矩阵之间的角度差（度）
        
        使用旋转矩阵的迹计算角度：angle = arccos((trace(R1^T @ R2) - 1) / 2)
        
        参数:
            R1: 第一个旋转矩阵 (3x3)
            R2: 第二个旋转矩阵 (3x3)
        
        返回:
            角度差（度）
        """
        R_diff = R1.T @ R2
        trace = np.trace(R_diff)
        # 限制在[-1, 3]范围内，避免数值误差
        # trace的范围是[-1, 3]，其中3表示完全一致，-1表示180度旋转
        trace = np.clip(trace, -1.0, 3.0)
        angle_rad = np.arccos((trace - 1.0) / 2.0)
        return np.rad2deg(angle_rad)

    def _should_update_cache_by_angle(self, current_T: np.ndarray) -> bool:
        """
        判断是否需要根据角度阈值更新缓存
        
        参数:
            current_T: 当前外参矩阵 (4x4)
        
        返回:
            True: 如果角度差超过阈值，需要更新
            False: 如果角度差在阈值内，可以使用缓存
        """
        return self._cache_mgr.should_update_cache_by_angle(
            current_T, self._compute_rotation_angle_diff
        )

    # ========== 统一投影 ==========
    def project_points(
        self,
        points: np.ndarray,
        color_type: str = "distance",
        attr_range: Optional[Tuple[float, float]] = None,
        color_filter_params: Optional[Dict] = None,
        range_filter_params: Optional[Dict] = None,
    ) -> Optional[np.ndarray]:
        """
        唯一投影入口：构建 ColorManager 映射表
        返回已过滤映射表（结构化数组），若无数据返回 None
        
        支持缓存机制：如果点云、内参未变化，且外参角度差在阈值内，使用缓存
        """
        if self._K is None or self._T is None:
            logger.error("[ProjectionManager] 内外参未设置，无法投影")
            return None
        if self._image_size is None:
            logger.error("[ProjectionManager] 图像尺寸未设置，无法投影")
            return None
        if points is None or len(points) == 0:
            logger.warning("[ProjectionManager] 输入点云为空")
            return None

        t0 = time.perf_counter()
        self._current_color_type = color_type
        self._projector.set_remove_backward_points(self._remove_backward_points)

        # ========== 缓存检查 ==========
        points_hash = hash(points.tobytes())
        K_hash = hash(self._K.tobytes()) if self._K is not None else None
        
        # 检查缓存
        cached_data = self._cache_mgr.get_pointcloud_cache(points_hash, K_hash)
        if cached_data is not None:
            # 检查角度阈值
            if not self._should_update_cache_by_angle(self._T):
                # 缓存有效，使用缓存的中间结果
                need_reproject = False
            else:
                need_reproject = True
        else:
            need_reproject = True

        if need_reproject:
            # ========== 执行投影 ==========
            points_xyz = points[:, :3].copy()
            intensities = points[:, 3].copy() if points.shape[1] >= 4 else np.zeros(len(points_xyz))
            distances = np.linalg.norm(points_xyz, axis=1)

            # 坐标变换
            points_cam = self._projector.transform_to_camera(points_xyz)

            # 🆕 根据畸变模式选择投影方式
            if self._distortion_mode == 'pointcloud_distort':
                # 点云加畸变模式：使用OpenCV的projectPoints直接投影（参考hujing_org_pick_pnp_py38的实现）
                import cv2
                
                # 1. 基础可见性过滤（前方点）
                if self._remove_backward_points:
                    front_mask = points_cam[:, 2] > 1e-6
                else:
                    front_mask = np.ones(len(points_cam), dtype=bool)
                
                if not np.any(front_mask):
                    logger.warning("[ProjectionManager] 投影后无有效点（相机前方）")
                    self._color_mgr.clear_filter()
                    self.clear_pointcloud_cache()
                    return None
                
                # 2. 使用OpenCV的projectPoints直接投影（自动处理畸变）
                points_3d_cam = points_cam[front_mask, :3].reshape(-1, 1, 3)  # (N, 1, 3)格式
                # ⚠️关键修复：
                # points_cam 已经是通过 self._T 变换到“相机坐标系”的点。
                # 如果这里再把 self._T 转成 rvec/tvec 传给 projectPoints，会导致外参被应用两次，
                # 典型表现就是投影整体发生大角度旋转/错位（例如顺时针90度）。
                # 因此：当输入点在相机系时，应使用单位外参（rvec=tvec=0）。
                rvec = np.zeros((3, 1), dtype=np.float64)
                tvec = np.zeros((3, 1), dtype=np.float64)
                
                # 使用OpenCV的projectPoints，传入畸变系数
                if self._distortion_coeffs is not None and self._original_K is not None:
                    distortion = self._distortion_coeffs
                else:
                    distortion = np.zeros(5)  # 无畸变
                
                # OpenCV会自动处理所有畸变计算，包括极端值情况
                points_2d_projected, _ = cv2.projectPoints(
                    points_3d_cam, rvec, tvec, self._original_K, distortion
                )
                
                # 转换为整数像素坐标
                points_2d_int = np.round(points_2d_projected.squeeze()).astype(int)
                
                # 3. 图像范围过滤（points_2d_int已经是前方点的投影结果）
                width, height = self._image_size
                in_image_mask = (
                    (points_2d_int[:, 0] >= 0)
                    & (points_2d_int[:, 0] < width)
                    & (points_2d_int[:, 1] >= 0)
                    & (points_2d_int[:, 1] < height)
                )
                
                # 4. 组合有效掩码（将in_image_mask映射回原始点云索引）
                valid_mask = np.zeros(len(points_cam), dtype=bool)
                valid_mask[front_mask] = in_image_mask
                
                if not np.any(valid_mask):
                    logger.warning("[ProjectionManager] 投影后无有效点（图像范围内）")
                    self._color_mgr.clear_filter()
                    self.clear_pointcloud_cache()
                    return None
                
                # 5. 提取有效点
                points_xyz_valid = points_xyz[valid_mask]
                points_cam_valid = points_cam[valid_mask, :3]
                intensities_valid = intensities[valid_mask]
                distances_valid = distances[valid_mask]
                # points_2d_int已经是前方点的投影结果，in_image_mask对应前方点的索引
                points_2d = points_2d_int[in_image_mask]  # 只取图像范围内的点
            else:
                # 图像去畸变模式（原有逻辑）
                points_2d, valid_mask = self._projector.project_to_image(points_cam, self._image_size)
                if not np.any(valid_mask):
                    logger.warning("[ProjectionManager] 投影后无有效点")
                    self._color_mgr.clear_filter()
                    self.clear_pointcloud_cache()
                    return None

                points_xyz_valid = points_xyz[valid_mask]
                points_cam_valid = points_cam[valid_mask, :3]
                intensities_valid = intensities[valid_mask]
                distances_valid = distances[valid_mask]

            # ========== 更新缓存 ==========
            # 估算新缓存的大小
            new_cache_size = (
                points_xyz_valid.nbytes +
                points_cam_valid.nbytes +
                points_2d.nbytes +
                intensities_valid.nbytes +
                distances_valid.nbytes
            ) / (1024 * 1024)  # 转换为MB
            
            # 检查当前缓存大小加上新缓存是否会超过限制
            current_size = self._cache_mgr.estimate_cache_size()
            if current_size + new_cache_size > self._cache_mgr.get_max_cache_size_mb():
                logger.warning(
                    f"[ProjectionManager] 缓存大小将超过限制 "
                    f"({current_size + new_cache_size:.2f}MB > {self._cache_mgr.get_max_cache_size_mb()}MB)，清除点云缓存"
                )
                self.clear_pointcloud_cache()
            
            # 保存到缓存
            T_hash = hash(self._T.tobytes()) if self._T is not None else None
            self._cache_mgr.set_pointcloud_cache(
                points_hash, K_hash, T_hash,
                {
                    'points_xyz': points_xyz_valid,
                    'points_cam': points_cam_valid,
                    'points_2d': points_2d,
                    'valid_mask': valid_mask,
                    'intensities': intensities_valid,
                    'distances': distances_valid,
                }
            )

            # 更新基准外参
            self._cache_mgr.reset_baseline_extrinsic(self._T)
        else:
            # ========== 使用缓存 ==========
            points_xyz_valid = cached_data['points_xyz']
            points_cam_valid = cached_data['points_cam']
            intensities_valid = cached_data['intensities']
            distances_valid = cached_data['distances']
            points_2d = cached_data['points_2d']

        # 构建映射表（使用缓存或新计算的数据）
        self._color_mgr.build_mapping_table(
            points_2d=points_2d,
            points_xyz=points_xyz_valid,
            intensities=intensities_valid,
            distances=distances_valid,
            cam_xyz=points_cam_valid,
            color_type=color_type,
        )

        # 应用过滤：颜色/范围/显示区间
        if color_filter_params is not None and "reference_attribute" in color_filter_params:
            self._color_mgr.apply_filter(
                "attribute",
                {
                    "reference": color_filter_params["reference_attribute"],
                    "threshold": color_filter_params["threshold"],
                    "attr_type": "distance" if color_type == "distance" else "intensity",
                },
            )

        if range_filter_params is not None:
            self._color_mgr.apply_filter(
                "range",
                {
                    "min_val": range_filter_params["min_val"],
                    "max_val": range_filter_params["max_val"],
                    "attr_type": range_filter_params.get("mode", "distance"),
                },
            )

        if attr_range is not None:
            self._color_mgr.apply_filter(
                "range",
                {"min_val": attr_range[0], "max_val": attr_range[1], "attr_type": "distance" if color_type == "distance" else "intensity"},
            )

        mapping = self._color_mgr.get_mapping_table(filtered=True)
        if mapping is None or len(mapping) == 0:
            logger.warning("[ProjectionManager] 过滤后无有效点")
            return None

        elapsed = time.perf_counter() - t0
        logger.debug(f"[ProjectionManager] 投影完成: 输入 {len(points):,} → 可见 {len(mapping):,}，耗时 {elapsed:.3f}s")
        return mapping

    # ========== 统一渲染 ==========
    def render_to_image(self, image: np.ndarray, point_size: int = 2, point_opacity: float = 1.0,
                        use_background_mode: bool = True) -> np.ndarray:
        if point_size is None:
            point_size = 2
        if point_opacity is None:
            point_opacity = 1.0
        mapping = self._color_mgr.get_mapping_table(filtered=True)
        if mapping is None or len(mapping) == 0:
            logger.warning("[ProjectionManager] 映射表为空，跳过渲染")
            return image.copy()

        # 根据背景模式选择底图
        if use_background_mode and self._background_mode == 'color':
            # 使用背景色
            h, w = image.shape[:2]
            current_size = (w, h)
            bg_color = self._cache_mgr.get_background_color()
            
            # 检查背景图像缓存
            cached_bg = self._cache_mgr.get_background_image(current_size, bg_color)
            if cached_bg is not None:
                base_image = cached_bg
            else:
                # 创建背景图像
                if bg_color is not None:
                    bg_image = np.full((h, w, 3), bg_color, dtype=np.uint8)
                    logger.debug(f"[ProjectionManager] 创建背景色图像: {w}x{h}, 颜色: {bg_color}")
                else:
                    bg_image = np.zeros((h, w, 3), dtype=np.uint8)
                    logger.debug(f"[ProjectionManager] 使用默认黑色背景: {w}x{h}")
                
                self._cache_mgr.set_background_image(bg_image, current_size, bg_color)
                base_image = bg_image.copy()
        else:
            # 使用传入的图像（通常是去畸变后的图像）
            base_image = image.copy()

        # 每个像素选取 LiDAR 距离最近的点
        selected: Dict[Tuple[int, int], np.ndarray] = {}
        for point in mapping:
            px, py = point["pixel_2d"]
            key = (int(px), int(py))
            if key not in selected or point["distance"] < selected[key]["distance"]:
                selected[key] = point

        points_2d = np.array([[k[0], k[1]] for k in selected.keys()], dtype=int)
        colors = np.array([p["color_bgr"] for p in selected.values()], dtype=np.uint8)

        return self._renderer.draw_points(
            image=base_image,
            points_2d=points_2d,
            colors=colors,
            point_size=point_size,
            point_opacity=point_opacity,
        )

    # ========== 查询接口 ==========
    def get_visible_points(self) -> Optional[np.ndarray]:
        mapping = self._color_mgr.get_mapping_table(filtered=True)
        if mapping is None or len(mapping) == 0:
            return None

        if self._current_color_type == "distance":
            attr = mapping["distance"]
        else:
            attr = mapping["intensity"]

        return np.column_stack([mapping["xyz_3d"], attr])

    def get_visible_points_count(self) -> int:
        mapping = self._color_mgr.get_mapping_table(filtered=True)
        return 0 if mapping is None else len(mapping)

    def find_nearest_point(self, px: int, py: int, search_radius: int = 5) -> Optional[Dict]:
        mapping_at_pixel = self._color_mgr.get_points_by_pixel(px, py)
        if len(mapping_at_pixel) > 0:
            idx = np.argmin(mapping_at_pixel["distance"])
            point = mapping_at_pixel[idx]
            return self._to_dict(point)

        mapping_in_radius = self._color_mgr.get_points_in_radius(px, py, search_radius)
        if len(mapping_in_radius) > 0:
            point = mapping_in_radius[0]  # 已按距离排序
            return self._to_dict(point)

        return None

    def get_lidar_3d_at_pixel(
        self,
        px: int,
        py: int,
        search_radius: int = 5,
        distance_range: Optional[Tuple[float, float]] = None,
        intensity_range: Optional[Tuple[float, float]] = None,
        color_filter: Optional[Dict] = None,
        return_attributes: bool = False,
    ) -> Union[List[Tuple[float, float, float]], List[Dict]]:
        """
        按像素位置与可选过滤条件筛选 lidar 3D 点。

        前置条件：需先调用 project_points(...) 构建映射表。

        参数:
            px, py: 像素坐标（图像坐标系）。
            search_radius: 像素邻域半径；0 表示仅该像素，>0 表示该范围内的投影点均为候选。
            distance_range: (min_d, max_d) 米，仅保留 lidar 距离在此区间内的点；None 表示不过滤。
            intensity_range: (min_i, max_i)，仅保留强度在此区间内的点；None 表示不过滤。
            color_filter: {"reference_color": [B, G, R], "threshold": float}，仅保留与参考色 BGR 欧氏距离 <= threshold 的点；None 表示不过滤。
            return_attributes: False 返回 List[Tuple[x,y,z]]；True 返回 List[Dict]（含 xyz, distance, intensity, pixel_2d, color_bgr 等）。

        返回:
            满足条件的 3D 点列表；无候选或过滤后为空时返回 []。
        """
        px, py = int(px), int(py)
        if search_radius < 0:
            search_radius = 0
        if distance_range is not None:
            min_d, max_d = distance_range
            if min_d > max_d:
                logger.warning("[ProjectionManager] get_lidar_3d_at_pixel: distance_range min > max，忽略距离过滤")
                distance_range = None
        if intensity_range is not None:
            min_i, max_i = intensity_range
            if min_i > max_i:
                logger.warning("[ProjectionManager] get_lidar_3d_at_pixel: intensity_range min > max，忽略强度过滤")
                intensity_range = None
        if color_filter is not None:
            if "reference_color" not in color_filter or "threshold" not in color_filter:
                logger.warning("[ProjectionManager] get_lidar_3d_at_pixel: color_filter 需含 reference_color 与 threshold，忽略颜色过滤")
                color_filter = None

        if search_radius == 0:
            candidates = self._color_mgr.get_points_by_pixel(px, py)
        else:
            candidates = self._color_mgr.get_points_in_radius(px, py, search_radius)
        if len(candidates) == 0:
            return []

        if distance_range is not None:
            min_d, max_d = distance_range
            mask = (candidates["distance"] >= min_d) & (candidates["distance"] <= max_d)
            candidates = candidates[mask]
        if len(candidates) == 0:
            return []

        if intensity_range is not None:
            min_i, max_i = intensity_range
            mask = (candidates["intensity"] >= min_i) & (candidates["intensity"] <= max_i)
            candidates = candidates[mask]
        if len(candidates) == 0:
            return []

        if color_filter is not None:
            ref = np.array(color_filter["reference_color"], dtype=np.float64)
            threshold = float(color_filter["threshold"])
            dist = np.linalg.norm(candidates["color_bgr"].astype(np.float64) - ref, axis=1)
            mask = dist <= threshold
            candidates = candidates[mask]
        if len(candidates) == 0:
            return []

        if return_attributes:
            return [self._to_dict(p) for p in candidates]
        return [tuple(p["xyz_3d"]) for p in candidates]

    def count_by_attribute(self, num_bins: int, min_val: float, max_val: float) -> np.ndarray:
        mapping = self._color_mgr.get_mapping_table(filtered=True)
        if mapping is None or len(mapping) == 0:
            return np.zeros(num_bins, dtype=int)

        attr_values = mapping["distance"] if self._current_color_type == "distance" else mapping["intensity"]
        bins = np.linspace(min_val, max_val, num_bins + 1)
        hist, _ = np.histogram(attr_values, bins=bins)
        return hist

    def count_rendered_by_attribute(self, num_bins: int, min_val: float, max_val: float) -> Optional[np.ndarray]:
        """
        统计实际渲染到图像上的点的属性分布
        使用与 render_to_image 相同的像素选择逻辑（每个像素最近的点）
        
        🔧 修复：统计应该基于实际渲染的颜色索引，而不是距离区间
        因为渲染时使用的是 normalized 值映射到颜色，而不是直接按距离区间映射
        """
        mapping = self._color_mgr.get_mapping_table(filtered=True)
        if mapping is None or len(mapping) == 0:
            return None

        # 使用与 render_to_image 相同的像素选择逻辑：每个像素选取 LiDAR 距离最近的点
        selected: Dict[Tuple[int, int], np.ndarray] = {}
        for point in mapping:
            px, py = point["pixel_2d"]
            key = (int(px), int(py))
            if key not in selected or point["distance"] < selected[key]["distance"]:
                selected[key] = point

        if len(selected) == 0:
            return None

        # 🔧 修复：基于 normalized 值统计颜色索引，而不是基于距离值
        # 因为渲染时使用的是 normalized → 颜色索引的映射
        normalized_values = []
        distance_values = []
        color_bgr_values = []
        for point in selected.values():
            normalized_values.append(point["normalized"])
            if self._current_color_type == "distance":
                distance_values.append(point["distance"])
            else:
                distance_values.append(point["intensity"])
            color_bgr_values.append(tuple(point["color_bgr"]))

        normalized_values = np.array(normalized_values)
        distance_values = np.array(distance_values)

        # 将 normalized 值映射到颜色索引 [0, num_bins-1]
        # 注意：ColorMapper.get_color_from_list 使用 ratio * (len(color_list) - 1) 计算索引
        # 所以这里也要使用相同的逻辑
        color_indices_float = normalized_values * (num_bins - 1)
        color_indices = np.clip(color_indices_float, 0, num_bins - 1).astype(int)
        hist = np.bincount(color_indices, minlength=num_bins)
        return hist

    def get_global_attr_range(self) -> Tuple[float, float]:
        """返回归一化使用的全局属性范围"""
        return self._color_mgr.get_global_attr_range()

    # ========== 归一化范围（V0.0.5 公开接口，替代直接访问 _color_mgr） ==========
    def set_normalization_range(self, min_val: float, max_val: float, color_type: Optional[str] = None) -> None:
        """
        设置归一化使用的属性范围，并据此重算 normalized 与颜色。
        用于「手动模式」：由调用方指定距离或强度范围，映射到 [0,1] 再着色。

        参数:
            min_val: 属性最小值（距离单位米或强度值）
            max_val: 属性最大值
            color_type: 可选，'distance' 或 'intensity'；不传则保持当前 _current_color_type
        """
        if color_type is not None:
            self._current_color_type = color_type
        self._color_mgr.set_normalization_range(min_val, max_val, color_type=color_type)

    def recompute_normalized_and_colors(self, use_auto_range: bool = False) -> None:
        """
        根据当前映射表重新计算归一化值与颜色。
        用于「自适应模式」或切换范围后刷新显示。

        参数:
            use_auto_range: True 时使用当前点云实际属性范围重算（自适应）；
                           False 时使用已通过 set_normalization_range 设置的范围重算（手动）。
        需在 project_points 之后调用；映射表为空时无效果。
        """
        if not self._color_mgr.is_valid():
            return
        if use_auto_range:
            self._color_mgr._compute_normalized_and_colors(use_filtered=False, use_manual_range=False)
        else:
            self._color_mgr._compute_normalized_and_colors(use_filtered=False, use_manual_range=True)

    def is_valid(self) -> bool:
        return self._color_mgr.is_valid()

    # ========== 颜色与展示接口（供 UI / 调用方使用，避免依赖 _color_mgr） ==========
    def get_current_color_type(self) -> str:
        """返回当前着色属性：'distance' 或 'intensity'。"""
        return self._current_color_type

    def get_color_scheme(self) -> Optional[List]:
        """返回当前 BGR 颜色列表；无配色时返回 None。"""
        scheme = self._color_mgr._color_scheme
        if scheme is None:
            return None
        return [tuple(c) for c in scheme]

    def get_filtered_point_count(self) -> int:
        """返回当前过滤后的映射表点数。"""
        return self._color_mgr.get_point_count(filtered=True)

    def get_filtered_mapping(self) -> Optional[np.ndarray]:
        """
        返回当前过滤后的映射表（只读视图）。
        字段：pixel_2d, xyz_3d, intensity, distance, color_bgr, normalized, cam_xyz。
        """
        return self._color_mgr.get_mapping_table(filtered=True)

    def get_points_at_pixel(self, px: int, py: int) -> List[Dict]:
        """
        返回该像素处所有映射点（list of dict）。
        每项含 pixel, xyz, cam_xyz, attr, normalized, color（与 find_nearest_point 一致）。
        """
        raw = self._color_mgr.get_points_by_pixel(int(px), int(py))
        if len(raw) == 0:
            return []
        return [self._to_dict(p) for p in raw]

    def get_points_in_radius(self, px: int, py: int, radius: int) -> List[Dict]:
        """
        返回像素邻域内映射点（按 LiDAR 距离排序），list of dict。
        每项含 pixel, xyz, cam_xyz, attr, normalized, color。
        """
        raw = self._color_mgr.get_points_in_radius(int(px), int(py), int(radius))
        if len(raw) == 0:
            return []
        return [self._to_dict(p) for p in raw]

    def apply_range_filter(self, min_val: float, max_val: float, mode: str = "distance") -> None:
        """
        对当前映射表按属性范围过滤。投影后调用，用于颜色区间过滤等。
        mode: 'distance' 或 'intensity'。
        """
        self._color_mgr.filter_by_range(min_val, max_val, attr_type=mode)

    def clear_filters(self) -> None:
        """清除当前范围/属性等过滤。"""
        self._color_mgr.clear_filter()

    def get_color_stats_for_colormap(
        self, num_bins: int, min_val: float, max_val: float
    ) -> Dict:
        """
        配色查看器一站式数据：点数量统计 + 渲染统计 + 全局范围。
        返回: {"point_counts": ndarray, "rendered_counts": ndarray, "global_range": (min, max)}。
        """
        point_counts = self.count_by_attribute(num_bins, min_val, max_val)
        rendered_counts = self.count_rendered_by_attribute(num_bins, min_val, max_val)
        global_range = self.get_global_attr_range()
        return {
            "point_counts": point_counts,
            "rendered_counts": rendered_counts if rendered_counts is not None else np.zeros(num_bins, dtype=int),
            "global_range": global_range,
        }

    def get_points_and_colors_for_render(self) -> Tuple[Optional[np.ndarray], Optional[np.ndarray]]:
        """
        与 render_to_image 相同逻辑：每像素取最近点，返回 (points_2d, colors_bgr)。
        供 UI 自绘或导出。无数据时返回 (None, None)。
        """
        mapping = self._color_mgr.get_mapping_table(filtered=True)
        if mapping is None or len(mapping) == 0:
            return None, None
        selected: Dict[Tuple[int, int], np.ndarray] = {}
        for point in mapping:
            px, py = point["pixel_2d"]
            key = (int(px), int(py))
            if key not in selected or point["distance"] < selected[key]["distance"]:
                selected[key] = point
        points_2d = np.array([[k[0], k[1]] for k in selected.keys()], dtype=int)
        colors_bgr = np.array([p["color_bgr"] for p in selected.values()], dtype=np.uint8)
        return points_2d, colors_bgr

    # ========== 缓存管理 ==========
    def clear_pointcloud_cache(self) -> None:
        """清除点云投影缓存"""
        self._cache_mgr.clear_pointcloud_cache()

    def reset_baseline_extrinsic(self, T: Optional[np.ndarray] = None) -> None:
        """
        重置基准外参（用于角度阈值判断）
        
        参数:
            T: 外参矩阵 (4x4)，如果为None则清除基准
        """
        self._cache_mgr.reset_baseline_extrinsic(T)
        if T is None:
            logger.debug("[ProjectionManager] 基准外参已清除")

    def force_update_cache(self) -> None:
        """强制更新缓存（忽略角度阈值）"""
        self._cache_mgr.reset_baseline_extrinsic(None)
        self.clear_pointcloud_cache()
        logger.debug("[ProjectionManager] 强制清除所有缓存")

    def get_cache_stats(self) -> Dict:
        """获取缓存统计信息"""
        return self._cache_mgr.get_cache_stats()

    # ========== 辅助 ==========
    def _to_dict(self, point) -> Dict:
        attr = point["distance"] if self._current_color_type == "distance" else point["intensity"]
        return {
            "pixel": (int(point["pixel_2d"][0]), int(point["pixel_2d"][1])),
            "xyz": tuple(point["xyz_3d"]),
            "cam_xyz": tuple(point["cam_xyz"]),
            "attr": float(attr),
            "normalized": float(point["normalized"]),
            "color": tuple(point["color_bgr"]),
        }
