import numpy as np

"""
相机内参矩阵 K
"""
K = np.array([
    [fx,  0,  cx],
    [0,   fy, cy],
    [0,   0,  1 ]
], dtype=float)

# fx, fy: 焦距（像素单位）
# cx, cy: 主点（光轴与传感器交点的像素坐标）

# 物理含义:
# fx = f / dx  (物理焦距 / 像素宽度)
# fy = f / dy  (物理焦距 / 像素高度)

"""
相机外参矩阵 [R|t]
"""
# R: 3×3 旋转矩阵（描述相机在世界的安装角度）
# t: 3×1 平移向量（描述相机光学中心在世界坐标的位置）

R = np.eye(3)  # 单位矩阵=无旋转
t = np.array([[0], [1.2], [0.5]])  # 安装高度1.2m, 前偏0.5m

# 外参 = 相机在世界中的姿态
extrinsic = np.hstack([R, t])  # 3×4 矩阵

import numpy as np
from dataclasses import dataclass, field
from typing import Tuple, List, Optional
import json

"""
IMS 座舱摄像头标定系统
覆盖: DMS/OMS/CPD 摄像头的内参外参标定、热漂移补偿、量产验证

参考: HackerNoon 摄像头标定指南 (2026-08)
"""

@dataclass
class CameraIntrinsics:
    """相机内参"""
    fx: float  # x方向焦距(像素)
    fy: float  # y方向焦距(像素)
    cx: float  # 主点x(像素)
    cy: float  # 主点y(像素)
    k1: float = 0  # 径向畸变
    k2: float = 0
    k3: float = 0
    p1: float = 0  # 切向畸变
    p2: float = 0
    
    @property
    def matrix(self) -> np.ndarray:
        return np.array([
            [self.fx, 0, self.cx],
            [0, self.fy, self.cy],
            [0, 0, 1]
        ], dtype=float)
    
    @property
    def distortion(self) -> np.ndarray:
        return np.array([
            self.k1, self.k2, self.p1, self.p2, self.k3
        ], dtype=float)


@dataclass
class CameraExtrinsics:
    """相机外参"""
    # 旋转矩阵 3×3
    R: np.ndarray  # 3×3
    # 平移向量 3×1
    t: np.ndarray  # 3×1
    
    @property
    def matrix(self) -> np.ndarray:
        return np.hstack([self.R, self.t.reshape(3, 1)])
    
    def to_euler(self) -> Tuple[float, float, float]:
        """旋转矩阵 -> 欧拉角(度)"""
        R = self.R
        pitch = np.degrees(np.arctan2(R[2, 0], 
            np.sqrt(R[0, 0]**2 + R[1, 0]**2)))
        yaw = np.degrees(np.arctan2(-R[1, 0], R[0, 0]))
        roll = np.degrees(np.arctan2(-R[2, 1], R[2, 2]))
        return pitch, yaw, roll


@dataclass 
class CalibrationResult:
    """标定结果"""
    camera_id: str
    intrinsics: CameraIntrinsics
    extrinsics: CameraExtrinsics
    reprojection_error: float  # 重投影误差(像素)
    calibration_time: str
    temperature: float  # 标定时温度(℃)
    valid: bool = True


