# -*- coding: utf-8 -*-
import logging
import sys
import threading
import uuid
from contextlib import closing
import socket
import flask
import time
import numpy as np
from flask import request
from ability_sdk.src.AbilitySDK import (ability_stub, ipc_server_handle, log,
                                        task)

from ability_sdk.src.AbilitySDK.task import TaskStatus, TaskState, update_task_status

import json


import math
import argparse
import os
from pathlib import Path
import re
from urllib.parse import urlparse

# 强制使用项目目录中的SDK（不使用WebSocket的版本）
project_root = os.path.dirname(os.path.abspath(__file__))
project_sdk_path = os.path.join(project_root, 'kuavo_humanoid_sdk')

# 移除系统安装的SDK路径
sys.path = [p for p in sys.path if 'site-packages/kuavo_humanoid_sdk' not in p]

# 将项目SDK路径插入到最前面
if project_sdk_path not in sys.path:
    sys.path.insert(0, project_sdk_path)

from kuavo_humanoid_sdk import KuavoSDK, KuavoRobot, KuavoRobotState, KuavoRobotTools, KuavoRobotHead
from kuavo_humanoid_sdk.interfaces.data_types import KuavoPose
def find_free_port():
    """Find a random available port."""
    with closing(socket.socket(socket.AF_INET, socket.SOCK_STREAM)) as s:
        s.bind(('', 0))
        s.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
        return s.getsockname()[1]
        
def _detect_ability_name(default: str = "HeadAbility") -> str:

    env_name = os.environ.get('ABILITY_NAME')
    if env_name:
        return env_name.strip()
    try:
        base = Path(sys.argv[0]).name
        # Strip special '.ability' suffix
        if base.endswith('.ability'):
            base = base[:-len('.ability')]
        # Strip generic extension if any remains (e.g., .exe, .bin, .py)
        if '.' in base:
            base = base.split('.', 1)[0]
        base = base or default
        # If caller's default endswith 'Ability' and the detected base does not, return the default
        # This enables calls like _detect_ability_name('HeadAbility') to yield 'HeadAbility' when binary is 'Head.ability'
        if default.endswith('Ability') and not base.endswith('Ability'):
            return default
        return base
    except Exception:
        return default

# ------------------ Config discovery & parsing helpers ------------------
# These helpers let us run from source or PyInstaller, and read robotUri either
# at the top level or nested under a 'config:' block in YAML/JSON-ish files.

LAST_CFG_PATH = None  # set by _load_robot_host_port for logging/diagnostics


def _iter_candidate_bases():
    bases = []
    # __file__ directory
    try:
        bases.append(Path(__file__).resolve())
    except Exception:
        pass
    # CWD
    try:
        bases.append(Path.cwd().resolve())
    except Exception:
        pass
    # PyInstaller temp dir
    meipass = getattr(sys, '_MEIPASS', None)
    if meipass:
        try:
            bases.append(Path(meipass).resolve())
        except Exception:
            pass
    return bases


essential_env_keys = ('ABILITY_ROOT', 'ABILITY_FRAMEWORK_HOME', 'ABILITY_HOME')


def _find_crs_dir() -> Path | None:
    """Walk upward from bases to find a directory containing a child named 'crs'.
    Prefer a parent that has BOTH 'crs' and 'packages' (siblings), but accept just 'crs' if needed.
    Honor environment overrides if they contain 'crs'.
    """
    # Environment overrides first
    for env_key in essential_env_keys:
        env_home = os.environ.get(env_key)
        if env_home:
            try:
                p = Path(env_home).resolve()
                crs = p / 'crs'
                if crs.exists() and crs.is_dir():
                    return crs
            except Exception:
                pass

    # Search upward from candidates
    candidates = _iter_candidate_bases()
    best_match = None
    for base in candidates:
        for parent in [base] + list(base.parents):
            crs = parent / 'crs'
            if crs.exists() and crs.is_dir():
                # Prefer if sibling 'packages' also exists
                packages = parent / 'packages'
                if packages.exists() and packages.is_dir():
                    return crs
                if best_match is None:
                    best_match = crs
    return best_match


