#!/usr/bin/env python3
"""
Demo API Server for Robot Control GUI
提供HTTP API接口给HTML页面调用机器人SDK
"""

# 设置OpenCV环境变量，避免多线程问题
import os
os.environ['OPENCV_FFMPEG_CAPTURE_OPTIONS'] = 'rtsp_transport;udp'
os.environ['OPENCV_VIDEOIO_PRIORITY_MSMF'] = '0'

import asyncio
import json
import logging
import threading
import time
import subprocess
import requests
import cv2
import io
import base64
import concurrent.futures
import shutil
from pathlib import Path
try:
    import numpy as np
except ImportError:
    print("警告: numpy未安装，PCD地图功能将不可用")
    print("请运行: pip install numpy")
    np = None
import struct
from typing import Optional, Dict, Any
import socket
import platform

# =============== 音频测试全局状态 ===============
AUDIO_REC_PROC: Optional[subprocess.Popen] = None
AUDIO_REC_FILE: Optional[str] = None
# 音频播放进程（异步播放/可停止）
AUDIO_PLAY_PROC: Optional[subprocess.Popen] = None

def _is_linux() -> bool:
    try:
        return platform.system().lower() == 'linux'
    except Exception:
        return False

def _cmd_exists(cmd: str) -> bool:
    try:
        return shutil.which(cmd) is not None
    except Exception:
        return False

def _run_cmd(cmd: list, timeout: float = 3.0) -> Dict[str, Any]:
    try:
        out = subprocess.run(cmd, stdout=subprocess.PIPE, stderr=subprocess.PIPE, text=True, timeout=timeout)
        return {
            'code': out.returncode,
            'stdout': out.stdout,
            'stderr': out.stderr
        }
    except subprocess.TimeoutExpired:
        return {'code': -1, 'stdout': '', 'stderr': 'timeout'}
    except Exception as e:
        return {'code': -2, 'stdout': '', 'stderr': str(e)}

def get_server_ip():
    """获取服务器的IP地址，用于返回正确的视频流URL"""
    try:
        # 尝试连接到一个公网地址来获取本机IP
        with socket.socket(socket.AF_INET, socket.SOCK_DGRAM) as s:
            s.connect(("8.8.8.8", 80))
            ip = s.getsockname()[0]
            # 过滤掉localhost地址
            if ip != "127.0.0.1":
                return ip
    except Exception:
        pass
    
    try:
        # 备用方法：获取所有网络接口的IP
        hostname = socket.gethostname()
        ip_list = socket.gethostbyname_ex(hostname)[2]
        for ip in ip_list:
            # 过滤掉localhost和回环地址
            if not ip.startswith("127.") and not ip.startswith("169.254."):
                return ip
    except Exception:
        pass
    
    try:
        # 最后的备用方案：通过主机名获取IP
        ip = socket.gethostbyname(socket.gethostname())
        if ip != "127.0.0.1":
            return ip
    except Exception:
        pass
    
    # 如果都失败了，返回localhost
    return "localhost"

# 获取服务器IP地址，用于构建正确的视频流URL
SERVER_IP = get_server_ip()
VIDEO_SERVER_BASE_URL = f"http://{SERVER_IP}:5011"
print(f"服务器IP: {SERVER_IP}")
print(f"视频服务器URL: {VIDEO_SERVER_BASE_URL}")
from collections import deque

from flask import Flask, request, jsonify, send_from_directory, send_file, Response, stream_template
from flask_cors import CORS

# 设置OpenCV为单线程模式，避免pthread问题
cv2.setNumThreads(1)

# 导入机器人SDK
import sys
import os
sys.path.append(os.path.dirname(os.path.dirname(os.path.abspath(__file__))))

from daystar_sdk.client import RobotClient
from daystar_sdk.websocket_client import WebSocketRobotClient, PtzfPosition, PtzfCmdVel
from daystar_sdk.proto import mcs_service_pb2

app = Flask(__name__)
CORS(app)  # 允许跨域请求

# 添加请求拦截器 - 捕获所有HTTP请求
@app.before_request
def log_request_info():
    """记录所有HTTP请求"""
    # print("\n" + "🌐" * 60)
    # print(f"🌐 [FLASK REQUEST] {request.method} {request.path}")
    # print(f"🌐 [CLIENT IP] {request.remote_addr}")
    # print(f"🌐 [USER AGENT] {request.headers.get('User-Agent', 'Unknown')}")
    # print("🌐" * 60 + "\n")
    logger.info(f"AddLog: Flask收到请求 - {request.method} {request.path}")

@app.after_request
def log_response_info(response):
    """记录所有HTTP响应"""
    # print(f"📤 [FLASK RESPONSE] {response.status_code} for {request.path}")
    return response

# 全局客户端实例
grpc_client: Optional[RobotClient] = None
websocket_client: Optional[WebSocketRobotClient] = None
websocket_loop: Optional[asyncio.AbstractEventLoop] = None

# 导航事件和状态存储
navigation_events = deque(maxlen=50)  # 最多存储50个事件
navigation_status = {
    'status': 'IDLE',
    'progress': 0,
    'current_target': None,
    'message': '系统就绪'
}
websocket_thread: Optional[threading.Thread] = None

# 云台状态存储
ptz_status = {
    'pan': 0.0,     # 水平角度
    'tilt': 0.0,    # 垂直角度  
    'zoom': 1.0,    # 缩放倍数
    'focus': 50,    # 聚焦值
    'light': False, # 灯光状态
    'moving': False # 是否在运动
}

# 雷达状态存储
lidar_status = {
    'state': 0,     # 雷达状态 (UNKNOWN=0, NORMAL=1, LOWFREQ=2, DISCONNECT=3)
    'timestamp': 0, # 时间戳
    'last_update': 0, # 最后更新时间
    'subscribed': False, # 是否已订阅
    'topic': None   # 当前订阅的topic
}

# 气体传感器状态存储
environment_sensor_status = {
    'sensors': {},  # 存储各类传感器数据 {type: value}
    'timestamp': 0, # 时间戳
    'last_update': 0, # 最后更新时间
    'subscribed': False, # 是否已订阅
    'topic': '/sensor/environment_sensor_data'  # topic路径
}

# 脚本输出缓存（用于推送到前端的SSE）
script_output_buffer = deque(maxlen=200)  # [{'ts': float, 'content': str}]

# 线程池用于处理可能阻塞的操作
executor = concurrent.futures.ThreadPoolExecutor(max_workers=5, thread_name_prefix="robot_api")

def safe_grpc_call(func, *args, timeout=3.0, **kwargs):
    """安全的GRPC调用，带超时处理"""
    try:
        future = executor.submit(func, *args, **kwargs)
        return future.result(timeout=timeout)
    except concurrent.futures.TimeoutError:
        logger.warning(f"GRPC调用超时: {func.__name__}")
        return None
    except Exception as e:
        logger.error(f"GRPC调用异常: {func.__name__} - {e}")
        return None

# 兼容性封装：确保以导航控制源调用上桩接口
def _enter_charge_with_source_nav() -> Optional[bool]:
    """尝试以 nav_control 作为来源调用 enter_charge。
    优先使用 SDK 暴露的方法；如无该方法，则回退直接调用底层 stub。
    返回 True/False 表示接口返回状态（若无法确定或异常，返回 None）。
    """
    global grpc_client
    if not grpc_client:
        return None
    try:
        # 优先使用 RobotClient 上的 enter_charge 方法（若SDK已实现）
        if hasattr(grpc_client, 'enter_charge'):
            return grpc_client.enter_charge(source=mcs_service_pb2.NavOrJoy.nav_control)  # type: ignore[attr-defined]
        # 回退方案：直接构造 EnableRequest 并调用底层 stub 的 EnterOrExitCharge
        # 注意：proto 定义为 EnableRequest，state/enable 含义按后端实现而定，这里仅传 True
        if hasattr(grpc_client, 'stub'):
            req = mcs_service_pb2.EnableRequest(enable=True)
            # 设置来源字段为 nav_control
            setattr(req, 'from', mcs_service_pb2.NavOrJoy.nav_control)
            resp = grpc_client.stub.EnterOrExitCharge(req)
            # CommonReply.state == 0 通常表示成功
            return getattr(resp, 'state', -1) == 0
    except Exception:
        logger.exception('enter_charge(nav_control) 调用异常')
        return None
    return None

# 兼容性封装：确保以导航控制源调用退桩接口
def _leave_charge_with_source_nav() -> Optional[int]:
    """尝试以 nav_control 作为来源调用 leave_charge。
    优先使用 SDK 暴露的方法；如无该方法，则回退直接调用底层 stub。
    返回值：1=成功, 3=失败, None=异常或无法确定
    """
    global grpc_client
    if not grpc_client:
        return None
    try:
        # 优先使用 RobotClient 上的 leave_charge 方法（若SDK已实现）
        if hasattr(grpc_client, 'leave_charge'):
            result = grpc_client.leave_charge(source=mcs_service_pb2.NavOrJoy.nav_control)  # type: ignore[attr-defined]
            # 假设返回值为 state: 1=成功, 3=失败
            if isinstance(result, int):
                return result
            elif isinstance(result, bool):
                return 1 if result else 3
            return None
        # 回退方案：直接构造 EnableRequest 并调用底层 stub 的 EnterOrExitCharge
        if hasattr(grpc_client, 'stub'):
            req = mcs_service_pb2.EnableRequest(enable=False)  # False 表示退桩
            # 设置来源字段为 nav_control
            setattr(req, 'from', mcs_service_pb2.NavOrJoy.nav_control)
            resp = grpc_client.stub.EnterOrExitCharge(req)
            # 返回 state 字段：1=成功, 3=失败
            state = getattr(resp, 'state', None)
            return state if state is not None else None
    except Exception:
        logger.exception('leave_charge(nav_control) 调用异常')
        return None
    return None

# =============== 通用工具：读取/轮询 common_status 字段 ===============
def _read_common_status_attr(attr_names):
    """从 grpc_client.get_common_status() 中读取指定字段（支持多个候选名）"""
    if not grpc_client:
        return None
    try:
        success, status = grpc_client.get_common_status()
        if success and status:
            for name in attr_names:
                if hasattr(status, name):
                    return getattr(status, name)
            # 兼容新结构：charge_status 内嵌 position_state / charge_switch_state
            try:
                cs = getattr(status, 'charge_status', None)
                if cs:
                    # 位置状态
                    if any(n in ['charge_pile_position_state', 'ChargePilePositionState'] for n in attr_names):
                        if hasattr(cs, 'position_state'):
                            return getattr(cs, 'position_state')
                    # 开关/对接状态
                    if any(n in ['charge_pile_switch_state', 'ChargePileSwitchState'] for n in attr_names):
                        if hasattr(cs, 'charge_switch_state'):
                            return getattr(cs, 'charge_switch_state')
            except Exception:
                pass
        return None
    except Exception as e:
        logger.warning(f"读取common_status字段异常: {e}")
        return None

def _poll_common_status_state(attr_names, success_value=2, fail_value=3, timeout_s=30.0, interval_s=0.5):
    """轮询 common_status 指定字段，直到等于 success_value 或 fail_value 或超时
    返回 (reached_success: bool, last_value: Any, elapsed: float)
    """
    start = time.time()
    last_val = None
    while True:
        last_val = _read_common_status_attr(attr_names)
        if last_val is not None:
            if last_val == success_value:
                return True, last_val, time.time() - start
            if last_val == fail_value:
                return False, last_val, time.time() - start
        if time.time() - start >= timeout_s:
            return False, last_val, time.time() - start
        time.sleep(interval_s)

# 云台位置回调函数
def on_ptzf_position_update(position_data):
    """处理云台位置更新回调"""
    try:
        global ptz_status
        
        # 从WebSocket回调数据中提取云台位置信息
        # 数据格式: {"header":{"frame_id":"PTZ_CAM","stamp":{"nanosec":...,"sec":...}}, 
        #           "ptzf_position":{"focus":28613,"pan":93.779...,"tilt":47.009...,"zoom":1.0}}
        if 'ptzf_position' in position_data:
            position = position_data['ptzf_position']
            ptz_status['pan'] = position.get('pan', ptz_status['pan'])
            ptz_status['tilt'] = position.get('tilt', ptz_status['tilt'])
            ptz_status['zoom'] = position.get('zoom', ptz_status['zoom'])
            ptz_status['focus'] = position.get('focus', ptz_status['focus'])
            
            logger.debug(f"云台位置更新: pan={ptz_status['pan']:.2f}, tilt={ptz_status['tilt']:.2f}, "
                        f"zoom={ptz_status['zoom']:.2f}, focus={ptz_status['focus']}")
        else:
            logger.warning(f"云台位置数据格式异常: {position_data}")
            
    except Exception as e:
        logger.error(f"处理云台位置更新异常: {e}")

# 雷达状态回调函数
def on_lidar_data(msg_data):
    """处理雷达状态数据回调"""
    try:
        global lidar_status
        
        # 解析雷达状态数据
        # 数据格式: {"timestamp": {"sec": 1758004782, "nanosec": 701002033}, "state": 0}
        if isinstance(msg_data, dict):
            # 提取状态值
            state = msg_data.get('state', 0)
            timestamp_data = msg_data.get('timestamp', {})
            
            # 更新雷达状态
            lidar_status['state'] = state
            lidar_status['timestamp'] = timestamp_data
            lidar_status['last_update'] = time.time()
            
            # 状态映射
            state_names = {0: 'UNKNOWN', 1: 'NORMAL', 2: 'LOWFREQ', 3: 'DISCONNECT'}
            state_name = state_names.get(state, f'UNKNOWN({state})')
            
            logger.debug(f"雷达状态更新: state={state} ({state_name}), "
                        f"timestamp_sec={timestamp_data.get('sec', 0)}, "
                        f"timestamp_nanosec={timestamp_data.get('nanosec', 0)}")
        else:
            logger.warning(f"雷达状态数据格式异常: {msg_data}")
            
    except Exception as e:
        logger.error(f"处理雷达状态更新异常: {e}")

# 气体传感器数据回调函数
def on_environment_sensor_data(msg_data):
    """处理气体传感器数据回调
    数据格式: {"type": 1-18, "value": "actual_value", "unit": "string", "timestamp": {...}}
    type值对应: 1=环境温度, 2=环境湿度, 3=风速, 4=雨量, 5=风向, 6=气压, 7=氧气, 8=SF6,
               9=声音检测, 10=烟感, 11=氢气, 12=硫化氢, 13=一氧化碳, 14=甲烷, 15=丙烷, 16=VOC,
               17=CO2(二氧化碳), 18=N2O(一氧化二氮)
    """
    try:
        global environment_sensor_status
        
        # 解析传感器数据
        if isinstance(msg_data, dict):
            sensor_type = msg_data.get('type', 0)
            value = msg_data.get('value', '')
            unit = msg_data.get('unit', '')
            timestamp_data = msg_data.get('timestamp', {})
            
            # 传感器类型映射
            sensor_names = {
                1: '环境温度', 2: '环境湿度', 3: '风速', 4: '雨量',
                5: '风向', 6: '气压', 7: '氧气', 8: 'SF6',
                9: '声音检测', 10: '烟感', 11: '氢气', 12: '硫化氢',
                13: '一氧化碳', 14: '甲烷', 15: '丙烷', 16: 'VOC',
                17: 'CO2(二氧化碳)', 18: 'N2O(一氧化二氮)'
            }
            
            sensor_name = sensor_names.get(sensor_type, f'未知传感器({sensor_type})')
            
            # 更新传感器数据
            environment_sensor_status['sensors'][sensor_type] = {
                'name': sensor_name,
                'value': value,
                'unit': unit,
                'timestamp': timestamp_data,
                'type': sensor_type
            }
            environment_sensor_status['timestamp'] = timestamp_data
            environment_sensor_status['last_update'] = time.time()
            
            logger.debug(f"气体传感器数据更新: type={sensor_type} ({sensor_name}), "
                        f"value={value}{unit}, "
                        f"timestamp_sec={timestamp_data.get('sec', 0)}")
        else:
            logger.warning(f"气体传感器数据格式异常: {msg_data}")
            
    except Exception as e:
        logger.error(f"处理气体传感器数据更新异常: {e}")

# 脚本输出消息回调函数
def on_script_output(msg_data: Dict[str, Any]):
    """处理脚本输出消息回调，适配ROS echo展示的结构
    期望结构（示例）：
      {
        "task_id": "factory_aging_task",
        "output_type": "stdout",
        "content": "[factory_aging_task] Cycle 1/8 completed.",
        "timestamp": {"sec": 1758716494, "nanosec": 906191853}
      }

    也兼容以下包裹形式：
      {"msg": {...}}
      {"data": {...}}
    若字段缺失，将尽量做鲁棒解析；SSE输出保持向后兼容：始终包含 ts 与 content，
    并在可用时附带 task_id 与 output_type。
    """
    try:
        # 尝试下钻一层常见包裹字段
        payload = msg_data if isinstance(msg_data, dict) else {}
        if isinstance(payload.get('msg'), dict):
            payload = payload.get('msg')
        elif isinstance(payload.get('data'), dict):
            payload = payload.get('data')

        # 提取字段
        task_id = payload.get('task_id') or payload.get('taskId') or payload.get('task')
        output_type = payload.get('output_type') or payload.get('outputType') or payload.get('type')
        content = payload.get('content')

        # 解析时间戳
        ts = time.time()
        ts_obj = payload.get('timestamp') or payload.get('time') or payload.get('ts')
        if isinstance(ts_obj, dict):
            try:
                sec = float(ts_obj.get('sec') or ts_obj.get('seconds') or 0)
                nsec = float(ts_obj.get('nanosec') or ts_obj.get('nsec') or 0)
                ts = sec + nsec / 1e9
            except Exception:
                pass
        elif isinstance(ts_obj, (int, float)):
            # 简单兼容：若为数字，按秒处理；若明显是毫秒/纳秒则做缩放
            val = float(ts_obj)
            if val > 1e12:      # 纳秒
                ts = val / 1e9
            elif val > 1e10:    # 毫秒
                ts = val / 1e3
            else:               # 秒
                ts = val

        # 内容兜底与清洗
        if content is None:
            content = json.dumps(payload if payload else msg_data, ensure_ascii=False)
        else:
            if isinstance(content, (dict, list)):
                content = json.dumps(content, ensure_ascii=False)
            else:
                content = str(content).rstrip('\n')

        item = {'ts': ts, 'content': content}
        if task_id:
            item['task_id'] = task_id
        if output_type:
            item['output_type'] = output_type

        script_output_buffer.append(item)
        logger.debug(f"[SCRIPT_OUTPUT] task={task_id or '-'} type={output_type or '-'} ts={ts}: {content}")
    except Exception as e:
        logger.error(f"处理脚本输出消息异常: {e}")

# 连接状态
connection_status = {
    'grpc_connected': False,
    'websocket_connected': False,
    'robot_host': '',
    'robot_port': 0,
    'websocket_host': '',
    'websocket_port': 0
}

# 导航事件回调函数
def on_navigation_result(result_type: str, data: Dict):
    """导航结果回调 - 用于HTML页面导航事件日志显示"""
    # 从数据中提取关键信息
    target_location = data.get('target_location', 'Unknown')
    task_id = data.get('task_id', 'Unknown')
    navigation_time = data.get('navigation_time', 0)
    timestamp = data.get('timestamp', time.strftime('%Y-%m-%d %H:%M:%S'))
    current_pose = data.get('current_pose', {})
    
    # 构造事件记录，用于事件日志显示
    event = {
        'timestamp': time.time(),
        'type': 'result',
        'result_type': result_type,
        'data': data,
        'message': f"导航{result_type}: 目标点位={target_location}, 耗时={navigation_time}秒",
        'target_location': target_location,
        'task_id': task_id,
        'navigation_time': navigation_time,
        'formatted_timestamp': timestamp
    }
    navigation_events.append(event)
    
    # 根据结果类型更新最终导航状态
    if result_type == 'complete':
        navigation_status['status'] = 'COMPLETED'
        navigation_status['message'] = f'导航到 {target_location} 成功完成'
        navigation_status['progress'] = 100
        navigation_status['current_target'] = target_location
    elif result_type == 'failed':
        navigation_status['status'] = 'FAILED'
        navigation_status['message'] = f'导航到 {target_location} 失败'
        navigation_status['progress'] = 0
        navigation_status['current_target'] = target_location
    elif result_type == 'cancelled':
        navigation_status['status'] = 'CANCELLED'
        navigation_status['message'] = f'导航到 {target_location} 被取消'
        navigation_status['progress'] = 0
        navigation_status['current_target'] = target_location
    
    logger.info(f"导航结果回调: {result_type} -> {target_location}, 数据: {data}")


def on_navigation_feedback(data: Dict):
    """导航反馈回调 - 用于HTML页面导航状态监控显示"""
    # 从数据中提取导航状态信息
    current_state = data.get('current_state', 0)
    distance_remaining = data.get('distance_remaining', 0.0)
    time_remaining = data.get('time_remaining', 0)
    target_location = data.get('target_location', 'Unknown')
    navigation_time = data.get('navigation_time', 0)
    current_pose = data.get('current_pose', {})
    task_id = data.get('task_id', 'Unknown')
    
    # 状态码映射
    state_map = {
        0: 'IDLE',
        1: 'PLANNING',
        2: 'NAVIGATING', 
        3: 'PAUSED',
        4: 'COMPLETED',
        5: 'FAILED'
    }
    
    status_text = state_map.get(current_state, f'UNKNOWN({current_state})')
    
    # 计算进度百分比 (基于剩余距离的估算)
    if distance_remaining > 0:
        # 假设初始距离，根据剩余距离计算进度
        estimated_total_distance = max(distance_remaining * 2, 1.0)  # 简单估算
        progress = max(0, min(100, int((estimated_total_distance - distance_remaining) / estimated_total_distance * 100)))
    else:
        progress = 100 if current_state == 4 else 0
    
    # 更新导航状态监控信息
    navigation_status.update({
        'status': status_text,
        'progress': progress,
        'current_target': target_location,
        'message': f'导航中: {status_text}, 剩余距离: {distance_remaining:.2f}m, 预计时间: {time_remaining}s',
        'distance_remaining': distance_remaining,
        'time_remaining': time_remaining,
        'navigation_time': navigation_time,
        'current_state': current_state,
        'task_id': task_id,
        'current_pose': current_pose
    })
    
    # 可选择是否将反馈也记录到事件日志中（如果需要详细日志）
    # 注意：通常反馈数据量较大，可能不需要全部记录到事件日志
    if current_state in [4, 5]:  # 只记录完成或失败状态的反馈
        feedback_event = {
            'timestamp': time.time(),
            'type': 'feedback',
            'data': data,
            'message': f"导航反馈: {status_text} - {target_location}",
            'current_state': current_state,
            'distance_remaining': distance_remaining
        }
        navigation_events.append(feedback_event)
    
    logger.info(f"导航反馈回调: 状态={status_text}, 目标={target_location}, 剩余距离={distance_remaining:.2f}m")
    
    logger.info(f"导航反馈回调: {data}")