class IMSCameraCalibrator:
    """IMS 摄像头标定器"""
    
    # IMS 摄像头配置
    CAMERAS = {
        'dms_front': {
            'name': 'DMS 前置摄像头',
            'sensor': 'OV2311',
            'resolution': '1600×1200',
            'fov': '60°',
            'position': '方向盘柱上方',
            'task': '疲劳/分心/眼动'
        },
        'oms_left': {
            'name': 'OMS 左侧摄像头',
            'sensor': 'OV2311',
            'resolution': '1600×1200',
            'fov': '120°',
            'position': '左B柱',
            'task': '乘员检测/CPD'
        },
        'oms_right': {
            'name': 'OMS 右侧摄像头',
            'sensor': 'OV2311',
            'resolution': '1600×1200',
            'fov': '120°',
            'position': '右B柱',
            'task': '乘员检测/CPD'
        },
        'cabin_wide': {
            'name': '座舱广角摄像头',
            'sensor': 'IMX390',
            'resolution': '1920×1080',
            'fov': '180°',
            'position': '车顶中控',
            'task': '全景座舱/行为'
        }
    }
    
    def __init__(self):
        self.results: dict[str, CalibrationResult] = {}
        self.thermal_drift_data: dict[str, list] = {}
    
    def calibrate_chessboard(self, camera_id: str, 
                              image_points: List[np.ndarray],
                              object_points: np.ndarray,
                              image_size: Tuple[int, int],
                              temperature: float = 25.0) -> CalibrationResult:
        """
        棋盘格标定
        
        Args:
            camera_id: 摄像头ID
            image_points: 各图像角点像素坐标列表
            object_points: 3D 棋盘格点坐标
            image_size: (width, height)
            temperature: 标定时温度
        """
        # 模拟标定过程
        np.random.seed(hash(camera_id) % 2**32)
        
        # 基于摄像头配置生成内参
        cam = self.CAMERAS[camera_id]
        w, h = map(int, cam['resolution'].split('×'))
        fov = int(cam['fov'].rstrip('°'))
        
        fx = w / (2 * np.tan(np.radians(fov) / 2))
        fy = fx  # 正方形像素
        cx = w / 2 + np.random.uniform(-3, 3)
        cy = h / 2 + np.random.uniform(-3, 3)
        
        # 畸变系数
        k1 = np.random.uniform(-0.3, -0.1)
        k2 = np.random.uniform(-0.1, 0.1)
        k3 = np.random.uniform(-0.05, 0.05)
        p1 = np.random.uniform(-0.005, 0.005)
        p2 = np.random.uniform(-0.005, 0.005)
        
        intrinsics = CameraIntrinsics(
            fx=round(fx, 2), fy=round(fy, 2),
            cx=round(cx, 2), cy=round(cy, 2),
            k1=round(k1, 4), k2=round(k2, 4),
            k3=round(k3, 4),
            p1=round(p1, 5), p2=round(p2, 5)
        )
        
        # 外参（模拟安装角度）
        positions = {
            'dms_front': {'pitch': -5, 'yaw': 0, 'roll': 0,
                          't': [0, 1.2, 0.3]},
            'oms_left': {'pitch': -10, 'yaw': 45, 'roll': 0,
                         't': [-0.5, 1.0, 0.5]},
            'oms_right': {'pitch': -10, 'yaw': -45, 'roll': 0,
                          't': [0.5, 1.0, 0.5]},
            'cabin_wide': {'pitch': -30, 'yaw': 0, 'roll': 0,
                           't': [0, 1.5, 0]}
        }
        
        pos = positions[camera_id]
        pitch = np.radians(pos['pitch'])
        yaw = np.radians(pos['yaw'])
        roll = np.radians(pos['roll'])
        
        Rx = np.array([[1, 0, 0],
                       [0, np.cos(pitch), -np.sin(pitch)],
                       [0, np.sin(pitch), np.cos(pitch)]])
        Ry = np.array([[np.cos(yaw), 0, np.sin(yaw)],
                       [0, 1, 0],
                       [-np.sin(yaw), 0, np.cos(yaw)]])
        Rz = np.array([[np.cos(roll), -np.sin(roll), 0],
                       [np.sin(roll), np.cos(roll), 0],
                       [0, 0, 1]])
        R = Rx @ Ry @ Rz
        t = np.array(pos['t'])
        
        extrinsics = CameraExtrinsics(R=R, t=t)
        
        # 重投影误差
        reproj_error = np.random.uniform(0.1, 0.5)
        
        result = CalibrationResult(
            camera_id=camera_id,
            intrinsics=intrinsics,
            extrinsics=extrinsics,
            reprojection_error=round(reproj_error, 3),
            calibration_time=f"2026-08-25T00:30:00",
            temperature=temperature,
            valid=reproj_error < 1.0
        )
        
        self.results[camera_id] = result
        return result
    
    def thermal_drift_test(self, camera_id: str,
                            temp_range: Tuple[float, float] = (-20, 80),
                            step: float = 10) -> dict:
        """
        热漂移测试
        
        Args:
            camera_id: 摄像头ID
            temp_range: (最低温度, 最高温度)
            step: 温度步长
        """
        if camera_id not in self.results:
            raise ValueError(f"{camera_id} 未标定")
        
        base = self.results[camera_id]
        drift_data = []
        
        for temp in np.arange(temp_range[0], temp_range[1] + step, step):
            # 热漂移模型: 焦距随温度变化 ~0.05%/℃
            temp_delta = temp - base.temperature
            fx_drift = base.intrinsics.fx * (1 + 0.0005 * temp_delta)
            fy_drift = base.intrinsics.fy * (1 + 0.0005 * temp_delta)
            cx_drift = base.intrinsics.cx + 0.02 * temp_delta
            cy_drift = base.intrinsics.cy + 0.02 * temp_delta
            
            # 外参漂移
            pitch_drift = 0.01 * temp_delta  # 度/℃
            
            drift_data.append({
                'temperature': round(temp, 1),
                'fx': round(fx_drift, 2),
                'fy': round(fy_drift, 2),
                'cx': round(cx_drift, 2),
                'cy': round(cy_drift, 2),
                'pitch_drift_deg': round(pitch_drift, 3),
                'fx_drift_pct': round((fx_drift - base.intrinsics.fx) 
                    / base.intrinsics.fx * 100, 3),
            })
        
        self.thermal_drift_data[camera_id] = drift_data
        return {
            'camera_id': camera_id,
            'base_temp': base.temperature,
            'temp_range': temp_range,
            'drift_data': drift_data
        }
    
    def mass_production_validation(self, batch_size: int = 100) -> dict:
        """
        量产验证（GRR分析）
        
        Returns:
            量产标定统计
        """
        np.random.seed(42)
        stats = []
        
        for i in range(batch_size):
            # 模拟每台设备的标定结果
            reproj_error = np.random.normal(0.3, 0.1)
            fx = 800 + np.random.normal(0, 2)
            pass_fail = reproj_error < 0.5 and abs(fx - 800) < 5
            
            stats.append({
                'unit_id': f'UNIT-{i:04d}',
                'reproj_error': round(reproj_error, 3),
                'fx': round(fx, 2),
                'pass': pass_fail
            })
        
        pass_rate = sum(s['pass'] for s in stats) / batch_size * 100
        errors = [s['reproj_error'] for s in stats]
        
        return {
            'batch_size': batch_size,
            'pass_rate': round(pass_rate, 1),
            'mean_reproj_error': round(np.mean(errors), 3),
            'std_reproj_error': round(np.std(errors), 3),
            'max_reproj_error': round(np.max(errors), 3),
            'cpk': round((0.5 - np.mean(errors)) / (3 * np.std(errors)), 2),
            'details': stats[:10]  # 前10个样本
        }


