# -*- coding: utf-8 -*-
"""
YOLOv8 单类分割 + MediaPipe Hand Landmarker + 光流追踪（多边形）
更新点（本版重点）：
- 左下角第二个进度条"距离(≈1)" 已完全替换为：ratio = 物体面积 / 手面积 的"接近 1 程度"可视化
  -> range_score = 1 - clamp(|ratio - 1| / RATIO_TOL, 0..1)
  -> 画面同时显示 ratio 数值；ratio<1 提示"向前靠近"，ratio>1 提示"后退"，在 [1±RATIO_TOL] 内为"保持"
其他特性：
- Enter 锁定：在分割掩码"内收 5px"的内边界上取光流点
- TRACK 期间：监控当前多边形外扩 40px 周边区域的分割，命中即重锁
- 成功判定：放宽"握持(Grasp)"启发式（拿瓶子无需特别紧）
- 手骨架单色渲染；测距箭头（端点定位线 + 箭头 + 像素值）
- 中文绘制优先 Pillow + 系统中文字体（避免问号）
"""

import os
import time
import threading
import math
import cv2
import numpy as np
import mediapipe as mp
from mediapipe.framework.formats import landmark_pb2
from ultralytics import YOLO
from ultralytics.utils.plotting import Colors
import bridge_io

# Day 26: 抑制 pygame 社区欢迎信息
import os
os.environ['PYGAME_HIDE_SUPPORT_PROMPT'] = "1"
import pygame  # 用于播放本地音频文件

from audio_player import play_audio_threadsafe
PERF_DEBUG = False        # 打印调试信息（False 关闭）
HAND_DOWNSCALE = 0.8      # HandLandmarker 的输入缩放 0.5=长宽各减半（≈1/4 像素量）
HAND_FPS_DIV = 1          # 人手每 2 帧跑一次（1=每帧；2=隔帧；3=每3帧）


# === 前端风格配色（BGR） + UI叠加管理（左下角按行堆叠） ===
FRONTEND_COLORS = {
    "text": (230, 237, 243),   # --text: #e6edf3
    "muted": (159, 176, 195),  # --muted: #9fb0c3
    "ok": (126, 231, 135),     # --ok: #7ee787
    "err": (128, 128, 255),    # --err: #ff8080 (BGR)
    "accent": (251, 218, 97),  # #61dafb 近似的强调色（BGR 取近似亮色）
}

# 底部指令按钮文本
CURRENT_COMMAND_TEXT = "—"

_UI_LINE = 0
_UI_H = 0
_UI_TR_LINE = 0  # 右上角逐行叠放计数
_UI_TOP_MARGIN = 12
_UI_RIGHT_MARGIN = 12
UNIFIED_FONT_PX = 12  # 统一字号


def ui_reset_overlay(img_h: int):
    """每帧调用一次，重置叠加行计数（改为右上角布局）。"""
    global _UI_LINE, _UI_H, _UI_TR_LINE
    _UI_LINE = 0
    _UI_TR_LINE = 0
    _UI_H = int(img_h)


def _ui_next_y_top(font_size: int) -> int:
    """返回右上角下一行的y(顶部对齐)，并推进行计数。"""
    global _UI_TR_LINE
    line_gap = max(4, int(font_size * 0.25))
    y_top = _UI_TOP_MARGIN + (_UI_TR_LINE * (font_size + line_gap))
    _UI_TR_LINE += 1
    return y_top


def set_current_command(text: str):
    global CURRENT_COMMAND_TEXT
    try:
        CURRENT_COMMAND_TEXT = str(text) if text else "—"
    except Exception:
        CURRENT_COMMAND_TEXT = "—"


def draw_command_pill(img_bgr: np.ndarray, label: str):
    """统一改为右上角白色文案。不再绘制底部圆角按钮。"""
    text_prefix = "当前指令："
    full_text = f"{text_prefix}{label if label else '—'}"
    # 直接用统一文本渲染
    draw_text_cn(img_bgr, full_text, (0, 0), font_size=UNIFIED_FONT_PX, color=(255,255,255), ui_hint=True)

try:
    from yoloe_backend import YoloEBackend
    _YOLOE_READY = True
except Exception as e:
    _YOLOE_READY = False
    print(f"[DETECTOR] YOLOE backend not ready: {e}", flush=True)

# ========= 路径参数（按需修改）=========
YOLO_MODEL_PATH = 'model/shoppingbest5.pt'
HAND_TASK_PATH  = 'model/hand_landmarker.task'

# ========= 摄像头 =========
CAM_INDEX = 0
INPUT_W, INPUT_H = 600, 480

# ========= 分割显示 =========
STROKE_WIDTH = 5  # 增加描边宽度，让黄框和绿框更粗
MASK_ALPHA   = 0.45
CONF_THRESHOLD = 0.20

# —— 单 prompt 识别（只显示一个类）——
PROMPT_NAME   = "AD_milk"
PROMPT_STRICT = True

# ========= 对齐条参数 =========
ALIGN_LOOSE_PCT      = 0.12   # 归一化距离阈（相对画面对角线）

# ========= 距离条参数（本版采用"ratio≈1"为目标）=========
RATIO_IDEAL          = 1.0    # 理想值：物体面积/手面积 ≈ 1
RATIO_TOL            = 0.25   # 容许偏离：±25% 内认为距离合适

# ========= 语音播报 =========
TTS_INTERVAL_SEC     = 1.0
ENABLE_TTS           = True

# ========= 光流（LK）与特征点 =========
LK_PARAMS = dict(winSize=(21, 21),
                 maxLevel=3,
                 criteria=(cv2.TERM_CRITERIA_EPS | cv2.TERM_CRITERIA_COUNT, 12, 0.03))
FEATURE_PARAMS = dict(maxCorners=600,
                      qualityLevel=0.001,
                      minDistance=5,
                      blockSize=7)

# ========= 关键参数：内收与周边监控 =========
INNER_OFFSET_PX_LOCK = 5     # Enter 锁定：掩码腐蚀像素，保证点在物体内部
EDGE_DILATE_PX       = 2     # 取内边界后小膨胀，利于提点
PERI_MONITOR_PX      = 40    # TRACK：监控多边形外扩 40px 的周边带
PERI_CHECK_EVERY     = 5     # 每隔 N 帧做一次周边分割检查，改为每帧

# ========= 轮廓精度参数 =========
CONTOUR_EPSILON_FACTOR = 0.002  # Douglas-Peucker算法的精度因子，越小越精细
TRACK_EPSILON_FACTOR = 0.003    # 追踪模式下的轮廓精度因子

# ========= YOLO实时矫正参数 =========
YOLO_CORRECTION_IOU_THRESHOLD = 0.2  # IoU阈值，越低越积极矫正
YOLO_CORRECTION_CONF_THRESHOLD = 0.15  # 置信度阈值，越低检测越敏感

# ========= 方向引导音频路径 =========
AUDIO_DIR = os.path.join(os.path.dirname(os.path.abspath(__file__)), "music")  # 相对路径
AUDIO_FILES = {
    "向上": os.path.join(AUDIO_DIR, "向上.wav"),
    "向下": os.path.join(AUDIO_DIR, "向下.wav"),
    "向左": os.path.join(AUDIO_DIR, "向左.wav"),
    "向右": os.path.join(AUDIO_DIR, "向右.wav"),
    "向前": os.path.join(AUDIO_DIR, "向前.wav"),
    "后退": os.path.join(AUDIO_DIR, "向后.wav"),
    "OK": os.path.join(AUDIO_DIR, "已对中.wav"),
}
GUIDANCE_INTERVAL_SEC = 1.5  # 引导播报间隔

# 初始化pygame音频
pygame.mixer.init()

# ========= 窗口 =========
WINDOW = "YOLO Seg + Flow Polygon (Peri-Relock) (Grab Guidance)"

# ======== MediaPipe 别名 ========
BaseOptions           = mp.tasks.BaseOptions
VisionRunningMode     = mp.tasks.vision.RunningMode
HandLandmarker        = mp.tasks.vision.HandLandmarker
HandLandmarkerOptions = mp.tasks.vision.HandLandmarkerOptions
HAND_CONNECTIONS      = mp.solutions.hands.HAND_CONNECTIONS

# ======== HandLandmarker 回调缓存 ========
_last_result = None  # (result, timestamp_ms)

def on_result(result: mp.tasks.vision.HandLandmarkerResult,
              output_image: mp.Image, timestamp_ms: int):
    global _last_result
    _last_result = (result, timestamp_ms)

def _to_proto(hand_lms) -> landmark_pb2.NormalizedLandmarkList:
    proto = landmark_pb2.NormalizedLandmarkList()
    proto.landmark.extend([
        landmark_pb2.NormalizedLandmark(x=p.x, y=p.y, z=p.z) for p in hand_lms
    ])
    return proto

