"""
示例 4: 多传感器并行处理（多进程模式）

本示例演示如何同时运行多个传感器：
- 每个传感器运行在独立的进程中
- 每个进程拥有独立的 CUDA 上下文
- 使用 MultiSensor 类简化多传感器管理

架构说明：
    进程 1 (传感器 0):  采集 → 光流 → HHD → 队列
    进程 2 (传感器 1):  采集 → 光流 → HHD → 队列
                                              ↓
    主进程:            显示 ← ───────────────┘

适用场景：多指触觉传感器、多视角采集、需要 GPU 加速的多路处理

注意事项：
- Windows 下必须在 if __name__ == '__main__' 中调用 mp.freeze_support()
- 每个传感器进程会占用独立的 GPU 显存
"""

import os
os.environ["OPENCV_VIDEOIO_MSMF_ENABLE_HW_TRANSFORMS"] = "0"  # Windows 兼容性设置

import cv2
import orisys
import multiprocessing as mp
import numpy as np


def main():
    # =========================================================================
    # 1. 配置多个传感器
    # =========================================================================
    # 每个传感器的配置是一个字典，包含：
    #   vid_src:  视频源（文件路径或摄像头索引）
    #   isstitch: 是否启用拼接模式
    #   cuda:     是否使用 GPU 加速
    #   cal_path: 标定文件路径
    configs = [
        {
            'vid_src': './s1.mp4',
            'isstitch': True,
            'cuda': True,
            'cal_path': './config/cal.npy',
            'config_name': "prototype"
        },
        {
            'vid_src': './s1.mp4',
            'isstitch': True,
            'cuda': True,
            'cal_path': './config/cal.npy',
            'config_name': "prototype"
        },
    ]
    
    # =========================================================================
    # 2. 使用上下文管理器启动多传感器
    # =========================================================================
    # MultiSensor 会自动：
    #   - 为每个配置创建独立的子进程
    #   - 在子进程中初始化 Sensor 实例
    #   - 持续采集和处理数据
    #   - 通过队列将结果传回主进程
    # compute_contact=True 启用接触点检测（深度图、质心、轮廓）
    with orisys.MultiSensor(configs, compute_contact=True) as multi:
        print("\n多传感器已启动，按 'q' 退出")
        
        # =====================================================================
        # 3. 迭代获取所有传感器的数据
        # =====================================================================
        # multi 是可迭代的，每次迭代返回所有传感器的最新数据列表
        # all_data = [data_sensor_0, data_sensor_1, ...]
        for all_data in multi:
            # 过滤掉 None（某些传感器可能暂时没有新数据）
            for data in [d for d in all_data if d]:
                sid = data['sensor_id']  # 传感器编号
                
                # 打印传感器状态
                print(f"传感器 {sid}: FPS={data['fps']:5.1f}, 法向力={data['fnormal']:8.2f}")
                
                # 显示形变箭头图
                if data['img'] is not None and data['flow'] is not None:
                    arrows = orisys.util.draw_arrows(
                        data['img'].copy(),
                        data['flow'],
                        threshold=5
                    )
                    cv2.imshow(f"Deformation {sid}", arrows)

                    
                if data['centroid'] is not None:
                    # print(f"触碰点: {data['centroid']}")
                    image_with_foe = orisys.util.draw_contact(data['img'].copy(), data['contour'], data['centroid'])
                    cv2.imshow(f"Contact Centroid {sid}", image_with_foe)
                
                if data['depth_map'] is not None and data['depth_map'].size > 0:
                    # 归一化散度值到0-255范围（使用绝对值，然后归一化）
                    div_abs = data['depth_map']
                    if div_abs.max() > div_abs.min():
                        div_normalized = ((div_abs - div_abs.min()) / (div_abs.max() - div_abs.min()) * 255).astype(np.uint8)
                    else:
                        div_normalized = np.zeros_like(div_abs, dtype=np.uint8)
                    cv2.imshow(f"Depth Map {sid}", cv2.applyColorMap(div_normalized, cv2.COLORMAP_JET))
                
            
            # 检查退出键
            if cv2.waitKey(1) & 0xFF == ord('q'):
                break
    
    # 上下文管理器退出时自动停止所有子进程
    cv2.destroyAllWindows()


if __name__ == '__main__':
    # Windows 下多进程必需的初始化
    mp.freeze_support()
    main()