BodyBalanceEvaluation/backend/devices/device_coordinator.py

#!/usr/bin/env python3
# -*- coding: utf-8 -*-
"""
设备协调器
负责统一管理和协调所有设备的生命周期、数据流和状态同步
"""

import threading
import time
import logging
from typing import Dict, List, Optional, Any, Callable
from collections import defaultdict
import json
from concurrent.futures import ThreadPoolExecutor, as_completed

try:
    from .camera_manager import CameraManager
    from .imu_manager import IMUManager
    from .pressure_manager import PressureManager
    from .femtobolt_manager import FemtoBoltManager
    from .utils.config_manager import ConfigManager
    from .utils.socket_manager import SocketManager
except ImportError:
    from camera_manager import CameraManager
    from imu_manager import IMUManager
    from pressure_manager import PressureManager
    from femtobolt_manager import FemtoBoltManager
    from utils.config_manager import ConfigManager
    from utils.socket_manager import SocketManager


class DeviceCoordinator:
    """设备协调器 - 统一管理所有设备"""
    
    def __init__(self, socketio, config_path: Optional[str] = None):
        """
        初始化设备协调器
        
        Args:
            socketio: SocketIO实例
            config_path: 配置文件路径
        """
        self.socketio = socketio
        self.logger = logging.getLogger(self.__class__.__name__)
        
        # 配置管理
        self.config_manager = ConfigManager(config_path)
        self.socket_manager = SocketManager(socketio)
        
        # 设备管理器
        self.devices: Dict[str, Any] = {}
        
        # 获取设备配置
        self.device_configs = self.config_manager.get_all_device_configs()
        
        # 状态管理
        self.is_initialized = False
        self.is_running = False
        self.coordinator_lock = threading.RLock()
        
        # 监控线程
        self.monitor_thread = None
        self.health_check_interval = 5.0  # 健康检查间隔（秒）
        
        # 事件回调
        self.event_callbacks: Dict[str, List[Callable]] = defaultdict(list)
        
        # 状态变化回调存储
        self._status_change_callback = None
        
        # 性能统计
        self.stats = {
            'start_time': None,
            'total_frames': 0,
            'device_errors': defaultdict(int),
            'reconnect_attempts': defaultdict(int)
        }
        
        # 线程池
        self.executor = ThreadPoolExecutor(max_workers=4, thread_name_prefix="DeviceCoord")
        
        self.logger.info("设备协调器初始化完成")
        
    def set_status_change_callback(self, callback: Callable):
        """
        设置状态变化回调函数
        
        Args:
            callback: 状态变化回调函数
        """
        self._status_change_callback = callback
        # 为已存在的设备注册回调
        for device_name, device in self.devices.items():
            if device and hasattr(device, 'add_status_change_callback'):
                device.add_status_change_callback(callback)
                self.logger.info(f"{device_name} 设备状态变化回调已注册")
        
    def initialize(self) -> bool:
        """
        初始化所有设备
        
        Returns:
            bool: 初始化是否成功
        """
        with self.coordinator_lock:
            if self.is_initialized:
                self.logger.warning("设备协调器已初始化")
                return True
                
            try:
                self.logger.info("开始初始化设备协调器...")
                
                # 注册Socket.IO命名空间
                self._register_namespaces()
                
                # 初始化设备（失败则降级继续）
                if not self._initialize_devices():
                    self.logger.warning("设备初始化失败，将以降级模式继续运行")
                
                # 启动监控线程
                self._start_monitor()
                
                self.is_initialized = True
                self.stats['start_time'] = time.time()
                
                self.logger.info("设备协调器初始化成功")
                self._emit_event('coordinator_initialized', {'devices': list(self.devices.keys())})
                
                return True
                
            except Exception as e:
                self.logger.error(f"设备协调器初始化失败: {e}")
                self._cleanup_devices()
                return False
                
    def _register_namespaces(self):
        """
        注册Socket.IO命名空间
        """
        namespace_mappings = {
            '/devices': 'devices',
            '/coordinator': 'coordinator'
        }
        
        for namespace, device_name in namespace_mappings.items():
            self.socket_manager.register_namespace(namespace, device_name)
            
        self.logger.info(f"已注册Socket.IO命名空间: {list(namespace_mappings.keys())}")
        
    def _initialize_devices(self) -> bool:
        """
        初始化所有设备
        
        Returns:
            bool: 初始化是否成功
        """
        try:
            # 并行初始化设备
            futures = []
            
            # FemtoBolt深度相机
            if self.device_configs.get('femtobolt', {}).get('enabled', False):
                future = self.executor.submit(self._init_femtobolt)
                futures.append(('femtobolt', future))

            # 普通相机：初始化两个实例（camera1 与 camera2）
            # camera1 使用 [CAMERA1] 配置；camera2 使用 [CAMERA2]（若不存在则回退为 device_index+1）
            if self.device_configs.get('camera1', {}).get('enabled', True):
                futures.append(('camera1', self.executor.submit(self._init_camera_by_name, 'camera1', 'CAMERA1')))
            if self.device_configs.get('camera2', {}).get('enabled', True):           
                futures.append(('camera2', self.executor.submit(self._init_camera_by_name, 'camera2', 'CAMERA2')))
                
            # IMU传感器
            if self.device_configs.get('imu', {}).get('enabled', False):
                future = self.executor.submit(self._init_imu)
                futures.append(('imu', future))
                
            # 压力传感器
            if self.device_configs.get('pressure', {}).get('enabled', False):
                future = self.executor.submit(self._init_pressure)
                futures.append(('pressure', future))
                

                
            # 等待所有设备初始化完成
            success_count = 0
            for device_name, future in futures:
                try:
                    result = future.result(timeout=30)  # 30秒超时
                    if result:
                        success_count += 1
                        self.logger.info(f"{device_name}设备初始化成功")
                    else:
                        self.logger.error(f"{device_name}设备初始化失败")
                except Exception as e:
                    self.logger.error(f"{device_name}设备初始化异常: {e}")
                    
            # 至少需要一个设备初始化成功
            if success_count == 0:
                self.logger.warning("没有设备初始化成功，但系统将继续运行")
                return False
                
            self.logger.info(f"设备初始化完成，成功: {success_count}/{len(futures)}")
            return True
            
        except Exception as e:
            self.logger.error(f"设备初始化失败: {e}")
            return False
            
    def _init_camera_by_name(self, device_name: str, section: str = 'CAMERA1') -> bool:
        """
        按名称初始化相机，支持 camera1/camera2 并覆盖配置段

        Args:
            device_name: 设备名称（如 'camera1'、'camera2'）
            section: 配置段名称（'CAMERA1' 或 'CAMERA2'）
        Returns:
            bool: 初始化是否成功
        """
        try:
            # 构造实例覆盖配置：优先读取目标配置段，否则回退到 [CAMERA1]
            cfg = {}
            parser = getattr(self.config_manager, 'config', None)
            base_cam = self.config_manager.get_device_config('camera1')
            if parser and parser.has_section(section):
                # 读取所有相关键
                def get_opt(sec, key, fallback=None):
                    try:
                        return parser.get(sec, key)
                    except Exception:
                        return fallback
                def get_int(sec, key, fallback=None):
                    try:
                        return parser.getint(sec, key)
                    except Exception:
                        return fallback
                def get_bool(sec, key, fallback=None):
                    try:
                        return parser.getboolean(sec, key)
                    except Exception:
                        return fallback
                enabled = get_bool(section, 'enabled', True)
                if not enabled:
                    self.logger.info(f"{device_name} 未启用，跳过初始化")
                    return False
                # 填充覆盖项
                idx2 = get_int(section, 'device_index', None)
                if idx2 is not None:
                    cfg['device_index'] = idx2
                w = get_int(section, 'width', None)
                h = get_int(section, 'height', None)
                f = get_int(section, 'fps', None)
                buf = get_int(section, 'buffer_size', None)
                fourcc = get_opt(section, 'fourcc', None)
                backend = get_opt(section, 'backend', None)
                if w is not None: cfg['width'] = w
                if h is not None: cfg['height'] = h
                if f is not None: cfg['fps'] = f
                if buf is not None: cfg['buffer_size'] = buf
                if fourcc is not None: cfg['fourcc'] = fourcc
                if backend is not None: cfg['backend'] = backend
            else:
                # section 不存在时：camera2 默认使用 device_index+1
                if device_name.lower() == 'camera2':
                    cfg['device_index'] = int(base_cam.get('device_index', 0)) + 1
                else:
                    cfg['device_index'] = int(base_cam.get('device_index', 0))
            
            camera = CameraManager(self.socketio, self.config_manager, device_name=device_name, instance_config=cfg)
            self.devices[device_name] = camera
            if camera.initialize():
                return True
            return False
        except Exception as e:
            self.logger.error(f"初始化{device_name}失败: {e}")
            return False
            
    def _init_imu(self) -> bool:
        """
        初始化IMU传感器
        
        Returns:
            bool: 初始化是否成功
        """
        try:
            imu = IMUManager(self.socketio, self.config_manager)
            self.devices['imu'] = imu
            if imu.initialize():
               return True
            return False
        except Exception as e:
            self.logger.error(f"初始化IMU失败: {e}")
            return False
            
    def _init_pressure(self) -> bool:
        """
        初始化压力传感器
        
        Returns:
            bool: 初始化是否成功
        """
        try:
            pressure = PressureManager(self.socketio, self.config_manager)
            self.devices['pressure'] = pressure
            if pressure.initialize():
               return True
            return False
        except Exception as e:
            self.logger.error(f"初始化压力传感器失败: {e}")
            return False
            
    def _init_femtobolt(self) -> bool:
        """
        初始化FemtoBolt深度相机
        
        Returns:
            bool: 初始化是否成功
        """
        try:
            femtobolt = FemtoBoltManager(self.socketio, self.config_manager)
            self.devices['femtobolt'] = femtobolt
            if femtobolt.initialize():                
                return True
            return False
        except Exception as e:
            self.logger.error(f"初始化FemtoBolt失败: {e}")
            return False
            
    def start_all_streaming(self) -> bool:
        """
        启动所有设备的数据流
        
        Returns:
            bool: 启动是否成功
        """
        with self.coordinator_lock:
            if not self.is_initialized:
                self.logger.error("设备协调器未初始化")
                return False
                
            if self.is_running:
                self.logger.warning("设备流已在运行")
                return True
                
            try:
                self.logger.info("启动所有设备数据流...")
                
                # 并行启动所有设备流
                futures = []
                for device_name, device in self.devices.items():
                    future = self.executor.submit(device.start_streaming)
                    futures.append((device_name, future))
                    
                # 等待所有设备启动完成
                success_count = 0
                for device_name, future in futures:
                    try:
                        result = future.result(timeout=10)  # 10秒超时
                        if result:
                            success_count += 1
                            self.logger.info(f"{device_name}数据流启动成功")
                        else:
                            self.logger.error(f"{device_name}数据流启动失败")
                    except Exception as e:
                        self.logger.error(f"{device_name}数据流启动异常: {e}")
                        
                self.is_running = success_count > 0
                
                if self.is_running:
                    self.logger.info(f"设备数据流启动完成，成功: {success_count}/{len(futures)}")
                    self._emit_event('streaming_started', {'active_devices': success_count})
                else:
                    self.logger.error("没有设备数据流启动成功")
                    
                return self.is_running
                
            except Exception as e:
                self.logger.error(f"启动设备数据流失败: {e}")
                return False
                
    def stop_all_streaming(self) -> bool:
        """
        停止所有设备的数据流
        
        Returns:
            bool: 停止是否成功
        """
        with self.coordinator_lock:
            if not self.is_running:
                self.logger.warning("设备流未运行")
                return True
                
            try:
                self.logger.info("停止所有设备数据流...")
                
                # 并行停止所有设备流
                futures = []
                for device_name, device in self.devices.items():
                    if hasattr(device, 'stop_streaming'):
                        future = self.executor.submit(device.stop_streaming)
                        futures.append((device_name, future))
                        
                # 等待所有设备停止完成
                for device_name, future in futures:
                    try:
                        future.result(timeout=5)  # 5秒超时
                        self.logger.info(f"{device_name}数据流已停止")
                    except Exception as e:
                        self.logger.error(f"停止{device_name}数据流异常: {e}")
                        
                self.is_running = False
                self.logger.info("所有设备数据流已停止")
                self._emit_event('streaming_stopped', {})
                
                return True
                
            except Exception as e:
                self.logger.error(f"停止设备数据流失败: {e}")
                return False
                
    
            
    def get_device_status(self, device_name: Optional[str] = None) -> Dict[str, Any]:
        """
        获取设备状态
        
        Args:
            device_name: 设备名称，None表示获取所有设备状态
            
        Returns:
            Dict[str, Any]: 设备状态信息
        """
        if device_name:
            if device_name in self.devices:
                return self.devices[device_name].get_status()
            else:
                return {'error': f'设备 {device_name} 不存在'}
        else:
            # 获取所有设备状态
            status = {
                'coordinator': {
                    'is_initialized': self.is_initialized,
                    'is_running': self.is_running,
                    'device_count': len(self.devices),
                    'uptime': time.time() - self.stats['start_time'] if self.stats['start_time'] else 0
                },
                'devices': {}
            }
            
            for name, device in self.devices.items():
                try:
                    status['devices'][name] = device.get_status()
                except Exception as e:
                    status['devices'][name] = {'error': str(e)}
                    
            return status
            
    def get_device(self, device_name: str) -> Optional[Any]:
        """
        获取指定设备实例
        
        Args:
            device_name: 设备名称
            
        Returns:
            Optional[Any]: 设备实例，不存在返回None
        """
        return self.devices.get(device_name)
    
    def get_device_managers(self) -> Dict[str, Any]:
        """
        获取所有设备管理器实例
        
        Returns:
            Dict[str, Any]: 设备管理器字典
        """
        return self.devices.copy()
    
    def start_all_connection_monitor(self) -> bool:
        """
        启动所有设备的连接监控
        
        Returns:
            bool: 启动是否成功
        """
        with self.coordinator_lock:
            if not self.is_initialized:
                self.logger.error("设备协调器未初始化")
                return False
                
            try:
                self.logger.info("启动所有设备连接监控...")
                
                success_count = 0
                for device_name, device in self.devices.items():
                    try:
                        # 对深度相机(femtobolt)和普通相机(camera1/camera2)直接跳过连接监控
                        if device_name in ['femtobolt', 'camera1', 'camera2', "imu"]:
                            continue                            
                            
                        if hasattr(device, '_start_connection_monitor'):
                            device._start_connection_monitor()
                            success_count += 1
                            self.logger.info(f"{device_name}设备连接监控已启动")
                        else:
                            self.logger.warning(f"{device_name}设备不支持连接监控")
                    except Exception as e:
                        self.logger.error(f"启动{device_name}设备失败: {e}")
                        
                self.logger.info(f"设备连接监控启动完成，成功: {success_count}/{len(self.devices)}")
                return success_count > 0
                
            except Exception as e:
                self.logger.error(f"启动设备连接监控失败: {e}")
                return False
    
    def stop_all_connection_monitor(self) -> bool:
        """
        停止所有设备的连接监控
        
        Returns:
            bool: 停止是否成功
        """
        with self.coordinator_lock:
            try:
                self.logger.info("停止所有设备连接监控...")
                
                success_count = 0
                for device_name, device in self.devices.items():
                    try:
                        # 对深度相机(femtobolt)和普通相机(camera1/camera2)直接跳过连接监控停止
                        if device_name in ['femtobolt', 'camera1', 'camera2', "imu"]:
                            self.logger.info(f"停止{device_name}设备推流")                           
                            continue
                            
                        if hasattr(device, '_stop_connection_monitor'):
                            device._stop_connection_monitor()
                            success_count += 1
                            self.logger.info(f"{device_name}设备连接监控已停止")
                        else:
                            self.logger.warning(f"{device_name}设备不支持连接监控")
                    except Exception as e:
                        self.logger.error(f"停止{device_name}设备失败: {e}")
                        
                self.logger.info(f"设备连接监控停止完成，成功: {success_count}/{len(self.devices)}")
                return True
                
            except Exception as e:
                self.logger.error(f"停止设备连接监控失败: {e}")
                return False
        
    def restart_device(self, device_name: str) -> bool:
        """
        彻底重启指定设备：停止推流，断开连接，销毁设备实例，重新创建实例，初始化，恢复推流        
        Args:
            device_name: 设备名称
            
        Returns:
            bool: 重启是否成功
        """
        if device_name not in self.devices:
            self.logger.error(f"设备 {device_name} 不存在")
            return False
            
        restart_start = time.time()
        device = self.devices[device_name]
        was_streaming = False
        
        try:
            self.logger.info(f"开始彻底重启设备: {device_name}")
            
            # 第一步：检查并停止数据流
            stop_start = time.time()
            if hasattr(device, 'is_streaming'):
                was_streaming = device.is_streaming
                
            if hasattr(device, 'stop_streaming') and was_streaming:
                self.logger.info(f"正在停止 {device_name} 设备推流...")
                try:
                    if not device.stop_streaming():
                        self.logger.warning(f"停止 {device_name} 推流失败，继续重启流程")
                    else:
                        self.logger.info(f"{device_name} 设备推流已停止")
                except Exception as e:
                    self.logger.warning(f"停止 {device_name} 推流异常: {e}，继续重启流程")
            
            stop_time = (time.time() - stop_start) * 1000
            
            # 第二步：断开连接并彻底清理资源
            cleanup_start = time.time()
            self.logger.info(f"正在彻底清理 {device_name} 设备...")
            
            # 断开连接但暂时不广播状态变化，避免重启过程中的状态冲突
            if hasattr(device, 'disconnect'):
                try:
                    device.disconnect()
                    self.logger.info(f"{device_name} 设备连接已断开")
                except Exception as e:
                    self.logger.warning(f"断开 {device_name} 连接异常: {e}")
            
            # 静默设置设备状态为未连接，不触发状态变化通知
            # 这样可以避免在重启过程中广播中间状态
            if hasattr(device, 'is_connected'):
                device.is_connected = False
                self.logger.info(f"{device_name} 设备状态已静默更新为未连接（重启过程中）")
            
            # 彻底清理资源
            if hasattr(device, 'cleanup'):
                try:
                    device.cleanup()
                    self.logger.info(f"{device_name} 设备资源已彻底清理")
                except Exception as e:
                    self.logger.warning(f"清理 {device_name} 资源异常: {e}")
            
            cleanup_time = (time.time() - cleanup_start) * 1000
            
            # 第三步：彻底销毁设备实例
            destroy_start = time.time()
            self.logger.info(f"正在销毁 {device_name} 设备实例...")
            
            # 从设备字典中移除
            old_device = self.devices.pop(device_name, None)
            if old_device:
                # 强制删除引用，帮助垃圾回收
                del old_device
                self.logger.info(f"{device_name} 设备实例已销毁")
            
            # 短暂等待，确保资源完全释放
            time.sleep(0.2)            
            destroy_time = (time.time() - destroy_start) * 1000
            
            # 第四步：重新创建设备实例
            create_start = time.time()
            self.logger.info(f"正在重新创建 {device_name} 设备实例...")
            
            new_device = None
            try:
                # 根据设备类型重新创建实例（仅支持 camera1/camera2）
                if device_name in ('camera1', 'camera2'):
                    try:
                        from .camera_manager import CameraManager
                    except ImportError:
                        from camera_manager import CameraManager
                    # 为 camera1/camera2 构造实例配置
                    section = 'CAMERA1' if device_name == 'camera1' else 'CAMERA2'
                    cfg = {}
                    parser = getattr(self.config_manager, 'config', None)
                    base_cam = self.config_manager.get_device_config('camera1')
                    if parser and parser.has_section(section):
                        def get_opt(sec, key, fallback=None):
                            try:
                                return parser.get(sec, key)
                            except Exception:
                                return fallback
                        def get_int(sec, key, fallback=None):
                            try:
                                return parser.getint(sec, key)
                            except Exception:
                                return fallback
                        def get_bool(sec, key, fallback=None):
                            try:
                                return parser.getboolean(sec, key)
                            except Exception:
                                return fallback
                        enabled = get_bool(section, 'enabled', True)
                        if not enabled:
                            raise Exception(f"{device_name} 未启用")
                        idx2 = get_int(section, 'device_index', None)
                        if idx2 is not None:
                            cfg['device_index'] = idx2
                        w = get_int(section, 'width', None)
                        h = get_int(section, 'height', None)
                        f = get_int(section, 'fps', None)
                        buf = get_int(section, 'buffer_size', None)
                        fourcc = get_opt(section, 'fourcc', None)
                        backend = get_opt(section, 'backend', None)
                        if w is not None: cfg['width'] = w
                        if h is not None: cfg['height'] = h
                        if f is not None: cfg['fps'] = f
                        if buf is not None: cfg['buffer_size'] = buf
                        if fourcc is not None: cfg['fourcc'] = fourcc
                        if backend is not None: cfg['backend'] = backend
                    else:
                        # section 不存在时：camera2 默认使用 [CAMERA1] 的 device_index + 1
                        if device_name == 'camera2':
                            cfg['device_index'] = int(base_cam.get('device_index', 0)) + 1
                        else:
                            cfg['device_index'] = int(base_cam.get('device_index', 0))
                    new_device = CameraManager(self.socketio, self.config_manager, device_name=device_name, instance_config=cfg)
                elif device_name == 'imu':
                    try:
                        from .imu_manager import IMUManager
                    except ImportError:
                        from imu_manager import IMUManager
                    new_device = IMUManager(self.socketio, self.config_manager)
                elif device_name == 'pressure':
                    try:
                        from .pressure_manager import PressureManager
                    except ImportError:
                        from pressure_manager import PressureManager
                    new_device = PressureManager(self.socketio, self.config_manager)
                elif device_name == 'femtobolt':
                    try:
                        from .femtobolt_manager import FemtoBoltManager
                    except ImportError:
                        from femtobolt_manager import FemtoBoltManager
                    new_device = FemtoBoltManager(self.socketio, self.config_manager)
                else:
                    raise ValueError(f"未知的设备类型: {device_name}")
                
                if new_device is None:
                    raise Exception("设备实例创建失败")
                
                # 将新设备实例添加到设备字典
                self.devices[device_name] = new_device
                create_time = (time.time() - create_start) * 1000
                self.logger.info(f"{device_name} 设备实例重新创建成功 (耗时: {create_time:.1f}ms)")
                
                # 重新注册状态变化回调（如果有的话）
                if hasattr(self, '_status_change_callback') and self._status_change_callback:
                    if hasattr(new_device, 'add_status_change_callback'):
                        new_device.add_status_change_callback(self._status_change_callback)
                        self.logger.info(f"{device_name} 设备状态变化回调已重新注册")
                
            except Exception as e:
                create_time = (time.time() - create_start) * 1000
                self.logger.error(f"重新创建 {device_name} 设备实例失败: {e} (耗时: {create_time:.1f}ms)")               
                return False
            
            # 第五步：初始化新设备实例
            init_start = time.time()
            self.logger.info(f"正在初始化新的 {device_name} 设备实例...")
            
            if not new_device.initialize():
                init_time = (time.time() - init_start) * 1000
                self.logger.error(f"{device_name} 设备初始化失败 (耗时: {init_time:.1f}ms)")
                # 初始化失败，从设备字典中移除
                self.devices.pop(device_name, None)               
                return False
            
            init_time = (time.time() - init_start) * 1000
            self.logger.info(f"{device_name} 设备初始化成功 (耗时: {init_time:.1f}ms)")
            
            # 设备初始化成功后，确保状态广播正确
            # 此时设备应该已经通过initialize()方法中的set_connected(True)触发了状态变化通知
            # 但为了确保状态一致性，我们再次确认状态
            if hasattr(new_device, 'is_connected') and new_device.is_connected:
                self.logger.info(f"{device_name} 设备重启后状态确认：已连接")
            else:
                self.logger.warning(f"{device_name} 设备重启后状态异常：未连接")
            
            # 第六步：如果之前在推流，则启动推流
            stream_time = 0
            if was_streaming and hasattr(new_device, 'start_streaming'):
                stream_start = time.time()
                self.logger.info(f"正在启动 {device_name} 设备推流...")
                try:
                    if not new_device.start_streaming():
                        stream_time = (time.time() - stream_start) * 1000
                        self.logger.error(f"启动 {device_name} 设备推流失败 (耗时: {stream_time:.1f}ms)")
                        return False
                    
                    stream_time = (time.time() - stream_start) * 1000
                    self.logger.info(f"{device_name} 设备推流已启动 (耗时: {stream_time:.1f}ms)")
                except Exception as e:
                    stream_time = (time.time() - stream_start) * 1000
                    self.logger.error(f"启动 {device_name} 推流异常: {e} (耗时: {stream_time:.1f}ms)")
                    return False
            
            # 计算总耗时并记录
            total_time = (time.time() - restart_start) * 1000
            self.logger.info(f"{device_name} 设备彻底重启完成 - 停止推流: {stop_time:.1f}ms, 清理资源: {cleanup_time:.1f}ms, 销毁实例: {destroy_time:.1f}ms, 创建实例: {create_time:.1f}ms, 初始化: {init_time:.1f}ms, 启动推流: {stream_time:.1f}ms, 总耗时: {total_time:.1f}ms")
            return True
                
        except Exception as e:
            total_time = (time.time() - restart_start) * 1000
            error_msg = f"彻底重启设备 {device_name} 异常: {e} (耗时: {total_time:.1f}ms)"
            self.logger.error(error_msg)            
            return False
            
    def _start_monitor(self):
        """
        启动监控线程
        """
        if self.monitor_thread and self.monitor_thread.is_alive():
            return
            
        self.monitor_thread = threading.Thread(
            target=self._monitor_worker,
            name="DeviceMonitor",
            daemon=True
        )
        self.monitor_thread.start()
        self.logger.info("设备监控线程已启动")
        
    def _monitor_worker(self):
        """
        监控工作线程
        """
        self.logger.info("设备监控线程开始运行")
        
        while self.is_initialized:
            try:
                # 检查设备健康状态
                for device_name, device in self.devices.items():
                    try:
                        status = device.get_status()
                        if not status.get('is_connected', False):
                            self.logger.warning(f"设备 {device_name} 连接丢失")
                            self.stats['device_errors'][device_name] += 1
                            
                            # 尝试重连
                            if self.stats['device_errors'][device_name] <= 3:
                                self.logger.info(f"尝试重连设备: {device_name}")
                                if self.restart_device(device_name):
                                    self.stats['device_errors'][device_name] = 0
                                    
                    except Exception as e:
                        self.logger.error(f"检查设备 {device_name} 状态异常: {e}")
                        
                # 发送状态更新
                if self.is_running:
                    status = self.get_device_status()
                    self.socket_manager.emit_to_namespace(
                        '/coordinator', 'status_update', status
                    )
                    
                time.sleep(self.health_check_interval)
                
            except Exception as e:
                self.logger.error(f"监控线程异常: {e}")
                time.sleep(1.0)
                
        self.logger.info("设备监控线程结束")
        
    def register_event_callback(self, event_name: str, callback: Callable):
        """
        注册事件回调
        
        Args:
            event_name: 事件名称
            callback: 回调函数
        """
        self.event_callbacks[event_name].append(callback)
        
    def _emit_event(self, event_name: str, data: Dict[str, Any]):
        """
        触发事件
        
        Args:
            event_name: 事件名称
            data: 事件数据
        """
        # 调用注册的回调
        for callback in self.event_callbacks[event_name]:
            try:
                callback(data)
            except Exception as e:
                self.logger.error(f"事件回调异常 {event_name}: {e}")
                
        # 发送到Socket.IO
        self.socket_manager.emit_to_namespace(
            '/coordinator', event_name, data
        )
        
    def _cleanup_devices(self):
        """
        清理所有设备
        """
        for device_name, device in self.devices.items():
            try:
                if hasattr(device, 'cleanup'):
                    device.cleanup()
                self.logger.info(f"设备 {device_name} 清理完成")
            except Exception as e:
                self.logger.error(f"清理设备 {device_name} 失败: {e}")
                
        self.devices.clear()
        
    def shutdown(self):
        """
        关闭设备协调器
        """
        with self.coordinator_lock:
            try:
                self.logger.info("关闭设备协调器...")
                
                # 停止数据流
                self.stop_all_streaming()
                
                # 停止监控
                self.is_initialized = False
                if self.monitor_thread and self.monitor_thread.is_alive():
                    self.monitor_thread.join(timeout=5.0)
                    
                # 清理设备
                self._cleanup_devices()
                
                # 关闭线程池
                self.executor.shutdown(wait=True)
                
                # 清理Socket管理器
                try:
                    self.socket_manager.cleanup_all()
                except Exception:
                    # 兼容旧接口
                    if hasattr(self.socket_manager, 'cleanup'):
                        self.socket_manager.cleanup()
                
                self.logger.info("设备协调器已关闭")
                
            except Exception as e:
                self.logger.error(f"关闭设备协调器失败: {e}")
                
    def __enter__(self):
        """上下文管理器入口"""
        return self
        
    def __exit__(self, exc_type, exc_val, exc_tb):
        """上下文管理器出口"""
        self.shutdown()