# 日志配置
logging.basicConfig(level=logging.INFO)
logger = logging.getLogger(__name__)

# 读取本地测试标志（通过命令行或环境变量传入）
SDK_LOCALTEST = False
try:
    # 命令行参数检查
    if '--localtest' in sys.argv:
        SDK_LOCALTEST = True
    # 环境变量兜底
    if os.environ.get('SDK_LOCALTEST', '0') in ('1', 'true', 'True', 'YES', 'yes'):
        SDK_LOCALTEST = True
except Exception:
    pass
logger.info(f"FrontEnd LocalTest mode = {SDK_LOCALTEST}")

# 连接状态
connection_status = {
    'grpc_connected': False,
    'websocket_connected': False,
    'robot_host': '',
    'robot_port': 0,
    'websocket_host': '',
    'websocket_port': 0
}

# 静态文件路由
@app.route('/')
def index():
    """主页重定向到demo页面"""
    # 默认首页仍提供 demo_gui.html
    return send_from_directory('.', 'demo_gui.html')

@app.route('/demo_gui.html')
def demo_gui():
    """提供demo GUI页面"""
    return send_from_directory('.', 'demo_gui.html')

@app.route('/factory_test.html')
def factory_test():
    """提供工厂测试页面"""
    resp = send_from_directory('.', 'factory_test.html')
    try:
        resp.headers['Cache-Control'] = 'no-store, no-cache, must-revalidate, max-age=0'
        resp.headers['Pragma'] = 'no-cache'
        resp.headers['Expires'] = '0'
    except Exception:
        pass
    return resp

@app.route('/factory')
def factory():
    """工厂入口路由 -> 统一入口页"""
    return send_from_directory('.', 'index.html')

@app.route('/aging_test.html')
def aging_test():
    """提供老化测试(任务循环)页面"""
    return send_from_directory('.', 'aging_test.html')


def run_websocket_loop():
    """在单独线程中运行WebSocket异步循环"""
    global websocket_loop
    websocket_loop = asyncio.new_event_loop()
    asyncio.set_event_loop(websocket_loop)
    websocket_loop.run_forever()


def ensure_websocket_loop():
    """确保WebSocket异步循环正在运行"""
    global websocket_thread, websocket_loop
    if websocket_thread is None or not websocket_thread.is_alive():
        websocket_thread = threading.Thread(target=run_websocket_loop, daemon=True)
        websocket_thread.start()
        time.sleep(0.1)  # 等待循环启动


# ==================== 连接管理接口 ====================

@app.route('/api/connect', methods=['POST'])
def connect_robot():
    """连接到机器人"""
    global grpc_client, websocket_client, websocket_loop
    
    try:
        data = request.get_json()
        host = data.get('host', '192.168.144.103')
        grpc_port = data.get('port', 50051)
        websocket_host = data.get('websocket_host', host)  # 如果没有提供WebSocket主机，使用GRPC主机
        websocket_port = data.get('websocket_port', 3232)
        
        logger.info(f"收到连接请求: GRPC={host}:{grpc_port}, WebSocket={websocket_host}:{websocket_port}")
        
        # 首先测试网络连通性
        logger.info(f"测试网络连通性到 {host}...")
        try:
            # 使用socket测试端口连通性
            import socket
            sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
            sock.settimeout(3)  # 3秒超时
            result = sock.connect_ex((host, grpc_port))
            sock.close()
            
            if result == 0:
                logger.info(f"✅ 端口 {host}:{grpc_port} 可达")
            else:
                logger.warning(f"❌ 端口 {host}:{grpc_port} 不可达，错误代码: {result}")
                return jsonify({
                    'success': False,
                    'message': f'无法连接到机器人服务器 {host}:{grpc_port}，请检查：\n1. 网络连接是否正常\n2. 机器人是否已开机\n3. 机器人服务是否正在运行',
                    'grpc_connected': False,
                    'websocket_connected': False,
                    'debug_info': f'端口测试失败: {result}'
                })
        except Exception as e:
            logger.error(f"网络连通性测试异常: {e}")
            return jsonify({
                'success': False,
                'message': f'网络连接测试失败: {str(e)}',
                'grpc_connected': False,
                'websocket_connected': False
            })

        logger.info(f"GRPC开始连接，主机: {host}, 端口: {grpc_port}")
        # 连接GRPC客户端
        try:
            grpc_client = RobotClient(host=host, port=grpc_port)
            # 测试连接 - 使用安全调用避免阻塞
            result = safe_grpc_call(grpc_client.get_mc_version, timeout=5.0)  # 增加超时时间
            if result and len(result) >= 2:
                success, version = result[0], result[1]
                if success:
                    connection_status['grpc_connected'] = True
                    connection_status['robot_host'] = host
                    connection_status['robot_port'] = grpc_port
                    logger.info(f"GRPC连接成功，版本: {version}")
                else:
                    connection_status['grpc_connected'] = False
                    logger.warning("GRPC连接失败")
            else:
                connection_status['grpc_connected'] = False
                logger.warning("GRPC连接测试超时")
        except Exception as e:
            logger.error(f"GRPC连接异常: {e}")
            connection_status['grpc_connected'] = False
        
        # 连接WebSocket客户端
        try:
            ensure_websocket_loop()
            websocket_client = WebSocketRobotClient(host=websocket_host, port=websocket_port)
            
            # 在WebSocket循环中连接
            future = asyncio.run_coroutine_threadsafe(
                websocket_client.connect(), websocket_loop
            )
            future.result(timeout=5)  # 等待连接完成
            
            if websocket_client.is_connected:
                # 首先尝试获取机器人名称
                try:
                    if grpc_client:
                        result = safe_grpc_call(grpc_client.get_robot_name, timeout=2.0)
                        if result and len(result) >= 2:
                            success, robot_name = result[0], result[1]
                            if success and robot_name:
                                logger.info(f"获取到机器人名称: {robot_name}")
                                # 设置WebSocket客户端的机器人名称
                                websocket_client.robot_name = robot_name
                                logger.info(f"WebSocket客户端机器人名称已设置为: {robot_name}")
                            else:
                                logger.warning("无法获取机器人名称，导航服务可能无法正常工作")
                        else:
                            logger.warning("获取机器人名称超时，导航服务可能无法正常工作")
                except Exception as e:
                    logger.error(f"获取机器人名称失败: {e}")
                
                # WebSocket连接成功后，立即发布云台控制主题
                try:
                    advertise_future = asyncio.run_coroutine_threadsafe(
                        websocket_client.advertise_ptzf_cmd_vel(), websocket_loop
                    )
                    advertise_success = advertise_future.result(timeout=3)
                    if advertise_success:
                        logger.info("云台控制主题发布成功")
                    else:
                        logger.warning("云台控制主题发布失败")
                except Exception as e:
                    logger.error(f"发布云台控制主题异常: {e}")
                
                # 订阅云台位置信息
                try:
                    subscribe_future = asyncio.run_coroutine_threadsafe(
                        websocket_client.subscribe_ptzf_position(on_ptzf_position_update), websocket_loop
                    )
                    subscribe_success = subscribe_future.result(timeout=3)
                    if subscribe_success:
                        logger.info("云台位置订阅成功")
                    else:
                        logger.warning("云台位置订阅失败")
                except Exception as e:
                    logger.error(f"订阅云台位置异常: {e}")
                
                # 注册导航事件回调
                try:
                    # 注册导航结果回调
                    result_future = asyncio.run_coroutine_threadsafe(
                        websocket_client.register_navigation_result(on_navigation_result), websocket_loop
                    )
                    result_success = result_future.result(timeout=3)
                    if result_success:
                        logger.info("导航结果回调注册成功")
                    else:
                        logger.warning("导航结果回调注册失败")
                    
                    # 注册导航反馈回调
                    feedback_future = asyncio.run_coroutine_threadsafe(
                        websocket_client.register_navigation_feedback(on_navigation_feedback), websocket_loop
                    )
                    feedback_success = feedback_future.result(timeout=3)
                    if feedback_success:
                        logger.info("导航反馈回调注册成功")
                    else:
                        logger.warning("导航反馈回调注册失败")
                        
                except Exception as e:
                    logger.error(f"注册导航回调异常: {e}")
                
                connection_status['websocket_connected'] = True
                connection_status['websocket_host'] = websocket_host
                connection_status['websocket_port'] = websocket_port
                logger.info(f"WebSocket连接成功: {websocket_host}:{websocket_port}")

                # 订阅脚本输出主题 /api/script_output
                try:
                    subscribe_future = asyncio.run_coroutine_threadsafe(
                        websocket_client.register_module_data(
                            topic="/api/script_output",
                            topic_type="api_msgs__ScriptOutput",
                            callback=on_script_output
                        ), websocket_loop
                    )
                    sub_ok = subscribe_future.result(timeout=3)
                    if sub_ok:
                        logger.info("脚本输出 /api/script_output 订阅成功")
                    else:
                        logger.warning("脚本输出 /api/script_output 订阅失败")
                except Exception as e:
                    logger.error(f"订阅脚本输出异常: {e}")
                
                # 订阅气体传感器数据主题
                try:
                    subscribe_future = asyncio.run_coroutine_threadsafe(
                        websocket_client.register_module_data(
                            topic="/sensor/environment_sensor_data",
                            topic_type="rms_msgs/msg/EnvironmentSensorData",
                            callback=on_environment_sensor_data
                        ), websocket_loop
                    )
                    sub_ok = subscribe_future.result(timeout=3)
                    if sub_ok:
                        environment_sensor_status['subscribed'] = True
                        logger.info("气体传感器数据 /sensor/environment_sensor_data 订阅成功")
                    else:
                        logger.warning("气体传感器数据 /sensor/environment_sensor_data 订阅失败")
                except Exception as e:
                    logger.error(f"订阅气体传感器数据异常: {e}")
            else:
                connection_status['websocket_connected'] = False
                logger.warning("WebSocket连接失败")
        except Exception as e:
            logger.error(f"WebSocket连接异常: {e}")
            connection_status['websocket_connected'] = False
        
        return jsonify({
            'success': connection_status['grpc_connected'] or connection_status['websocket_connected'],
            'grpc_connected': connection_status['grpc_connected'],
            'websocket_connected': connection_status['websocket_connected'],
            'message': f"连接到 GRPC:{host}:{grpc_port}, WebSocket:{websocket_host}:{websocket_port}"
        })
        
    except Exception as e:
        logger.error(f"连接失败: {e}")
        return jsonify({'success': False, 'message': str(e)}), 500


@app.route('/api/disconnect', methods=['POST'])
def disconnect_robot():
    """断开机器人连接"""
    global grpc_client, websocket_client, websocket_loop
    
    try:
        # 断开GRPC连接
        if grpc_client:
            grpc_client.channel.close()
            grpc_client = None
            connection_status['grpc_connected'] = False
        
        # 断开WebSocket连接
        if websocket_client and websocket_loop:
            future = asyncio.run_coroutine_threadsafe(
                websocket_client.disconnect(), websocket_loop
            )
            future.result(timeout=3)
            websocket_client = None
            connection_status['websocket_connected'] = False
        
        return jsonify({'success': True, 'message': '已断开连接'})
        
    except Exception as e:
        logger.error(f"断开连接失败: {e}")
        return jsonify({'success': False, 'message': str(e)}), 500


@app.route('/api/status', methods=['GET'])
def get_connection_status():
    """获取连接状态"""
    return jsonify(connection_status)


@app.route('/api/config', methods=['GET'])
def get_frontend_config():
    """提供前端运行所需的最小配置，例如是否启用本地测试模式。
    返回示例：{"localtest": true}
    """
    try:
        return jsonify({
            'success': True,
            'localtest': bool(SDK_LOCALTEST)
        })
    except Exception as e:
        logger.error(f"/api/config 异常: {e}")
        return jsonify({'success': False, 'message': str(e)}), 500


# ==================== 机器人基础信息接口 ====================

# ==================== 本地音频设备与录音/播放 ====================
@app.route('/api/audio/devices', methods=['GET'])
def audio_list_devices():
    """列出Linux主机上的音频输入/输出设备 (arecord -l / aplay -l)。"""
    if not _is_linux():
        return jsonify({'success': False, 'message': '仅支持Linux主机'}), 400
    if not (_cmd_exists('arecord') and _cmd_exists('aplay')):
        return jsonify({'success': False, 'message': '需要安装 alsa-utils (arecord/aplay)'}), 400
    rec = _run_cmd(['arecord', '-l'], timeout=3.0)
    play = _run_cmd(['aplay', '-l'], timeout=3.0)
    return jsonify({
        'success': True,
        'input_devices': rec.get('stdout', ''),
        'output_devices': play.get('stdout', '')
    })

@app.route('/api/audio/start_record', methods=['POST'])
def audio_start_record():
    """启动录音，保存为WAV临时文件。支持可选参数: device, duration, rate, channels."""
    global AUDIO_REC_PROC, AUDIO_REC_FILE
    if not _is_linux():
        return jsonify({'success': False, 'message': '仅支持Linux主机'}), 400
    if not _cmd_exists('arecord'):
        return jsonify({'success': False, 'message': 'arecord 未安装(alsa-utils)'}), 400
    if AUDIO_REC_PROC and AUDIO_REC_PROC.poll() is None:
        return jsonify({'success': False, 'message': '录音已在进行中'}), 400
    data = request.get_json(silent=True) or {}
    device = data.get('device')  # 如: hw:0,0 或 plughw:1,0
    duration = data.get('duration')  # 秒，可选；若未提供则持续录制直到停止
    rate = int(data.get('rate', 16000))
    channels = int(data.get('channels', 1))

    # 录音保存目录
    out_dir = Path('/tmp')
    out_dir.mkdir(parents=True, exist_ok=True)
    ts = int(time.time())
    AUDIO_REC_FILE = str(out_dir / f'record_{ts}.wav')

    cmd = ['arecord', '-q', '-f', 'S16_LE', '-r', str(rate), '-c', str(channels)]
    if device:
        cmd.extend(['-D', str(device)])
    cmd.extend(['-t', 'wav', AUDIO_REC_FILE])
    try:
        if duration:
            # 有时长：同步录制，子进程自行退出
            duration = int(duration)
            proc = subprocess.Popen(cmd)
            AUDIO_REC_PROC = proc
            logger.info(f"开始录音(定时 {duration}s): {' '.join(cmd)} -> {AUDIO_REC_FILE}")
            # 启动后台线程在duration后停止
            def _stop_after():
                time.sleep(max(0, duration))
                try:
                    if proc.poll() is None:
                        proc.terminate()
                except Exception:
                    pass
            threading.Thread(target=_stop_after, daemon=True).start()
        else:
            # 无时长：持续录制，待手动停止
            AUDIO_REC_PROC = subprocess.Popen(cmd)
            logger.info(f"开始录音: {' '.join(cmd)} -> {AUDIO_REC_FILE}")
        return jsonify({'success': True, 'file': AUDIO_REC_FILE})
    except Exception as e:
        logger.error(f"启动录音失败: {e}")
        AUDIO_REC_PROC = None
        AUDIO_REC_FILE = None
        return jsonify({'success': False, 'message': str(e)}), 500

@app.route('/api/audio/stop_record', methods=['POST'])
def audio_stop_record():
    """停止当前录音，返回文件路径。"""
    global AUDIO_REC_PROC
    if not _is_linux():
        return jsonify({'success': False, 'message': '仅支持Linux主机'}), 400
    if AUDIO_REC_PROC is None:
        return jsonify({'success': False, 'message': '没有正在进行的录音'}), 400
    try:
        if AUDIO_REC_PROC.poll() is None:
            AUDIO_REC_PROC.terminate()
            try:
                AUDIO_REC_PROC.wait(timeout=3)
            except subprocess.TimeoutExpired:
                AUDIO_REC_PROC.kill()
        AUDIO_REC_PROC = None
        return jsonify({'success': True, 'file': AUDIO_REC_FILE})
    except Exception as e:
        logger.error(f"停止录音失败: {e}")
        return jsonify({'success': False, 'message': str(e)}), 500

@app.route('/api/audio/play', methods=['POST'])
def audio_play():
    """播放声音：若未提供file则播放最近一次录音文件；支持可选参数 device。
    若已有播放在进行中，将先行停止旧的播放进程。
    该接口异步返回，播放在后台进行，可通过 /api/audio/status 查询 playing 状态。
    """
    if not _is_linux():
        return jsonify({'success': False, 'message': '仅支持Linux主机'}), 400
    if not _cmd_exists('aplay'):
        return jsonify({'success': False, 'message': 'aplay 未安装(alsa-utils)'}), 400
    data = request.get_json(silent=True) or {}
    file_path = data.get('file') or AUDIO_REC_FILE
    device = data.get('device')  # 如: hw:0,0
    if not file_path or not os.path.exists(file_path):
        return jsonify({'success': False, 'message': '音频文件不存在'}), 400
    cmd = ['aplay', '-q']
    if device:
        cmd.extend(['-D', str(device)])
    cmd.append(file_path)
    try:
        global AUDIO_PLAY_PROC
        # 若已有播放，先停止
        if AUDIO_PLAY_PROC and AUDIO_PLAY_PROC.poll() is None:
            try:
                AUDIO_PLAY_PROC.terminate()
                AUDIO_PLAY_PROC.wait(timeout=2)
            except Exception:
                try:
                    AUDIO_PLAY_PROC.kill()
                except Exception:
                    pass
        # 启动新播放进程（异步）
        AUDIO_PLAY_PROC = subprocess.Popen(cmd)
        logger.info(f"开始播放: {' '.join(cmd)}")
        return jsonify({'success': True})
    except Exception as e:
        logger.error(f"播放异常: {e}")
        return jsonify({'success': False, 'message': str(e)}), 500

@app.route('/api/audio/stop_play', methods=['POST'])
def audio_stop_play():
    """停止当前的音频播放。"""
    global AUDIO_PLAY_PROC
    if not _is_linux():
        return jsonify({'success': False, 'message': '仅支持Linux主机'}), 400
    if AUDIO_PLAY_PROC is None:
        return jsonify({'success': False, 'message': '没有正在进行的播放'}), 400
    try:
        if AUDIO_PLAY_PROC.poll() is None:
            AUDIO_PLAY_PROC.terminate()
            try:
                AUDIO_PLAY_PROC.wait(timeout=3)
            except subprocess.TimeoutExpired:
                AUDIO_PLAY_PROC.kill()
        AUDIO_PLAY_PROC = None
        return jsonify({'success': True})
    except Exception as e:
        logger.error(f"停止播放失败: {e}")
        return jsonify({'success': False, 'message': str(e)}), 500

@app.route('/api/audio/status', methods=['GET'])
def audio_status():
    """返回录音状态与最近文件。"""
    return jsonify({
        'success': True,
        'recording': AUDIO_REC_PROC is not None and AUDIO_REC_PROC.poll() is None,
        'playing': AUDIO_PLAY_PROC is not None and AUDIO_PLAY_PROC.poll() is None,
        'last_file': AUDIO_REC_FILE
    })

@app.route('/api/audio/list_records', methods=['GET'])
def audio_list_records():
    """列出 /tmp 目录下由本工具生成的录音文件 record_*.wav，按修改时间倒序。"""
    try:
        base = Path('/tmp')
        if not base.exists():
            return jsonify({'success': True, 'files': []})
        files = []
        for p in sorted(base.glob('record_*.wav'), key=lambda x: x.stat().st_mtime, reverse=True):
            try:
                st = p.stat()
                files.append({
                    'name': p.name,
                    'path': str(p),
                    'size': st.st_size,
                    'mtime': st.st_mtime
                })
            except Exception:
                continue
        # 限制最多返回200条
        return jsonify({'success': True, 'files': files[:200]})
    except Exception as e:
        return jsonify({'success': False, 'message': str(e)}), 500

@app.route('/api/robot/version', methods=['GET'])
def get_mc_version():
    """获取MC版本"""
    if not grpc_client:
        return jsonify({'success': False, 'message': '未连接到机器人'}), 400
    
    try:
        result = safe_grpc_call(grpc_client.get_mc_version, timeout=2.0)
        if result and len(result) >= 2:
            success, version = result[0], result[1]
            return jsonify({'success': success, 'version': version})
        else:
            return jsonify({'success': False, 'message': '获取版本信息超时'}), 500
    except Exception as e:
        return jsonify({'success': False, 'message': str(e)}), 500


@app.route('/api/robot/name', methods=['GET'])
def get_robot_name():
    """获取机器人名称"""
    if not grpc_client:
        return jsonify({'success': False, 'message': '未连接到机器人'}), 400
    
    try:
        result = safe_grpc_call(grpc_client.get_robot_name, timeout=2.0)
        if result and len(result) >= 2:
            success, name = result[0], result[1]
            return jsonify({'success': success, 'name': name})
        else:
            return jsonify({'success': False, 'message': '获取机器人名称超时'}), 500
    except Exception as e:
        return jsonify({'success': False, 'message': str(e)}), 500