# —— 手骨架单色渲染 —— #
def draw_hands_mono(img_bgr, hand_lms, color=(0, 255, 255), r=2, t=2):
    mp_drawing = mp.solutions.drawing_utils
    landmark_spec   = mp_drawing.DrawingSpec(color=color, thickness=-1, circle_radius=r)
    connection_spec = mp_drawing.DrawingSpec(color=color, thickness=t,  circle_radius=r)
    if hasattr(hand_lms, "landmark"):
        proto = hand_lms
    else:
        proto = _to_proto(hand_lms)
    mp_drawing.draw_landmarks(
        img_bgr,
        landmark_list=proto,
        connections=HAND_CONNECTIONS,
        landmark_drawing_spec=landmark_spec,
        connection_drawing_spec=connection_spec,
    )

def norm_name(s: str) -> str:
    return "".join(str(s).lower().split())

# ======== TTS（pyttsx3）========
class Speaker:
    def __init__(self, enable=True):
        self.enable = enable
        self._engine = None
        self._lock = threading.Lock()
        if enable:
            try:
                import pyttsx3
                self._engine = pyttsx3.init()
                self._engine.setProperty('rate', 190)
                self._engine.setProperty('volume', 1.0)
            except Exception:
                self._engine = None
                self.enable = False

    def say_async(self, text: str):
        if not self.enable or not text:
            return
        def _run():
            try:
                with self._lock:
                    self._engine.stop()
                    self._engine.say(text)
                    self._engine.iterate()
                    t0 = time.time()
                    while self._engine.isBusy() and (time.time() - t0) < 1.2:
                        self._engine.iterate()
                        time.sleep(0.01)
            except Exception:
                pass
        threading.Thread(target=_run, daemon=True).start()

# ======== 中文文本绘制（优先 Pillow）========
_PIL_OK = False
_FONT_PATH = None
def _init_font():
    global _PIL_OK, _FONT_PATH
    try:
        from PIL import ImageFont  # noqa
        _PIL_OK = True
    except Exception:
        _PIL_OK = False
        return
    candidates = [
        # Linux 中文字体路径 (Ubuntu/Debian)
        "/usr/share/fonts/truetype/wqy/wqy-zenhei.ttc",
        "/usr/share/fonts/truetype/wqy/wqy-microhei.ttc",
        "/usr/share/fonts/opentype/noto/NotoSansCJK-Regular.ttc",
        "/usr/share/fonts/truetype/noto/NotoSansCJK-Regular.ttc",
        "/usr/share/fonts/truetype/droid/DroidSansFallbackFull.ttf",
    ]
    for p in candidates:
        if os.path.exists(p):
            _FONT_PATH = p
            return
    _PIL_OK = False
_init_font()

def draw_text_cn(img_bgr, text, xy, font_size=20, color=(255,255,255), stroke=None, ui_hint=True):
    """
    统一的文本绘制：
    - 默认采用前端风格：小字体、左下角按行堆叠(ui_hint=True)。
    - 若 ui_hint=False 则按传入 xy 精确定位（用于贴近目标的小标注）。
    """
    # 统一样式：微软雅黑 + 固定字号 + 纯白
    color = (255, 255, 255)
    font_size = int(UNIFIED_FONT_PX)

    H, W = img_bgr.shape[:2]
    # 右上角堆叠布局：计算y顶边，并按文本宽度右对齐
    y_top = _ui_next_y_top(font_size) if ui_hint else _ui_next_y_top(font_size)
    # 先估算文本尺寸
    tw = th = 0
    font_obj = None

    if _PIL_OK and _FONT_PATH:
        try:
            from PIL import Image, ImageDraw, ImageFont
            font_obj = ImageFont.truetype(_FONT_PATH, font_size)
            # 计算文本尺寸
            bbox = ImageDraw.Draw(Image.new('RGB', (1,1))).textbbox((0,0), text, font=font_obj)
            tw = max(1, bbox[2] - bbox[0])
            th = max(1, bbox[3] - bbox[1])
        except Exception:
            pass
    if _PIL_OK and _FONT_PATH and font_obj is not None:
        try:
            from PIL import Image, ImageDraw
            img_rgb = cv2.cvtColor(img_bgr, cv2.COLOR_BGR2RGB)
            pil_img = Image.fromarray(img_rgb)
            draw = ImageDraw.Draw(pil_img)
            x = max(8, W - _UI_RIGHT_MARGIN - tw)
            y = y_top
            draw.text((x, y), text, fill=(255,255,255), font=font_obj)
            img_bgr[:] = cv2.cvtColor(np.asarray(pil_img), cv2.COLOR_RGB2BGR)
            return
        except Exception:
            pass
    # OpenCV 回退：估算尺寸并右对齐
    if tw <= 0 or th <= 0:
        scale = font_size/24.0
        (tw, th), _ = cv2.getTextSize(text, cv2.FONT_HERSHEY_SIMPLEX, scale, 2)
    x = max(8, W - _UI_RIGHT_MARGIN - int(tw))
    y_baseline = int(y_top + th)
    cv2.putText(img_bgr, text, (x, y_baseline), cv2.FONT_HERSHEY_SIMPLEX, font_size/24.0, color, 2, cv2.LINE_AA)

# ======== 工具函数 ========
def clamp01(x): return max(0.0, min(1.0, x))

def draw_progress_bars(vis, align_score, range_score):
    """第一条=对齐，第二条=距离(≈1)，对应 ratio 与 1 的接近程度"""
    H, W = vis.shape[:2]
    bar_w = int(W * 0.28)
    bar_h = 12
    gap   = 8
    x0    = 12
    y0    = H - 2*bar_h - gap - 12
    # 背景
    cv2.rectangle(vis, (x0, y0), (x0 + bar_w, y0 + bar_h), (50, 50, 50), -1)
    cv2.rectangle(vis, (x0, y0 + bar_h + gap), (x0 + bar_w, y0 + 2*bar_h + gap), (50, 50, 50), -1)
    # 填充
    cv2.rectangle(vis, (x0, y0), (x0 + int(bar_w * clamp01(align_score)), y0 + bar_h), (0, 220, 0), -1)
    cv2.rectangle(vis, (x0, y0 + bar_h + gap), (x0 + int(bar_w * clamp01(range_score)), y0 + 2*bar_h + gap), (0, 180, 255), -1)
    draw_text_cn(vis, "对齐",       (x0, y0 - 18),                 font_size=18, color=(180,180,180))
    draw_text_cn(vis, "距离(≈1)",   (x0, y0 + bar_h + gap - 18),   font_size=18, color=(180,180,180))

def polygon_center_and_area(poly):
    if poly is None or len(poly) < 3:
        return None, 0.0
    poly = np.array(poly, dtype=np.float32)
    M = cv2.moments(poly)
    if abs(M["m00"]) < 1e-6:
        c = np.mean(poly, axis=0)
        return (float(c[0]), float(c[1])), 0.0
    cx = float(M["m10"] / M["m00"])
    cy = float(M["m01"] / M["m00"])
    area = float(cv2.contourArea(poly.astype(np.int32)))
    return (cx, cy), area

def hand_bbox_and_area(lms, W, H):
    xs = [int(p.x * W) for p in lms]
    ys = [int(p.y * H) for p in lms]
    if not xs or not ys:
        return None, 0.0
    x0, y0, x1, y1 = min(xs), min(ys), max(xs), max(ys)
    w = max(1, x1 - x0)
    h = max(1, y1 - y0)
    area = float(w * h)
    return (x0, y0, w, h), area

# ======== 手势：握持(Grasp) 识别（放宽版启发式）========
THUMB_INDEX_CLOSE = 0.34   # 放宽
FINGERTIP_NEAR    = 0.44   # 放宽
MIN_CURLED_COUNT  = 1      # 放宽

def detect_grasp(hand_lms, W, H):
    box, _ = hand_bbox_and_area(hand_lms, W, H)
    if not box:
        return False, 0.0
    x0, y0, w0, h0 = box
    hand_diag = float(np.hypot(w0, h0)) + 1e-6
    palm_idx = [0, 5, 9, 13, 17]
    px = np.mean([hand_lms[i].x * W for i in palm_idx])
    py = np.mean([hand_lms[i].y * H for i in palm_idx])
    palm = np.array([px, py], dtype=np.float32)
    t4 = np.array([hand_lms[4].x * W, hand_lms[4].y * H], dtype=np.float32)
    t8 = np.array([hand_lms[8].x * W, hand_lms[8].y * H], dtype=np.float32)
    thumb_index_dist = float(np.linalg.norm(t4 - t8)) / hand_diag
    tips = [12, 16, 20]
    dists = []
    for i in tips:
        ti = np.array([hand_lms[i].x * W, hand_lms[i].y * H], dtype=np.float32)
        dists.append(float(np.linalg.norm(ti - palm)) / hand_diag)
    curled_cnt = sum(1 for d in dists if d < FINGERTIP_NEAR)
    cond1 = (thumb_index_dist < THUMB_INDEX_CLOSE)
    cond2 = (curled_cnt >= MIN_CURLED_COUNT)
    score = 0.5 * (1.0 - min(thumb_index_dist / THUMB_INDEX_CLOSE, 1.0)) + \
            0.5 * min(curled_cnt / 3.0, 1.0)
    return (cond1 and cond2), score