def _extract_field(text: str, key: str) -> str | None:
    """Extract a simple string field from YAML/JSON-ish text without extra deps.
    Supports both JSON style ("key": "value") and YAML style (key: value),
    and allows leading spaces (for nested under 'config:').
    """
    # JSON style: "key": "..."
    m = re.search(rf'"{re.escape(key)}"\s*:\s*"([^"]+)"', text)
    if m:
        return m.group(1)
    # YAML style: key: value (quotes optional)
    m = re.search(rf'^[ \t]*{re.escape(key)}\s*:\s*([\"\']?)([^\n\r#\"\']+)\1\s*$', text, re.MULTILINE)
    if m:
        return m.group(2).strip()
    return None


def _find_config_for_ability(ability_name: str) -> Path | None:
    """Scan crs/*.yaml|*.yml and pick the one whose abilityName matches ability_name.
    If multiple match, prefer filename containing the ability name (case-insensitive), else the first.
    """
    crs_dir = _find_crs_dir()
    if not crs_dir:
        return None
    yaml_files = list(crs_dir.glob('*.yaml')) + list(crs_dir.glob('*.yml'))
    matches = []
    for p in yaml_files:
        try:
            text = p.read_text(encoding='utf-8')
        except Exception:
            continue
        name = _extract_field(text, 'abilityName')
        if name and name.strip() == ability_name:
            matches.append((p, text))
    if not matches:
        return None
    # Prefer file name containing the ability name (loose heuristic)
    lower = ability_name.lower()
    for p, _ in matches:
        if lower in p.name.lower():
            return p
    return matches[0][0]


def _find_crs_config_file() -> tuple[Path | None, str]:
    crs_dir = _find_crs_dir()
    if not crs_dir:
        return None, 'crs dir not found'
    # First try ability-specific yaml selection
    p = _find_config_for_ability(ABILITY_NAME or '')
    if p:
        return p, 'matched by abilityName'
    # Fallback to generic names
    candidates = [
        crs_dir / 'config',
        crs_dir / 'config.yaml',
        crs_dir / 'config.yml',
        crs_dir / 'config.json',
    ]
    for p in candidates:
        if p.exists() and p.is_file():
            return p, 'fallback generic config name'
    return None, 'no known config file'


def _parse_robot_uri_from_text(text: str) -> str | None:
    """Extract robotUri from either top-level or nested under 'config:'.
    Tries JSON style and YAML style. Quotes are optional in YAML.
    """
    # Try top-level first
    uri = _extract_field(text, 'robotUri')
    if uri:
        return uri
    # If nested, try to roughly capture a 'config: ... robotUri: ...' block
    # This is already handled by allowing leading spaces in _extract_field, so return None
    return None


def _load_robot_host_port(default_host: str = '192.168.20.131', default_port: int = 9090) -> tuple[str, int]:
    """Read ws host/port from crs yaml/json. Priority:
    1) YAML whose abilityName == RAW executable name (e.g., 'Head')
    2) YAML whose abilityName == fallback from _detect_ability_name('HeadAbility') (e.g., 'HeadAbility')
    3) Generic config files in crs (config/config.yaml/...)
    Otherwise fall back to defaults.
    """
    global LAST_CFG_PATH, ABILITY_NAME

    # --- Debug: starting lookup
    try:
        primary_name = (ABILITY_NAME or '').strip()
        fallback_name = _detect_ability_name('HeadAbility')
        logging.warning(f"[CFG] lookup start: primary='{primary_name}', fallback='{fallback_name}'")
    except Exception:
        primary_name = (ABILITY_NAME or '').strip()
        fallback_name = 'HeadAbility'

    cfg_path = None

    # Step 1: try primary (raw) name first
    if primary_name:
        cfg_path = _find_config_for_ability(primary_name)
        if cfg_path:
            try:
                logging.warning(f"[CFG] matched by primary abilityName: {primary_name} -> {cfg_path}")
            except Exception:
                pass

    # Step 2: if not found, try fallback ability name
    if not cfg_path and fallback_name and fallback_name != primary_name:
        retry = _find_config_for_ability(fallback_name)
        if retry:
            cfg_path = retry
            try:
                # Update global so downstream logs/config reflect the final name
                ABILITY_NAME = fallback_name
                logging.warning(f"[CFG] matched by fallback abilityName: {fallback_name} -> {cfg_path}")
            except Exception:
                pass

    # Step 3: generic fallback files in crs
    if not cfg_path:
        cfg_path_generic, reason = _find_crs_config_file()
        cfg_path = cfg_path_generic
        try:
            crs_dir = _find_crs_dir()
            logging.warning(f"[CFG] generic fallback: reason='{reason}', crs_dir={crs_dir}, chosen={cfg_path}")
            if not cfg_path and crs_dir:
                yfiles = list(crs_dir.glob('*.yaml')) + list(crs_dir.glob('*.yml'))
                logging.warning(f"[CFG] yaml candidates: {[p.name for p in yfiles]}")
        except Exception:
            pass

    # If still nothing, return defaults
    if not cfg_path:
        LAST_CFG_PATH = None
        logging.warning(f"[CFG] no config matched; using defaults {default_host}:{default_port}")
        return default_host, default_port

    # Parse the chosen file
    try:
        text = cfg_path.read_text(encoding='utf-8')
        # Extract robotUri even if nested under config
        uri = _extract_field(text, 'robotUri')
        if not uri:
            # Fallback to permissive regex (legacy)
            m = re.search(r'robotUri\s*:\s*[\"\']?([^\n\r#\"\']+)', text)
            uri = m.group(1).strip() if m else None
        if not uri:
            LAST_CFG_PATH = str(cfg_path)
            logging.warning(f"[CFG] robotUri not found in {cfg_path}; using defaults {default_host}:{default_port}")
            return default_host, default_port
        parsed = urlparse(uri)
        host = parsed.hostname or default_host
        port = parsed.port or default_port
        LAST_CFG_PATH = str(cfg_path)
        logging.warning(f"[CFG] using {host}:{port} from {cfg_path}")
        return host, int(port)
    except Exception as e:
        LAST_CFG_PATH = str(cfg_path)
        logging.warning(f"[CFG] failed to parse {cfg_path}: {e}; using defaults {default_host}:{default_port}")
        return default_host, default_port