@app.route('/api/robot/battery', methods=['GET'])
def get_battery_info():
    """获取电池信息"""
    if not grpc_client:
        return jsonify({'success': False, 'message': '未连接到机器人'}), 400
    
    try:
        # 使用安全调用避免阻塞
        result_level = safe_grpc_call(grpc_client.rps_get_battery_level, timeout=1.5)
        result_current = safe_grpc_call(grpc_client.rps_get_battery_current, timeout=1.5)
        result_power = safe_grpc_call(grpc_client.rps_get_battery_output_power, timeout=1.5)
        result_charging = safe_grpc_call(grpc_client.rps_get_charging_status, timeout=1.5)
        result_low = safe_grpc_call(grpc_client.rps_check_battery_low, timeout=1.5)
        
        # 解析结果
        level = None
        if result_level and len(result_level) >= 2:
            success_level, level = result_level[0], result_level[1]
            level = level if success_level else None
            
        current = None
        if result_current and len(result_current) >= 2:
            success_current, current = result_current[0], result_current[1]
            current = current if success_current else None
            
        power = None
        if result_power and len(result_power) >= 2:
            success_power, power = result_power[0], result_power[1]
            power = power if success_power else None
            
        charging = None
        if result_charging and len(result_charging) >= 2:
            success_charging, charging = result_charging[0], result_charging[1]
            charging = charging if success_charging else None
            
        low = None
        if result_low and len(result_low) >= 2:
            success_low, low = result_low[0], result_low[1]
            low = low if success_low else None
        
        return jsonify({
            'success': True,
            'level': level,
            'current': current,
            'power': power,
            'charging': charging,
            'low': low
        })
    except Exception as e:
        return jsonify({'success': False, 'message': str(e)}), 500


@app.route('/api/robot/camera', methods=['GET'])
def get_camera():
    """获取相机图像数据（代理相机API避免CORS问题）"""
    logger.info(f"get_camera 调用，当前连接状态: {connection_status}")
    try:
        # 从连接状态中获取网段信息
        robot_host = connection_status.get('robot_host', '')
        if not robot_host:
            return jsonify({
                'success': False, 
                'message': '未连接到机器人，无法获取相机数据'
            }), 400

        # 构建相机API URL
        # 从robot_host提取网段，例如 192.168.100.105 -> 192.168.100
        host_parts = robot_host.split('.')
        logger.info(f"host_parts: {host_parts}")
        if len(host_parts) >= 3:
            network_segment = '.'.join(host_parts[:3])
            camera_url = f"http://{network_segment}.105:9000/api/v1/get_rgbd"
        else:
            # 备用方案：使用默认网段
            camera_url = "http://192.168.144.105:9000/api/v1/get_rgbd"

        #camera_url = "http://192.168.1.105:9000/api/v1/get_rgbd" # Debug IS2.0 机器地址

        logger.info(f"获取相机数据，URL: {camera_url}")
        
        # 服务器端请求相机数据（避免CORS问题）
        response = requests.get(camera_url, timeout=5)
        
        if response.status_code == 200:
            camera_data = response.json()
            
            # 根据实际JSON格式提取数据
            # 格式: {"code": 200, "data": {"depth_content": "...", "rgb_content": "..."}, "message": "success", "timestamp": "..."}
            code = camera_data.get("code")
            message = camera_data.get("message", "")
            timestamp = camera_data.get("timestamp", "")
            data_obj = camera_data.get("data", {})
            
            if code == 200 and data_obj:
                # 提取RGB图像数据
                rgb_content = data_obj.get("rgb_content")
                depth_content = data_obj.get("depth_content")  # 深度数据，可选
                
                if rgb_content:
                    return jsonify({
                        "success": True,
                        "data": rgb_content,  # base64编码的RGB图像数据
                        "depth_data": depth_content,  # 深度数据（可选）
                        "timestamp": timestamp,
                        "message": "相机RGB数据获取成功",
                        "camera_message": message
                    })
                else:
                    return jsonify({
                        "success": False, 
                        "message": "相机返回数据中缺少RGB图像数据(rgb_content字段为空)"
                    })
            else:
                return jsonify({
                    "success": False, 
                    "message": f"相机API返回错误代码: {code}, 消息: {message}"
                })
        else:
            return jsonify({
                "success": False, 
                "message": f"相机API返回错误: HTTP {response.status_code}"
            })
            
    except requests.exceptions.Timeout:
        return jsonify({
            "success": False, 
            "message": "相机API请求超时，请检查网络连接"
        }), 500
    except requests.exceptions.ConnectionError:
        return jsonify({
            "success": False, 
            "message": "无法连接到相机服务，请检查相机是否正常运行"
        }), 500
    except Exception as e:
        logger.error(f"获取相机数据异常: {e}")
        return jsonify({
            "success": False, 
            "message": f"获取相机数据失败: {str(e)}"
        }), 500


# ==================== 雷达状态接口 ====================

@app.route('/api/robot/lidar/subscribe', methods=['POST'])
def subscribe_lidar_status():
    """订阅雷达状态"""
    if not websocket_client:
        return jsonify({'success': False, 'message': '未连接到WebSocket'}), 400
    
    try:
        global lidar_status
        
        if lidar_status['subscribed']:
            return jsonify({
                'success': True,
                'message': '雷达状态已订阅'
            })
        
        # 获取机器人名称并格式化为topic格式
        if not grpc_client:
            return jsonify({'success': False, 'message': '未连接到GRPC客户端，无法获取机器人名称'}), 400
        
        result = safe_grpc_call(grpc_client.get_robot_name, timeout=2.0)
        if not result or len(result) < 2 or not result[0]:
            return jsonify({'success': False, 'message': '无法获取机器人名称'}), 400
        
        robot_name = result[1]  # 格式如 "Bot IS 024"
        logger.info(f"获取到机器人名称: {robot_name}")
        
        # 将机器人名称转换为topic格式: "Bot IS 024" -> "IS20_024"
        # 提取 "IS 024" 部分并转换为 "IS20_024"
        if "IS " in robot_name:
            # 提取 "IS " 后面的部分
            is_part = robot_name.split("IS ")[1].strip()  # "024"
            formatted_name = f"IS20_{is_part}"  # "IS20_024"
        else:
            # 如果格式不符合预期，使用原始名称但去掉空格
            formatted_name = robot_name.replace(" ", "_")
        
        topic = f"/{formatted_name}/system/lidar_state"
        logger.info(f"构建雷达状态topic: {topic}")
        
        # 注册雷达数据订阅
        future = asyncio.run_coroutine_threadsafe(
            websocket_client.register_module_data(
                topic=topic,
                topic_type="rms_msgs__LidarState", 
                callback=on_lidar_data
            ), websocket_loop
        )
        success = future.result(timeout=5)
        
        if success:
            lidar_status['subscribed'] = True
            lidar_status['topic'] = topic  # 保存使用的topic
            logger.info(f"雷达状态订阅成功: {topic}")
            return jsonify({
                'success': True,
                'message': f'雷达状态订阅成功: {topic}'
            })
        else:
            logger.error(f"雷达状态订阅失败: {topic}")
            return jsonify({
                'success': False,
                'message': f'雷达状态订阅失败: {topic}'
            })
            
    except Exception as e:
        logger.error(f"订阅雷达状态异常: {e}")
        return jsonify({'success': False, 'message': str(e)}), 500


@app.route('/api/robot/lidar/unsubscribe', methods=['POST'])
def unsubscribe_lidar_status():
    """取消订阅雷达状态"""
    if not websocket_client:
        return jsonify({'success': False, 'message': '未连接到WebSocket'}), 400
    
    try:
        global lidar_status
        
        if not lidar_status['subscribed']:
            return jsonify({
                'success': True,
                'message': '雷达状态未订阅'
            })
        
        # 获取之前订阅时使用的topic
        topic = lidar_status.get('topic', '/system/lidar_state')  # 使用保存的topic，如果没有则使用默认值
        logger.info(f"取消订阅雷达状态topic: {topic}")
        
        # 取消雷达数据订阅
        future = asyncio.run_coroutine_threadsafe(
            websocket_client.unregister_module_data(topic), websocket_loop
        )
        success = future.result(timeout=5)
        
        if success:
            lidar_status['subscribed'] = False
            lidar_status['topic'] = None  # 清除保存的topic
            logger.info(f"雷达状态取消订阅成功: {topic}")
            return jsonify({
                'success': True,
                'message': f'雷达状态取消订阅成功: {topic}'
            })
        else:
            logger.error(f"雷达状态取消订阅失败: {topic}")
            return jsonify({
                'success': False,
                'message': f'雷达状态取消订阅失败: {topic}'
            })
            
    except Exception as e:
        logger.error(f"取消订阅雷达状态异常: {e}")
        return jsonify({'success': False, 'message': str(e)}), 500


@app.route('/api/robot/lidar_status', methods=['GET'])
def get_lidar_status():
    """获取雷达状态"""
    try:
        global lidar_status
        
        # 状态映射
        state_names = {0: 'UNKNOWN', 1: 'NORMAL', 2: 'LOWFREQ', 3: 'DISCONNECT'}
        state_name = state_names.get(lidar_status['state'], f'UNKNOWN({lidar_status["state"]})')
        
        # 计算数据新鲜度
        time_since_update = time.time() - lidar_status['last_update'] if lidar_status['last_update'] > 0 else 999
        is_fresh = time_since_update < 10  # 10秒内的数据认为是新鲜的
        
        return jsonify({
            'success': True,
            'message': '获取雷达状态成功',
            'data': {
                'state': lidar_status['state'],
                'state_name': state_name,
                'subscribed': lidar_status['subscribed'],
                'topic': lidar_status.get('topic', None),
                'timestamp': lidar_status['timestamp'],
                'last_update': lidar_status['last_update'],
                'time_since_update': time_since_update,
                'is_fresh': is_fresh,
                'is_normal': lidar_status['state'] == 1,  # 状态为1表示正常
                'description': {
                    0: '状态未知',
                    1: '正常工作',
                    2: '频率过低',
                    3: '连接断开'
                }.get(lidar_status['state'], '未知状态')
            }
        })
        
    except Exception as e:
        logger.error(f"获取雷达状态异常: {e}")
        return jsonify({'success': False, 'message': str(e)}), 500


# ==================== 气体传感器接口 ====================

@app.route('/api/robot/environment_sensor_status', methods=['GET'])
def get_environment_sensor_status():
    """获取气体传感器状态"""
    try:
        global environment_sensor_status
        
        # 计算数据新鲜度
        time_since_update = time.time() - environment_sensor_status['last_update'] if environment_sensor_status['last_update'] > 0 else 999
        is_fresh = time_since_update < 10  # 10秒内的数据认为是新鲜的
        
        # 格式化传感器数据
        sensors_list = []
        for sensor_type, sensor_data in environment_sensor_status['sensors'].items():
            sensors_list.append({
                'type': sensor_type,
                'name': sensor_data['name'],
                'value': sensor_data['value'],
                'unit': sensor_data['unit'],
                'timestamp': sensor_data['timestamp']
            })
        
        return jsonify({
            'success': True,
            'message': '获取气体传感器状态成功',
            'data': {
                'sensors': sensors_list,
                'sensor_count': len(sensors_list),
                'subscribed': environment_sensor_status['subscribed'],
                'topic': environment_sensor_status['topic'],
                'timestamp': environment_sensor_status['timestamp'],
                'last_update': environment_sensor_status['last_update'],
                'time_since_update': time_since_update,
                'is_fresh': is_fresh
            }
        })
        
    except Exception as e:
        logger.error(f"获取气体传感器状态异常: {e}")
        return jsonify({'success': False, 'message': str(e)}), 500


# ==================== 机器人控制接口 ====================

@app.route('/api/robot/posture', methods=['POST'])
def set_posture():
    """控制机器人姿态（站立/趴下）- 直接调用，不做状态判断"""
    if not grpc_client:
        return jsonify({'success': False, 'message': '未连接到机器人'}), 400
    
    try:
        data = request.get_json()
        stand_up = data.get('stand_up', True)
        
        # 使用安全调用避免阻塞
        result = safe_grpc_call(grpc_client.lie_down_or_stand_up, stand_up, timeout=5.0)
        success = result if result is not None else False
        
        return jsonify({
            'success': success, 
            'message': f"机器人{'站立' if stand_up else '趴下'}{'成功' if success else '失败'}"
        })
    except Exception as e:
        return jsonify({'success': False, 'message': str(e)}), 500


@app.route('/api/robot/driver', methods=['POST'])
def set_driver_enable():
    """启用/禁用驱动器"""
    if not grpc_client:
        return jsonify({'success': False, 'message': '未连接到机器人'}), 400
    
    try:
        data = request.get_json()
        enable = data.get('enable', True)
        
        success = grpc_client.driver_enable(enable)
        return jsonify({
            'success': success, 
            'message': f"驱动器{'启用' if enable else '禁用'}{'成功' if success else '失败'}"
        })
    except Exception as e:
        return jsonify({'success': False, 'message': str(e)}), 500


@app.route('/api/robot/light', methods=['POST'])
def set_light_style():
    """设置灯光样式"""
    if not grpc_client:
        return jsonify({'success': False, 'message': '未连接到机器人'}), 400
    
    try:
        data = request.get_json()
        style = data.get('style', 0)
        
        success = grpc_client.set_rps_light_style(style)
        return jsonify({
            'success': success, 
            'message': f"设置灯光样式 {style} {'成功' if success else '失败'}"
        })
    except Exception as e:
        return jsonify({'success': False, 'message': str(e)}), 500


# ==================== 机器人运动控制接口 ====================

@app.route('/api/robot/velocity', methods=['POST'])
def set_robot_velocity():
    """控制机器人速度（线速度和角速度）"""
    if not grpc_client:
        return jsonify({'success': False, 'message': '未连接到机器人'}), 400
    
    try:
        data = request.get_json()
        linear_x = data.get('linear_x', 0.0)  # 前进/后退速度 (m/s)
        linear_y = data.get('linear_y', 0.0)  # 左右移动速度 (m/s)
        angular_z = data.get('angular_z', 0.0)  # 旋转速度 (rad/s)
        
        # 调用SDK的速度控制接口，支持全方向运动
        success = grpc_client.direction_movement(linear_x=linear_x, linear_y=linear_y, angular_z=angular_z)
        
        if success:
            if linear_x == 0 and linear_y == 0 and angular_z == 0:
                action_desc = "停止运动"
            else:
                action_desc = f"运动控制: X={linear_x:.2f}m/s, Y={linear_y:.2f}m/s, Z={angular_z:.2f}rad/s"
            
            return jsonify({
                'success': True, 
                'message': f"{action_desc} 执行成功"
            })
        else:
            return jsonify({
                'success': False, 
                'message': '速度控制命令发送失败'
            })
    except Exception as e:
        return jsonify({'success': False, 'message': str(e)}), 500


@app.route('/api/robot/height', methods=['POST'])
def set_robot_height():
    """调整机器人身体高度"""
    if not grpc_client:
        return jsonify({'success': False, 'message': '未连接到机器人'}), 400
    
    try:
        data = request.get_json()
        height_percent = data.get('height', 50)  # 高度百分比 0-100
        
        # 验证百分比范围
        if height_percent < 0 or height_percent > 100:
            return jsonify({
                'success': False, 
                'message': f"高度百分比必须在0-100范围内，当前值: {height_percent}"
            })
        
        # 将百分比转换为实际高度范围 25-45 厘米
        # 公式: actual_height_cm = 25 + (height_percent / 100) * (45 - 25)
        min_height_cm = 25  # 最低高度 25厘米
        max_height_cm = 45  # 最高高度 45厘米
        actual_height_cm = min_height_cm + (height_percent / 100.0) * (max_height_cm - min_height_cm)
        actual_height_cm = int(round(actual_height_cm))  # 转换为整数厘米
        
        logger.info(f"高度调整: 百分比{height_percent}% -> 实际高度{actual_height_cm}厘米")
        
        # 调用SDK的高度调整接口 - 传入整数厘米值
        success = grpc_client.body_height_adjust(actual_height_cm)
        return jsonify({
            'success': success, 
            'message': f"身体高度调整为{height_percent}% (实际高度: {actual_height_cm}厘米) ({'成功' if success else '失败'})"
        })
    except Exception as e:
        return jsonify({'success': False, 'message': str(e)}), 500


@app.route('/api/robot/speed_limit', methods=['POST'])
def set_robot_speed_limit():
    """设置机器人移动速度限制"""
    if not grpc_client:
        return jsonify({'success': False, 'message': '未连接到机器人'}), 400
    
    try:
        data = request.get_json()
        speed_percent = data.get('speed', 50)  # 速度百分比 0-100
        
        # 调用SDK的速度调整接口 - 使用真实的方法名
        success = grpc_client.speed_adjust(speed_percent)
        return jsonify({
            'success': success, 
            'message': f"移动速度限制设为{speed_percent}% ({'成功' if success else '失败'})"
        })
    except Exception as e:
        return jsonify({'success': False, 'message': str(e)}), 500


@app.route('/api/robot/gait', methods=['POST'])
def set_robot_gait():
    """设置机器人步态模式"""
    if not grpc_client:
        return jsonify({'success': False, 'message': '未连接到机器人'}), 400
    
    try:
        data = request.get_json()
        gait_mode = data.get('gait', 'walk')
        
        # 步态类型映射到场景类型（基于SceneType枚举）
        gait_map = {
            'walk': 2,      # 行走 -> walking = 2
            'rl': 9,        # RL -> rl_walk = 9
            'slope': 8,     # 斜坡 -> stone = 8 (石头/斜坡地形)
            'stairs': 3     # 楼梯 -> stairs = 3
        }
        
        scene_type = gait_map.get(gait_mode, 2)  # 默认步行模式
        success = grpc_client.set_scene(scene_type)
        
        gait_names = {
            'walk': '行走',
            'rl': 'RL',
            'slope': '斜坡',
            'stairs': '楼梯'
        }
            
        return jsonify({
            'success': success, 
            'message': f"步态设置为{gait_names[gait_mode]}模式 ({'成功' if success else '失败'})"
        })
    except Exception as e:
        return jsonify({'success': False, 'message': str(e)}), 500


@app.route('/api/robot/obstacle_avoidance', methods=['POST'])
def set_obstacle_avoidance():
    """设置机器人停障功能"""
    if not grpc_client:
        return jsonify({'success': False, 'message': '未连接到机器人'}), 400
    
    try:
        data = request.get_json()
        enable = data.get('enable', True)
        
        success = grpc_client.set_guardian_switch(enable)
        
        return jsonify({
            'success': success, 
            'message': f"停障功能{'启动' if enable else '关闭'} ({'成功' if success else '失败'})"
        })
    except Exception as e:
        return jsonify({'success': False, 'message': str(e)}), 500


@app.route('/api/robot/action', methods=['POST'])
def perform_robot_action():
    """执行机器人动作演示"""
    if not grpc_client:
        return jsonify({'success': False, 'message': '未连接到机器人'}), 400
    
    try:
        data = request.get_json()
        action = data.get('action', 'greet')
        
        # 动作类型映射到演示模式
        action_map = {
            'greet': 1,      # 打招呼
            'shake_head': 2, # 摇头
            'wag_tail': 3    # 摆尾
        }
        
        mode = action_map.get(action, 1)
        success = grpc_client.demo_control(mode, True)  # 启用对应的演示模式
        
        action_names = {
            'greet': '打招呼',
            'shake_head': '摇头',
            'wag_tail': '摆尾'
        }
            
        return jsonify({
            'success': success, 
            'message': f"{action_names[action]}动作 ({'执行成功' if success else '执行失败'})"
        })
    except Exception as e:
        return jsonify({'success': False, 'message': str(e)}), 500


@app.route('/api/test', methods=['GET'])
def test_endpoint():
    """测试端点，用于验证服务器连接"""
    print("\n" + "=" * 80)
    print("🎯🎯🎯 [TEST ENDPOINT] 测试端点被访问! 🎯🎯🎯")
    print("=" * 80 + "\n")
    logger.info("AddLog: 测试端点被访问")
    return jsonify({'success': True, 'message': '测试成功', 'timestamp': time.time()})


@app.route('/api/python/validate', methods=['POST'])
def validate_python_code():
    """校验传入的Python代码语法，返回是否通过与错误定位信息"""
    try:
        data = request.get_json(silent=True) or {}
        code = data.get('code', '')
        if not isinstance(code, str):
            return jsonify({'success': False, 'message': 'code必须为字符串'}), 400
        if code.strip() == '':
            return jsonify({'success': False, 'message': '代码不能为空', 'error': {'msg': 'empty code'}})
        import ast
        try:
            ast.parse(code)
            return jsonify({'success': True, 'message': '语法检查通过'})
        except SyntaxError as e:
            err = {
                'msg': e.msg,
                'lineno': getattr(e, 'lineno', None),
                'col': getattr(e, 'offset', None),
                'text': getattr(e, 'text', None)
            }
            return jsonify({'success': False, 'message': '语法错误', 'error': err})
    except Exception as e:
        logger.exception('validate_python_code 异常')
        return jsonify({'success': False, 'message': f'校验异常: {str(e)}'}), 500


# ==================== 上/退桩充电接口 ====================

@app.route('/api/robot/enter_charge', methods=['POST'])
def api_enter_charge():
    """上桩充电（简化版）：
    - 仅调用一次 enter_charge（以 nav_control 来源）启动上桩流程
    - 随后通过读取 common_status 的 position_state 与 charge_switch_state 轮询确认
    - 结果以最终的 pos_state 与 switch_state 判定是否上桩成功（不再分别返回 ok_find/ok_dock）
    字段含义约定（默认值与旧逻辑保持一致，支持覆盖）：
      ChargePilePositionState: 1=进行中, 2=查找/定位成功, 3=失败
      ChargePileSwitchState:   1=进行中, 2=对接成功,   3=失败
    超时：使用 find_timeout_s + dock_timeout_s 的总和作为整体超时（可覆盖）。
    """
    if not grpc_client:
        return jsonify({'success': False, 'message': '未连接到机器人'}), 400

    try:
        body = request.get_json(silent=True) or {}
        pos_success = int(body.get('position_success_value', 2))
        pos_fail = int(body.get('position_fail_value', 3))
        switch_success = int(body.get('switch_success_value', 4))
        switch_fail = int(body.get('switch_fail_value', 6))
        find_timeout = float(body.get('find_timeout_s', 45.0))
        dock_timeout = float(body.get('dock_timeout_s', 60.0))
        total_timeout = float(body.get('total_timeout_s', find_timeout + dock_timeout))

        logger.info('[CHARGE] enter_charge(nav_control): 启动上桩流程')
        trig = _enter_charge_with_source_nav()
        logger.info(f'[CHARGE] enter_charge(nav_control) 返回: {trig}')

        # 合并轮询：在总超时时间内反复读取两个状态，依据最终 switch_state 判定成功/失败
        start_t = time.time()
        pos_state = _read_common_status_attr(['charge_pile_position_state', 'ChargePilePositionState'])
        switch_state = _read_common_status_attr(['charge_pile_switch_state', 'ChargePileSwitchState'])
        success_flag = False
        timed_out = False
        while True:
            now = time.time()
            if now - start_t >= total_timeout:
                timed_out = True
                break
            # 更新最新状态
            cur_pos = _read_common_status_attr(['charge_pile_position_state', 'ChargePilePositionState'])
            cur_sw = _read_common_status_attr(['charge_pile_switch_state', 'ChargePileSwitchState'])
            if cur_pos is not None:
                pos_state = cur_pos
            if cur_sw is not None:
                switch_state = cur_sw
            

            # 以开关/对接状态为准判断成败；位置状态仅用于辅助展示
            if switch_state == switch_success:
                success_flag = True
                break
            if switch_state == switch_fail:
                success_flag = False
                break
            time.sleep(0.5)

            # 打印合并轮询中的当前状态
            logger.info(f"[CHARGE] 轮询状态: position_state={pos_state}, switch_state={switch_state}, success_flag={success_flag}")

        elapsed_total = time.time() - start_t
        msg = '上桩成功' if success_flag else ('上桩超时' if timed_out else '上桩失败')
        return jsonify({
            'success': bool(success_flag),
            'position_state': pos_state,
            'switch_state': switch_state,
            'elapsed': elapsed_total,
            'message': msg
        })

    except Exception as e:
        logger.exception('[CHARGE] enter_charge 处理异常')
        return jsonify({'success': False, 'message': f'enter_charge 异常: {str(e)}'}), 500