# ======== 内收后的边界提点 ========
def inner_offset_edge(mask_bin, offset_px=5, edge_dilate_px=2):
    if offset_px > 0:
        k = cv2.getStructuringElement(cv2.MORPH_ELLIPSE, (2*offset_px+1, 2*offset_px+1))
        eroded = cv2.erode(mask_bin.astype(np.uint8), k, iterations=1)
    else:
        eroded = mask_bin.astype(np.uint8)
    edges = cv2.Canny(eroded*255, 50, 150)
    if edge_dilate_px > 0:
        k2 = cv2.getStructuringElement(cv2.MORPH_ELLIPSE, (2*edge_dilate_px+1, 2*edge_dilate_px+1))
        edges = cv2.dilate(edges, k2, iterations=1)
    return edges  # uint8 0/255

# ======== YOLO 分割：全帧或 ROI 内选择最佳 mask ========
def find_best_mask(frame_bgr, yolo, W, H, target_cls_id, conf_thr=0.10, roi_rect=None):
    results = yolo(frame_bgr, verbose=False)
    best_mask = None
    best_score = 0.0
    if results and results[0].masks is not None:
        r0 = results[0]
        for mask_t, conf_t, cls_t in zip(r0.masks.data, r0.boxes.conf, r0.boxes.cls):
            cls_id = int(cls_t.item())
            conf_value = float(conf_t.item())
            if target_cls_id is not None and cls_id != target_cls_id:
                continue
            if conf_value < conf_thr:
                continue
            mask_np = mask_t.detach().cpu().numpy()
            mask_rz = cv2.resize(mask_np, (W, H), interpolation=cv2.INTER_LINEAR)
            mask_bin = (mask_rz > 0.5).astype(np.uint8)

            if roi_rect is not None:
                x0, y0, x1, y1 = roi_rect
                x0, y0 = max(0, x0), max(0, y0)
                x1, y1 = min(W-1, x1), min(H-1, y1)
                roi = np.zeros_like(mask_bin, dtype=np.uint8)
                roi[y0:y1+1, x0:x1+1] = 1
                overlap = (mask_bin & roi).sum()
                score = float(overlap)
            else:
                score = float(mask_bin.sum())

            if score > best_score:
                best_score = score
                best_mask = mask_bin
    return best_mask