ABILITY_NAME = None
class Head(ability_stub.AbilityInterface):
    def __init__(self):
        # Determine ability name from packaged binary (e.g., 'HeadAbility.ability' -> 'HeadAbility')
        global ABILITY_NAME
        # Use raw executable prefix first (e.g., 'Head.ability' -> 'Head')
        base = Path(sys.argv[0]).name
        if base.endswith('.ability'):
            base = base[:-len('.ability')]
        if '.' in base:
            base = base.split('.', 1)[0]
        ABILITY_NAME = base or 'Head'
        logging.warning(f"[CFG] Detected RAW primary='{ABILITY_NAME}', fallback='{_detect_ability_name('HeadAbility')}'")

        # 初始化 SDK - 添加更详细的错误处理
        try:
            if not KuavoSDK().Init(log_level='INFO'):
                logging.error("KuavoSDK init failed")
                logging.error("请确保已经启动 ROS 和相关节点:")
                logging.error("  1. 启动 roscore")
                logging.error("  2. 启动机器人控制器，例如:")
                logging.error("     roslaunch humanoid_controllers load_kuavo_real.launch")
                logging.error("     或")
                logging.error("     roslaunch humanoid_controllers load_kuavo_mujoco_sim.launch")
                sys.exit(1)
        except Exception as e:
            logging.error(f"KuavoSDK 初始化失败: {e}")
            logging.error("请确保:")
            logging.error("  1. ROS 环境已正确配置 (source setup.bash)")
            logging.error("  2. roscore 正在运行")
            logging.error("  3. 机器人控制器节点正在运行")
            logging.error("  4. 检查 ROS_MASTER_URI 环境变量")
            sys.exit(1)
            
        self.robot = KuavoRobot()
        self.robot_state = KuavoRobotState()
        self.robot_tools = KuavoRobotTools()
        self.robot_head = KuavoRobotHead()
        logging.warning("HeadAbility initialized")
        logging.warning(f"HeadAbility abilityName='{ABILITY_NAME}' from executable")
        self.app = flask.Flask(__name__)
        self.server_thread = None
        self.port = None
        self.server = None
        self.running_flag = False
        self.head_joint_control_flag = False
        self.target_tracking_flag = False
        self.track_position = None
        # 启动任务路由
        @self.app.route('/api/task/start_task', methods=['POST'])
        def start_task_route():
            try:
                data = json.loads(request.data)
                if data is None:
                    return {"error": "Invalid or missing JSON data"}, 400
                task_type = data.get("task_type", -1)
                if task_type != 0 and task_type != 1 :
                    return {"error": "Unsupported task_type or no task_type provided"}, 401
                # 根据任务类型选择执行函数
                if task_type == 0:
                    result = task.execute_task_simple(
                        self.head_joint_control_func,
                        executor_id=uuid.uuid4(),
                        input=data.get("payload")
                    )
                    return result, 200
                elif task_type == 1:
                    result = task.execute_task_simple(
                        self.target_tracking_func,
                        executor_id=uuid.uuid4(),
                        input=data.get("payload")
                    )
                    return result, 200
                elif task_type == 2:
                    result = task.execute_task_simple(
                        self.head_current_pose,
                        executor_id=uuid.uuid4(),
                        input=data.get("payload")
                    )
                    return result, 200
            except Exception as e:
                return {"error": f"Failed to process request: {str(e)}"}, 400
               
        
        # 状态路由
        @self.app.route('/api/status', methods=['GET'])
        def status_route():
            task_type = request.args.get('task_type', type=int)
            output_type = request.args.get('output_type', type=int)
            
            # 关节角反馈 (task_type=0, output_type=0)
            if task_type == 0 and output_type == 0:
                return self.robot_state.head_joint_state()
            
            return {"message": "Unsupported status query"}

        # 取消任务路由
        @self.app.route('/api/task/cancel_task', methods=['POST'])
        def cancel_task_route():
            try:
                data = request.get_json()
                if data is None:
                    return {"error": "Invalid or missing JSON data"}, 400
                task_type = data.get("task_type", -1)
                if task_type != 1:
                    return {"error": "Unsupported task_type or no task_type provided"}, 401
                if task_type == 0:
                    result = task.execute_task_simple(
                        self.cancel_head_joint_control_func,
                        executor_id=uuid.uuid4(),
                        input=data.get("payload")
                    )
                    return result,200
                if task_type == 1:  # 仅目标追踪任务支持取消
                    result = task.execute_task_simple(
                        self.CancelTargetTracking,
                        executor_id=uuid.uuid4(),
                        input=data.get("payload")
                    )
                    return result,200    
            except Exception as e:
                return {"error": f"Failed to process request: {str(e)}"}, 400
    def head_joint_control_func(self, input: dict = None, task_status: TaskStatus = None) -> dict:
        """
        头部关节角控制函数 - 符合API接口规范
        
        输入参数 (按照API文档):
            - yaw: yaw角度 (弧度，必填)
            - pitch: pitch角度 (弧度，必填)
            - yaw_threshold: yaw角度阈值 (弧度，可选，默认0.15)
            - pitch_threshold: pitch角度阈值 (弧度，可选，默认0.15)
            - timeout: 超时时间 (秒，可选，默认30)
            - interval: feedback间隔 (秒，可选，默认0.3)
        
        返回:
            执行过程中payload包含: {"yaw": float, "pitch": float}
            执行成功后payload为空
        """
        func_start = time.time()
        
        if self.running_flag:
            print("[Head] ERROR: Another task is already running")
            return {"message": "another task is running", "state": task.TaskState.error}
        
        # 验证必填参数
        if 'yaw' not in input or 'pitch' not in input:
            print("[Head] ERROR: Missing required parameters: yaw and pitch")
            return {"message": "Missing required parameters: yaw and pitch", "state": task.TaskState.error}
        
        # 解析参数 - 调整默认值以提高成功率
        target_yaw = float(input.get('yaw', 0.0))
        target_pitch = float(input.get('pitch', 0.0))
        
        # 处理阈值参数：如果传入0或None，使用默认值0.15
        yaw_threshold_input = input.get("yaw_threshold", 0.15)
        pitch_threshold_input = input.get("pitch_threshold", 0.15)
        
        yaw_threshold = float(yaw_threshold_input) if yaw_threshold_input and float(yaw_threshold_input) > 0 else 0.15
        pitch_threshold = float(pitch_threshold_input) if pitch_threshold_input and float(pitch_threshold_input) > 0 else 0.15
        
        timeout = float(input.get("timeout", 30.0))  # 增加到30秒
        interval = float(input.get("interval", 0.3))  # 减小到0.3秒，更频繁检查
        
        print(f"[Head] 参数解析: 输入yaw_threshold={yaw_threshold_input}, 使用yaw_threshold={yaw_threshold:.4f}rad")
        print(f"[Head] 参数解析: 输入pitch_threshold={pitch_threshold_input}, 使用pitch_threshold={pitch_threshold:.4f}rad")
        
        print(f"[Head] ========== 开始头部控制任务 ==========")
        print(f"[Head] 目标位置: yaw={math.degrees(target_yaw):.2f}° ({target_yaw:.4f}rad), pitch={math.degrees(target_pitch):.2f}° ({target_pitch:.4f}rad)")
        print(f"[Head] 参数设置: yaw_threshold={math.degrees(yaw_threshold):.2f}°, pitch_threshold={math.degrees(pitch_threshold):.2f}°")
        print(f"[Head] 参数设置: timeout={timeout}s, interval={interval}s")
        logging.info(f"[Head] Moving to: yaw={math.degrees(target_yaw):.1f}°, pitch={math.degrees(target_pitch):.1f}°")
        
        # 发送控制命令
        print(f"[Head] 发送控制命令到机器人...")
        control_result = self.robot_head.control_head(yaw=target_yaw, pitch=target_pitch)
        print(f"[Head] 控制命令发送结果: {control_result}")
        
        if not control_result:
            print("[Head] ERROR: Control command failed")
            return {"message": "Control failed, please check the parameters", "state": task.TaskState.error}
        
        self.running_flag = True
        self.head_joint_control_flag = True
        start_time = time.time()
        check_count = 0
        
        print(f"[Head] 开始等待头部到达目标位置...")
        
        # 等待到达目标位置
        while self.head_joint_control_flag and (time.time() - start_time) < timeout:
            try:
                check_count += 1
                elapsed = time.time() - start_time
                
                # 获取当前头部状态
                head_state = self.robot_state.head_joint_state()
                if head_state and hasattr(head_state, 'position') and len(head_state.position) >= 2:
                    current_yaw = head_state.position[0]
                    current_pitch = head_state.position[1]
                    
                    # 计算误差
                    yaw_error = abs(target_yaw - current_yaw)
                    pitch_error = abs(target_pitch - current_pitch)
                    
                    print(f"[Head] 检查#{check_count} (耗时{elapsed:.2f}s): "
                          f"当前yaw={math.degrees(current_yaw):.2f}° (误差{math.degrees(yaw_error):.2f}°), "
                          f"当前pitch={math.degrees(current_pitch):.2f}° (误差{math.degrees(pitch_error):.2f}°)")
                    
                    # 更新任务状态 - 按照API规范返回当前yaw和pitch
                    task_status.payload["yaw"] = current_yaw
                    task_status.payload["pitch"] = current_pitch
                    task.update_task_status(task_status)
                    
                    # 检查是否到达
                    print(f"[Head] 阈值检查: yaw_threshold={math.degrees(yaw_threshold):.2f}° ({yaw_threshold:.4f}rad), "
                          f"pitch_threshold={math.degrees(pitch_threshold):.2f}° ({pitch_threshold:.4f}rad)")
                    yaw_reached = self._angle_check(current_yaw, target_yaw, yaw_threshold)
                    pitch_reached = self._angle_check(current_pitch, target_pitch, pitch_threshold)
                    
                    print(f"[Head] 到达状态: yaw_reached={yaw_reached}, pitch_reached={pitch_reached}")
                    
                    if yaw_reached and pitch_reached:
                        print(f"[Head] ✓ 成功到达目标位置！总耗时{elapsed:.2f}s，检查次数{check_count}")
                        logging.info(f"[Head] Target position reached")
                        break
                else:
                    print(f"[Head] WARNING: 无法获取有效的头部状态数据")
            except Exception as e:
                print(f"[Head] ERROR: 获取头部状态失败: {e}")
                logging.warning(f"[Head] Failed to get head state: {e}")
            
            time.sleep(interval)
        
        # 确定返回状态
        elapsed_total = time.time() - start_time
        if not self.head_joint_control_flag:
            print(f"[Head] ✗ 任务被取消 (耗时{elapsed_total:.2f}s)")
            ret = {"message": "Task cancelled", "state": task.TaskState.cancelled}
        elif time.time() - start_time >= timeout:
            print(f"[Head] ✗ 任务超时 (耗时{elapsed_total:.2f}s，超时限制{timeout}s)")
            print(f"[Head] 建议: 增加timeout参数或放宽threshold参数")
            ret = {"message": "Task timeout", "state": task.TaskState.error}
        else:
            print(f"[Head] ✓ 任务成功完成 (耗时{elapsed_total:.2f}s)")
            ret = {"message": "Arrived", "state": task.TaskState.finished}
        
        self.running_flag = False
        self.head_joint_control_flag = False
        func_end = time.time()
        print(f"[Head] ========== 头部控制任务结束 ==========")
        logging.info(f"[Head] task_type=0 head control duration: {func_end - func_start:.3f}s")
        return ret
    def head_current_pose(self, input: dict = None, task_status: TaskStatus = None) -> dict:
        yaw = self.robot_state.head_joint_state().position[0]
        pitch = self.robot_state.head_joint_state().position[1]
        ret = {"message": "success", "state": task.TaskState.finished, "output": {"yaw": yaw, "pitch": pitch}}
        return ret
    def target_tracking_func(self, input: dict = None, task_status: TaskStatus = None) -> dict:
        if self.running_flag:
            return {"message": "another task is running", "state": task.TaskState.error}
        if 'source' not in input or input.get("source") not in ["camera", "base", "odom"]:
            return {"message": "no valid source provided", "state": task.TaskState.error}
        if input.get("source") in ["camera"]:
            return {"message": "not implemented yet", "state": task.TaskState.error}
        if 'position' not in input or 'x' not in input.get("position") or 'y' not in input.get("position") or 'z' not in input.get("position"):
            return {"message": "missing required para", "state": task.TaskState.error}
        if 'camera_height' not in input:
            return {"message": "missing camera_height", "state": task.TaskState.error}
        interval = input.get("interval", 1)
        source = input.get("source")
        camera_height = input.get("camera_height", 0.75)  # 摄像头在机器人坐标系下高度
        if source == "odom":
            self.track_position = input.get("position")
        elif source == "base":
            base_pose = KuavoPose(
                position=[input.get("position").get("x"), input.get("position").get("y"), input.get("position").get("z")],
                orientation=[0, 0, 0, 1] # 使用默认值
            )
        # 获取base_link到odom的坐标变换
            base_to_odom = self.robot_tools.get_tf_transform("odom", "base_link")
            # 确保返回的是可迭代对象
            if not isinstance(base_to_odom.position, (list, tuple, np.ndarray)):
                return {"message": "TF变换位置信息格式错误","state":task.TaskState.error}
            if not isinstance(base_to_odom.orientation, (list, tuple, np.ndarray)):
                return {"message": "TF变换姿态信息格式错误","state":task.TaskState.error}
            # 转换目标姿态到odom坐标系
            pose_odom = self._transform_to_odom(base_pose, base_to_odom)
            self.track_position = {"x": pose_odom.position[0], "y": pose_odom.position[1], "z": pose_odom.position[2]} 
        self.running_flag = True
        self.target_tracking_flag = True
        while self.target_tracking_flag :
            if not self._enable_head_tracking(
                self.track_position,
                camera_height=camera_height,
            ):
                ret = {"message": "Head Control Failed, or target can not be tracking"}
                self.running_flag = False
                self.target_tracking_flag = False
                return ret
            time.sleep(interval)
        ret = {"message": "Task Cancel"}
        self.running_flag = False
        self.target_tracking_flag = False
        return ret
    def _transform_to_odom(self, pose, transform):
        """将姿态从base_link转换到odom坐标系"""
	        # 位置转换（显式转换为numpy数组）
        pos_base = np.array(pose.position)
        transform_pos = np.array(transform.position)
        
        # 使用显式类型转换确保运算正确
        rotated_pos = self._quaternion_rotate(
            np.array(transform.orientation),  # 确保四元数是numpy数组
            pos_base
        )
        pos_odom = transform_pos + rotated_pos
        
	        # 姿态转换（显式转换为numpy数组）
        transform_quat = np.array(transform.orientation)
        pose_quat = np.array(pose.orientation)
        rot_odom = self._quaternion_multiply(transform_quat, pose_quat)
        
        # 转换为Python原生类型
        return KuavoPose(
            position=tuple(pos_odom.tolist()),
            orientation=rot_odom  # rot_odom 已经是列表，不需要转换
        )
    def _quaternion_rotate(self, q, v):
        """
        使用四元数旋转向量
        q: 四元数 [x, y, z, w]
        v: 三维向量 [x, y, z]
        """
        q = np.array(q)
        v = np.array(v)
        q_conj = np.array([-q[0], -q[1], -q[2], q[3]])
        v_quat = np.array([v[0], v[1], v[2], 0.0])
        rotated = self._quaternion_multiply(self._quaternion_multiply(q, v_quat), q_conj)
        return rotated[:3]
    
    def _quaternion_multiply(self, q1, q2):
        """
        四元数乘法，用于组合旋转
        q1, q2: 两个四元数 [x, y, z, w]
        """
        x1, y1, z1, w1 = q1
        x2, y2, z2, w2 = q2
        
        w = w1 * w2 - x1 * x2 - y1 * y2 - z1 * z2
        x = w1 * x2 + x1 * w2 + y1 * z2 - z1 * y2
        y = w1 * y2 + y1 * w2 + z1 * x2 - x1 * z2
        z = w1 * z2 + z1 * w2 + x1 * y2 - y1 * x2
        
        return [x, y, z, w]
    # def _track_loop(self, target_position, interval):
    #     while self.target_tracking_flag :
    #         if not self._enable_head_tracking(
    #             position=target_position
    #         ):
    #             ret = {"message": "Head Control Failed, or target can not be tracking"}
    #             self.running_flag = False
    #             self.target_tracking_flag = False
    #         time.sleep(interval)
    #     self.running_flag = False
    #     self.target_tracking_flag = False
    
    def CancelTargetTracking(self, input: dict = None, task_status: TaskStatus = None) -> dict:
        func_start = time.time()
        if not self.running_flag or not self.target_tracking_flag :
            return {"message": "TargetTracking Task is not running"}
        self.running_flag = False
        self.target_tracking_flag  = False
        func_end = time.time()
        logging.info(f"[Head] task_type=2 CancelTargetTracking duration: {func_end - func_start:.3f}s")
        return {"message": "Task Cancel success"}

    def cancel_head_joint_control_func(self, input: dict = None, task_status: TaskStatus = None) -> dict:
        if not self.running_flag or not self.head_joint_control_flag :
            return {"message": "head_joint_control Task is not running"}
        self.running_flag = False
        self.target_tracking_flag = False
        if not self.robot_head.control_head(self.robot_state.head_joint_state().position[0], self.robot_state.head_joint_state().position[1]):
            return {"message": "Task is canceled, but failed to stay current pose"}
        return {"message": "Task Cancel success"}
    
    def _enable_head_tracking(self, position, camera_height=0) -> bool:
        """
        启用头部跟踪功能，使头部始终追踪指定的空间位置和朝向。

        Args:
            position (dict): 必须包含
                - position: dict，目标位置，格式如 {"x": float, "y": float, "z": float}
            
        Returns:
            bool: 如果启用成功返回True，否则返回False。
        """
        # 计算目标相对于机器人的方向
        dx = position['x'] - self.robot_state.robot_position()[0]
        dy = position['y'] - self.robot_state.robot_position()[1]
        dz = position['z'] - self.robot_state.robot_position()[2] - camera_height
        robot_yaw = self._extract_yaw_from_quaternion(self.robot_state.robot_orientation())
	        # 计算yaw和pitch角度来指向目标（结果为弧度）
        global_yaw_rad = math.atan2(dy, dx)
        distance = math.sqrt(dx*dx + dy*dy)
        if distance < 1e-6:  # 如果距离过小，避免除零
            print("目标与机器人位置过近，无法计算角度")
            return False
        pitch_rad = math.atan2(-dz, distance)
    
        # 标准化 pitch 角到 [-π, π]
        pitch_rad = math.atan2(math.sin(pitch_rad), math.cos(pitch_rad))
        relative_yaw_rad = global_yaw_rad - robot_yaw
        relative_yaw_rad = math.atan2(math.sin(relative_yaw_rad), math.cos(relative_yaw_rad))
        relative_yaw_deg = math.degrees(relative_yaw_rad)
        # 转换为角度
        pitch_deg = math.degrees(pitch_rad)
        
        # 检查yaw角度是否超过65度
        if abs(relative_yaw_deg) > 65:
            print("yaw can not be traced")
            print(abs(relative_yaw_deg))
            return False
            
	        # 限制yaw角度：小于30度直接使用，大于30度使用30度
        if abs(relative_yaw_deg) <= 30:
            final_yaw_rad = relative_yaw_rad
        else:
            final_yaw_rad = math.radians(30 if relative_yaw_deg > 0 else -30)
            
        # 检查pitch角度是否超过50度
        if abs(pitch_deg) > 50:
            print("pitch can not be traced")
            print(abs(pitch_deg))
            return False
            
	        # 限制pitch角度：小于15度直接使用，大于15度使用15度
        if abs(pitch_deg) <= 15:
            final_pitch_rad = pitch_rad
        else:
            final_pitch_rad = math.radians(15 if pitch_deg > 0 else -15)
            
        
        # 控制头部指向目标（输入为弧度）
        return self.robot_head.control_head(yaw=final_yaw_rad, pitch=final_pitch_rad)
    
    def _angle_check(self, current_angle, target_angle, angle_threshold=0.1):
        """检查机器人头部当前朝向与目标朝向的差异
        
        Args:
            yaw_angle_target: 目标朝向角度（弧度）
            angle_threshold: 角度阈值（弧度），小于此阈值认为已到位
            
        Returns:
            bool: 是否成功到达目标朝向
        """
        
        # 计算角度差
        angle_diff = target_angle - current_angle
        # 标准化到[-pi, pi]
        while angle_diff > math.pi:
            angle_diff -= 2 * math.pi
        while angle_diff < -math.pi:
            angle_diff += 2 * math.pi
        
        # 输出当前状态
        print(f"头部当前朝向: {math.degrees(current_angle):.2f}度, 目标朝向: {math.degrees(target_angle):.2f}度, 差值: {math.degrees(abs(angle_diff)):.2f}度")
        print(f"  angle_diff={angle_diff:.4f}rad, angle_threshold={angle_threshold:.4f}rad, 判断: {abs(angle_diff)} < {angle_threshold} = {abs(angle_diff) < angle_threshold}")
        
        # 检查是否已到位
        if abs(angle_diff) < angle_threshold:
            print(f"✓ 机器人头部已旋转到位!")
            return True
        else:
            print(f"✗ 未到位，继续等待...")
            return False
    
    def _extract_yaw_from_quaternion(self, quaternion):
        """从四元数中提取yaw角
        
        Args:
            quaternion: 四元数 (x, y, z, w)
            
        Returns:
            float: yaw角（弧度）
        """
        if not quaternion or len(quaternion) != 4:
            print("无法获取有效的四元数")
            return 0.0
            
        # 计算yaw角 (围绕z轴的旋转)
        # 四元数到欧拉角的简化计算，仅提取yaw
        x, y, z, w = quaternion
        yaw = math.atan2(2.0 * (w * z + x * y), 1.0 - 2.0 * (y * y + z * z))
        return yaw
    
    def _extract_pitch_from_quaternion(self, quaternion):
        """从四元数中提取pitch角
        
        Args:
            quaternion: 四元数 (x, y, z, w)
            
        Returns:
            float: pitch角（弧度）
        """
        if not quaternion or len(quaternion) != 4:
            print("无法获取有效的四元数")
            return 0.0
            
        # 计算pitch角 (围绕x轴的旋转)
        x, y, z, w = quaternion
        pitch = math.asin(2.0 * (w * y - z * x))
        return pitch

    def on_start(self):
        logging.warning("on_start called")
    
    def on_connect(self):
        # Find a random available port
        self.port = find_free_port()
        self.ability_info.ability_port = self.port
        
        # Create a threading.Event to signal server shutdown
        self.stop_event = threading.Event()
        
        def run_flask():
            try:
                from werkzeug.serving import make_server
                self.server = make_server('0.0.0.0', self.port, self.app, threaded=True)
                self.server.serve_forever()
            except Exception as e:
                logging.error(f"Flask server error: {e}")
                
        self.server_thread = threading.Thread(target=run_flask)
        self.server_thread.daemon = True
        self.server_thread.start()
        logging.warning(f"on_connect called, Flask server started on port {self.port}")


    def on_disconnect(self):
        logging.warning("on_disconnect called")
        if self.server_thread and self.server:
            try:
                self.stop_event.set()
                self.server.shutdown()
                self.server.server_close()
                # Send shutdown request to the server
                self.server_thread.join(timeout=3.0)
                if self.server_thread.is_alive():
                    logging.warning("Server thread did not terminate within timeout")
                else:
                    logging.warning("Flask server thread terminated")
            except Exception as e:
                logging.error(f"Error during server shutdown: {e}")
            finally:
                self.server_thread = None
                self.stop_event = None
                self.server = None
                logging.warning("Flask server stopped")
        else:
            logging.warning("No active server thread to stop")

    def on_terminate(self):
        logging.warning("on_terminate called")


if __name__ == "__main__":
    log.configure_logging(sys.argv[0])
    ability = Head()
    handle = ipc_server_handle.HttpIpcServerHandle(sys.argv, ability)
    handle.run()
    logging.warning("HeadAbility process ended")