def test_restart_device(device_name=None):
    """
    测试设备重启功能
    
    该测试方法演示如何使用restart_device方法进行设备的彻底重启，
    包括模拟设备初始化、推流、重启和状态验证的完整流程。
    
    Args:
        device_name (str, optional): 指定要测试的设备名称。如果为None，则自动选择第一个可用设备。
                                   可选值: 'camera1', 'camera2', 'imu', 'pressure', 'femtobolt'
    """
    import time
    import threading
    from unittest.mock import Mock
    
    print("=" * 60)
    print("设备协调器重启功能测试")
    print("=" * 60)
    
    # 创建模拟的SocketIO（使用真实配置文件）
    mock_socketio = Mock()
    
    try:
        # 创建设备协调器实例
        print("1. 创建设备协调器...")
        coordinator = DeviceCoordinator(mock_socketio)
        
        # 初始化设备协调器
        print("2. 初始化设备协调器...")
        if not coordinator.initialize():
            print("❌ 设备协调器初始化失败")
            return False
        
        print("✅ 设备协调器初始化成功")
        print(f"   已初始化设备: {list(coordinator.devices.keys())}")
        
        # 等待设备稳定
        time.sleep(1)
        
        # 选择要测试的设备
        available_devices = list(coordinator.devices.keys())
        if not available_devices:
            print("❌ 没有可用的设备进行测试")
            return False
        
        # 根据参数选择测试设备（仅支持 camera1/camera2/imu/pressure/femtobolt）
        if device_name:
            allowed = {'camera1', 'camera2', 'imu', 'pressure', 'femtobolt'}
            if device_name in available_devices and device_name in allowed:
                test_device = device_name
                print(f"3. 使用指定的测试设备: {test_device}")
            else:
                print(f"❌ 指定的设备 '{device_name}' 不存在或不受支持")
                print(f"   可用设备: {available_devices}")
                return False
        else:
            test_device = available_devices[0]  # 选择第一个设备进行测试
            print(f"3. 自动选择测试设备: {test_device}")
            print(f"   可用设备: {available_devices}")
        
        # 获取设备初始状态
        device = coordinator.devices[test_device]
        initial_streaming = getattr(device, 'is_streaming', False)
        initial_connected = getattr(device, 'is_connected', False)
        
        print(f"   设备初始状态 - 连接: {initial_connected}, 推流: {initial_streaming}")
        
        # 如果设备未推流，先启动推流
        if hasattr(device, 'start_streaming') and not initial_streaming:
            print("4. 启动设备推流...")
            if device.start_streaming():
                print("✅ 设备推流启动成功")
                time.sleep(0.5)  # 等待推流稳定
            else:
                print("⚠️  设备推流启动失败，继续测试")
        
        # 记录重启前的设备实例ID
        old_device_id = id(device)
        print(f"   重启前设备实例ID: {old_device_id}")
        
        # 执行设备重启
        print("5. 执行设备重启...")
        restart_start = time.time()
        
        restart_success = coordinator.restart_device(test_device)
        
        restart_time = (time.time() - restart_start) * 1000
        
        if restart_success:
            print(f"✅ 设备重启成功 (总耗时: {restart_time:.1f}ms)")
            
            # 验证设备实例是否已更换
            new_device = coordinator.devices.get(test_device)
            if new_device:
                new_device_id = id(new_device)
                print(f"   重启后设备实例ID: {new_device_id}")
                
                if new_device_id != old_device_id:
                    print("✅ 设备实例已成功更换")
                else:
                    print("❌ 设备实例未更换，可能重启不彻底")
                
                # 检查设备状态
                new_connected = getattr(new_device, 'is_connected', False)
                new_streaming = getattr(new_device, 'is_streaming', False)
                
                print(f"   重启后设备状态 - 连接: {new_connected}, 推流: {new_streaming}")
                
                # 验证推流状态恢复
                if initial_streaming and new_streaming:
                    print("✅ 推流状态已正确恢复")
                elif not initial_streaming and not new_streaming:
                    print("✅ 推流状态保持一致")
                else:
                    print("⚠️  推流状态与预期不符")
                
            else:
                print("❌ 重启后设备实例丢失")
                
        else:
            print(f"❌ 设备重启失败 (耗时: {restart_time:.1f}ms)")       
        print("8. 清理资源...")
        coordinator.shutdown()
        print("✅ 资源清理完成")
        
        print("=" * 60)
        print("测试完成")
        print("=" * 60)
        
        return restart_success
        
    except Exception as e:
        print(f"❌ 测试过程中发生异常: {e}")
        import traceback
        traceback.print_exc()
        return False


if __name__ == "__main__":
    """
    直接运行此文件时执行设备重启测试
    """
    print("启动设备协调器重启功能测试...")
    
    # 检查命令行参数
    device_name = None
    
    try:
        # 设置日志级别
        import logging
        logging.basicConfig(
            level=logging.INFO,
            format='%(asctime)s - %(name)s - %(levelname)s - %(message)s'
        )
        
        # 执行测试
        # 可选值: 'camera1', 'camera2', 'imu', 'pressure', 'femtobolt'
        success = test_restart_device('pressure')
        
        if success:
            print("\n🎉 所有测试通过！")
        else:
            print("\n❌ 测试失败，请检查日志")
            
    except KeyboardInterrupt:
        print("\n⚠️  测试被用户中断")
    except Exception as e:
        print(f"\n❌ 测试启动失败: {e}")
        import traceback
        traceback.print_exc()