@app.route('/api/robot/leave_charge', methods=['POST'])
def api_leave_charge():
    """退桩接口：以 nav_control 模式调用 SDK leave_charge，并轮询开关状态确认结果。
    通过 switch_state 判断：1=成功，3=失败。
    """
    if not grpc_client:
        return jsonify({'success': False, 'message': '未连接到机器人'}), 400
    try:
        body = request.get_json(silent=True) or {}
        wait_release = bool(body.get('wait_release', False))
        release_timeout = float(body.get('release_timeout_s', 20.0))

        logger.info('[CHARGE] leave_charge: 退桩 (nav_control 模式)')
        # 使用 nav_control 模式调用退桩
        _leave_charge_with_source_nav()

        if wait_release:
            # 轮询确认退桩是否完成（开关状态：1=成功，3=失败）
            ok_release, last_val, elapsed = _poll_common_status_state(
                ['charge_pile_switch_state', 'ChargePileSwitchState'],
                success_value=1,  # 退桩成功时 switch_state = 1
                fail_value=3,     # 退桩失败时 switch_state = 3
                timeout_s=release_timeout, interval_s=0.5
            )
            success_flag = (last_val == 1)
            return jsonify({
                'success': success_flag,
                'message': '退桩成功' if success_flag else ('退桩失败' if last_val == 3 else '退桩超时'),
                'switch_state': last_val,
                'elapsed_release': elapsed
            })

        # 不等待确认，立即读取当前状态
        switch_state = _read_common_status_attr(['charge_pile_switch_state', 'ChargePileSwitchState'])
        success_flag = (switch_state == 1)
        
        return jsonify({
            'success': success_flag,
            'message': '退桩命令已发送' if switch_state is None else ('退桩成功' if success_flag else '退桩失败'),
            'switch_state': switch_state
        })
    except Exception as e:
        logger.exception('[CHARGE] leave_charge 处理异常')
        return jsonify({'success': False, 'message': f'leave_charge 异常: {str(e)}'}), 500

@app.route('/api/robot/call_method', methods=['POST'])
def call_method():
    """调用机器人SDK的call_method方法"""
    if not grpc_client:
        return jsonify({'success': False, 'message': '未连接到机器人'}), 400
    
    try:
        data = request.get_json()
        method_name = data.get('method_name')
        script_name = data.get('script_name')
        request_id = data.get('request_id', 'default-request')
        
        if not method_name:
            return jsonify({'success': False, 'message': '缺少method_name参数'}), 400
        
        # 特殊处理标零方法
        if method_name == "ReCalibration":
            return handle_recalibration(data)
        
        if not script_name:
            return jsonify({'success': False, 'message': '缺少script_name参数'}), 400
        
        logger.info(f"调用方法: {method_name}, 脚本: {script_name}, 请求ID: {request_id}")
        
        # 调用SDK的call_method方法
        result = grpc_client.call_method(method_name, script_name=script_name, request_id=request_id)
        
        # 打印返回值详细信息
        print(f"\n{'='*60}")
        print(f"🔍 [CALL_METHOD RESULT] 方法调用返回值详情:")
        print(f"📋 方法名: {method_name}")
        print(f"📋 脚本名: {script_name}")
        print(f"📋 请求ID: {request_id}")
        print(f"📋 返回值类型: {type(result)}")
        print(f"📋 返回值内容: {result}")
        print(f"{'='*60}\n")
        
        logger.info(f"call_method返回值: 类型={type(result)}, 内容={result}")
        
        # 解析result中的嵌套JSON结构
        success = False
        exit_code = None
        parsed_result = None
        script_success = False
        
        try:
            if isinstance(result, dict):
                # result格式: {'success': True, 'result': '{"exit_code":0,...}', ...}
                api_success = result.get('success', False)
                
                if api_success:
                    # 解析result中的'result'字段（JSON字符串）
                    result_json_str = result.get('result', '{}')
                    if isinstance(result_json_str, str):
                        import json
                        parsed_result = json.loads(result_json_str)
                        exit_code = parsed_result.get('exit_code')
                        script_success = parsed_result.get('success', False)
                        
                        print(f"🔍 [PARSED RESULT] 解析后的脚本执行结果:")
                        print(f"📋 脚本执行成功: {script_success}")
                        print(f"📋 退出代码: {exit_code}")
                        print(f"📋 脚本名称: {parsed_result.get('script_name', 'unknown')}")
                        print(f"📋 正常退出: {parsed_result.get('exited_normally', False)}")
                        
                        # 根据exit_code判断最终成功状态
                        success = api_success and script_success and (exit_code == 0)
                    else:
                        success = api_success
                else:
                    success = api_success
            elif isinstance(result, tuple) and len(result) >= 2:
                # 兼容旧格式: (success, result_dict)
                api_success, result_dict = result[0], result[1]
                
                if api_success and isinstance(result_dict, dict):
                    # 解析result_dict中的'result'字段（JSON字符串）
                    result_json_str = result_dict.get('result', '{}')
                    if isinstance(result_json_str, str):
                        import json
                        parsed_result = json.loads(result_json_str)
                        exit_code = parsed_result.get('exit_code')
                        script_success = parsed_result.get('success', False)
                        
                        print(f"🔍 [PARSED RESULT] 解析后的脚本执行结果:")
                        print(f"📋 脚本执行成功: {script_success}")
                        print(f"📋 退出代码: {exit_code}")
                        print(f"📋 脚本名称: {parsed_result.get('script_name', 'unknown')}")
                        print(f"📋 正常退出: {parsed_result.get('exited_normally', False)}")
                        
                        # 根据exit_code判断最终成功状态
                        success = api_success and script_success and (exit_code == 0)
                    else:
                        success = api_success
                else:
                    success = api_success
            elif isinstance(result, bool):
                success = result
            else:
                success = bool(result)
                
        except Exception as parse_error:
            logger.error(f"解析result异常: {parse_error}")
            print(f"❌ [PARSE ERROR] 解析结果时出错: {parse_error}")
            success = False
        
        return jsonify({
            'success': success, 
            'exit_code': exit_code,
            'script_success': script_success,
            'parsed_result': parsed_result,
            'raw_result': result,  # 保留原始返回值
            'message': f"方法调用{'成功' if success else '失败'}: {method_name}({script_name}), 退出代码: {exit_code}"
        })
        
    except Exception as e:
        logger.error(f"call_method异常: {e}")
        return jsonify({'success': False, 'message': str(e)}), 500


def handle_recalibration(data):
    """处理标零(重新校准)请求"""
    if not grpc_client:
        return jsonify({'success': False, 'message': '未连接到机器人'}), 400
    
    try:
        # 从请求中获取参数
        enable = data.get('enable', True)  # 默认启用校准
        source = data.get('source', 2)  # 默认API控制源(0:JOY, 1:NAV, 2:API)
        request_id = data.get('request_id', 'recalibration-request')
        
        logger.info(f"执行标零校准: enable={enable}, source={source}, request_id={request_id}")
        
        # 调用SDK的重新校准方法
        result = safe_grpc_call(grpc_client.re_calibrate_all_driver_and_save, enable, source, timeout=30.0)
        
        if result is not None:
            logger.info(f"标零校准命令发送结果: {result}")
            
            # 获取当前的标定状态
            calibrate_state = 0  # 默认未知状态
            
            try:
                # 直接调用get_common_status，它返回(success, status)格式
                success_flag, status_obj = grpc_client.get_common_status()
                logger.info(f"get_common_status调用结果: success={success_flag}, status类型={type(status_obj)}")
                
                if success_flag and status_obj and hasattr(status_obj, 'calibrate_pos_state'):
                    calibrate_state = status_obj.calibrate_pos_state
                    logger.info(f"获取到标定状态: {calibrate_state}")
                else:
                    logger.warning(f"获取标定状态失败: success={success_flag}, status有效={status_obj is not None}")
                    
            except Exception as e:
                logger.error(f"获取common_status时发生异常: {e}")
                logger.error(f"异常详情: {e.__class__.__name__}: {str(e)}")
            
            # 根据calibrate_pos_state判断标定结果
            # 1: 正在标定, 2: 标定成功, 3: 标定失败
            if calibrate_state == 1:
                success = True
                message = "标定命令已发送，正在进行标定"
                status_text = "进行中"
            elif calibrate_state == 2:
                success = True
                message = "标定成功"
                status_text = "成功"
            elif calibrate_state == 3:
                success = False
                message = "标定失败"
                status_text = "失败"
            else:
                success = bool(result)
                message = f"标零校准命令发送{'成功' if result else '失败'}，状态未知"
                status_text = "未知"
            
            logger.info(f"标零校准最终结果: success={success}, calibrate_state={calibrate_state}, message={message}")
            
            return jsonify({
                'success': success,
                'message': message,
                'data': {
                    'enable': enable,
                    'source': source,
                    'command_result': result,
                    'calibrate_pos_state': calibrate_state,
                    'calibrate_status': status_text
                }
            })
        else:
            logger.error("标零校准调用超时或失败")
            return jsonify({
                'success': False,
                'message': '标零校准调用超时或失败'
            })
            
    except Exception as e:
        logger.error(f"标零校准异常: {e}")
        return jsonify({'success': False, 'message': f'标零校准异常: {str(e)}'}), 500

@app.route('/api/robot/control_mode', methods=['POST'])
def set_control_mode():
    """设置机器人控制模式（传参为整型枚举值）。
    请求JSON示例: {"mode": 2}
    SDK枚举: mcs_service_pb2.NavOrJoy
      nall_control = 0, nav_control = 1, joy_control = 2, uwb_control = 3
    实际调用: RobotClient.set_nav_or_joy_control(mode:int)
    """
    if not grpc_client:
        return jsonify({'success': False, 'message': '未连接到机器人'}), 400
    try:
        data = request.get_json(silent=True) or {}
        if 'mode' not in data:
            return jsonify({'success': False, 'message': '缺少必需参数: mode(int)'}), 400

        raw_mode = data.get('mode')
        if not isinstance(raw_mode, int):
            return jsonify({'success': False, 'message': 'mode 必须为整数(0..3)'}), 400

        enum_value = raw_mode
        if enum_value < 0 or enum_value > 3:
            return jsonify({'success': False, 'message': f'不支持的mode取值: {enum_value}，有效范围: 0..3'}), 400

        rev_map = {
            getattr(mcs_service_pb2.NavOrJoy, 'nall_control', 0): 'null',
            mcs_service_pb2.NavOrJoy.nav_control: 'nav',
            mcs_service_pb2.NavOrJoy.joy_control: 'joy',
            mcs_service_pb2.NavOrJoy.uwb_control: 'uwb'
        }
        enum_name = rev_map.get(enum_value, f'unknown({enum_value})')

        logger.info(f"设置控制模式: {enum_name}({enum_value})")
        result = safe_grpc_call(grpc_client.set_nav_or_joy_control, enum_value, timeout=5.0)
        success = bool(result)
        return jsonify({
            'success': success,
            'message': f"设置控制模式为 {enum_name}({enum_value}) {'成功' if success else '失败'}",
            'mode': enum_value,
            'mode_name': enum_name
        })
    except Exception as e:
        logger.exception('设置控制模式异常')
        return jsonify({'success': False, 'message': f'设置控制模式异常: {str(e)}'}), 500

@app.route('/api/robot/common_status', methods=['GET'])
def get_common_status():
    """获取机器人综合状态信息"""
    # 添加最明显的HTTP请求接收标识
    # print("\n" + "=" * 80)
    print("�🚨🚨 [HTTP REQUEST RECEIVED] /api/robot/common_status 接口被调用! 🚨🚨🚨")
    # print("=" * 80 + "\n")
    # logger.info("AddLog: HTTP请求已接收 - /api/robot/common_status")
    # print("⭐" * 50)
    # print("⭐ 后端接收到获取系统状态的请求!")
    # print("⭐" * 50)
    
    if not grpc_client:
        logger.error('AddLog: grpc_client为None，未连接到机器人')
        return jsonify({'success': False, 'message': '未连接到机器人'}), 400
    
    # 记录请求日志
    # logger.info('AddLog: get_common_status接口被调用，grpc_client状态正常')
    # logger.info(f'AddLog: grpc_client类型: {type(grpc_client)}')
    # 初始化默认值
    battery_level = 0
    battery_current = 0.0
    battery_power = 0.0
    charging_status = False
    estop_status = False
    battery_low = False
    error_code = None
    # 运控默认对象
    class DummyStatus:
        driver_enable_state = 0
        belie_or_stand = 0
        guardian_switch = False
        joy_mode = 2
        cur_speed = 0.0
        cur_height = 0.0
        max_speed = 0.0
        max_height = 0.0
        cur_scene = 2
        emergency = False
        error_code = None
        uwb_state = ''
        net_time_gap = 0
        battery_level = 0.0
        calibrate_pos_state = None
        charge_pile_position_state = None  # 充电桩定位状态
        charge_pile_switch_state = None    # 充电开关接触状态
    common_status = DummyStatus()
    # 运控状态
    try:
        # logger.info('AddLog: 开始调用grpc_client.get_common_status()')
        success, status = grpc_client.get_common_status()
        # logger.info(f'AddLog: get_common_status调用完成，success={success}, status类型={type(status)}')
        if success and status:
            common_status = status
            logger.info(f'AddLog: get_common_status成功，获取到状态数据: driver_enable_state={status.driver_enable_state}, 获取标零状态={status.calibrate_pos_state}')
            # 兼容新结构：如果状态里有 charge_status，则将嵌套值映射到旧字段上（便于前端/轮询代码复用）
            try:
                cs = getattr(status, 'charge_status', None)
                if cs is not None:
                    # 标零状态若有则保留已有，否则尝试从 charge/battery info 中推断（此处不推断，只做兼容）
                    if hasattr(cs, 'position_state'):
                        setattr(common_status, 'charge_pile_position_state', getattr(cs, 'position_state'))
                    if hasattr(cs, 'charge_switch_state'):
                        setattr(common_status, 'charge_pile_switch_state', getattr(cs, 'charge_switch_state'))
            except Exception:
                pass
        else:
            logger.warning(f'AddLog: get_common_status失败，success={success}, status={status}，使用默认值')
    except Exception as e:
        logger.error(f'AddLog: get_common_status异常: {e}')
    if True:
        # 电池相关
        try:
            # logger.info('AddLog: 开始调用grpc_client.rps_get_battery_level()')
            success, level, _ = grpc_client.rps_get_battery_level()
            # logger.info(f'AddLog: rps_get_battery_level调用完成，success={success}, level={level}')
            if success:
                battery_level = level
        except Exception as e:
            logger.warning(f'AddLog: 获取电池电量失败: {e}')
        try:
            # logger.info('AddLog: 开始调用grpc_client.rps_get_battery_current()')
            success, current, _ = grpc_client.rps_get_battery_current()
            # logger.info(f'AddLog: rps_get_battery_current调用完成，success={success}, current={current}')
            if success:
                battery_current = round(current, 2)
        except Exception as e:
            logger.warning(f'AddLog: 获取电池电流失败: {e}')
        try:
            success, power, _ = grpc_client.rps_get_battery_output_power()
            if success:
                battery_power = round(power, 1)
        except Exception as e:
            logger.warning(f'获取电池功率失败: {e}')
        try:
            success, charging, _ = grpc_client.rps_get_charging_status()
            if success:
                charging_status = charging
        except Exception as e:

            logger.warning(f'获取充电状态失败: {e}')
        try:
            success, estop, _ = grpc_client.rps_get_estop_state()
            if success:
                estop_status = estop
        except Exception as e:
            logger.warning(f'获取急停状态失败: {e}')
        try:
            success, low, _ = grpc_client.rps_check_battery_low()
            if success:
                battery_low = low
        except Exception as e:
            logger.warning(f'检查低电量状态失败: {e}')
    try:
        success, latest_error = grpc_client.get_latest_error()
        if success and latest_error:
            error_code = latest_error
    except Exception as e:
        logger.warning(f'获取错误信息失败: {e}')
    
    # 构建综合状态数据
    status_data = {
        'battery_level': battery_level,
        'current': battery_current,
        'power': battery_power,
        'charging': charging_status,
        'battery_low': battery_low,
        'estop': estop_status,
        'driver_enabled': common_status.driver_enable_state == 1,
        'posture': '站立' if common_status.belie_or_stand == 3 else '趴下',
        'control_mode': common_status.joy_mode,
        'guardian_switch': common_status.guardian_switch,
        'cur_speed': round(common_status.cur_speed, 2),
        'cur_height': round(common_status.cur_height, 2),
        'max_speed': round(common_status.max_speed, 2),
        'max_height': round(common_status.max_height, 2),
        'cur_scene': common_status.cur_scene,
        'emergency': common_status.emergency,
        'error_code': error_code or common_status.error_code,
        'net_time_gap': common_status.net_time_gap,
        'uwb_state': common_status.uwb_state,
        'calibrate_pos_state': getattr(common_status, 'calibrate_pos_state', None),
        # 兼容旧字段名：优先取映射后的旧字段，若没有则从嵌套charge_status读取
        'charge_pile_position_state': (
            getattr(common_status, 'charge_pile_position_state', None)
            if getattr(common_status, 'charge_pile_position_state', None) is not None
            else (getattr(getattr(common_status, 'charge_status', None), 'position_state', None) if hasattr(common_status, 'charge_status') else None)
        ),
        'charge_pile_switch_state': (
            getattr(common_status, 'charge_pile_switch_state', None)
            if getattr(common_status, 'charge_pile_switch_state', None) is not None
            else (getattr(getattr(common_status, 'charge_status', None), 'charge_switch_state', None) if hasattr(common_status, 'charge_status') else None)
        ),
        # 直接透传新结构，便于前端如需取更多充电相关字段
        'charge_status': {
            'charge_tile_name': getattr(getattr(common_status, 'charge_status', None), 'charge_tile_name', None) if hasattr(common_status, 'charge_status') else None,
            'position_state': getattr(getattr(common_status, 'charge_status', None), 'position_state', None) if hasattr(common_status, 'charge_status') else None,
            'charge_switch_state': getattr(getattr(common_status, 'charge_status', None), 'charge_switch_state', None) if hasattr(common_status, 'charge_status') else None,
            'charge_succeeded': getattr(getattr(common_status, 'charge_status', None), 'charge_succeeded', None) if hasattr(common_status, 'charge_status') else None,
        },
        'temperature': 35.0,
        'voltage': 24.0
    }
    # logger.info(f"AddLog: 获取系统状态: battery_level={battery_level}, driver_enabled={status_data['driver_enabled']}, posture={status_data['posture']}")
    return jsonify({'success': True, 'data': status_data})



# ==================== 云台控制接口 ====================

@app.route('/api/ptz/position', methods=['POST'])
def set_ptz_position():
    """设置云台位置"""
    if not websocket_client:
        return jsonify({'success': False, 'message': '未连接到WebSocket'}), 400
    
    try:
        data = request.get_json()
        logger.info(f"收到云台位置设置请求: {data}")
        
        position = PtzfPosition(
            pan=float(data.get('pan', 0.0)),
            tilt=float(data.get('tilt', 0.0)),
            zoom=float(data.get('zoom', 0.0)),
            focus=int(data.get('focus', 0))
        )
        relative = bool(data.get('relative', False))
        
        logger.info(f"设置云台位置: pan={position.pan}, tilt={position.tilt}, zoom={position.zoom}, focus={position.focus}, relative={relative}")
        
        future = asyncio.run_coroutine_threadsafe(
            websocket_client.set_ptzf_position(position, relative), websocket_loop
        )
        success = future.result(timeout=10)
        
        # 更新本地云台状态
        if success:
            global ptz_status
            if relative:
                # 相对移动，累加到当前位置
                ptz_status['pan'] += position.pan
                ptz_status['tilt'] += position.tilt
                ptz_status['zoom'] += position.zoom
                ptz_status['focus'] += position.focus
            else:
                # 绝对移动，直接设置位置
                ptz_status['pan'] = position.pan
                ptz_status['tilt'] = position.tilt
                ptz_status['zoom'] = position.zoom
                ptz_status['focus'] = position.focus
        
        message = f"设置云台位置{'成功' if success else '失败'}"
        if relative:
            message = f"相对{message}"
        else:
            message = f"绝对{message}"
            
        logger.info(f"云台位置设置结果: success={success}, message={message}")
        logger.info(f"更新后的云台状态: {ptz_status}")
        
        return jsonify({
            'success': success, 
            'message': message
        })
    except Exception as e:
        error_msg = f"设置云台位置异常: {str(e)}"
        logger.error(error_msg)
        return jsonify({'success': False, 'message': error_msg}), 500