# ==================== 测试 ====================
if __name__ == "__main__":
    print("=" * 70)
    print("IMS 摄像头标定系统测试")
    print("=" * 70)
    
    calibrator = IMSCameraCalibrator()
    
    # 1. 标定所有摄像头
    print(f"\n{'摄像头':<20} {'fx':>8} {'fy':>8} {'cx':>8} {'cy':>8} "
          f"{'k1':>8} {'误差(px)':>10} {'通过':>6}")
    print("-" * 70)
    
    for cam_id in IMSCameraCalibrator.CAMERAS:
        result = calibrator.calibrate_chessboard(
            cam_id, 
            image_points=[np.random.rand(48, 2) for _ in range(15)],
            object_points=np.random.rand(48, 3),
            image_size=(1600, 1200),
            temperature=25.0
        )
        
        i = result.intrinsics
        e = result.extrinsics.to_euler()
        cam_name = IMSCameraCalibrator.CAMERAS[cam_id]['name']
        
        print(f"{cam_name:<20} {i.fx:>8.1f} {i.fy:>8.1f} "
              f"{i.cx:>8.1f} {i.cy:>8.1f} {i.k1:>8.4f} "
              f"{result.reprojection_error:>10.3f} "
              f"{'✅' if result.valid else '❌':>6}")
        
        print(f"  安装角度: pitch={e[0]:.1f}° yaw={e[1]:.1f}° "
              f"roll={e[2]:.1f}°")
    
    # 2. 热漂移测试
    print(f"\n{'='*70}")
    print("热漂移测试 (DMS 前置摄像头)")
    print(f"{'='*70}")
    
    drift = calibrator.thermal_drift_test('dms_front', (-20, 80))
    print(f"\n{'温度(℃)':>8} {'fx':>8} {'fx漂移%':>10} {'cx':>8} "
          f"{'pitch漂移(°)':>12}")
    print("-" * 55)
    
    for d in drift['drift_data']:
        print(f"{d['temperature']:>8.1f} {d['fx']:>8.1f} "
              f"{d['fx_drift_pct']:>10.3f}% {d['cx']:>8.1f} "
              f"{d['pitch_drift_deg']:>12.3f}")
    
    # 3. 量产验证
    print(f"\n{'='*70}")
    print("量产验证 (100台设备)")
    print(f"{'='*70}")
    
    mp = calibrator.mass_production_validation(100)
    print(f"\n  批量大小: {mp['batch_size']}")
    print(f"  通过率: {mp['pass_rate']}%")
    print(f"  平均重投影误差: {mp['mean_reproj_error']:.3f} px")
    print(f"  最大重投影误差: {mp['max_reproj_error']:.3f} px")
    print(f"  标准差: {mp['std_reproj_error']:.3f} px")
    print(f"  Cpk: {mp['cpk']}")
    
    print(f"\n  前10个样本:")
    print(f"  {'设备ID':<12} {'误差(px)':>10} {'fx':>8} {'通过':>6}")
    for s in mp['details']:
        print(f"  {s['unit_id']:<12} {s['reproj_error']:>10.3f} "
              f"{s['fx']:>8.1f} {'✅' if s['pass'] else '❌':>6}")