"""
ZF LIFETEC红外安全带佩戴检测算法框架

基于IR摄像头图像流，实现安全带路径追踪与误用判断

硬件需求:
- IR摄像头：850nm或940nm波段，≥2MP，≥30fps
- 红外补光：940nm LED阵列
- 安全带：ZF LIFETEC Infrared Belt

依赖: opencv-python>=4.8, scikit-image>=0.21, numpy>=1.24
"""

import cv2
import numpy as np
from skimage.filters import gabor_filter
from skimage.measure import label, regionprops
from typing import Tuple, List, Optional

class InfraredBeltDetector:
    """
    红外安全带检测器
    
    核心流程:
    1. IR图像预处理（降噪、增强）
    2. 基于光谱特征的带体分割
    3. 带体路径追踪（从肩点到 buckle）
    4. 佩戴路径分类（正确/误用）
    """
    
    def __init__(self, config: dict):
        # IR图像参数
        self.ir_wavelength = config.get('ir_wavelength', 940)  # nm
        self.resolution = config.get('resolution', (1920, 1080))
        self.fps = config.get('fps', 30)
        
        # 带体检测参数
        self.belt_width_px = config.get('belt_width_px', 45)  # 安全带宽度约45mm，约45px
        self.min_belt_length_px = config.get('min_belt_length', 200)
        
        # 误用分类阈值
        self.shoulder_angle_threshold = config.get('shoulder_angle_threshold', 25)  # 度
        self.chest_coverage_threshold = config.get('chest_coverage', 0.6)  # 胸部覆盖比例
        
        # Gabor滤波器参数（带体纹理检测）
        self.gabor_freq = 0.1  # 空间频率
        self.gabor_theta = 0   # 方向（动态调整）
        
    def preprocess_ir_image(self, ir_frame: np.ndarray) -> np.ndarray:
        """
        IR图像预处理
        
        Args:
            ir_frame: 原始IR图像 (H, W), uint16 或 uint8
            
        Returns:
            enhanced: 增强后的二值化图像
        """
        # 1. 降噪
        denoised = cv2.fastNlMeansDenoising(ir_frame.astype(np.uint8), None, 10, 7, 21)
        
        # 2. 自适应阈值分割（分离高反射安全带）
        #    红外安全带在IR图像中呈现高亮
        _, binary = cv2.threshold(
            denoised, 0, 255, 
            cv2.THRESH_BINARY + cv2.THRESH_OTSU
        )
        
        # 3. 形态学操作（去除小噪声，连接断裂区域）
        kernel = cv2.getStructuringElement(cv2.MORPH_RECT, (5, 5))
        cleaned = cv2.morphologyEx(binary, cv2.MORPH_CLOSE, kernel)
        cleaned = cv2.morphologyEx(cleaned, cv2.MORPH_OPEN, kernel)
        
        return cleaned
    
    def extract_belt_path(self, binary: np.ndarray) -> Optional[List[Tuple[int, int]]]:
        """
        从二值化图像中提取安全带路径
        
        Returns:
            belt_path: 安全带中心线坐标列表 [(x1,y1), (x2,y2), ...]
            None: 未检测到安全带
        """
        # 连通区域分析
        labeled = label(binary > 0)
        regions = regionprops(labeled)
        
        # 筛选：面积、长宽比符合安全带特征的区域
        candidates = []
        for region in regions:
            min_row, min_col, max_row, max_col = region.bbox
            width = max_col - min_col
            height = max_row - min_row
            aspect_ratio = max(width, height) / max(min(width, height), 1)
            
            # 安全带是细长区域
            if aspect_ratio > 4 and region.area > 500:
                candidates.append(region)
        
        if not candidates:
            return None
        
        # 选择最长的候选区域
        best = max(candidates, key=lambda r: r.bbox_area)
        
        # 提取中心线（骨架化）
        skeleton = cv2.ximgproc.thinning(
            (labeled == best.label).astype(np.uint8) * 255
        )
        
        # 按y坐标排序获取路径
        points = np.where(skeleton > 0)
        if len(points[0]) < 10:
            return None
        
        # 沿y方向排序
        sorted_idx = np.argsort(points[0])
        path = list(zip(points[1][sorted_idx], points[0][sorted_idx]))
        
        return path
    
    def classify_belt_position(
        self, 
        belt_path: List[Tuple[int, int]], 
        body_landmarks: dict
    ) -> Tuple[str, float]:
        """
        分类安全带佩戴方式
        
        Args:
            belt_path: 安全带路径坐标
            body_landmarks: 人体关键点
                - shoulder: (x, y) 肩部位置
                - chest: (x, y) 胸部中心
                - hip: (x, y) 髋部位置
                - buckle: (x, y) 锁扣位置
                
        Returns:
            classification: "correct” / “under_arm” / “behind_back” / “unknown”
            confidence: 置信度 0-1
        """
        if len(belt_path) < 5:
            return "unknown", 0.0
        
        path = np.array(belt_path)
        shoulder = np.array(body_landmarks['shoulder'])
        chest = np.array(body_landmarks['chest'])
        buckle = np.array(body_landmarks['buckle'])
        
        # 1. 计算带体在肩-胸段的走向角度
        shoulder_to_chest = chest - shoulder
        belt_near_shoulder = path[0] - shoulder  # 带体起点相对肩部
        angle = np.arctan2(
            shoulder_to_chest[0] * belt_near_shoulder[1] - 
            shoulder_to_chest[1] * belt_near_shoulder[0],
            shoulder_to_chest[0] * belt_near_shoulder[0] + 
            shoulder_to_chest[1] * belt_near_shoulder[1]
        )
        angle_deg = np.degrees(np.abs(angle))
        
        # 2. 检查带体是否在肩部外侧（正确）还是内侧（误用）
        belt_x_at_shoulder_y = path[np.argmin(np.abs(path[:, 1] - shoulder[1])), 0]
        belt_on_shoulder_side = belt_x_at_shoulder_y >= shoulder[0]
        
        # 3. 检查带体是否穿过胸部区域
        chest_region_belt = path[
            (path[:, 1] > shoulder[1]) & 
            (path[:, 1] < chest[1] + 50)
        ]
        chest_coverage = len(chest_region_belt) / len(path) if len(path) > 0 else 0
        
        # 4. 分类
        if angle_deg < self.shoulder_angle_threshold and chest_coverage > self.chest_coverage_threshold:
            return "correct", 0.9
        elif belt_x_at_shoulder_y < shoulder[0] - 30:
            # 带体在肩部内侧（可能从腋下穿过）
            return "under_arm", 0.75
        elif chest_coverage < 0.3:
            # 带体未覆盖胸部（可能在背后）
            return "behind_back", 0.7
        else:
            return "unknown", 0.5
    
    def process_frame(
        self, 
        ir_frame: np.ndarray, 
        body_landmarks: Optional[dict] = None
    ) -> dict:
        """
        完整处理一帧IR图像
        
        Returns:
            result: {
                'belt_detected': bool,
                'classification': str,
                'confidence': float,
                'belt_path': list,
                'warning_level': int  # 0=none, 1=audio-visual
            }
        """
        # 预处理
        binary = self.preprocess_ir_image(ir_frame)
        
        # 提取路径
        belt_path = self.extract_belt_path(binary)
        
        if belt_path is None:
            return {
                'belt_detected': False,
                'classification': 'no_belt',
                'confidence': 0.9,
                'warning_level': 1
            }
        
        # 分类（需要人体关键点）
        if body_landmarks:
            cls, conf = self.classify_belt_path(belt_path, body_landmarks)
        else:
            cls, conf = "detected", 1.0
        
        warning = 1 if cls in ('under_arm', 'behind_back', 'no_belt') else 0
        
        return {
            'belt_detected': True,
            'classification': cls,
            'confidence': conf,
            'belt_path': belt_path,
            'warning_level': warning
        }


# 实际测试
if __name__ == "__main__":
    # 模拟IR图像（实际使用时替换为真实IR摄像头数据）
    detector = InfraredBeltDetector({
        'ir_wavelength': 940,
        'resolution': (1920, 1080),
        'fps': 30,
    })
    
    # 模拟一张IR图像
    np.random.seed(42)
    test_image = np.random.randint(50, 80, (1080, 1920), dtype=np.uint8)
    
    # 模拟安全带（从上到下的亮条）
    for y in range(200, 800):
        for x in range(900, 945):
            test_image[y, x] = 200  # 高反射率
    
    # 人体关键点
    landmarks = {
        'shoulder': (960, 200),
        'chest': (920, 500),
        'hip': (900, 800),
        'buckle': (850, 850),
    }
    
    result = detector.process_frame(test_image, landmarks)
    print(f"检测结果: {result['classification']}")
    print(f"置信度: {result['confidence']:.2f}")
    print(f"警告级别: {result['warning_level']}")