@app.route('/api/ptz/move', methods=['POST'])
def move_ptz():
    """控制云台移动"""
    if not websocket_client:
        return jsonify({'success': False, 'message': '未连接到WebSocket'}), 400
    
    try:
        data = request.get_json()
        direction = data.get('direction')
        speed = data.get('speed', 0.5)
        
        if direction == 'pan_left':
            future = asyncio.run_coroutine_threadsafe(
                websocket_client.move_pan(speed), websocket_loop
            )
        elif direction == 'pan_right':
            future = asyncio.run_coroutine_threadsafe(
                websocket_client.move_pan(-speed), websocket_loop
            )
        elif direction == 'tilt_up':
            future = asyncio.run_coroutine_threadsafe(
                websocket_client.move_tilt(speed), websocket_loop
            )
        elif direction == 'tilt_down':
            future = asyncio.run_coroutine_threadsafe(
                websocket_client.move_tilt(-speed), websocket_loop
            )
        elif direction == 'zoom_in':
            future = asyncio.run_coroutine_threadsafe(
                websocket_client.move_zoom(speed), websocket_loop
            )
        elif direction == 'zoom_out':
            future = asyncio.run_coroutine_threadsafe(
                websocket_client.move_zoom(-speed), websocket_loop
            )
        elif direction == 'stop':
            future = asyncio.run_coroutine_threadsafe(
                websocket_client.stop_all_movement(), websocket_loop
            )
        else:
            return jsonify({'success': False, 'message': '无效的方向参数'}), 400
        
        future.result(timeout=3)
        return jsonify({'success': True, 'message': f'云台{direction}命令已发送'})
        
    except Exception as e:
        return jsonify({'success': False, 'message': str(e)}), 500


@app.route('/api/ptz/light', methods=['POST'])
def set_ptz_light():
    """控制云台灯光"""
    if not websocket_client:
        return jsonify({'success': False, 'message': '未连接到WebSocket'}), 400
    
    try:
        data = request.get_json()
        enable = data.get('enable', True)
        
        future = asyncio.run_coroutine_threadsafe(
            websocket_client.light_cmd(enable), websocket_loop
        )
        success = future.result(timeout=3)
        
        # 更新本地云台状态
        if success:
            global ptz_status
            ptz_status['light'] = enable
        
        return jsonify({
            'success': success, 
            'message': f"云台灯光{'开启' if enable else '关闭'}{'成功' if success else '失败'}"
        })
    except Exception as e:
        return jsonify({'success': False, 'message': str(e)}), 500


@app.route('/api/ptz/init', methods=['POST'])
def init_ptz():
    """初始化云台控制"""
    if not websocket_client:
        return jsonify({'success': False, 'message': '未连接到WebSocket'}), 400
    
    try:
        # 确保云台控制主题已发布
        future = asyncio.run_coroutine_threadsafe(
            websocket_client.advertise_ptzf_cmd_vel(), websocket_loop
        )
        success = future.result(timeout=5)
        
        if success:
            logger.info("云台初始化成功 - 控制主题已发布")
            return jsonify({
                'success': True, 
                'message': '云台初始化成功，现在可以进行控制操作'
            })
        else:
            logger.error("云台初始化失败 - 控制主题发布失败")
            return jsonify({
                'success': False, 
                'message': '云台初始化失败，无法发布控制主题'
            })
    except Exception as e:
        logger.error(f"云台初始化异常: {e}")
        return jsonify({'success': False, 'message': f'云台初始化异常: {str(e)}'}), 500


@app.route('/api/ptz/status', methods=['GET'])
def get_ptz_status():
    """获取云台状态"""
    if not websocket_client:
        return jsonify({'success': False, 'message': '未连接到WebSocket'}), 400
    
    try:
        global ptz_status
        # 返回当前云台状态
        return jsonify({
            'success': True,
            'message': '获取云台状态成功',
            'data': ptz_status.copy()
        })
    except Exception as e:
        logger.error(f"获取云台状态异常: {e}")
        return jsonify({'success': False, 'message': f'获取云台状态异常: {str(e)}'}), 500


@app.route('/api/ptz/reset', methods=['POST'])
def reset_ptz():
    """重置云台到初始位置"""
    if not websocket_client:
        return jsonify({'success': False, 'message': '未连接到WebSocket'}), 400
    
    try:
        # 重置到中心位置
        from daystar_sdk.websocket_client import PtzfPosition
        reset_position = PtzfPosition(pan=0.0, tilt=0.0, zoom=1.0, focus=50)
        
        future = asyncio.run_coroutine_threadsafe(
            websocket_client.set_ptzf_position(reset_position, relative=False), websocket_loop
        )
        success = future.result(timeout=10)
        
        # 更新本地云台状态
        if success:
            global ptz_status
            ptz_status['pan'] = 0.0
            ptz_status['tilt'] = 0.0
            ptz_status['zoom'] = 1.0
            ptz_status['focus'] = 50
        
        if success:
            logger.info("云台重置成功")
            return jsonify({
                'success': True, 
                'message': '云台重置成功，已回到初始位置'
            })
        else:
            logger.error("云台重置失败")
            return jsonify({
                'success': False, 
                'message': '云台重置失败'
            })
    except Exception as e:
        logger.error(f"云台重置异常: {e}")
        return jsonify({'success': False, 'message': f'云台重置异常: {str(e)}'}), 500


@app.route('/api/ptz/focus/auto', methods=['POST'])
def auto_focus():
    """自动对焦"""
    if not websocket_client:
        return jsonify({'success': False, 'message': '未连接到WebSocket'}), 400
    
    try:
        future = asyncio.run_coroutine_threadsafe(
            websocket_client.auto_focus(focus_mode=1), websocket_loop
        )
        success = future.result(timeout=8)
        
        return jsonify({
            'success': success, 
            'message': f"自动对焦{'成功' if success else '失败'}"
        })
    except Exception as e:
        return jsonify({'success': False, 'message': str(e)}), 500


@app.route('/api/ptz/focus/manual', methods=['POST'])
def manual_focus():
    """手动对焦"""
    if not websocket_client:
        return jsonify({'success': False, 'message': '未连接到WebSocket'}), 400
    
    try:
        data = request.get_json()
        direction = data.get('direction', 'near')
        
        # 根据方向设置聚焦速度
        speed = 30000 if direction == 'far' else 4000  # 正值远焦，负值近焦
        
        future = asyncio.run_coroutine_threadsafe(
            websocket_client.move_focus(speed), websocket_loop
        )
        future.result(timeout=3)
        
        return jsonify({
            'success': True, 
            'message': f"{'远距离' if direction == 'far' else '近距离'}对焦启动成功"
        })
    except Exception as e:
        return jsonify({'success': False, 'message': str(e)}), 500


@app.route('/api/ptz/focus/stop', methods=['POST'])
def stop_focus():
    """停止对焦"""
    if not websocket_client:
        return jsonify({'success': False, 'message': '未连接到WebSocket'}), 400
    
    try:
        future = asyncio.run_coroutine_threadsafe(
            websocket_client.move_focus(0.0), websocket_loop  # 速度为0停止对焦
        )
        future.result(timeout=3)
        
        return jsonify({
            'success': True, 
            'message': '停止对焦成功'
        })
    except Exception as e:
        return jsonify({'success': False, 'message': str(e)}), 500


@app.route('/api/ptz/focus/position', methods=['POST'])
def set_focus_position():
    """设置聚焦位置"""
    if not websocket_client:
        return jsonify({'success': False, 'message': '未连接到WebSocket'}), 400
    
    try:
        data = request.get_json()
        position = int(data.get('position', 20000))
        
        # 聚焦位置范围通常在4000-40000之间
        if position < 4000 or position > 40000:
            return jsonify({'success': False, 'message': '聚焦位置超出范围(4000-40000)'}), 400
        
        # 使用move_focus设置聚焦位置，参考manual_focus的实现
        future = asyncio.run_coroutine_threadsafe(
            websocket_client.move_focus(position), websocket_loop
        )
        future.result(timeout=5)
        
        # 更新状态
        ptz_status['focus'] = position
        
        return jsonify({
            'success': True, 
            'message': f'设置聚焦位置到 {position} 成功'
        })
    except Exception as e:
        return jsonify({'success': False, 'message': str(e)}), 500


# ==================== 导航接口 ====================

@app.route('/api/nav/goal', methods=['POST'])
def nav_to_goal():
    """导航到目标点"""
    if not websocket_client:
        return jsonify({'success': False, 'message': '未连接到WebSocket'}), 400
    
    try:
        data = request.get_json()
        x = data.get('x', 0.0)
        y = data.get('y', 0.0)
        yaw = data.get('yaw', 0.0)
        
        future = asyncio.run_coroutine_threadsafe(
            websocket_client.navigate_to_xyz(x, y, yaw), websocket_loop
        )
        success = future.result(timeout=5)
        
        return jsonify({
            'success': success, 
            'message': f"导航到目标点 ({x}, {y}, {yaw}) {'成功' if success else '失败'}"
        })
    except Exception as e:
        return jsonify({'success': False, 'message': str(e)}), 500


@app.route('/api/nav/cancel', methods=['POST'])
def cancel_navigation():
    """取消导航"""
    if not websocket_client:
        return jsonify({'success': False, 'message': '未连接到WebSocket'}), 400
    
    try:
        # 确保机器人名称已设置
        if grpc_client:
            try:
                success, robot_name = grpc_client.get_robot_name()
                if not success or not robot_name:
                    logger.warning("无法获取机器人名称，取消导航可能失败")
                else:
                    logger.info(f"当前机器人名称: {robot_name}")
            except Exception as e:
                logger.error(f"获取机器人名称异常: {e}")
        
        future = asyncio.run_coroutine_threadsafe(
            websocket_client.cancel_navigation(""), websocket_loop
        )
        success = future.result(timeout=3)
        
        return jsonify({
            'success': success, 
            'message': f"取消导航{'成功' if success else '失败'}"
        })
    except Exception as e:
        return jsonify({'success': False, 'message': str(e)}), 500


@app.route('/api/nav/location', methods=['POST'])
def navigate_to_location():
    """导航到指定位置点"""
    if not websocket_client:
        return jsonify({'success': False, 'message': '未连接到WebSocket'}), 400
    
    try:
        data = request.get_json()
        location_name = data.get('location_name', '')
        
        if not location_name:
            return jsonify({'success': False, 'message': '位置名称不能为空'}), 400
        
        # 确保机器人名称已设置
        if grpc_client:
            try:
                success, robot_name = grpc_client.get_robot_name()
                if not success or not robot_name:
                    logger.warning("无法获取机器人名称，导航可能失败")
                else:
                    logger.info(f"当前机器人名称: {robot_name}")
            except Exception as e:
                logger.error(f"获取机器人名称异常: {e}")
        
        # 重置导航状态，准备开始新的导航
        navigation_status['status'] = 'PLANNING'
        navigation_status['message'] = f'开始导航到 {location_name}'
        navigation_status['current_target'] = location_name
        navigation_status['progress'] = 0
        
        future = asyncio.run_coroutine_threadsafe(
            websocket_client.navigate_to_location(location_name), websocket_loop
        )
        result = future.result(timeout=10)
        
        logger.info(f"导航API调用结果: {result}, 类型: {type(result)}")
        
        # 由于导航是异步的，不依赖于立即返回值，而是通过回调函数处理结果
        # 只要调用没有抛出异常，就认为是成功启动导航
        return jsonify({
            'success': True, 
            'message': f"导航到位置 {location_name} 已启动，请通过导航状态监控进度",
            'data': result
        })
    except Exception as e:
        logger.error(f"导航API调用异常: {e}")
        return jsonify({'success': False, 'message': str(e)}), 500


@app.route('/api/nav/locations', methods=['GET'])
def get_available_locations():
    """获取可用的导航位置列表"""
    if not websocket_client:
        return jsonify({'success': False, 'message': '未连接到WebSocket'}), 400
    
    try:
        future = asyncio.run_coroutine_threadsafe(
            websocket_client.get_available_locations(), websocket_loop
        )
        
        try:
            result = future.result(timeout=8)  # 增加超时时间
            
            if result and result.get('success') and 'location_names' in result:
                # 从机器人返回的数据中提取点位名称列表
                location_names = result['location_names']
                return jsonify({
                    'success': True, 
                    'message': f'获取到{len(location_names)}个导航点位',
                    'data': {
                        'locations': location_names,
                        'total_count': len(location_names)
                    }
                })
            elif result and not result.get('success'):
                return jsonify({
                    'success': False, 
                    'message': result.get('message', '获取点位列表失败'),
                    'data': {'locations': [], 'total_count': 0}
                })
            else:
                return jsonify({
                    'success': False, 
                    'message': '未获取到有效的位置数据',
                    'data': {'locations': [], 'total_count': 0}
                })
        except Exception as e:
            return jsonify({
                'success': False, 
                'message': f'获取点位列表超时或异常: {str(e)}',
                'data': {'locations': [], 'total_count': 0}
            })
            
    except Exception as e:
        return jsonify({'success': False, 'message': f'请求处理异常: {str(e)}'}), 500


@app.route('/api/nav/location/add', methods=['POST'])
def add_location():
    """添加当前位置为导航点"""
    if not websocket_client:
        return jsonify({'success': False, 'message': '未连接到WebSocket'}), 400
    
    try:
        data = request.get_json()
        location_name = data.get('location_name', '')
        
        if not location_name:
            return jsonify({'success': False, 'message': '位置名称不能为空'}), 400
        
        # 增加超时时间，并添加更好的错误处理
        future = asyncio.run_coroutine_threadsafe(
            websocket_client.add_location(location_name, timeout=20), websocket_loop
        )
        
        try:
            result = future.result(timeout=25)  # 比WebSocket超时多5秒
            
            if result and result.get('success'):
                return jsonify({
                    'success': True, 
                    'message': f"添加位置 {location_name} 成功",
                    'data': result
                })
            else:
                error_msg = result.get('message', '添加位置失败') if result else '添加位置失败'
                return jsonify({
                    'success': False, 
                    'message': f"添加位置 {location_name} 失败: {error_msg}"
                })
        except Exception as e:
            return jsonify({
                'success': False, 
                'message': f"添加位置操作超时或异常: {str(e)}"
            })
            
    except Exception as e:
        return jsonify({'success': False, 'message': f'请求处理异常: {str(e)}'}), 500


@app.route('/api/nav/location/delete', methods=['POST'])
def delete_location():
    """删除导航位置点"""
    if not websocket_client:
        return jsonify({'success': False, 'message': '未连接到WebSocket'}), 400
    
    try:
        data = request.get_json()
        location_name = data.get('location_name', '')
        
        if not location_name:
            return jsonify({'success': False, 'message': '位置名称不能为空'}), 400
        
        future = asyncio.run_coroutine_threadsafe(
            websocket_client.delete_location(location_name), websocket_loop
        )
        result = future.result(timeout=5)
        
        return jsonify({
            'success': result is not None, 
            'message': f"删除位置 {location_name} {'成功' if result else '失败'}",
            'data': result
        })
    except Exception as e:
        return jsonify({'success': False, 'message': str(e)}), 500


@app.route('/api/nav/status', methods=['GET'])
def get_navigation_status():
    """获取导航状态"""
    try:
        return jsonify({
            'success': True, 
            'message': '获取导航状态成功',
            'data': navigation_status.copy()  # 返回状态的副本
        })
    except Exception as e:
        return jsonify({'success': False, 'message': str(e)}), 500


@app.route('/api/nav/events', methods=['GET'])
def get_navigation_events():
    """获取导航事件列表"""
    try:
        # 转换deque为列表，并按时间倒序排列
        events_list = list(navigation_events)
        events_list.reverse()  # 最新的事件在前
        
        return jsonify({
            'success': True,
            'message': f'获取到{len(events_list)}个导航事件',
            'data': {
                'events': events_list,
                'total_count': len(events_list)
            }
        })
    except Exception as e:
        return jsonify({'success': False, 'message': str(e)}), 500


@app.route('/api/nav/events/clear', methods=['POST'])
def clear_navigation_events():
    """清空导航事件"""
    try:
        navigation_events.clear()
        return jsonify({
            'success': True,
            'message': '导航事件已清空'
        })
    except Exception as e:
        return jsonify({'success': False, 'message': str(e)}), 500


@app.route('/api/nav/current_pose', methods=['GET'])
def get_current_pose():
    """获取当前位置"""
    if not websocket_client:
        return jsonify({'success': False, 'message': '未连接到WebSocket'}), 400
    
    try:
        future = asyncio.run_coroutine_threadsafe(
            websocket_client.get_current_pose(), websocket_loop
        )
        result = future.result(timeout=10)  # 增加超时时间到10秒
        
        if result and result.get('success'):
            # 从机器人返回的数据中提取位置信息
            location_pose = result.get('location_pose', {})
            position = location_pose.get('position', {})
            orientation = location_pose.get('orientation', {})
            
            current_pose = {
                'x': position.get('x', 0.0),
                'y': position.get('y', 0.0),
                'z': position.get('z', 0.0),
                'orientation': {
                    'x': orientation.get('x', 0.0),
                    'y': orientation.get('y', 0.0),
                    'z': orientation.get('z', 0.0),
                    'w': orientation.get('w', 1.0)
                },
                'frame_id': 'map'
            }
            
            return jsonify({
                'success': True,
                'message': '获取当前位置成功',
                'data': current_pose
            })
        else:
            return jsonify({
                'success': False,
                'message': result.get('message', '获取当前位置失败') if result else '获取当前位置失败'
            })
    except Exception as e:
        return jsonify({'success': False, 'message': str(e)}), 500


@app.route('/api/nav/localization_state', methods=['GET'])
def get_localization_state():
    """获取定位状态"""
    if not websocket_client:
        return jsonify({'success': False, 'message': '未连接到WebSocket'}), 400
    
    try:
        future = asyncio.run_coroutine_threadsafe(
            websocket_client.get_localization_state(), websocket_loop
        )
        result = future.result(timeout=5)
        
        if result and result.get('success'):
            # 从机器人返回的数据中提取定位状态
            loc_state = result.get('loc_state', 0)
            
            # 状态映射: 0=NORMAL, 1=LACKDATA, 2=LOST
            status_map = {
                0: 'NORMAL',
                1: 'LACKDATA', 
                2: 'LOST'
            }
            
            is_localized = (loc_state == 0)  # 只有NORMAL状态才认为是定位成功
            status = status_map.get(loc_state, 'UNKNOWN')
            confidence = 0.95 if is_localized else (0.5 if loc_state == 1 else 0.1)
            
            localization_state = {
                'is_localized': is_localized,
                'confidence': confidence,
                'status': status,
                'message': result.get('message', '定位状态获取成功'),
                'loc_state': loc_state
            }
            
            return jsonify({
                'success': True,
                'message': '获取定位状态成功',
                'data': localization_state
            })
        else:
            return jsonify({
                'success': False,
                'message': result.get('message', '获取定位状态失败') if result else '获取定位状态失败'
            })
    except Exception as e:
        return jsonify({'success': False, 'message': str(e)}), 500


@app.route('/api/nav/reset_localization', methods=['POST'])
def reset_localization():
    """重置定位"""
    if not websocket_client:
        return jsonify({'success': False, 'message': '未连接到WebSocket'}), 400
    
    try:
        # 注意：WebSocket客户端目前不提供重置定位的服务接口
        # 这是一个临时的模拟实现，返回成功状态
        # 如果后续机器人端提供了重置定位服务，可以调用类似：
        # result = await websocket_client._call_service("reset_localization", {}, service_type="navigation")
        
        return jsonify({
            'success': True,
            'message': '定位重置成功 (模拟实现)'
        })
    except Exception as e:
        return jsonify({'success': False, 'message': str(e)}), 500

# ==================== 地图管理模块接口-待开发 ====================
@app.route('/api/nav/mapping/start', methods = ['POST'])
def start_mapping():
    """开始建图"""
    if not websocket_client:
        return jsonify({'success': False, 'message': '未连接到WebSocket'}), 400
    try:
        logger.info('[MAPPING] HTTP /api/nav/mapping/start called')
        # 开始建图
        fut = asyncio.run_coroutine_threadsafe(
            websocket_client.start_mapping(), websocket_loop
        )
        result = fut.result(timeout=10)
        # 打印和记录返回的 result 值（类型与内容）
        try:
            print("\n" + "=" * 80)
            print("🗺️ [MAPPING][START] start_mapping 返回结果:")
            print(f"👉 类型: {type(result)}")
            print(f"👉 内容: {result}")
            print("=" * 80 + "\n")
        except Exception as _e:
            logger.warning(f"[MAPPING][START] 打印result异常: {_e}")
        logger.info(f"[MAPPING][START] result type={type(result)} content={result}")
        ok = bool(result and result.get('success'))
        if ok:
            logger.info("建图已启动")
        else:
            logger.warning(f"启动建图失败: {result.get('message') if isinstance(result, dict) else result}")
        return jsonify({
            'success': ok,
            'message': '建图已启动' if ok else f"启动建图失败: {result.get('message') if isinstance(result, dict) else '未知错误'}",
            'data': result
        })
    except Exception as e:
        return jsonify({'success': False, 'message': str(e)}), 500
    
@app.route('/api/nav/mapping/stop', methods = ['POST'])
def stop_mapping():
    """结束建图"""
    if not websocket_client:
        return jsonify({'success': False, 'message': '未连接到WebSocket'}), 400
    try:
        # 结束建图并保存，同时可选择自动加载
        body = request.get_json(silent=True) or {}
        map_name = body.get('map_name') or body.get('name') or 'my_map'
        auto_reload = bool(body.get('auto_reload', True))
        logger.info(f'[MAPPING] HTTP /api/nav/mapping/stop called map_name={map_name} auto_reload={auto_reload}')

        fut = asyncio.run_coroutine_threadsafe(
            websocket_client.stop_mapping(map_name, auto_reload=auto_reload), websocket_loop
        )
        result = fut.result(timeout=20)
        # 打印和记录返回的 result 值（类型与内容）
        try:
            print("\n" + "=" * 80)
            print("🗺️ [MAPPING][STOP] stop_mapping 返回结果:")
            print(f"👉 类型: {type(result)}")
            print(f"👉 内容: {result}")
            print("=" * 80 + "\n")
        except Exception as _e:
            logger.warning(f"[MAPPING][STOP] 打印result异常: {_e}")
        logger.info(f"[MAPPING][STOP] result type={type(result)} content={result}")
        ok = bool(result and result.get('success'))
        if ok:
            logger.info("结束建图成功并已处理地图")
        else:
            logger.warning(f"结束建图失败: {result.get('message') if isinstance(result, dict) else result}")
        return jsonify({
            'success': ok,
            'message': '结束建图成功并已处理地图' if ok else f"结束建图失败: {result.get('message') if isinstance(result, dict) else '未知错误'}",
            'data': result
        })
    except Exception as e:
        return jsonify({'success': False, 'message': str(e)}), 500