# ======== 工程化：测距箭头（端点定位线 + 箭头 + 像素值）========
def draw_measure_arrow(img, p1, p2, txt=None):
    p1 = (int(p1[0]), int(p1[1]))
    p2 = (int(p2[0]), int(p2[1]))
    # 端点定位线
    def end_cap(pt, size=8, color=(255,255,255), t=1):
        x, y = pt
        cv2.line(img, (x - size, y), (x + size, y), color, t, cv2.LINE_AA)
        cv2.line(img, (x, y - size), (x, y + size), color, t, cv2.LINE_AA)
    end_cap(p1, size=7, color=(255,255,255), t=1)
    end_cap(p2, size=7, color=(255,255,255), t=1)
    # 箭头
    cv2.arrowedLine(img, p1, p2, (255,255,255), 2, cv2.LINE_AA, tipLength=0.18)
    # 文本
    if txt is None:
        d = int(np.hypot(p2[0]-p1[0], p2[1]-p1[1]))
        txt = f"{d}px"
    mid = ((p1[0]+p2[0])//2, (p1[1]+p2[1])//2)
    font = cv2.FONT_HERSHEY_SIMPLEX
    fs, th = 0.6, 2
    (tw, th_text), _ = cv2.getTextSize(txt, font, fs, th)
    pad = 4
    x0 = mid[0] - tw//2 - pad
    y0 = mid[1] - th_text - 6
    x1 = mid[0] + tw//2 + pad
    y1 = mid[1] + 6
    cv2.rectangle(img, (x0, y0), (x1, y1), (32,32,32), -1)
    cv2.putText(img, txt, (x0+pad, y1-6), font, fs, (255,255,255), th, cv2.LINE_AA)

# 添加绘制虚线的函数
def draw_dashed_line(img, pt1, pt2, color=(255, 255, 255), thickness=2, dash_length=10, gap_length=5):
    """绘制虚线"""
    pt1 = np.array(pt1, dtype=np.float32)
    pt2 = np.array(pt2, dtype=np.float32)
    line_vec = pt2 - pt1
    line_len = np.linalg.norm(line_vec)
    if line_len < 1:
        return
    
    line_vec = line_vec / line_len  # 单位向量
    
    # 绘制虚线段
    current_pos = 0
    while current_pos < line_len:
        start_pos = current_pos
        end_pos = min(current_pos + dash_length, line_len)
        
        start_pt = pt1 + line_vec * start_pos
        end_pt = pt1 + line_vec * end_pos
        
        cv2.line(img, tuple(start_pt.astype(int)), tuple(end_pt.astype(int)), color, thickness)
        
        current_pos += dash_length + gap_length

# 添加绘制手部轮廓的函数
def draw_hand_contour(img, hand_lms, W, H, color=(255, 255, 255), thickness=1):
    """绘制手部landmarks的凸包轮廓"""
    # 获取所有手部关键点
    points = []
    for lm in hand_lms:
        x = int(lm.x * W)
        y = int(lm.y * H)
        points.append([x, y])
    
    if len(points) > 3:
        points = np.array(points, dtype=np.int32)
        # 计算凸包
        hull = cv2.convexHull(points)
        # 绘制凸包轮廓
        cv2.polylines(img, [hull], True, color, thickness)

# 检测手和物体是否接触
def check_hand_object_contact(hand_box, poly, overlap_threshold=0.15):
    """
    检测手的边界框和物体多边形是否有重叠
    返回: (是否接触, 重叠比例)
    """
    if hand_box is None or poly is None or len(poly) < 3:
        return False, 0.0
    
    # 获取手的边界框
    hx, hy, hw, hh = hand_box
    hand_rect = np.array([
        [hx, hy],
        [hx + hw, hy],
        [hx + hw, hy + hh],
        [hx, hy + hh]
    ], dtype=np.int32)
    
    # 创建掩码来计算重叠
    H = int(max(hy + hh, np.max(poly[:, 1])) + 10)
    W = int(max(hx + hw, np.max(poly[:, 0])) + 10)
    
    hand_mask = np.zeros((H, W), dtype=np.uint8)
    cv2.fillPoly(hand_mask, [hand_rect], 1)
    
    obj_mask = np.zeros((H, W), dtype=np.uint8)
    cv2.fillPoly(obj_mask, [poly.astype(np.int32)], 1)
    
    # 计算重叠
    intersection = np.logical_and(hand_mask, obj_mask).sum()
    hand_area = hand_mask.sum()
    
    # 重叠比例（相对于手的面积）
    overlap_ratio = intersection / max(1.0, hand_area)
    
    return overlap_ratio > overlap_threshold, overlap_ratio

# 添加方向判断函数
def get_guidance_direction(hand_center, object_center, hand_area, object_area, hand_box=None, poly=None):
    """
    根据手心和物体中心位置，以及面积比，返回引导方向
    返回: (方向文字, 是否需要前后调整)
    """
    if hand_center is None or object_center is None:
        return None, None
    
    # 首先检查手和物体是否接触
    is_touching = False
    overlap_ratio = 0.0
    if hand_box is not None and poly is not None:
        is_touching, overlap_ratio = check_hand_object_contact(hand_box, poly, overlap_threshold=0.1)
    
    hx, hy = hand_center
    ox, oy = object_center
    
    # 计算水平和垂直偏差
    dx = ox - hx  # 正数表示物体在右边
    dy = oy - hy  # 正数表示物体在下边
    
    # 如果手和物体已经接触，直接返回"向前"
    if is_touching:
        return "向前", f"接触度: {overlap_ratio:.1%}"
    
    # 如果没有接触，引导上下左右
    # 判断主要方向
    h_threshold = 30  # 水平偏差阈值（像素）
    v_threshold = 30  # 垂直偏差阈值（像素）
    
    h_dir = None
    v_dir = None
    
    # 水平方向
    if abs(dx) > h_threshold:
        h_dir = "向右" if dx > 0 else "向左"
    
    # 垂直方向
    if abs(dy) > v_threshold:
        v_dir = "向下" if dy > 0 else "向上"
    
    # 选择偏移最大的方向
    if abs(dx) > abs(dy) and h_dir:
        # 水平偏移更大
        return h_dir, v_dir
    elif v_dir:
        # 垂直偏移更大或相等
        return v_dir, h_dir
    else:
        # 已经在中心附近但还没接触，提示靠近
        distance = np.sqrt(dx**2 + dy**2)
        if distance < 50:  # 很近但还没接触
            return "向前", "请缓慢靠近"
        else:
            return "保持", None

# 播放音频的函数
def play_guidance_audio(direction):
    """播放方向引导音频"""
    # 直接调用新的音频播放函数
    play_audio_threadsafe(direction)
    # 同步更新底部按钮的指令文本
    try:
        if isinstance(direction, str) and direction.strip():
            set_current_command(direction.strip())
    except Exception:
        pass

# 添加居中判断函数
def get_center_guidance(object_center, frame_center, threshold=30):
    """
    判断物体是否在画面中心，返回引导方向
    返回: (方向文字, 是否已居中)
    """
    if object_center is None:
        return None, False
    
    ox, oy = object_center
    cx, cy = frame_center
    
    dx = cx - ox  # 正数表示需要向右移动
    dy = cy - oy  # 正数表示需要向下移动
    
    # 判断是否已经居中
    distance = np.sqrt(dx**2 + dy**2)
    if distance < threshold:
        return "已居中", True
    
    # 判断主要方向（对调左右和上下）
    if abs(dx) > abs(dy):
        return "向左" if dx > 0 else "向右", False  # 对调了
    else:
        return "向上" if dy > 0 else "向下", False  # 对调了

def main(headless: bool = False, prompt_name: str = None, stop_event=None):

    # OpenCV 优化
    try:
        import cv2
        cv2.setUseOptimized(True)
        cv2.setNumThreads(2)   # 视 CPU 核心数而定；树莓派类设备可设 1
    except Exception:
        pass




    # 如果传入了 prompt_name，使用它替换全局的 PROMPT_NAME
    global PROMPT_NAME
    if prompt_name:
        PROMPT_NAME = prompt_name
        print(f"[YOLOMEDIA] Using dynamic prompt: {PROMPT_NAME}")
    
    speaker = Speaker(ENABLE_TTS)
    last_tts_ts = 0.0
    MODE = "SEGMENT"  # 模式：SEGMENT -> FLASH -> CENTER_GUIDE -> TRACK
    colors = Colors()

    FRAME_IDX    = 0
    last_mask    = None      # 上一帧"目标掩膜"（用于 IoU 降噪）
    flow_mask    = None      # 光流外推得到的掩膜（你现有代码里会更新它）
    flow_grace   = 0         # YOLOE 丢检后，允许光流顶住的计数
    last_seen_ts = 0.0       # 最近一次 YOLOE 成功检测的时间戳
    locked_id    = None      # （可选）若你在 tracker 里记录了 id，可在下面选择相同 id
    # 刷新/容错参数（可按需微调）
    REDETECT_EVERY = 5       # 每 5 帧强制"信任 YOLOE 一次"
    FLOW_GRACE_MAX = 8       # YOLOE 连续丢检时，光流最多顶 8 帧
    IOU_MIN_KEEP   = 0.20    # 新/旧掩膜 IoU 太低时，用平滑合成，避免闪烁



    print("[INIT] 加载 YOLO 模型...")
    # NOTE: shoppingbest 不再用于找东西流程；如其他模式仍需，可保留 yolo = YOLO(...) 但不在本流程使用
    # yolo = YOLO(YOLO_MODEL_PATH)

    # —— 直接启用 YOLOE 文本提示后端（不再先查 shoppingbest）——
    use_yoloe = False
    yoloe_backend = None
    if _YOLOE_READY:
        try:
            yoloe_backend = YoloEBackend()                  # 可用 YOLOE_MODEL_PATH 环境变量指定模型
            yoloe_backend.set_text_classes([PROMPT_NAME])   # 文本类别
            use_yoloe = True
            print(f"[DETECTOR] YOLOE text-prompt backend enabled for: {PROMPT_NAME}", flush=True)
        except Exception as e:
            print(f"[DETECTOR] YOLOE init failed: {e}", flush=True)
    else:
        print("[DETECTOR] YOLOE backend not ready (import failed)", flush=True)

    # 类名映射（YOLOE 模式下简化）
    if use_yoloe:
        # YOLOE 模式下，只有一个目标类
        id_to_name = {0: PROMPT_NAME}
        name_to_id = {norm_name(PROMPT_NAME): 0}
        target_cls_id = 0
    else:
        # 如果将来需要支持传统 YOLO，可以在这里初始化
        id_to_name = {}
        name_to_id = {}
        target_cls_id = None

    # 目标类已在上面的 YOLOE 模式中设置

    print(f"[CLASS] target id={target_cls_id}, name={id_to_name.get(target_cls_id, 'N/A')}")
    print(f"[阈值] conf >= {CONF_THRESHOLD:.2f}")

    # Hand Landmarker
    print("[INIT] 初始化 Hand Landmarker...")
    base = BaseOptions(model_asset_path=HAND_TASK_PATH)
    hand_options = HandLandmarkerOptions(
        base_options=base,
        running_mode=VisionRunningMode.LIVE_STREAM,
        num_hands=1,
        min_hand_detection_confidence=0.40,
        min_hand_presence_confidence=0.50,
        min_tracking_confidence=0.70,
        result_callback=on_result
    )
    landmarker = HandLandmarker.create_from_options(hand_options)

    W = None
    H = None
    print("[Bridge] 等待 ESP32 画面 ...")

    # [headless] 仅在非 headless 时创建窗口（原逻辑保留，外层加判断）
    if not headless:
        cv2.namedWindow(WINDOW, cv2.WINDOW_NORMAL)

    # 光流缓存
    old_gray = None
    p0 = None
    lock_edge_debug = None     # 调试可视化：内边界
    track_frame_count = 0      # 控制周边监控频率
    last_poly_box = None       # 当前多边形外接矩形

    fps_hist = []
    
    # 添加自动锁定相关变量
    auto_lock_start_time = None  # 开始检测到物体的时间
    auto_lock_delay = 1.0        # 1秒后自动锁定
    last_detected_mask = None    # 最后检测到的mask
    
    # 添加闪烁动画相关变量
    flash_start_time = None      # 闪烁开始时间
    flash_duration = 1.0         # 闪烁持续时间（秒）
    flash_frequency = 1          # 闪烁频率（Hz） - 只闪一次
    flash_mask = None            # 用于闪烁的mask
    flash_color = (0, 255, 255)  # 闪烁颜色（黄色）

    # 添加引导相关变量
    last_guidance_time = 0
    last_guidance_direction = None

    # 添加居中引导相关变量
    center_guide_mask = None      # 用于居中引导的mask
    center_guide_start = None     # 居中引导开始时间
    center_threshold = 30         # 居中判定阈值（像素）
    last_center_guide_time = 0   # 上次居中引导语音时间
    center_reached = False        # 是否已经到达中心

    # 添加抓取跟踪相关变量
    grasp_tracking_frames = []  # 存储最近的手和物体位置
    grasp_tracking_duration = 1.0  # 需要持续1秒
    grasp_movement_threshold = 10  # 最小移动像素阈值（提高阈值）
    grasp_detected = False  # 是否已经检测到抓取
    grasp_start_time = None  # 开始检测到协同移动的时间
    
    # 背景参考点（用于检测相机移动） - 移到这里初始化
    background_points = None
    old_background_gray = None

    try:
        while True:
            # 检查停止事件
            if stop_event and stop_event.is_set():
                print("[YOLOMEDIA] Stop event detected, exiting...")
                break
                
            frame = bridge_io.wait_raw_bgr(timeout_sec=0.5)
            if frame is None:
                # 没取到帧就继续等（ESP32还没连上或暂时无新帧）
                # [headless] 给出 1ms 让出调度，避免空转
                if headless:
                    cv2.waitKey(1)
                continue
            
            # 每帧重置 UI 文字叠加到左下角
            H, W = frame.shape[:2]
            ui_reset_overlay(H)

            vis = frame.copy()
            t_now = time.time()

            # 抽帧 + 降采样（人手识别）
            if FRAME_IDX % HAND_FPS_DIV == 0:
                rgb = cv2.cvtColor(frame, cv2.COLOR_BGR2RGB)
                if HAND_DOWNSCALE and HAND_DOWNSCALE != 1.0:
                    small = cv2.resize(rgb, None, fx=HAND_DOWNSCALE, fy=HAND_DOWNSCALE, interpolation=cv2.INTER_AREA)
                    mp_image = mp.Image(image_format=mp.ImageFormat.SRGB, data=small)
                else:
                    mp_image = mp.Image(image_format=mp.ImageFormat.SRGB, data=rgb)
                landmarker.detect_async(mp_image, int(t_now * 1000))
            # 否则跳过，复用上一次 _last_result；Landmarker 会自己做 tracking


            # 取手心、手框、握持（放宽版）
            hand_center = None
            hand_area = None
            hand_box = None
            grasp_now = False
            grasp_score = 0.0
            if _last_result is not None:
                res, _ = _last_result
                if res.hand_landmarks and len(res.hand_landmarks) > 0:
                    l0 = res.hand_landmarks[0]
                    
                    # 绘制手部骨骼
                    draw_hands_mono(vis, l0, color=(0, 255, 255), r=2, t=2)
                    
                    # 绘制手部轮廓（替代矩形框）
                    draw_hand_contour(vis, l0, W, H, color=(255, 255, 255), thickness=1)
                    
                    xs = [p.x * W for p in l0]
                    ys = [p.y * H for p in l0]
                    hand_center = (float(sum(xs)/len(xs)), float(sum(ys)/len(ys)))
                    hand_box, hand_area = hand_bbox_and_area(l0, W, H)
                    # 注释掉矩形框绘制
                    # if hand_box:
                    #     x0, y0, w0, h0 = hand_box
                    #     cv2.rectangle(vis, (x0, y0), (x0+w0, y0+h0), (0,255,255), 1)
                    grasp_now, grasp_score = detect_grasp(l0, W, H)
                    draw_text_cn(vis, f"握持评分: {grasp_score:.2f}", (10, 70), font_size=18, color=(0, 180, 255))
                    

            if MODE == "SEGMENT":
                # —— 仅 YOLOE：每帧文本提示分割 + 取最大目标（删掉 shoppingbest 与重复 YOLOE 段）——
                FRAME_IDX += 1
                candidate_masks = []
                detected_object = False

                if use_yoloe and yoloe_backend is not None:
                    # 每帧都跑；persist=True 便于维持目标 ID
                    det = yoloe_backend.segment(frame, conf=0.20, iou=0.45, persist=True)
                    H, W = frame.shape[:2]

                    # 选一个掩膜：优先与 locked_id 相同；否则面积最大
                    chosen_idx = None
                    if det["masks"]:
                        if locked_id is not None and det["ids"] and (locked_id in det["ids"]):
                            chosen_idx = det["ids"].index(locked_id)
                        else:
                            areas = [int(m.sum()) for m in det["masks"]]
                            chosen_idx = int(np.argmax(areas))

                    if chosen_idx is not None:
                        m = det["masks"][chosen_idx]
                        if m.shape[:2] != (H, W):
                            m = cv2.resize(m, (W, H), interpolation=cv2.INTER_NEAREST)

                        mask_bin = (m > 0).astype(np.uint8)
                        candidate_masks.append({
                            "mask": mask_bin,
                            "area": int(mask_bin.sum()),
                            "name": PROMPT_NAME,
                            "cls_id": 0,
                            "conf": 0.99,
                        })
                        detected_object = True

                        # 简单可视化（半透明叠层 + 轮廓），不影响你后面的逻辑
                        colored = np.zeros_like(frame, dtype=np.uint8)
                        colored[mask_bin == 1] = (0, 255, 255)
                        vis = cv2.addWeighted(vis, 1.0, colored, MASK_ALPHA, 0)
                        contours, _ = cv2.findContours(mask_bin, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_NONE)
                        if contours:
                            # 选择最大轮廓并进行适度平滑
                            largest_contour = max(contours, key=cv2.contourArea)
                            # 使用Douglas-Peucker算法适度简化，保持更多细节
                            epsilon = CONTOUR_EPSILON_FACTOR * cv2.arcLength(largest_contour, True)  # 更小的epsilon保留更多细节
                            smoothed_contour = cv2.approxPolyDP(largest_contour, epsilon, True)
                            cv2.drawContours(vis, [smoothed_contour], -1, (0, 255, 255), STROKE_WIDTH)

                        # 记录 id，减少目标跳变
                        if det["ids"] and len(det["ids"]) > chosen_idx and det["ids"][chosen_idx] is not None:
                            locked_id = int(det["ids"][chosen_idx])

                else:
                    # YOLOE 未就绪：提示并保持原画面（不阻塞前端）
                    draw_text_cn(vis, "YOLOE 未就绪，显示原始画面", (10, 100), font_size=22, color=(0, 215, 255))

                # 选择面积最大的mask  ←—— 这一行下面开始保留你的原代码

                # 选择面积最大的mask
                if candidate_masks:
                    # 按面积降序排序
                    candidate_masks.sort(key=lambda x: x['area'], reverse=True)
                    largest_mask_info = candidate_masks[0]
                    last_detected_mask = largest_mask_info['mask']
                    
                    # 可选：在最大的物体上添加特殊标记
                    contours, _ = cv2.findContours(last_detected_mask, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_NONE)
                    if contours:
                        # 找到最大轮廓的中心
                        M = cv2.moments(contours[0])
                        if M["m00"] != 0:
                            cx = int(M["m10"] / M["m00"])
                            cy = int(M["m01"] / M["m00"])
                            # 在最大物体中心画一个圆圈标记
                            cv2.circle(vis, (cx, cy), 8, (0, 255, 0), 2)
                            cv2.circle(vis, (cx, cy), 12, (0, 255, 0), 1)
                            # 目标标签：保持就地标注
                            draw_text_cn(vis, "目标", (cx + 15, cy - 5), font_size=16, color=FRONTEND_COLORS["ok"], ui_hint=False)
                    
                    # 显示检测信息
                    if len(candidate_masks) > 1:
                        draw_text_cn(vis, f"检测到{len(candidate_masks)}个物体，选择最大的（面积: {largest_mask_info['area']}）", 
                                   (10, H - 30), font_size=16, color=(255, 255, 0))
                
                # 自动锁定逻辑
                if detected_object and last_detected_mask is not None:
                    if auto_lock_start_time is None:
                        auto_lock_start_time = t_now
                        print(f"[AUTO] 检测到物体，选择最大的（面积: {np.sum(last_detected_mask)}），开始倒计时...")
                        #play_guidance_audio("检测到物体")  # 添加这行
                    
                    elapsed = t_now - auto_lock_start_time
                    remaining = auto_lock_delay - elapsed
                    
                    if remaining > 0:
                        # 显示倒计时（移动到左下角，前端风格）
                        draw_text_cn(vis, f"检测到物体，{remaining:.1f}秒后自动锁定", (10, 100), font_size=16, color=FRONTEND_COLORS["text"], stroke=(0,0,0))
                        
                        # 绘制锁定框 - 使用虚线框表示正在准备锁定
                        if last_detected_mask is not None:
                            contours, _ = cv2.findContours(last_detected_mask, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_NONE)
                            if contours:
                                # 找到最大轮廓
                                largest_contour = max(contours, key=cv2.contourArea)
                                # 简化轮廓
                                epsilon = CONTOUR_EPSILON_FACTOR * cv2.arcLength(largest_contour, True)
                                smoothed_contour = cv2.approxPolyDP(largest_contour, epsilon, True)
                                
                                # 根据倒计时进度改变颜色亮度
                                progress = 1.0 - (remaining / auto_lock_delay)
                                color_intensity = int(100 + 155 * progress)  # 从100到255
                                lock_color = (0, color_intensity, color_intensity)  # 黄色渐亮
                                
                                # 绘制虚线轮廓
                                pts = smoothed_contour.reshape(-1, 2)
                                for i in range(len(pts)):
                                    pt1 = tuple(pts[i])
                                    pt2 = tuple(pts[(i + 1) % len(pts)])
                                    # 使用虚线效果（通过绘制短线段）
                                    draw_dashed_line(vis, pt1, pt2, color=lock_color, thickness=3, 
                                                   dash_length=15, gap_length=8)
                    else:
                        # 进入闪烁模式
                        print("[AUTO] 进入闪烁动画模式")
                        MODE = "FLASH"
                        flash_start_time = t_now
                        flash_mask = last_detected_mask.copy()
                        auto_lock_start_time = None
                        play_guidance_audio("检测到物体") 
                else:
                    # 没有检测到物体，重置计时器
                    if auto_lock_start_time is not None:
                        print("[AUTO] 物体丢失，重置倒计时")
                    auto_lock_start_time = None
                    last_detected_mask = None
                    draw_text_cn(vis, "分割中... 等待检测到物体", (10, 100), font_size=16, color=FRONTEND_COLORS["muted"])

            elif MODE == "FLASH":
                # 闪烁动画模式
                if flash_start_time is not None and flash_mask is not None:
                    elapsed = t_now - flash_start_time
                    
                    if elapsed < flash_duration:
                        # 计算渐入渐出效果
                        # 前0.3秒渐入，中间0.4秒保持，后0.3秒渐出
                        if elapsed < 0.3:
                            # 渐入阶段
                            alpha = elapsed / 0.3 * 0.8  # 0到0.8
                        elif elapsed < 0.7:
                            # 保持阶段
                            alpha = 0.8
                        else:
                            # 渐出阶段
                            alpha = (1.0 - elapsed) / 0.3 * 0.8  # 0.8到0
                        
                        # 绘制闪烁的mask
                        colored = np.zeros_like(frame, dtype=np.uint8)
                        colored[flash_mask == 1] = flash_color
                        vis = cv2.addWeighted(vis, 1.0 - alpha, colored, alpha, 0)
                        
                        # 绘制轮廓（固定粗细，颜色渐变）
                        contours, _ = cv2.findContours(flash_mask, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_NONE)
                        if contours:
                            # 轮廓颜色也跟随alpha变化
                            contour_color = tuple(int(c * (0.5 + alpha * 0.5)) for c in flash_color)
                            cv2.drawContours(vis, contours, -1, contour_color, STROKE_WIDTH + 1)
                        
                        # 显示提示文字（左下角）
                        draw_text_cn(vis, "正在锁定目标...", (10, 100), font_size=18, color=FRONTEND_COLORS["accent"]) 
                    else:
                        # 闪烁结束，初始化光流追踪并进入居中引导模式
                        print("[AUTO] 闪烁结束，初始化光流追踪")
                        edge_mask = inner_offset_edge(flash_mask, offset_px=INNER_OFFSET_PX_LOCK, edge_dilate_px=EDGE_DILATE_PX)
                        gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
                        pts = cv2.goodFeaturesToTrack(gray, mask=edge_mask, **FEATURE_PARAMS)
                        
                        if pts is not None and len(pts) >= 8:
                            p0 = pts
                            old_gray = gray
                            MODE = "CENTER_GUIDE"
                            lock_edge_debug = edge_mask.copy()
                            track_frame_count = 0
                            center_guide_start = t_now
                            center_reached = False
                            flash_start_time = None
                            flash_mask = None
                            last_detected_mask = None
                            print(f"[LOCK] 内边界特征点数={len(p0)} → CENTER_GUIDE")
                        else:
                            print("[LOCK] 内边界特征点不足，返回检测模式")
                            MODE = "SEGMENT"
                            flash_start_time = None
                            flash_mask = None
                            last_detected_mask = None
            
            elif MODE == "CENTER_GUIDE":
                # 居中引导模式（使用光流追踪）
                gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
                poly_center = None
                poly_area = 0.0
                
                if old_gray is not None and p0 is not None and len(p0) >= 5:
                    # 光流追踪
                    p1, st, err = cv2.calcOpticalFlowPyrLK(old_gray, gray, p0, None, **LK_PARAMS)
                    if p1 is not None and st is not None:
                        good_new = p1[st == 1]
                        if len(good_new) >= 5:
                            p0 = good_new.reshape(-1, 1, 2)
                            hull = cv2.convexHull(good_new.reshape(-1,1,2))
                            poly = hull.reshape(-1, 2)
                            
                            if len(poly) >= 3:
                                H, W = frame.shape[:2]

                                # 把当前光流多边形 rasterize 成掩膜（便于与 YOLOE 掩膜做 IoU）
                                poly_mask = np.zeros((H, W), dtype=np.uint8)
                                cv2.fillPoly(poly_mask, [poly.astype(np.int32)], 1)

                                # 降频：每3帧用 YOLOE 重新检测，其余帧依赖光流维持
                                need_reseed = False
                                new_det_mask = None

                                if use_yoloe and yoloe_backend is not None and (FRAME_IDX % 3 == 0):
                                    # 添加调试信息
                                    if FRAME_IDX % 30 == 0:  # 每30帧打印一次
                                        print(f"[YOLOE] 实时检测第 {FRAME_IDX} 帧")
                                    det = yoloe_backend.segment(frame, conf=0.20, iou=0.45, persist=True)
                                    if det["masks"]:
                                        # 取面积最大的那个
                                        areas = [int(m.sum()) for m in det["masks"]]
                                        j = int(np.argmax(areas))
                                        m = det["masks"][j]
                                        if m.shape[:2] != (H, W):
                                            m = cv2.resize(m, (W, H), interpolation=cv2.INTER_NEAREST)
                                        new_det_mask = (m > 0).astype(np.uint8)

                                        # 和当前光流多边形的 IoU
                                        inter = np.logical_and(new_det_mask, poly_mask).sum()
                                        union = np.logical_or(new_det_mask, poly_mask).sum() + 1e-6
                                        iou   = inter / union

                                        # IoU 太低，说明漂了：用 YOLOE 的掩膜重播种光流
                                        # 降低阈值，让 YOLOE 更容易更新光流
                                        if iou < 0.5:  # 从 IOU_MIN_KEEP (0.20) 提高到 0.5
                                            need_reseed = True
                                            # 用新掩膜的「内边界特征点」播种
                                            edge_mask = inner_offset_edge(new_det_mask, offset_px=INNER_OFFSET_PX_LOCK, edge_dilate_px=EDGE_DILATE_PX)
                                            gray2 = gray  # 本帧灰度图已在上面算过
                                            pts = cv2.goodFeaturesToTrack(gray2, mask=edge_mask, **FEATURE_PARAMS)
                                            if pts is not None and len(pts) >= 8:
                                                p0 = pts
                                                old_gray = gray2
                                                # 更新 last_mask，便于下游逻辑一致
                                                last_mask = new_det_mask.copy()
                                                last_seen_ts = time.time()
                                                flow_grace = 0
                                                print("[RESEED] YOLOE 低 IoU 触发重播种（已更新光流特征点）")

                                # 如果这帧没重播种，但 YOLOE 有结果且与 poly 很接近，可以做一次"平滑融合"，抑制抖动
                                if (not need_reseed) and (new_det_mask is not None):
                                    inter = np.logical_and(new_det_mask, poly_mask).sum()
                                    union = np.logical_or(new_det_mask, poly_mask).sum() + 1e-6
                                    iou   = inter / union
                                    # 降低融合阈值，让 YOLOE 结果更容易被采用
                                    if iou < 0.95:  # 从 0.90 提高到 0.95
                                        # 增加 YOLOE 的权重，让实时检测更明显
                                        poly_mask = ((0.8 * new_det_mask + 0.2 * poly_mask) > 0.5).astype(np.uint8)
                                        # 用更新后的 poly_mask 回写到可视化与引导的后续变量（如果你下游用的是 last_detected_mask/last_mask）
                                        last_mask = poly_mask.copy()
                                                                                # 更新多边形轮廓，让可视化实时更新
                                        contours, _ = cv2.findContours(poly_mask, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_NONE)
                                        if contours:
                                            # 找到最大轮廓
                                            largest_contour = max(contours, key=cv2.contourArea)
                                            # 使用精细的轮廓处理，保留更多细节
                                            epsilon = TRACK_EPSILON_FACTOR * cv2.arcLength(largest_contour, True)
                                            poly = cv2.approxPolyDP(largest_contour, epsilon, True).reshape(-1, 2)
                                            # 注释掉凸包处理，保留原始轮廓细节
                                            # hull = cv2.convexHull(poly.reshape(-1,1,2))
                                            # poly = hull.reshape(-1, 2)
                                            # 重新计算特征点
                                            edge_mask = inner_offset_edge(poly_mask, offset_px=INNER_OFFSET_PX_LOCK, edge_dilate_px=EDGE_DILATE_PX)
                                            pts = cv2.goodFeaturesToTrack(gray, mask=edge_mask, **FEATURE_PARAMS)
                                            if pts is not None and len(pts) >= 5:
                                                p0 = pts

                                # 绘制追踪的多边形 - 使用更粗的线条
                                cv2.polylines(vis, [poly.astype(np.int32)], isClosed=True, color=(0,255,255), thickness=STROKE_WIDTH)
                                
                                # 计算多边形中心
                                poly_center, poly_area = polygon_center_and_area(poly)
                                
                                if poly_center:
                                    object_center = (int(poly_center[0]), int(poly_center[1]))
                                    
                                    # 画面中心
                                    frame_center = (W // 2, H // 2)
                                    
                                    # 绘制物品中心点
                                    cv2.circle(vis, object_center, 8, (0, 255, 0), -1)
                                    cv2.circle(vis, object_center, 12, (0, 255, 0), 2)
                                    
                                    # 绘制画面中心十字
                                    cv2.line(vis, (frame_center[0] - 20, frame_center[1]), 
                                            (frame_center[0] + 20, frame_center[1]), (255, 255, 255), 2)
                                    cv2.line(vis, (frame_center[0], frame_center[1] - 20), 
                                            (frame_center[0], frame_center[1] + 20), (255, 255, 255), 2)
                                    
                                    # 绘制引导虚线
                                    draw_dashed_line(vis, object_center, frame_center, 
                                                   color=(255, 255, 0), thickness=2, 
                                                   dash_length=10, gap_length=5)
                                    
                                    # 获取引导方向
                                    direction, is_centered = get_center_guidance(object_center, frame_center, center_threshold)
                                    
                                    if not center_reached:
                                        if is_centered:
                                            # 到达中心，播放OK音效
                                            center_reached = True
                                            last_center_guide_time = t_now
                                            play_guidance_audio("OK")
                                            try:
                                                bridge_io.send_ui_final("✓ 物品已居中！")
                                            except Exception:
                                                pass
                                            draw_text_cn(vis, "✓ 物品已居中！", (10, 60), font_size=18, color=FRONTEND_COLORS["ok"]) 
                                        else:
                                            # 显示引导文字
                                            msg = f"请将物品移到画面中心: {direction}"
                                            try:
                                                # 节流：每次语音播报也推一次final
                                                if t_now - last_center_guide_time > GUIDANCE_INTERVAL_SEC:
                                                    bridge_io.send_ui_final(msg)
                                            except Exception:
                                                pass
                                            draw_text_cn(vis, msg, 
                                                       (10, 40), font_size=18, color=FRONTEND_COLORS["text"])
                                            
                                            # 显示距离信息
                                            dx = frame_center[0] - object_center[0]
                                            dy = frame_center[1] - object_center[1]
                                            distance = int(np.sqrt(dx**2 + dy**2))
                                            draw_text_cn(vis, f"距离: {distance}px", 
                                                       (10, 60), font_size=16, color=FRONTEND_COLORS["muted"])
                                            
                                            # 播放语音引导
                                            if t_now - last_center_guide_time > GUIDANCE_INTERVAL_SEC:
                                                play_guidance_audio(direction)
                                                last_center_guide_time = t_now
                                    else:
                                        # 已经居中，显示成功信息
                                        try:
                                            bridge_io.send_ui_final("✓ 物品已成功移到中心！")
                                        except Exception:
                                            pass
                                        draw_text_cn(vis, "✓ 物品已成功移到中心！", 
                                                   (10, 60), font_size=18, color=FRONTEND_COLORS["ok"])
                                        
                                        # 等待1秒后进入手部追踪模式
                                        if t_now - last_center_guide_time > 1.0:
                                            print("[CENTER] 进入手部追踪模式")
                                            try:
                                                bridge_io.send_ui_final("进入手部追踪模式")
                                            except Exception:
                                                pass
                                            MODE = "TRACK"
                                            # 保持当前的光流追踪状态
                                else:
                                    # 多边形中心计算失败，显示警告
                                    draw_text_cn(vis, "正在追踪物体...", (10, 100), font_size=20, color=(255, 255, 0))
                        else:
                            # 光流点数不足，尝试重新检测
                            MODE = "SEGMENT"
                            old_gray = None
                            p0 = None
                            print("[CENTER] 光流追踪失败，返回检测模式")
                
                old_gray = gray

            else:  # MODE == "TRACK"
                # 手部追踪模式（原有逻辑保持不变）
                align_score = 0.0
                range_score = 0.0
                ratio = None

                gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
                track_frame_count += 1

                relock_done = False
                poly_center = None
                poly_area = 0.0

                # 初始化camera_movement为默认值
                camera_movement = np.array([0.0, 0.0])
                
                # 初始化或更新背景参考点（在物体多边形外部取点）
                if background_points is None or track_frame_count % 30 == 0:
                    # 在画面四角取一些背景特征点
                    mask_for_bg = np.ones((H, W), dtype=np.uint8) * 255
                    if last_poly_box:
                        x, y, w, h = last_poly_box
                        # 扩大区域，排除物体和手
                        expand = 100
                        x1 = max(0, x - expand)
                        y1 = max(0, y - expand)
                        x2 = min(W, x + w + expand)
                        y2 = min(H, y + h + expand)
                        mask_for_bg[y1:y2, x1:x2] = 0
                    
                    # 在背景区域提取特征点
                    try:
                        bg_pts = cv2.goodFeaturesToTrack(gray, maxCorners=20, 
                                                       qualityLevel=0.1, 
                                                       minDistance=30, 
                                                       mask=mask_for_bg)
                        if bg_pts is not None and len(bg_pts) >= 5:
                            background_points = bg_pts
                            old_background_gray = gray.copy()
                    except Exception as e:
                        #print(f"[TRACK] 背景特征点提取失败: {e}")
                        background_points = None
                
                # 计算背景移动（相机移动）
                if old_background_gray is not None and background_points is not None and len(background_points) > 0:
                    try:
                        bg_p1, bg_st, _ = cv2.calcOpticalFlowPyrLK(
                            old_background_gray, gray, background_points, None, **LK_PARAMS
                        )
                        if bg_p1 is not None and bg_st is not None:
                            good_bg_old = background_points[bg_st == 1]
                            good_bg_new = bg_p1[bg_st == 1]
                            if len(good_bg_new) >= 3 and len(good_bg_old) >= 3:
                                # 计算背景的平均移动
                                bg_movement = np.mean(good_bg_new - good_bg_old, axis=0)
                                camera_movement = bg_movement.reshape(2)
                                background_points = good_bg_new.reshape(-1, 1, 2)
                                old_background_gray = gray.copy()
                    except Exception as e:
                        print(f"[TRACK] 背景光流计算失败: {e}")
                        camera_movement = np.array([0.0, 0.0])

                if old_gray is not None and p0 is not None and len(p0) >= 5:
                    p1, st, err = cv2.calcOpticalFlowPyrLK(old_gray, gray, p0, None, **LK_PARAMS)
                    if p1 is not None and st is not None:
                        good_new = p1[st == 1]
                        if len(good_new) >= 5:
                            p0 = good_new.reshape(-1, 1, 2)
                            hull = cv2.convexHull(good_new.reshape(-1,1,2))
                            poly = hull.reshape(-1, 2)
                            
                            if len(poly) >= 3:
                                # 统一的 YOLOE 实时检测和校正（每帧）
                                latest_det_mask = None
                                if use_yoloe and yoloe_backend is not None:
                                    # 添加调试信息
                                    if track_frame_count % 30 == 0:  # 每30帧打印一次
                                        print(f"[YOLOE] TRACK模式实时检测第 {track_frame_count} 帧")
                                    
                                    # YOLOE 实时检测（统一调用，避免重复）
                                    det = yoloe_backend.segment(frame, conf=YOLO_CORRECTION_CONF_THRESHOLD, iou=0.45, persist=True)
                                    if det["masks"]:
                                        # 取面积最大的那个
                                        areas = [int(m.sum()) for m in det["masks"]]
                                        j = int(np.argmax(areas))
                                        m = det["masks"][j]
                                        if m.shape[:2] != (H, W):
                                            m = cv2.resize(m, (W, H), interpolation=cv2.INTER_NEAREST)
                                        latest_det_mask = (m > 0).astype(np.uint8)
                                        
                                        # 和当前光流多边形的 IoU
                                        poly_mask = np.zeros((H, W), dtype=np.uint8)
                                        cv2.fillPoly(poly_mask, [poly.astype(np.int32)], 1)
                                        inter = np.logical_and(latest_det_mask, poly_mask).sum()
                                        union = np.logical_or(latest_det_mask, poly_mask).sum() + 1e-6
                                        iou = inter / union
                                        
                                        # 降低IoU阈值，更积极地校正
                                        if iou > YOLO_CORRECTION_IOU_THRESHOLD:  # 使用可配置阈值
                                            # 用 YOLOE 结果更新多边形
                                            contours, _ = cv2.findContours(latest_det_mask, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_NONE)
                                            if contours:
                                                largest_contour = max(contours, key=cv2.contourArea)
                                                # 使用更精细的轮廓处理，减少过度简化
                                                epsilon = TRACK_EPSILON_FACTOR * cv2.arcLength(largest_contour, True)
                                                poly = cv2.approxPolyDP(largest_contour, epsilon, True).reshape(-1, 2)
                                                
                                                # 更新光流特征点
                                                edge_mask = inner_offset_edge(latest_det_mask, offset_px=INNER_OFFSET_PX_LOCK, edge_dilate_px=EDGE_DILATE_PX)
                                                pts = cv2.goodFeaturesToTrack(gray, mask=edge_mask, **FEATURE_PARAMS)
                                                if pts is not None and len(pts) >= 5:
                                                    p0 = pts
                                                    #print(f"[TRACK] YOLOE 实时校正，IoU: {iou:.3f}")
                                
                                # 检查是否接触，决定轮廓颜色
                                is_touching = False
                                overlap_ratio = 0.0
                                if hand_box is not None and poly is not None:
                                    is_touching, overlap_ratio = check_hand_object_contact(hand_box, poly, overlap_threshold=0.1)
                                
                                # 绘制多边形（可能已被 YOLOE 更新）- 使用更粗的线条
                                if is_touching:
                                    # 接触时用亮绿色，并添加发光效果
                                    poly_color = (0, 255, 127)
                                    # 绘制一个更粗的外层轮廓作为发光效果
                                    cv2.polylines(vis, [poly.astype(np.int32)], isClosed=True, 
                                                color=(127, 255, 127), thickness=STROKE_WIDTH + 4)
                                    # 添加半透明的填充效果
                                    overlay = vis.copy()
                                    cv2.fillPoly(overlay, [poly.astype(np.int32)], (0, 255, 0))
                                    cv2.addWeighted(overlay, 0.15, vis, 0.85, 0, vis)
                                else:
                                    # 未接触时用普通绿色
                                    poly_color = (0, 255, 0)
                                cv2.polylines(vis, [poly.astype(np.int32)], isClosed=True, color=poly_color, thickness=STROKE_WIDTH)
                                # 多边形质心与面积
                                poly_center, poly_area = polygon_center_and_area(poly)
                                if poly_center:
                                    pc = (int(poly_center[0]), int(poly_center[1]))
                                    cv2.circle(vis, pc, 6, (0,255,0), -1)

                                # 多边形外接矩形（用于周边监控）
                                x, y, w, h = cv2.boundingRect(poly.astype(np.int32))
                                last_poly_box = (x, y, w, h)

                                # ====== 对齐分数（第一条）======
                                if hand_center and poly_center:
                                    hc = np.array(hand_center, dtype=np.float32)
                                    oc = np.array(poly_center, dtype=np.float32)
                                    dist = float(np.linalg.norm(oc - hc))
                                    diag = float(np.linalg.norm([W, H]))
                                    align_score = 1.0 - min(dist/(ALIGN_LOOSE_PCT*diag + 1e-6), 1.0)
                                    
                                    # 绘制虚线引导（替代原来的实线箭头）
                                    draw_dashed_line(vis, (hc[0], hc[1]), (oc[0], oc[1]), 
                                                   color=(255, 255, 0), thickness=2, 
                                                   dash_length=15, gap_length=10)
                                    
                                    # 方向引导
                                    direction, secondary = get_guidance_direction(
                                        hand_center, poly_center, hand_area, poly_area,
                                        hand_box, poly
                                    )
                                    
                                    if direction and direction != "保持":
                                        # 根据是否接触显示不同颜色
                                        if direction == "向前":
                                            # 手已经接触物体，用绿色显示
                                            guide_color = (0, 255, 0)  # 绿色
                                            draw_text_cn(vis, f"引导: {direction} - 伸手抓取", (W//2 - 80, 40), 
                                                       font_size=24, color=guide_color, stroke=(0, 0, 0))
                                        else:
                                            # 还未接触，用黄色显示
                                            guide_color = (0, 255, 255)  # 黄色
                                            draw_text_cn(vis, f"引导: {direction}", (W//2 - 60, 40), 
                                                       font_size=24, color=guide_color, stroke=(0, 0, 0))
                                        
                                        # 显示次要信息（接触度或其他方向）
                                        if secondary:
                                            if isinstance(secondary, str):
                                                # 接触度信息
                                                draw_text_cn(vis, secondary, (W//2 - 60, 70), 
                                                           font_size=18, color=(0, 255, 0))
                                            else:
                                                # 其他方向信息
                                                draw_text_cn(vis, f"（或 {secondary}）", (W//2 - 60, 70), 
                                                           font_size=18, color=(200, 200, 200))
                                        
                                        # 播放语音引导 - 确保每个方向都会播放
                                        if t_now - last_guidance_time > GUIDANCE_INTERVAL_SEC:
                                            # 检查方向是否改变，或者时间间隔足够
                                            if direction != last_guidance_direction or t_now - last_guidance_time > GUIDANCE_INTERVAL_SEC * 2:
                                                play_guidance_audio(direction)
                                                last_guidance_direction = direction
                                                last_guidance_time = t_now
                                                print(f"[GUIDE] 播放引导音频: {direction}")
                                else:
                                    align_score = 0.0

                                # 显示接触状态
                                is_touching, overlap_ratio = check_hand_object_contact(hand_box, poly, overlap_threshold=0.1)
                                if is_touching:
                                    draw_text_cn(vis, f"状态: 已接触 ({overlap_ratio:.1%})", (10, 95), 
                                               font_size=16, color=(0, 255, 0))
                                else:
                                    # 计算手和物体的距离
                                    if hand_center and poly_center:
                                        distance = np.sqrt((hand_center[0] - poly_center[0])**2 + 
                                                         (hand_center[1] - poly_center[1])**2)
                                        draw_text_cn(vis, f"距离: {distance:.0f}px", (10, 95), 
                                                   font_size=16, color=FRONTEND_COLORS["muted"])

                                # 成功条件：握持（放宽）
                                if (_last_result and _last_result[0].hand_landmarks and len(_last_result[0].hand_landmarks) > 0):
                                    l0 = _last_result[0].hand_landmarks[0]
                                    grasp_now, grasp_score = detect_grasp(l0, W, H)
                                else:
                                    grasp_now, grasp_score = False, 0.0
             
                                # guidance_msg 相关代码已经集成到上面的引导逻辑中

                                # ===== 周边监控 & 重新锁定（复用YOLO结果）=====
                                if (track_frame_count % PERI_CHECK_EVERY == 0) and (last_poly_box is not None) and (latest_det_mask is not None):
                                    # 直接使用刚才的YOLO检测结果，避免重复调用
                                    px, py, pw, ph = last_poly_box
                                    x0 = max(0, px - PERI_MONITOR_PX)
                                    y0 = max(0, py - PERI_MONITOR_PX)
                                    x1 = min(W - 1, px + pw + PERI_MONITOR_PX)
                                    y1 = min(H - 1, py + ph + PERI_MONITOR_PX)
                                    
                                    # 检查周边区域是否有更好的检测结果
                                    peri_area = latest_det_mask[y0:y1, x0:x1].sum()
                                    total_area = latest_det_mask.sum()
                                    
                                    # 如果周边区域有显著检测结果，重新锁定
                                    if peri_area > total_area * 0.1:  # 周边有10%以上的检测面积
                                        edge_mask = inner_offset_edge(latest_det_mask, offset_px=INNER_OFFSET_PX_LOCK, edge_dilate_px=EDGE_DILATE_PX)
                                        pts = cv2.goodFeaturesToTrack(gray, mask=edge_mask, **FEATURE_PARAMS)
                                        if pts is not None and len(pts) >= 8:
                                            p0 = pts
                                            old_gray = gray
                                            lock_edge_debug = edge_mask.copy()
                                            #print(f"[PERI] 周边重锁定，特征点数={len(p0)}")
                            else:
                                MODE = "SEGMENT"; old_gray = None; p0 = None; lock_edge_debug = None
                        else:
                            MODE = "SEGMENT"; old_gray = None; p0 = None; lock_edge_debug = None
                    else:
                        MODE = "SEGMENT"; old_gray = None; p0 = None; lock_edge_debug = None
                else:
                    MODE = "SEGMENT"; old_gray = None; p0 = None; lock_edge_debug = None

  

                if MODE == "SEGMENT":
                    draw_text_cn(vis, "追踪丢失 → 正在重新识别。按 Enter 重新锁定", (10, 100), font_size=22, color=(0,0,255))

                old_gray = gray

            # FPS（移动到左下角样式）
            if 'fps_hist' not in locals():
                fps_hist = []
            fps_hist.append(t_now)
            if len(fps_hist) > 30:
                fps_hist.pop(0)
            fps = 0.0 if len(fps_hist) < 2 else (len(fps_hist)-1)/(fps_hist[-1]-fps_hist[0])
            draw_text_cn(vis, f"FPS: {fps:.1f}", (10, 40), font_size=16, color=FRONTEND_COLORS["ok"]) 

            # 右下角显示"内边界/最近一次锁定"的调试图
            if lock_edge_debug is not None:
                # 极小缩放并放在右下角
                small = cv2.resize(lock_edge_debug, (0,0), fx=0.22, fy=0.22, interpolation=cv2.INTER_NEAREST)
                sh, sw = small.shape[:2]
                small_bgr = cv2.cvtColor(small, cv2.COLOR_GRAY2BGR)
                # 右下角位置，留 10-12px 边距
                x1 = max(8, W - sw - 12)
                y1 = max(8, H - sh - 12)
                y2 = y1 + sh
                x2 = x1 + sw
                vis[y1:y2, x1:x2] = small_bgr
                # 标签置于图上方紧贴，使用更小字号
                #draw_text_cn(vis, "内边界", (x1, y1 - 8), font_size=12, color=FRONTEND_COLORS["muted"], ui_hint=False)

            # 底部中间的"当前指令"按钮（始终绘制，文案随音频同步）
            draw_command_pill(vis, CURRENT_COMMAND_TEXT)

            # 展示（无论 headless 与否，都会推给前端）
            bridge_io.send_vis_bgr(vis)

            # [headless] 只有非 headless 时才弹窗与键盘交互；headless 下用 waitKey(1) 让出调度
            if not headless:
                cv2.imshow(WINDOW, vis)
                key = cv2.waitKey(1) & 0xFF
                if key in (27, ord('q')):
                    break
                elif key == ord('r'):
                    MODE = "SEGMENT"; old_gray = None; p0 = None; lock_edge_debug = None
                elif key == 13:  # Enter：从 SEGMENT 锁定并开始 TRACK（内收 5px）
                    if MODE == "SEGMENT":
                        # 使用 YOLOE 进行手动锁定
                        if use_yoloe and yoloe_backend is not None:
                            det = yoloe_backend.segment(frame, conf=CONF_THRESHOLD, iou=0.45, persist=True)
                            if det["masks"]:
                                # 取面积最大的那个
                                areas = [int(m.sum()) for m in det["masks"]]
                                j = int(np.argmax(areas))
                                m = det["masks"][j]
                                if m.shape[:2] != (H, W):
                                    m = cv2.resize(m, (W, H), interpolation=cv2.INTER_NEAREST)
                                best_mask = (m > 0.5).astype(np.uint8)
                            else:
                                best_mask = None
                        else:
                            best_mask = None
                        if best_mask is not None:
                            edge_mask = inner_offset_edge(best_mask, offset_px=INNER_OFFSET_PX_LOCK, edge_dilate_px=EDGE_DILATE_PX)
                            gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
                            pts = cv2.goodFeaturesToTrack(gray, mask=edge_mask, **FEATURE_PARAMS)
                            if pts is not None and len(pts) >= 8:
                                p0 = pts
                                old_gray = gray
                                MODE = "TRACK"
                                lock_edge_debug = edge_mask.copy()
                                track_frame_count = 0
                                print(f"[LOCK] 内边界特征点数={len(p0)} → TRACK")
                            else:
                                print("[LOCK] 内边界特征点不足，请调整画面后重试。")
                        else:
                            print("[LOCK] 当前帧未找到有效分割，请重试。")
            else:
                # headless 下也调用一次 waitKey(1)，让 OpenCV 的计时器/回调得到机会，且避免 CPU 忙等
                cv2.waitKey(1)
                
                # 在 headless 模式下检查停止事件
                if stop_event and stop_event.is_set():
                    print("[YOLOMEDIA] Received stop signal in headless mode")
                    break

    finally:
        try:
            landmarker.close()
        except Exception:
            pass
        #cap.release()
        # [headless] 仅在非 headless 时销毁窗口
        if not headless:
            cv2.destroyAllWindows()


if __name__ == "__main__":
    main()