@app.route('/api/nav/mapping/import', methods = ['POST'])
def import_map():
    """导入地图"""
    if not websocket_client:
        return jsonify({'success': False, 'message': '未连接到WebSocket'}), 400
    try:
        return jsonify({
            'success': True,
        })
    except Exception as e:
        return jsonify({'success': False, 'message': str(e)}), 500

@app.route('/api/nav/mapping/export', methods = ['POST'])
def export_map():
    """导出地图"""
    if not websocket_client:
        return jsonify({'success': False, 'message': '未连接到WebSocket'}), 400
    try:
        return jsonify({
            'success': True,
        })
    except Exception as e:
        return jsonify({'success': False, 'message': str(e)}), 500

@app.route('/api/nav/mapping/relocalization', methods = ['POST'])
def relocalization():
    """重定位"""
    if not websocket_client:
        return jsonify({'success': False, 'message': '未连接到WebSocket'}), 400
    try:
        return jsonify({
            'success': True,
        })
    except Exception as e:
        return jsonify({'success': False, 'message': str(e)}), 500

@app.route('/api/nav/mapping/apply', methods = ['POST'])
def apply_mapping_parameters():
    """应用参数"""
    if not websocket_client:
        return jsonify({'success': False, 'message': '未连接到WebSocket'}), 400
    try:
        return jsonify({
            'success': True,
        })
    except Exception as e:
        return jsonify({'success': False, 'message': str(e)}), 500

@app.route('/api/nav/mapping/reset', methods = ['POST'])
def reset_mapping_parameters():
    """恢复默认"""
    if not websocket_client:
        return jsonify({'success': False, 'message': '未连接到WebSocket'}), 400
    try:
        return jsonify({
            'success': True,
        })
    except Exception as e:
        return jsonify({'success': False, 'message': str(e)}), 500


# ==================== 视频流接口 ====================

# 改进的视频流管理器 - 使用后台线程和帧缓存
class VideoStreamManager:
    def __init__(self):
        self.streams = {}
        self.frame_cache = {}  # 存储最新帧
        self.cache_locks = {}  # 帧缓存锁
        
    def create_stream(self, stream_type):
        """创建新的视频流"""
        if stream_type not in self.streams:
            self.streams[stream_type] = {
                'url': None,
                'active': False,
                'capture': None,
                'thread': None,
                'lock': threading.Lock(),
                'stop_event': threading.Event()
            }
            self.frame_cache[stream_type] = None
            self.cache_locks[stream_type] = threading.Lock()
    
    def _capture_frames(self, stream_type):
        """后台线程函数：持续捕获视频帧"""
        stream_info = self.streams[stream_type]
        frame_count = 0
        last_log_time = time.time()
        
        logger.info(f"开始后台帧捕获线程: {stream_type}")
        
        while not stream_info['stop_event'].is_set():
            try:
                with stream_info['lock']:
                    if stream_info['capture'] is None:
                        break
                    
                    # 非阻塞读取，设置短超时
                    ret, frame = stream_info['capture'].read()
                
                if ret and frame is not None:
                    # 编码帧为JPEG
                    ret_encode, buffer = cv2.imencode('.jpg', frame, [
                        cv2.IMWRITE_JPEG_QUALITY, 70,
                        cv2.IMWRITE_JPEG_OPTIMIZE, 1
                    ])
                    
                    if ret_encode:
                        # 更新帧缓存
                        with self.cache_locks[stream_type]:
                            self.frame_cache[stream_type] = buffer.tobytes()
                        
                        frame_count += 1
                        # 每30帧记录一次日志
                        if time.time() - last_log_time > 3.0:
                            logger.debug(f"{stream_type}后台捕获帧数: {frame_count}")
                            last_log_time = time.time()
                else:
                    logger.warning(f"{stream_type}后台帧捕获失败，等待重试")
                    time.sleep(0.1)
                    
            except Exception as e:
                logger.error(f"{stream_type}后台帧捕获异常: {e}")
                time.sleep(0.1)
            
            # 控制帧率 - 约15fps避免过度占用CPU
            time.sleep(0.066)
        
        logger.info(f"后台帧捕获线程结束: {stream_type}")
    
    def start_stream(self, stream_type, url):
        """启动视频流"""
        self.create_stream(stream_type)
        stream_info = self.streams[stream_type]
        
        if stream_info['active']:
            return True, f"{stream_type}视频流已在运行"
        
        try:
            with stream_info['lock']:
                # 关闭现有连接
                if stream_info['capture']:
                    stream_info['capture'].release()
                
                # 创建新的VideoCapture
                cap = cv2.VideoCapture(url, cv2.CAP_FFMPEG)
                
                # 优化设置
                cap.set(cv2.CAP_PROP_BUFFERSIZE, 1)
                cap.set(cv2.CAP_PROP_FPS, 15)
                cap.set(cv2.CAP_PROP_FRAME_WIDTH, 640)
                cap.set(cv2.CAP_PROP_FRAME_HEIGHT, 480)
                cap.set(cv2.CAP_PROP_OPEN_TIMEOUT_MSEC, 5000)
                cap.set(cv2.CAP_PROP_READ_TIMEOUT_MSEC, 2000)
                
                # 测试连接
                ret, test_frame = cap.read()
                if not ret:
                    cap.release()
                    return False, f"无法连接到{stream_type}视频流"
                
                stream_info['url'] = url
                stream_info['capture'] = cap
                stream_info['active'] = True
                stream_info['stop_event'].clear()
                
                # 启动后台帧捕获线程
                stream_info['thread'] = threading.Thread(
                    target=self._capture_frames,
                    args=(stream_type,),
                    daemon=True,
                    name=f"VideoCapture-{stream_type}"
                )
                stream_info['thread'].start()
                
                logger.info(f"成功启动{stream_type}视频流后台处理")
                return True, f"{stream_type}视频流启动成功"
                
        except Exception as e:
            logger.error(f"启动{stream_type}视频流失败: {e}")
            return False, f"启动{stream_type}视频流失败: {str(e)}"
    
    def stop_stream(self, stream_type):
        """停止视频流"""
        if stream_type not in self.streams:
            return True, f"{stream_type}视频流不存在"
        
        stream_info = self.streams[stream_type]
        
        if not stream_info['active']:
            return True, f"{stream_type}视频流未运行"
        
        try:
            # 设置停止标志
            stream_info['stop_event'].set()
            stream_info['active'] = False
            
            # 等待后台线程结束
            if stream_info['thread'] and stream_info['thread'].is_alive():
                stream_info['thread'].join(timeout=2.0)
            
            # 关闭capture
            with stream_info['lock']:
                if stream_info['capture']:
                    stream_info['capture'].release()
                    stream_info['capture'] = None
            
            # 清理帧缓存
            with self.cache_locks[stream_type]:
                self.frame_cache[stream_type] = None
            
            logger.info(f"成功停止{stream_type}视频流")
            return True, f"{stream_type}视频流已停止"
            
        except Exception as e:
            logger.error(f"停止{stream_type}视频流失败: {e}")
            return False, f"停止{stream_type}视频流失败: {str(e)}"
    
    def get_latest_frame(self, stream_type):
        """获取最新帧（非阻塞）"""
        if stream_type not in self.frame_cache:
            return None
        
        with self.cache_locks[stream_type]:
            return self.frame_cache[stream_type]
    
    def is_active(self, stream_type):
        """检查流是否活跃"""
        if stream_type not in self.streams:
            return False
        return self.streams[stream_type]['active']
    
    def cleanup_all(self):
        """清理所有视频流"""
        for stream_type in list(self.streams.keys()):
            self.stop_stream(stream_type)

# ==================== 简易任务管理 (老化测试) ====================

"""任务执行层说明

本文件早期版本内置了一个本地线程版 TaskManager, 逐行 exec 用户脚本用于老化测试。
现在 SDK 已提供基于 WebSocket 的真实任务接口: create_task/execute_task/task_status/
pause_task/resume_task/stop_task/delete_task/get_all_tasks。

为确保兼容, 我们引入一个特性开关 USE_LOCAL_TASK_FALLBACK (环境变量或常量)。
默认使用 SDK 任务接口; 若 SDK 不可用或需要离线演示可开启本地模式。

新增: 简单循环(loop_count<=0 表示无限) 由本地 orchestrator 调用 SDK 的 execute_task
实现; 若后续 SDK 原生支持循环可直接移除该逻辑。
"""

USE_LOCAL_TASK_FALLBACK = os.environ.get("USE_LOCAL_TASK_FALLBACK", "0") == "1"

class TaskState:
    CREATED = 'CREATED'
    RUNNING = 'RUNNING'
    PAUSED = 'PAUSED'
    STOPPED = 'STOPPED'
    COMPLETED = 'COMPLETED'
    ERROR = 'ERROR'

class TaskInfo:
    def __init__(self, task_id: str, script_content: str, timeout: int,
                 loop_count: int = 1, loop_interval_sec: float = 1.0):
        self.task_id = task_id
        self.script_content = script_content
        self.timeout = timeout
        # 循环控制: loop_count <=0 表示无限循环
        self.loop_count = loop_count
        self.loop_interval_sec = loop_interval_sec
        self.state = TaskState.CREATED
        self.thread: Optional[threading.Thread] = None
        self._pause_event = threading.Event()
        self._stop_event = threading.Event()
        self._pause_event.set()  # 未暂停时保持set
        self.create_time = time.time()
        self.start_time: Optional[float] = None
        self.end_time: Optional[float] = None
        self.last_heartbeat = time.time()
        self.run_count = 0  # 循环次数（完成的轮次）
        self.success_count = 0
        self.error_count = 0
        self.last_loop_start: Optional[float] = None
        self.error: Optional[str] = None

    def to_dict(self):
        return {
            'task_id': self.task_id,
            'state': self.state,
            'state_name': self.state,
            'timeout': self.timeout,
            'loop_count': self.loop_count,
            'loop_interval_sec': self.loop_interval_sec,
            'create_time': self.create_time,
            'start_time': self.start_time,
            'end_time': self.end_time,
            'duration': (self.end_time - self.start_time) if self.start_time and self.end_time else None,
            'last_heartbeat': self.last_heartbeat,
            'loop_finished': self.run_count,
            'success_count': self.success_count,
            'error_count': self.error_count,
            'last_loop_start': self.last_loop_start,
            'error': self.error
        }

class LocalTaskManager:
    """保留原本本地实现(仅在 USE_LOCAL_TASK_FALLBACK 时启用)"""
    def __init__(self):
        self._tasks: Dict[str, TaskInfo] = {}
        self._lock = threading.Lock()

    def create_task(self, task_id: str, script_content: str, timeout: int, loop_count: int = 1, loop_interval_sec: float = 1.0) -> bool:
        with self._lock:
            if task_id in self._tasks:
                return False
            self._tasks[task_id] = TaskInfo(task_id, script_content, timeout, loop_count, loop_interval_sec)
            return True
    def get_task(self, tid: str):
        return self._tasks.get(tid)
    def list_tasks(self):
        return [t.to_dict() for t in self._tasks.values()]
    def delete_task(self, tid: str):
        t = self._tasks.get(tid)
        if not t or t.state in (TaskState.RUNNING, TaskState.PAUSED):
            return False
        self._tasks.pop(tid, None);return True
    # 简化: 不再维护执行, 鼓励使用 SDK; 若需要可复制旧逻辑
    def execute_task(self, tid: str):
        t = self.get_task(tid)
        if not t:
            return False
        t.state = TaskState.COMPLETED
        t.start_time = t.end_time = time.time()
        return True
    def pause_task(self, tid: str): return False
    def resume_task(self, tid: str): return False
    def stop_task(self, tid: str): return False

local_task_manager = LocalTaskManager()

# SDK 循环任务调度器: 在服务器侧用线程循环调用 execute_task / task_status
class SdkLoopTask:
    def __init__(self, task_id: str, loop_count: int, loop_interval_sec: float, timeout: int, script_content: str):
        self.task_id = task_id
        self.loop_count = loop_count
        self.loop_interval_sec = loop_interval_sec
        self.timeout = timeout
        self.script_content = script_content
        self.thread: Optional[threading.Thread] = None
        self._stop = threading.Event()
        self._pause = threading.Event(); self._pause.set()
        self.run_count = 0
        self.success = 0
        self.errors = 0
        self.last_error = None
        self.start_time = None
        self.end_time = None
        self.state = TaskState.CREATED

    def to_dict(self):
        return {
            'task_id': self.task_id,
            'state': self.state,
            'state_name': self.state,
            'loop_count': self.loop_count,
            'loop_interval_sec': self.loop_interval_sec,
            'loop_finished': self.run_count,
            'success_count': self.success,
            'error_count': self.errors,
            'error': self.last_error,
            'start_time': self.start_time,
            'end_time': self.end_time,
            'duration': (self.end_time - self.start_time) if self.start_time and self.end_time else None
        }

class SdkLoopTaskOrchestrator:
    def __init__(self):
        self._loop_tasks: Dict[str, SdkLoopTask] = {}
        self._lock = threading.Lock()

    def create(self, task_id: str, script_content: str, timeout: int, loop_count: int, loop_interval_sec: float):
        with self._lock:
            if task_id in self._loop_tasks:
                return False
            self._loop_tasks[task_id] = SdkLoopTask(task_id, loop_count, loop_interval_sec, timeout, script_content)
            return True
    def get(self, task_id: str):
        return self._loop_tasks.get(task_id)
    def list(self):
        return [t.to_dict() for t in self._loop_tasks.values()]
    def delete(self, task_id: str):
        t = self._loop_tasks.get(task_id)
        if not t or t.state in (TaskState.RUNNING, TaskState.PAUSED):
            return False
        self._loop_tasks.pop(task_id, None);return True
    def pause(self, task_id: str):
        t = self.get(task_id); 
        if not t or t.state != TaskState.RUNNING: return False
        t.state = TaskState.PAUSED; t._pause.clear(); return True
    def resume(self, task_id: str):
        t = self.get(task_id); 
        if not t or t.state != TaskState.PAUSED: return False
        t.state = TaskState.RUNNING; t._pause.set(); return True
    def stop(self, task_id: str):
        t = self.get(task_id); 
        if not t or t.state not in (TaskState.RUNNING, TaskState.PAUSED): return False
        t._stop.set(); t._pause.set(); t.state = TaskState.STOPPED; return True

    def execute(self, task_id: str):
        t = self.get(task_id)
        if not t: return False
        if t.state in (TaskState.RUNNING, TaskState.PAUSED): return True
        def _runner():
            t.start_time = time.time(); t.state = TaskState.RUNNING
            loop_index = 0
            while True:
                if t._stop.is_set(): break
                if t.loop_count > 0 and loop_index >= t.loop_count: break
                loop_index += 1; t.run_count = loop_index
                # 1) 如果脚本尚未以持久化方式创建, 则调用 create_task
                if t.script_content:
                    try:
                        fut = asyncio.run_coroutine_threadsafe(websocket_client.create_task(t.task_id, t.script_content, t.timeout), websocket_loop)
                        create_ret = fut.result(timeout=10)
                        if not create_ret or not create_ret.get('success'):
                            t.errors += 1; t.last_error = f"create_task失败: {create_ret}"; break
                    except Exception as e:
                        t.errors += 1; t.last_error = f"create_task异常: {e}"; break
                    # 脚本只需要创建一次
                    t.script_content = ""
                # 2) 执行任务
                try:
                    fut2 = asyncio.run_coroutine_threadsafe(websocket_client.execute_task(task_id=t.task_id, timeout=t.timeout), websocket_loop)
                    exec_ret = fut2.result(timeout=t.timeout+10)
                    if not exec_ret or not exec_ret.get('success'):
                        t.errors += 1; t.last_error = f"execute_task失败: {exec_ret}"; break
                    t.success += 1
                except Exception as e:
                    t.errors += 1; t.last_error = f"execute_task异常: {e}"; break
                # 3) 间隔等待/暂停
                end_wait = time.time() + t.loop_interval_sec
                while time.time() < end_wait:
                    if t._stop.is_set(): break
                    if not t._pause.is_set(): time.sleep(0.1); continue
                    time.sleep(0.1)
                if t._stop.is_set(): break
                if t.loop_count > 0 and loop_index >= t.loop_count: break
            if t.state not in (TaskState.STOPPED, TaskState.ERROR):
                t.state = TaskState.COMPLETED if not t._stop.is_set() else TaskState.STOPPED
            t.end_time = time.time()
        t.thread = threading.Thread(target=_runner, name=f"SdkLoop-{task_id}", daemon=True)
        t.thread.start(); return True

sdk_loop_orchestrator = SdkLoopTaskOrchestrator()

# ============= 任务管理 REST 接口 (/api/task/*) =============

# --- 实时任务状态订阅支持 ---
# 使用 SDK 新增的 register_task_status / unregister_task_status 接口, 将回调结果放入内存队列
task_status_cache: Dict[str, Dict[str, Any]] = {}
task_status_subscribed: Dict[str, bool] = {}  # 仍保留按task标记, 但底层SDK是全局订阅
task_status_lock = threading.Lock()

def _on_task_status_update(task_id: str, data: Dict[str, Any]):
    """SDK 回调: 更新任务状态缓存"""
    with task_status_lock:
        now = time.time()
        entry = data.copy() if isinstance(data, dict) else {'raw': str(data)}
        entry['task_id'] = task_id
        entry['ts'] = now
        task_status_cache[task_id] = entry
    logger.debug(f"[TASK][STATUS][CB] task_id={task_id} state={entry.get('state')} updated")

def ensure_task_status_subscription(task_id: str):
    """确保任务状态全局订阅已建立。
    SDK 的 register_task_status 是全局通道(不按 task_id 过滤)，我们仍然按 task_id 做本地标记，
    方便避免重复调用。首次任意任务调用时建立一次全局订阅。"""
    if not websocket_client:
        return False
    # 若已经有任何一个订阅成功则直接返回
    with task_status_lock:
        if any(task_status_subscribed.values()):
            task_status_subscribed[task_id] = True
            return True
        # 若当前task尚未标记，先占位 False
        task_status_subscribed.setdefault(task_id, False)
    try:
        async def _do_register():
            return await websocket_client.register_task_status(lambda data: _on_task_status_update(data.get('task_id') or task_id, data))
        fut = asyncio.run_coroutine_threadsafe(_do_register(), websocket_loop)
        ret = fut.result(timeout=5)
        ok = bool(ret)
        with task_status_lock:
            for k in list(task_status_subscribed.keys()):
                task_status_subscribed[k] = ok
            task_status_subscribed[task_id] = ok
        logger.info(f"[TASK][STATUS][SUB][GLOBAL] subscribed={ok} trigger_task={task_id}")
        return ok
    except Exception as e:
        logger.exception(f"[TASK][STATUS][SUB][GLOBAL] 订阅失败 error={e}")
        return False

def remove_task_status_subscription(task_id: str):
    """由于底层是全局订阅，这里在所有任务都被标记为不需要后才真正取消。"""
    if not websocket_client:
        return False
    with task_status_lock:
        task_status_subscribed[task_id] = False
        # 若仍有其它任务需要订阅则不做真正取消
        if any(v for v in task_status_subscribed.values()):
            logger.debug(f"[TASK][STATUS][UNSUB] skip global unsubscribe still others active")
            return True
    try:
        async def _do_unregister():
            return await websocket_client.unregister_task_status()
        fut = asyncio.run_coroutine_threadsafe(_do_unregister(), websocket_loop)
        fut.result(timeout=5)
        logger.info(f"[TASK][STATUS][UNSUB][GLOBAL] 全局取消订阅成功")
        return True
    except Exception as e:
        logger.exception(f"[TASK][STATUS][UNSUB][GLOBAL] 取消订阅失败 error={e}")
        return False

@app.route('/api/task/create', methods=['POST'])
def api_task_create():
    data = request.get_json(force=True, silent=True) or {}
    task_id = data.get('task_id','').strip()
    script_content = data.get('script_content','').strip()
    timeout = int(data.get('timeout',0))
    loop_count = int(data.get('loop_count',1))
    loop_interval_sec = float(data.get('loop_interval_sec',1.0))
    if not task_id or not script_content:
        logger.warning(f"[TASK][CREATE] 参数缺失 task_id={task_id!r} script_len={len(script_content)}")
        return jsonify({'success': False, 'message': 'task_id 和 script_content 不能为空'})
    if USE_LOCAL_TASK_FALLBACK:
        logger.info(f"[TASK][CREATE][LOCAL] 创建任务 {task_id} loop_count={loop_count} interval={loop_interval_sec}s timeout={timeout}")
        if local_task_manager.create_task(task_id, script_content, timeout, loop_count, loop_interval_sec):
            return jsonify({'success': True,'message':'创建成功(local)','loop_count':loop_count,'loop_interval_sec':loop_interval_sec,'backend':'local'})
        logger.warning(f"[TASK][CREATE][LOCAL] 任务已存在 {task_id}")
        return jsonify({'success': False,'message':'任务已存在','backend':'local'})
    # SDK 模式: 先记录本地 orchestrator 结构(用于循环) 再调用 create_task
    logger.info(f"[TASK][CREATE][SDK] 准备创建 {task_id} loop_count={loop_count} interval={loop_interval_sec}s timeout={timeout}")
    created = sdk_loop_orchestrator.create(task_id, script_content, timeout, loop_count, loop_interval_sec)
    if not created:
        logger.warning(f"[TASK][CREATE][SDK] 任务已存在 {task_id}")
        return jsonify({'success': False,'message':'任务已存在','backend':'sdk'})
    if not websocket_client:
        logger.error(f"[TASK][CREATE][SDK] WebSocket 未连接, task_id={task_id}")
        return jsonify({'success': False,'message':'WebSocket未连接','backend':'sdk'})
    try:
        fut = asyncio.run_coroutine_threadsafe(websocket_client.create_task(task_id, script_content, timeout), websocket_loop)
        ret = fut.result(timeout=timeout+5)
        logger.info(f"[TASK][CREATE][SDK] 调用结果 task_id={task_id} success={ret and ret.get('success')} message={ret and ret.get('message')}")
        return jsonify({'success': bool(ret and ret.get('success')),'message': ret.get('message') if ret else '未知错误','backend':'sdk','loop_count':loop_count,'loop_interval_sec':loop_interval_sec})
    except Exception as e:
        logger.exception(f"[TASK][CREATE][SDK] 异常 task_id={task_id} error={e}")
        return jsonify({'success': False,'message': f'create_task异常: {e}','backend':'sdk'})

@app.route('/api/task/list', methods=['GET'])
def api_task_list():
    if USE_LOCAL_TASK_FALLBACK:
        logger.info("[TASK][LIST][LOCAL] 列出全部任务")
        return jsonify({'success': True,'tasks': local_task_manager.list_tasks(),'backend':'local'})
    # 合并 orchestrator 记录与 SDK get_all_tasks
    sdk_ids = []
    if websocket_client:
        try:
            logger.info("[TASK][LIST][SDK] get_all_task 0")
            fut = asyncio.run_coroutine_threadsafe(websocket_client.get_all_tasks(), websocket_loop)
            logger.info("[TASK][LIST][SDK] get_all_task 1")
            ret = fut.result(timeout=8)
            logger.info("[TASK][LIST][SDK] get_all_task 2")
            # 调试输出原始返回结果
            try:
                logger.info(f"[TASK][LIST][SDK] get_all_tasks 原始返回: {ret}")
            except Exception:
                pass
            if ret and ret.get('success'):
                raw_ids = ret.get('task_ids')  # 可能为 None / list / 单个字符串
                if isinstance(raw_ids, list):
                    sdk_ids = raw_ids
                elif raw_ids is None:
                    sdk_ids = []
                else:
                    # 避免出现非列表(比如字符串)导致后续逻辑混乱
                    sdk_ids = [raw_ids]
                try:
                    logger.info(f"[TASK][LIST][SDK] SDK返回任务数={len(sdk_ids)} raw_type={type(raw_ids).__name__}")
                except Exception:
                    logger.info("[TASK][LIST][SDK] 统计任务数量时出现非预期类型, 已忽略")
        except Exception:
            logger.exception("[TASK][LIST][SDK] 获取SDK任务列表异常")
            pass
    loop_entries = {t['task_id']: t for t in sdk_loop_orchestrator.list()}
    tasks = []
    for tid in sdk_ids:
        base = loop_entries.get(tid, {'task_id': tid, 'state': 'UNKNOWN', 'state_name': 'UNKNOWN'})
        tasks.append(base)
    # 可能存在尚未创建成功的 loop 任务
    for tid, info in loop_entries.items():
        if tid not in [t['task_id'] for t in tasks]:
            tasks.append(info)
    logger.debug(f"[TASK][LIST][SDK] 返回任务总数={len(tasks)} (包含loop记录)")
    return jsonify({'success': True,'tasks': tasks,'backend':'sdk'})

@app.route('/api/task/execute', methods=['POST'])
def api_task_execute():
    data = request.get_json(force=True, silent=True) or {}
    task_id = data.get('task_id','').strip()
    script_content = data.get('script_content','').strip()
    trial_run = bool(data.get('trial_run'))
    inline_timeout = int(data.get('timeout',0))
    if not task_id:
        logger.warning("[TASK][EXECUTE] 缺少 task_id")
        return jsonify({'success': False,'message':'task_id 不能为空'})
    if USE_LOCAL_TASK_FALLBACK:
        # 若提供脚本且任务不存在, 作为一次性试运行创建后立即执行
        t = local_task_manager.get_task(task_id)
        if script_content and not t:
            created = local_task_manager.create_task(task_id, script_content, inline_timeout, loop_count=1, loop_interval_sec=0)
            logger.info(f"[TASK][EXECUTE][LOCAL][INLINE] 试运行创建 task_id={task_id} created={created}")
        logger.info(f"[TASK][EXECUTE][LOCAL] 执行任务 {task_id} trial_run={trial_run}")
        if local_task_manager.execute_task(task_id):
            return jsonify({'success': True,'message':'任务已执行(local)','backend':'local','trial_run': trial_run})
        return jsonify({'success': False,'message':'任务不存在','backend':'local','trial_run': trial_run})
    if not websocket_client:
        return jsonify({'success': False,'message':'WebSocket未连接','backend':'sdk'})
    # 如果本地存在循环配置则走循环执行，否则直接执行一次
    loop_task = sdk_loop_orchestrator.get(task_id)
    if loop_task:
        logger.info(f"[TASK][EXECUTE][SDK] 循环任务启动 task_id={task_id} loop_count={loop_task.loop_count}")
        if sdk_loop_orchestrator.execute(task_id):
            # 确保订阅状态
            ensure_task_status_subscription(task_id)
            return jsonify({'success': True,'message':'循环任务已启动','backend':'sdk','mode':'loop'})
        logger.error(f"[TASK][EXECUTE][SDK] 循环任务启动失败 {task_id}")
        return jsonify({'success': False,'message':'循环任务启动失败','backend':'sdk','mode':'loop'})
    # 非循环: 直接调用一次 execute_task (不依赖本地存在)
    # 若前端传入脚本(试运行), 先尝试临时 create_task 覆盖/刷新脚本
    if script_content:
        try:
            fut_c = asyncio.run_coroutine_threadsafe(websocket_client.create_task(task_id, script_content, inline_timeout), websocket_loop)
            create_ret = fut_c.result(timeout=inline_timeout+5)
            logger.info(f"[TASK][EXECUTE][SDK][INLINE] create_task 返回 task_id={task_id} success={create_ret and create_ret.get('success')} msg={create_ret and create_ret.get('message')}")
        except Exception as e:
            logger.exception(f"[TASK][EXECUTE][SDK][INLINE] create_task 异常 task_id={task_id} error={e}")
    try:
        fut = asyncio.run_coroutine_threadsafe(websocket_client.execute_task(task_id=task_id, timeout=0), websocket_loop)
        ret = fut.result(timeout=130)
        ok = bool(ret and ret.get('success'))
        logger.info(f"[TASK][EXECUTE][SDK] 直接执行任务 task_id={task_id} success={ok} msg={ret and ret.get('message')} trial_run={trial_run} inline_script={bool(script_content)}")
        if ok:
            ensure_task_status_subscription(task_id)
        return jsonify({'success': ok,'message': ret.get('message') if ret else '未知错误','backend':'sdk','mode':'single','sdk_execute': ret,'trial_run': trial_run,'inline_script': bool(script_content)})
    except Exception as e:
        logger.exception(f"[TASK][EXECUTE][SDK] execute_task 异常 task_id={task_id} error={e}")
        return jsonify({'success': False,'message': f'执行异常: {e}','backend':'sdk','mode':'single','trial_run': trial_run,'inline_script': bool(script_content)})

@app.route('/api/task/pause', methods=['POST'])
def api_task_pause():
    data = request.get_json(force=True, silent=True) or {}
    task_id = data.get('task_id','').strip()
    if USE_LOCAL_TASK_FALLBACK:
        return jsonify({'success': False,'message':'本地模式不支持暂停','backend':'local'})
    logger.info(f"[TASK][PAUSE][SDK] 请求暂停 {task_id}")
    loop_updated = False
    loop_task = sdk_loop_orchestrator.get(task_id)
    if loop_task:
        # 尝试同步本地循环状态(忽略失败, 以远端为准)
        loop_updated = sdk_loop_orchestrator.pause(task_id)
    sdk_result = None
    if websocket_client:
        try:
            fut = asyncio.run_coroutine_threadsafe(websocket_client.pause_task(task_id), websocket_loop)
            sdk_result = fut.result(timeout=6)
            logger.info(f"[TASK][PAUSE][SDK] SDK调用结果 task_id={task_id} success={sdk_result and sdk_result.get('success')} msg={sdk_result and sdk_result.get('message')}")
        except Exception as e:
            logger.exception(f"[TASK][PAUSE][SDK] SDK pause_task 异常 task_id={task_id} error={e}")
            sdk_result = {'success': False, 'message': f'SDK pause_task 异常: {e}'}
    success = bool(sdk_result and sdk_result.get('success'))
    resp = {
        'success': success,
        'message': '已请求暂停' if success else '暂停失败',
        'backend': 'sdk',
        'loop_updated': loop_updated
    }
    if sdk_result is not None:
        resp['sdk_pause'] = sdk_result
    return jsonify(resp)

@app.route('/api/task/resume', methods=['POST'])
def api_task_resume():
    data = request.get_json(force=True, silent=True) or {}
    task_id = data.get('task_id','').strip()
    if USE_LOCAL_TASK_FALLBACK:
        return jsonify({'success': False,'message':'本地模式不支持恢复','backend':'local'})
    logger.info(f"[TASK][RESUME][SDK] 请求恢复 {task_id}")
    loop_updated = False
    if sdk_loop_orchestrator.get(task_id):
        loop_updated = sdk_loop_orchestrator.resume(task_id)
    sdk_result = None
    if websocket_client:
        try:
            fut = asyncio.run_coroutine_threadsafe(websocket_client.resume_task(task_id), websocket_loop)
            sdk_result = fut.result(timeout=6)
            logger.info(f"[TASK][RESUME][SDK] SDK调用结果 task_id={task_id} success={sdk_result and sdk_result.get('success')} msg={sdk_result and sdk_result.get('message')}")
        except Exception as e:
            logger.exception(f"[TASK][RESUME][SDK] SDK resume_task 异常 task_id={task_id} error={e}")
            sdk_result = {'success': False, 'message': f'SDK resume_task 异常: {e}'}
    success = bool(sdk_result and sdk_result.get('success'))
    resp = {
        'success': success,
        'message': '已请求恢复' if success else '恢复失败',
        'backend': 'sdk',
        'loop_updated': loop_updated
    }
    if sdk_result is not None:
        resp['sdk_resume'] = sdk_result
    return jsonify(resp)

@app.route('/api/task/stop', methods=['POST'])
def api_task_stop():
    data = request.get_json(force=True, silent=True) or {}
    task_id = data.get('task_id','').strip()
    if USE_LOCAL_TASK_FALLBACK:
        return jsonify({'success': False,'message':'本地模式不支持停止','backend':'local'})
    logger.info(f"[TASK][STOP][SDK] 请求停止 {task_id}")
    loop_updated = False
    if sdk_loop_orchestrator.get(task_id):
        loop_updated = sdk_loop_orchestrator.stop(task_id)
    sdk_result = None
    if websocket_client:
        try:
            fut = asyncio.run_coroutine_threadsafe(websocket_client.stop_task(task_id), websocket_loop)
            sdk_result = fut.result(timeout=6)
            logger.info(f"[TASK][STOP][SDK] SDK调用结果 task_id={task_id} success={sdk_result and sdk_result.get('success')} msg={sdk_result and sdk_result.get('message')}")
        except Exception as e:
            logger.exception(f"[TASK][STOP][SDK] SDK stop_task 异常 task_id={task_id} error={e}")
            sdk_result = {'success': False, 'message': f'SDK stop_task 异常: {e}'}
    success = bool(sdk_result and sdk_result.get('success'))
    if success:
        # 任务生命周期终止后可选择自动取消订阅（保留最后状态）
        remove_task_status_subscription(task_id)
    resp = {
        'success': success,
        'message': '已请求停止' if success else '停止失败',
        'backend': 'sdk',
        'loop_updated': loop_updated
    }
    if sdk_result is not None:
        resp['sdk_stop'] = sdk_result
    return jsonify(resp)

@app.route('/api/task/delete', methods=['POST'])
def api_task_delete():
    data = request.get_json(force=True, silent=True) or {}
    task_id = data.get('task_id','').strip()
    if USE_LOCAL_TASK_FALLBACK:
        logger.info(f"[TASK][DELETE][LOCAL] 删除任务 {task_id}")
        if local_task_manager.delete_task(task_id):
            return jsonify({'success': True,'message':'删除成功','backend':'local'})
        return jsonify({'success': False,'message':'删除失败','backend':'local'})
    logger.info(f"[TASK][DELETE][SDK] 删除任务 {task_id}")
    loop_deleted = False
    if sdk_loop_orchestrator.get(task_id):
        loop_deleted = sdk_loop_orchestrator.delete(task_id)
    sdk_result = None
    if websocket_client:
        try:
            fut = asyncio.run_coroutine_threadsafe(websocket_client.delete_task(task_id), websocket_loop)
            sdk_result = fut.result(timeout=6)
            logger.info(f"[TASK][DELETE][SDK] SDK delete_task 调用完成 task_id={task_id} success={sdk_result and sdk_result.get('success')}")
        except Exception as e:
            logger.exception(f"[TASK][DELETE][SDK] SDK delete_task 调用异常 task_id={task_id} error={e}")
            sdk_result = {'success': False, 'message': f'SDK delete_task 异常: {e}'}
    success = bool(sdk_result and sdk_result.get('success')) or loop_deleted
    if success:
        remove_task_status_subscription(task_id)
    return jsonify({'success': success,'message':'删除请求已发送' if success else '删除失败','backend':'sdk','loop_deleted': loop_deleted,'sdk_delete': sdk_result})

@app.route('/api/task/status', methods=['GET'])
def api_task_status():
    task_id = request.args.get('task_id','').strip()
    if USE_LOCAL_TASK_FALLBACK:
        t = local_task_manager.get_task(task_id)
        if not t: return jsonify({'success': False,'message':'任务不存在','backend':'local'})
        return jsonify({'success': True,'status': t.to_dict(),'backend':'local'})
    status_dict = {}
    loop_task = sdk_loop_orchestrator.get(task_id)
    if loop_task:
        status_dict.update(loop_task.to_dict())
    # 优先使用实时缓存 (若已订阅)
    with task_status_lock:
        cached = task_status_cache.get(task_id)
        if cached:
            status_dict['realtime'] = cached
            # 覆盖 sdk_state
            if 'state' in cached:
                status_dict['sdk_state'] = cached.get('state')
    # 如果未订阅且前端主动查询, 尝试发起订阅 (懒加载)
    if not status_dict.get('realtime'):
        ensure_task_status_subscription(task_id)
    if not status_dict:
        return jsonify({'success': False,'message':'任务不存在(本地无记录且远端查询失败)','backend':'sdk'})
    status_dict.setdefault('task_id', task_id)
    return jsonify({'success': True,'status': status_dict,'backend':'sdk'})

@app.route('/api/task/status/stream')
def api_task_status_stream():
    """Server-Sent Events (SSE) 推送所有已订阅任务状态更新
    前端示例:
      const es = new EventSource('/api/task/status/stream');
      es.onmessage = ev => { const data = JSON.parse(ev.data); ... }
    """
    def gen():
        last_snapshot = {}
        while True:
            time.sleep(1)
            payload = []
            with task_status_lock:
                for tid, info in task_status_cache.items():
                    prev = last_snapshot.get(tid)
                    # 简单变更检测(时间戳或状态不同)
                    if not prev or prev.get('ts') != info.get('ts') or prev.get('state') != info.get('state'):
                        payload.append(info)
                        last_snapshot[tid] = info.copy()
            if payload:
                yield f"data: {json.dumps({'tasks': payload, 'ts': time.time()})}\n\n"
    return Response(gen(), mimetype='text/event-stream')

@app.route('/api/script_output/stream')
def api_script_output_stream():
        """SSE推送脚本输出内容（从 /api/script_output 订阅缓冲）
        前端订阅示例：
            const es = new EventSource('/api/script_output/stream');
            es.onmessage = ev => { const data = JSON.parse(ev.data); /* data.items: [{ts, content}] */ };
        """
        def gen():
                last_idx = 0
                while True:
                        time.sleep(0.5)
                        # 快照复制，避免并发问题
                        items = list(script_output_buffer)
                        if last_idx < len(items):
                                new_items = items[last_idx:]
                                last_idx = len(items)
                                payload = {'items': new_items, 'ts': time.time()}
                                yield f"data: {json.dumps(payload, ensure_ascii=False)}\n\n"
        return Response(gen(), mimetype='text/event-stream')

# ================= 工厂预加载功能 =================
@app.route('/api/factory/preload', methods=['POST'])
def api_factory_preload():
    """预加载工厂测试所需资源 (精简版):
    1. 拷贝 map.pcd 到目标地图目录
    2. 拷贝 default_nav_points/*.yaml 到 points 目录
    (不再拷贝/创建任务脚本)
    """
    base_dir = Path(__file__).parent / 'factory_test'
    map_src = base_dir / 'map.pcd'
    nav_dir = base_dir / 'default_nav_points'
    target_root = Path('/home/daystarp/sysb-deployment-dev/project/demo_v1.0.0')
    map_target_dir = target_root / 'map'
    points_target_dir = target_root / 'daystar_api' / 'points'
    summary = {'map': None, 'nav_points': []}
    try:
        # 地图
        if map_src.exists():
            map_target_dir.mkdir(parents=True, exist_ok=True)
            shutil.copy2(str(map_src), str(map_target_dir / map_src.name))
            summary['map'] = {'src': str(map_src), 'dst': str(map_target_dir / map_src.name)}
        else:
            logger.warning(f"[FACTORY][PRELOAD] map.pcd 不存在: {map_src}")
        # 点位
        if nav_dir.exists():
            points_target_dir.mkdir(parents=True, exist_ok=True)
            for f in nav_dir.glob('*.yaml'):
                shutil.copy2(str(f), str(points_target_dir / f.name))
                summary['nav_points'].append({'file': f.name})
        else:
            logger.warning(f"[FACTORY][PRELOAD] 默认点位目录缺失: {nav_dir}")
        logger.info(f"[FACTORY][PRELOAD] 完成 summary={summary}")
        return jsonify({'success': True,'summary': summary})
    except Exception as e:
        logger.exception(f"[FACTORY][PRELOAD] 预加载异常 error={e}")
        return jsonify({'success': False,'message': f'预加载异常: {e}','summary': summary})

@app.route('/api/factory/aging_task_script', methods=['GET'])
def api_factory_aging_task_script():
    """返回 factory_aging_task.py 内容(若存在)
    搜索顺序:
      1. 与本文件同级 factory_aging_task.py
      2. factory_test/factory_aging_task.py
      3. target deployment scripts 目录下 factory_aging_task.py
    """
    candidates = []
    try:
        base_dir = Path(__file__).parent
        candidates.append(base_dir / 'factory_aging_task.py')
        candidates.append(base_dir / 'factory_test' / 'factory_aging_task.py')
        target_root = Path('/home/daystarp/sysb-deployment-dev/project/demo_v1.0.0')
        candidates.append(target_root / 'daystar_api' / 'scripts' / 'factory_aging_task.py')
        for p in candidates:
            if p.exists():
                try:
                    content = p.read_text(encoding='utf-8')
                except Exception:
                    content = p.read_text(errors='ignore')
                return jsonify({'success': True, 'path': str(p), 'content': content})
        return jsonify({'success': False, 'message': '未找到 factory_aging_task.py', 'searched': [str(x) for x in candidates]}), 404
    except Exception as e:
        logger.exception(f"[FACTORY][AGING_TASK] 读取异常: {e}")
        return jsonify({'success': False, 'message': f'读取异常: {e}'})

# 创建全局视频流管理器
video_manager = VideoStreamManager()

# 视频流管理
video_streams = {
    'rgb': {
        'url': '',
        'active': False,
        'capture': None,
        'lock': threading.Lock()
    },
    'ir': {
        'url': '',
        'active': False,
        'capture': None,
        'lock': threading.Lock()
    }
}

def generate_frames(stream_type):
    """非阻塞视频帧生成器 - 使用预缓存的帧"""
    logger.info(f"开始生成{stream_type}视频流")
    
    # 确保流已创建
    video_manager.create_stream(stream_type)
    
    frame_count = 0
    last_frame_time = time.time()
    no_frame_count = 0
    max_no_frame = 100  # 最多等待100次无帧情况后结束
    
    while video_manager.is_active(stream_type):
        try:
            # 从缓存获取最新帧（非阻塞）
            frame_bytes = video_manager.get_latest_frame(stream_type)
            
            if frame_bytes is not None:
                # 控制帧率
                current_time = time.time()
                if current_time - last_frame_time < 0.066:  # 约15fps
                    time.sleep(0.01)
                    continue
                
                frame_count += 1
                no_frame_count = 0
                last_frame_time = current_time
                
                # 每30帧输出一次日志
                if frame_count % 30 == 0:
                    logger.debug(f"{stream_type}流式传输第{frame_count}帧")
                
                # 生成多部分响应格式
                yield (b'--frame\r\n'
                       b'Content-Type: image/jpeg\r\n\r\n' + frame_bytes + b'\r\n')
            else:
                no_frame_count += 1
                if no_frame_count > max_no_frame:
                    logger.warning(f"{stream_type}长时间无帧数据，结束流式传输")
                    break
                
                # 等待新帧
                time.sleep(0.033)  # 30ms
                
        except Exception as e:
            logger.error(f"生成{stream_type}视频帧异常: {e}")
            break
    
    logger.info(f"{stream_type}视频流生成结束，共{frame_count}帧")


@app.route('/api/video/config', methods=['POST'])
def config_video_streams():
    """配置视频流地址"""
    try:
        data = request.get_json()
        rgb_url = data.get('rgb_url', '')
        ir_url = data.get('ir_url', '')
        
        # 更新视频流配置
        if rgb_url:
            video_streams['rgb']['url'] = rgb_url
        if ir_url:
            video_streams['ir']['url'] = ir_url
        
        logger.info(f"视频流配置更新: RGB={rgb_url}, IR={ir_url}")
        
        return jsonify({
            'success': True,
            'message': '视频流配置成功',
            'rgb_url': video_streams['rgb']['url'],
            'ir_url': video_streams['ir']['url']
        })
        
    except Exception as e:
        logger.error(f"配置视频流失败: {e}")
        return jsonify({'success': False, 'message': str(e)}), 500


@app.route('/api/video/start/<stream_type>', methods=['POST'])
def start_video_stream(stream_type):
    """启动指定的视频流 - 代理到独立视频服务器（完全非阻塞）"""
    logger.info(f"start_video_stream {stream_type}")
    
    if stream_type not in video_streams:
        return jsonify({'success': False, 'message': '无效的视频流类型'}), 400
    
    try:
        # 获取视频流URL
        stream_url = video_streams[stream_type]['url']
        
        if not stream_url:
            return jsonify({
                'success': False,
                'message': f'{stream_type}视频流地址未配置'
            }), 400
        
        logger.info(f"代理启动{stream_type}视频流: {stream_url}")
        
        # 启动独立视频服务器（如果尚未运行）
        try:
            import requests
            
            # 检查视频服务器是否运行
            try:
                response = requests.get(f'{VIDEO_SERVER_BASE_URL}/health', timeout=2)
                if response.status_code != 200:
                    raise Exception("视频服务器未响应")
            except:
                # 启动视频服务器
                logger.info("启动独立视频服务器...")
                import subprocess
                subprocess.Popen([
                    sys.executable, 'video_stream_server.py'
                ], cwd=os.path.dirname(os.path.abspath(__file__)),
                   stdout=subprocess.PIPE, 
                   stderr=subprocess.PIPE,
                   creationflags=subprocess.CREATE_NEW_PROCESS_GROUP if os.name == 'nt' else 0)
                
                # 等待服务器启动
                time.sleep(3)
            
            # 通过HTTP请求启动视频流
            response = requests.get(
                f'{VIDEO_SERVER_BASE_URL}/start/{stream_type}',
                params={'url': stream_url},
                timeout=10
            )
            
            if response.status_code == 200:
                result = response.json()
                if result['success']:
                    # 更新本地状态
                    video_streams[stream_type]['active'] = True
                    
                    logger.info(f"✅ {stream_type}视频流启动成功（独立服务器）")
                    
                    return jsonify({
                        'success': True,
                        'message': f'{stream_type}视频流启动成功（独立服务器）',
                        'stream_url': f'{VIDEO_SERVER_BASE_URL}/stream/{stream_type}'
                    })
                else:
                    return jsonify({
                        'success': False,
                        'message': f'视频服务器启动失败: {result["message"]}'
                    }), 500
            else:
                return jsonify({
                    'success': False,
                    'message': '无法连接到视频服务器'
                }), 500
                
        except Exception as e:
            logger.error(f"代理启动{stream_type}视频流失败: {e}")
            return jsonify({
                'success': False,
                'message': f'启动视频流失败: {str(e)}'
            }), 500
        
    except Exception as e:
        logger.error(f"启动{stream_type}视频流异常: {e}")
        return jsonify({'success': False, 'message': str(e)}), 500


@app.route('/api/video/stop/<stream_type>', methods=['POST'])
def stop_video_stream(stream_type):
    """停止指定的视频流 - 代理到独立视频服务器（完全非阻塞）"""
    if stream_type not in video_streams:
        return jsonify({'success': False, 'message': '无效的视频流类型'}), 400
    
    try:
        logger.info(f"代理停止{stream_type}视频流")
        
        # 通过HTTP请求停止视频流
        try:
            import requests
            response = requests.get(f'{VIDEO_SERVER_BASE_URL}/stop/{stream_type}', timeout=5)
            
            if response.status_code == 200:
                result = response.json()
                
                # 更新本地状态
                video_streams[stream_type]['active'] = False
                
                logger.info(f"✅ {stream_type}视频流已停止（独立服务器）")
                
                return jsonify({
                    'success': result['success'],
                    'message': result['message']
                })
            else:
                return jsonify({
                    'success': False,
                    'message': '无法连接到视频服务器'
                }), 500
                
        except requests.exceptions.ConnectionError:
            # 如果视频服务器未运行，直接返回成功
            video_streams[stream_type]['active'] = False
            return jsonify({
                'success': True,
                'message': f'{stream_type}视频流已停止（服务器未运行）'
            })
        except Exception as e:
            return jsonify({
                'success': False,
                'message': f'停止视频流失败: {str(e)}'
            }), 500
        
    except Exception as e:
        logger.error(f"停止{stream_type}视频流异常: {e}")
        return jsonify({'success': False, 'message': str(e)}), 500


def cleanup_all_video_streams():
    """清理所有视频流资源 - 使用新的管理器"""
    logger.info("正在清理所有视频流资源...")
    
    try:
        # 使用新的视频流管理器清理
        video_manager.cleanup_all()
        
        # 同步清理老的video_streams状态（向后兼容）
        for stream_type in video_streams:
            video_streams[stream_type]['active'] = False
            if 'capture' in video_streams[stream_type]:
                video_streams[stream_type]['capture'] = None
                
        logger.info("视频流资源清理完成")
        
    except Exception as e:
        logger.error(f"清理视频流资源时出错: {e}")


@app.route('/api/video/stream/<stream_type>')
def video_stream(stream_type):
    """视频流端点 - 重定向到独立视频服务器（完全非阻塞）"""
    if stream_type not in video_streams:
        return "Invalid stream type", 400
    
    # 重定向到独立视频服务器，完全避免主线程阻塞
    from flask import redirect
    return redirect(f'{VIDEO_SERVER_BASE_URL}/stream/{stream_type}', code=302)


@app.route('/api/video/status', methods=['GET'])
def get_video_status():
    """获取所有视频流状态"""
    try:
        status = {}
        for stream_type, stream_info in video_streams.items():
            status[stream_type] = {
                'url': stream_info['url'],
                'active': stream_info['active'],
                'connected': stream_info['capture'] is not None
            }
        
        return jsonify({
            'success': True,
            'streams': status
        })
        
    except Exception as e:
        logger.error(f"获取视频流状态异常: {e}")
        return jsonify({'success': False, 'message': str(e)}), 500


@app.route('/api/video/snapshot/<stream_type>', methods=['POST'])
def take_snapshot(stream_type):
    """截取视频流快照"""
    if stream_type not in video_streams:
        return jsonify({'success': False, 'message': '无效的视频流类型'}), 400
    
    try:
        stream_info = video_streams[stream_type]
        
        if not stream_info['active'] or stream_info['capture'] is None:
            return jsonify({
                'success': False,
                'message': f'{stream_type}视频流未激活'
            }), 400
        
        # 读取当前帧
        ret, frame = stream_info['capture'].read()
        if not ret:
            return jsonify({
                'success': False,
                'message': '无法读取视频帧'
            }), 500
        
        # 编码为JPEG
        ret, buffer = cv2.imencode('.jpg', frame)
        if not ret:
            return jsonify({
                'success': False,
                'message': '无法编码图像'
            }), 500
        
        # 转换为base64
        image_base64 = base64.b64encode(buffer).decode('utf-8')
        
        timestamp = time.strftime('%Y%m%d_%H%M%S')
        filename = f'{stream_type}_snapshot_{timestamp}.jpg'
        
        return jsonify({
            'success': True,
            'message': '截图成功',
            'filename': filename,
            'image_data': f'data:image/jpeg;base64,{image_base64}'
        })
        
    except Exception as e:
        logger.error(f"截取{stream_type}快照异常: {e}")
        return jsonify({'success': False, 'message': str(e)}), 500


# 自动配置视频流地址
def auto_config_video_streams():
    """根据连接状态自动配置视频流地址"""
    try:
        if connection_status.get('websocket_host'):
            # 使用WebSocket主机作为视频流主机
            host = connection_status['websocket_host']
            video_streams['rgb']['url'] = f'rtsp://{host}:554/rgb'
            video_streams['ir']['url'] = f'rtsp://{host}:554/ir'
            logger.info(f"自动配置视频流地址: RGB={video_streams['rgb']['url']}, IR={video_streams['ir']['url']}")
    except Exception as e:
        logger.error(f"自动配置视频流地址失败: {e}")


# ==================== PCD点云地图处理 ====================

def parse_pcd_file(file_path):
    """解析PCD文件并返回点云数据（包含RGB信息）"""
    try:
        points = []
        
        # 获取文件大小信息
        file_size = os.path.getsize(file_path)
        logger.info(f"开始解析PCD文件: {file_path}, 大小: {file_size / (1024*1024):.2f} MB")
        
        with open(file_path, 'rb') as f:
            header_ended = False
            data_format = 'ascii'
            fields = []
            size = []
            type_info = []
            count = []
            points_count = 0
            
            # 读取头部信息
            while not header_ended:
                line = f.readline().decode('utf-8', errors='ignore').strip()
                
                if line.startswith('FIELDS'):
                    fields = line.split()[1:]
                elif line.startswith('SIZE'):
                    size = [int(x) for x in line.split()[1:]]
                elif line.startswith('TYPE'):
                    type_info = line.split()[1:]
                elif line.startswith('COUNT'):
                    count = [int(x) for x in line.split()[1:]]
                elif line.startswith('POINTS'):
                    points_count = int(line.split()[1])
                elif line.startswith('DATA'):
                    data_format = line.split()[1]
                    header_ended = True
            
            has_rgb = 'rgb' in fields
            logger.info(f"PCD文件信息: 字段={fields}, 点数={points_count}, 格式={data_format}, 包含RGB={has_rgb}")
            
            # 对于大型文件，预先计算采样率
            max_points_to_load = 100000  # 最大加载点数
            sample_rate = 1
            if points_count > max_points_to_load:
                sample_rate = max(1, points_count // max_points_to_load)
                logger.info(f"大型文件采样: 每{sample_rate}个点取1个")
            
            # 读取点云数据
            if data_format == 'ascii':
                # ASCII格式
                point_counter = 0
                for _ in range(points_count):
                    line = f.readline().decode('utf-8', errors='ignore').strip()
                    if line:
                        point_counter += 1
                        # 采样处理
                        if point_counter % sample_rate != 0:
                            continue
                            
                        values = line.split()
                        if len(values) >= 3:
                            try:
                                x, y, z = float(values[0]), float(values[1]), float(values[2])
                                # 过滤无效点和异常值
                                if not (np.isnan(x) or np.isnan(y) or np.isnan(z)) and abs(x) < 1000 and abs(y) < 1000 and abs(z) < 1000:
                                    if has_rgb and len(values) >= 4:
                                        # 处理RGB数据
                                        rgb_value = int(float(values[3]))
                                        r = (rgb_value >> 16) & 0xFF
                                        g = (rgb_value >> 8) & 0xFF
                                        b = rgb_value & 0xFF
                                        points.extend([x, y, z, r/255.0, g/255.0, b/255.0])  # 归一化RGB到0-1
                                    else:
                                        points.extend([x, y, z, 0.5, 0.5, 0.5])  # 默认灰色
                            except ValueError:
                                continue
                        
                        # 每10000个点输出一次进度
                        if len(points) // 6 % 10000 == 0 and len(points) > 0:
                            logger.info(f"已处理 {len(points) // 6} 个点...")
            
            elif data_format == 'binary':
                # 二进制格式
                point_step = sum(size)
                point_counter = 0
                xyz_offset = 0
                rgb_offset = 12  # xyz后面的位置
                
                # 计算RGB字段的偏移量
                if has_rgb:
                    for i, field in enumerate(fields):
                        if field == 'rgb':
                            rgb_offset = sum(size[:i])
                            break
                
                for _ in range(points_count):
                    point_data = f.read(point_step)
                    point_counter += 1
                    
                    # 采样处理
                    if point_counter % sample_rate != 0:
                        continue
                        
                    if len(point_data) >= 12:  # 至少需要xyz三个float
                        try:
                            x, y, z = struct.unpack('fff', point_data[:12])
                            if not (np.isnan(x) or np.isnan(y) or np.isnan(z)) and abs(x) < 1000 and abs(y) < 1000 and abs(z) < 1000:
                                if has_rgb and len(point_data) >= rgb_offset + 4:
                                    # 读取RGB数据
                                    rgb_value = struct.unpack('I', point_data[rgb_offset:rgb_offset+4])[0]
                                    r = (rgb_value >> 16) & 0xFF
                                    g = (rgb_value >> 8) & 0xFF
                                    b = rgb_value & 0xFF
                                    points.extend([x, y, z, r/255.0, g/255.0, b/255.0])
                                else:
                                    points.extend([x, y, z, 0.5, 0.5, 0.5])  # 默认灰色
                        except struct.error:
                            continue
                    
                    # 每10000个点输出一次进度
                    if len(points) // 6 % 10000 == 0 and len(points) > 0:
                        logger.info(f"已处理 {len(points) // 6} 个点...")
        
        final_count = len(points) // 6
        logger.info(f"PCD文件解析完成，有效点数: {final_count}")
        return points
        
    except Exception as e:
        logger.error(f"解析PCD文件失败: {e}")
        raise


@app.route('/api/navigation/load_pcd_map', methods=['POST'])
def load_pcd_map():
    """加载PCD点云地图文件"""
    logger.info("load_pcd_map called")
    if np is None:
        return jsonify({
            'success': False,
            'message': 'numpy未安装，无法处理PCD文件。请运行: pip install numpy'
        }), 500
        
    try:
        data = request.get_json()
        file_path = data.get('file_path', '')
        logger.info(f"PCD文件路径: {file_path}")
        
        if not file_path:
            return jsonify({
                'success': False,
                'message': '请提供PCD文件路径'
            }), 400
        
        # 路径处理策略:
        # - 若 file_path 精确等于 './map.pcd' 则使用现有相对路径转换(相对本文件目录)
        # - 否则按传入路径直接检查和加载(不做相对路径转换)
        if file_path == './map.pcd':
            if not os.path.isabs(file_path):
                # 获取当前脚本所在目录
                script_dir = os.path.dirname(os.path.abspath(__file__))
                original_path = file_path
                file_path = os.path.join(script_dir, file_path)
                logger.info(f"相对路径转换: {original_path} -> {file_path}")
        # 其它值：不进行路径转换，直接按传入路径检查文件是否存在
        
        # 检查文件是否存在
        if not os.path.exists(file_path):
            logger.error(f"文件不存在: {file_path}")
            return jsonify({
                'success': False,
                'message': f'PCD文件不存在: {file_path}'
            }), 404
        
        # 解析PCD文件
        logger.info(f"开始加载PCD文件: {file_path}")
        point_data = parse_pcd_file(file_path)
        
        return jsonify({
            'success': True,
            'message': f'PCD地图加载成功',
            'point_data': point_data,
            'point_count': len(point_data) // 6,  # 每个点包含x,y,z,r,g,b
            'file_path': file_path
        })
        
    except Exception as e:
        logger.error(f"加载PCD地图失败: {e}")
        return jsonify({
            'success': False,
            'message': f'加载失败: {str(e)}'
        }), 500


@app.route('/api/navigation/pcd_info', methods=['GET'])
def get_pcd_info():
    """获取PCD文件信息（不加载完整数据）"""
    try:
        file_path = request.args.get('file_path', '')
        
        if not file_path or not os.path.exists(file_path):
            return jsonify({
                'success': False,
                'message': '文件不存在'
            }), 404
        
        # 只读取头部信息
        with open(file_path, 'r', encoding='utf-8', errors='ignore') as f:
            lines = []
            for _ in range(20):  # 读取前20行应该包含所有头部信息
                line = f.readline().strip()
                if not line or line.startswith('DATA'):
                    break
                lines.append(line)
        
        # 解析基本信息
        info = {'fields': [], 'points': 0, 'format': 'unknown'}
        for line in lines:
            if line.startswith('FIELDS'):
                info['fields'] = line.split()[1:]
            elif line.startswith('POINTS'):
                info['points'] = int(line.split()[1])
            elif line.startswith('DATA'):
                info['format'] = line.split()[1]
        
        return jsonify({
            'success': True,
            'info': info,
            'file_path': file_path
        })
        
    except Exception as e:
        logger.error(f"获取PCD信息失败: {e}")
        return jsonify({
            'success': False,
            'message': str(e)
        }), 500


@app.route('/api/navigation/create_demo_pcd', methods=['POST'])
def create_demo_pcd():
    """创建一个演示用的PCD文件"""
    if np is None:
        return jsonify({
            'success': False,
            'message': 'numpy未安装，无法创建PCD文件。请运行: pip install numpy'
        }), 500
        
    try:
        data = request.get_json()
        file_path = data.get('file_path', './demo_map.pcd')
        
        # 处理相对路径
        if not os.path.isabs(file_path):
            script_dir = os.path.dirname(os.path.abspath(__file__))
            file_path = os.path.join(script_dir, file_path)
        
        # 创建演示点云数据（一个简单的房间结构）
        points = []
        
        # 创建房间墙壁
        for x in np.linspace(-5, 5, 100):
            points.extend([x, -5, 0])  # 下墙
            points.extend([x, 5, 0])   # 上墙
        
        for y in np.linspace(-5, 5, 100):
            points.extend([-5, y, 0])  # 左墙
            points.extend([5, y, 0])   # 右墙
        
        # 添加一些障碍物
        for x in np.linspace(-2, 2, 50):
            for y in np.linspace(-1, 1, 20):
                points.extend([x, y, 0])
        
        # 添加一些随机点
        for _ in range(1000):
            x = np.random.uniform(-4, 4)
            y = np.random.uniform(-4, 4)
            z = np.random.uniform(-0.5, 0.5)
            points.extend([x, y, z])
        
        # 创建PCD文件
        os.makedirs(os.path.dirname(file_path) if os.path.dirname(file_path) else '/tmp', exist_ok=True)
        
        with open(file_path, 'w') as f:
            f.write("# .PCD v0.7 - Point Cloud Data file format\n")
            f.write("VERSION 0.7\n")
            f.write("FIELDS x y z\n")
            f.write("SIZE 4 4 4\n")
            f.write("TYPE F F F\n")
            f.write("COUNT 1 1 1\n")
            f.write(f"WIDTH {len(points) // 3}\n")
            f.write("HEIGHT 1\n")
            f.write("VIEWPOINT 0 0 0 1 0 0 0\n")
            f.write(f"POINTS {len(points) // 3}\n")
            f.write("DATA ascii\n")
            
            for i in range(0, len(points), 3):
                f.write(f"{points[i]:.6f} {points[i+1]:.6f} {points[i+2]:.6f}\n")
        
        logger.info(f"演示PCD文件已创建: {file_path}, 点数: {len(points) // 3}")
        
        return jsonify({
            'success': True,
            'message': f'演示PCD文件创建成功',
            'file_path': file_path,
            'point_count': len(points) // 3
        })
        
    except Exception as e:
        logger.error(f"创建演示PCD文件失败: {e}")
        return jsonify({
            'success': False,
            'message': str(e)
        }), 500


@app.route('/api/navigation/load_rgb_image', methods=['POST'])
def load_rgb_image():
    """加载RGB图像文件"""
    try:
        data = request.get_json()
        file_path = data.get('file_path', '')
        
        if not file_path:
            return jsonify({
                'success': False,
                'message': '文件路径不能为空'
            }), 400
            
        # 处理相对路径
        if not os.path.isabs(file_path):
            script_dir = os.path.dirname(os.path.abspath(__file__))
            file_path = os.path.join(script_dir, file_path)
        
        # 检查文件是否存在
        if not os.path.exists(file_path):
            return jsonify({
                'success': False,
                'message': f'文件不存在: {file_path}'
            }), 404
            
        # 检查是否为支持的图像格式
        supported_formats = ['.jpg', '.jpeg', '.png', '.gif', '.bmp', '.webp']
        file_ext = os.path.splitext(file_path)[1].lower()
        if file_ext not in supported_formats:
            return jsonify({
                'success': False,
                'message': f'不支持的图像格式: {file_ext}。支持的格式: {", ".join(supported_formats)}'
            }), 400
        
        # 获取图像信息
        file_size = os.path.getsize(file_path)
        file_name = os.path.basename(file_path)
        
        # 返回文件信息
        return jsonify({
            'success': True,
            'message': 'RGB图像信息获取成功',
            'file_path': file_path,
            'file_name': file_name,
            'file_size': file_size,
            'format': file_ext[1:].upper()
        })
        
    except Exception as e:
        print(f"加载RGB图像失败: {e}")
        return jsonify({
            'success': False,
            'message': f'加载RGB图像失败: {str(e)}'
        }), 500


@app.route('/api/navigation/serve_image/<path:filename>')
def serve_image(filename):
    """提供图像文件服务"""
    try:
        # 安全检查：防止路径遍历攻击
        filename = os.path.basename(filename)
        script_dir = os.path.dirname(os.path.abspath(__file__))
        file_path = os.path.join(script_dir, filename)
        
        if os.path.exists(file_path):
            return send_file(file_path)
        else:
            return jsonify({
                'success': False,
                'message': f'图像文件不存在: {filename}'
            }), 404
            
    except Exception as e:
        return jsonify({
            'success': False,
            'message': f'提供图像文件失败: {str(e)}'
        }), 500

# ==================== 航插测试接口 ====================
@app.route('/api/robot/usb_check', methods=['POST'])
def check_usb():
    """检测USB设备状态"""
    try:
        # 获取当前文件所在目录的路径
        current_dir = os.path.dirname(os.path.abspath(__file__))
        # 构建脚本路径
        script_path = os.path.join(current_dir, 'factory_test', 'jetson_usb.sh')
        # 检查脚本是否存在
        if not os.path.exists(script_path):
            return jsonify({
                'status': 'error',
                'message': 'USB检测脚本不存在'
            }), 500
        # 执行脚本
        result = subprocess.run(
            ['bash', script_path],
            capture_output=True,
            text=True,
            timeout = 10  # 30秒超时
        )
        # 根据返回码判断USB状态
        if result.returncode == 0:
            usb_status = 'inserted'
            message = '检测到U盘插入'
        else:
            usb_status = 'not_found'
            message = '未检测到U盘'
        # 返回执行结果
        return jsonify({
            'status': 'success',
            'usb_status': usb_status,
            'message': message,
            'returncode': result.returncode,
            'stdout': result.stdout,
            'stderr': result.stderr
        })
    except subprocess.TimeoutExpired:
        return jsonify({
            'status': 'error',
            'message': 'USB检测超时'
        }), 500
    except Exception as e:
        return jsonify({
            'status': 'error',
            'message': f'USB检测失败: {str(e)}'
        }), 500

if __name__ == '__main__':
    print("启动Demo API服务器...")
    print(f"访问 http://{SERVER_IP}:5010 来测试API")
    print("可用页面:")
    print(f"  - 主控制面板: http://{SERVER_IP}:5010/demo_gui.html")
    print(f"  - 工厂测试页面: http://{SERVER_IP}:5010/factory")
    print(f"  - 工厂测试页面: http://{SERVER_IP}:5010/factory_test.html")
    
    try:
        # 使用多进程模式提高并发性能
        app.run(host='0.0.0.0', port=5010, debug=True, threaded=True, processes=1)
    except KeyboardInterrupt:
        print("\n正在关闭服务器...")
        cleanup_all_video_streams()
        print("服务器已关闭")
