"""
Touch 部署客户端（NexBench Remote SDK）— 与 server_test.sh / PolicyClient 交互。

与 transport_client_touch_zc.py 的差异：
  - 图像与机械臂控制走 lumos_remote（absolute_pose），不再用本地 USB 相机 / DualTouchRobot
  - 策略推理仍走 superbot.transport.client.PolicyClient（tcp://localhost:5562）
  - 控制下发参考 absolute_pose_chunk_demo：整段 chunk 一次 step（exec_frequency）

superbot 映射（与 dual_touch_new / rotation.py 对齐，务必保留）：
  1. proprio / action 为 20D：
       [L_xyz(3), L_r6d(6), L_grip(1), R_xyz(3), R_r6d(6), R_grip(1)]
     R6D 布局与 nexbench / eef_6d_new_reg 一致（Zhou interleaved）
  2. 执行前 abs_6d_2_abs_euler：R6D → extrinsic XYZ 欧拉角（弧度），再展成
     双臂 absolute_pose chunk 行：[L_xyz,L_rpy,L_g, R_xyz,R_rpy,R_g] = 14D
  3. GRIPPER_SCALE：训练侧夹爪近似「米 / 0.85」；nexbench 侧为 [0,1]
     观测：nex_grip[0,1] → meters → /GRIPPER_SCALE 再发给 server
     执行：server 输出 *GRIPPER_SCALE → meters → /GRIPPER_MAX → nex [0,1]（量化到 0.1）
  4. 松灵坐标系旋转对齐（对齐 dual_touch_new）分两侧独立开关：
     --apply-eef-rot-obs：观测侧硬件→policy（组 proprio）
     --apply-eef-rot-exec：执行侧 policy→硬件（下发 absolute_pose chunk）

用法见 deployment/client_touch_nex.sh。
"""

from __future__ import annotations

import argparse
import base64
import io
import json
import os
import select
import sys
import termios
import threading
import time
import tty
import urllib.error
import urllib.parse
import urllib.request
import uuid
from datetime import datetime
from typing import Any

import cv2
import numpy as np
from scipy.signal import butter, filtfilt  # noqa: F401 — 须在 json_numpy.patch 前加载 scipy
from scipy.spatial.transform import Rotation as R

import json_numpy

json_numpy.patch()

from loguru import logger

from superbot.transport.client import PolicyClient
from superbot.utils.rotation import abs_6d_2_abs_euler, rotation_6d_to_matrix

# ---------------------------------------------------------------------------
# lumos_remote（nexbench）路径
# ---------------------------------------------------------------------------
_NEX_SDK_CANDIDATES = [
    os.environ.get("NEXBENCH_SDK_ROOT", ""),
    # chunk + exec_frequency 需要 V1.3+；只取第一个命中，不要再插 V1.2
    "/mnt/samsungdisk/nexbenchremotesdk/src",
    "/mnt/samsungdisk/nexbenchremotesdk",
]


def _resolve_lumos_remote_root() -> str:
    for raw in _NEX_SDK_CANDIDATES:
        if not raw:
            continue
        root = os.path.abspath(os.path.expanduser(raw))
        if os.path.isdir(os.path.join(root, "lumos_remote")):
            return root
        src = os.path.join(root, "src")
        if os.path.isdir(os.path.join(src, "lumos_remote")):
            return src
    raise ImportError(
        "找不到 lumos_remote V1.3+。请 pip install -e "
        "'/home/lumos6/nexbenchremotesdk_V1.3/nexbenchremotesdk[ws]' "
        "或 export NEXBENCH_SDK_ROOT=/path/to/.../src"
    )


def _ensure_lumos_remote() -> None:
    """强制使用 V1.3+（含 Action.exec_frequency），避免环境里旧版 V1.2 抢先。"""
    root = _resolve_lumos_remote_root()

    # 清掉已导入的旧包 / 旧路径，否则会继续打到 V1.2
    for mod_name in list(sys.modules):
        if mod_name == "lumos_remote" or mod_name.startswith("lumos_remote."):
            del sys.modules[mod_name]
    for p in list(sys.path):
        if "nexbenchremotesdk_V1.2" in str(p) and p != root:
            try:
                sys.path.remove(p)
            except ValueError:
                pass
    if root in sys.path:
        sys.path.remove(root)
    sys.path.insert(0, root)

    import lumos_remote  # noqa: F401
    from lumos_remote.types import Action as _ActionCheck

    if "exec_frequency" not in getattr(_ActionCheck, "__dataclass_fields__", {}):
        raise ImportError(
            f"已加载的 lumos_remote 不含 exec_frequency（路径={lumos_remote.__file__}）。"
            "请改用 nexbenchremotesdk_V1.3。"
        )
    logger.info(f"lumos_remote: {lumos_remote.__file__}")


_ensure_lumos_remote()
from lumos_remote import (  # noqa: E402
    Action,
    Config,
    LumosClient,
    LumosError,
    Observation,
)

# ---------------------------------------------------------------------------
# 与 transport_client_touch_zc 对齐的常量
# ---------------------------------------------------------------------------
PROPRIO_POS_BIAS = 0.0
GRIPPER_SCALE = 0.85
GRIPPER_DISTANCE_MAX_M = 0.085  # DualTouchRobot.GRIPPER_DISTANCE_MAX_M
CHUNK_SKIP = 0
DEFAULT_EXEC_STEPS = 10
OBS_HEAD_SIZE = (640, 480)
OBS_WRIST_SIZE = (640, 480)

# nexbench 默认 endpoint（与 absolute_pose_demo 一致）
WS_CONTROL_BASE = "ws://124.174.105.207:8222/control"
WS_FEEDBACK_BASE = "ws://124.174.105.207:8222/stream"
HTTP_BASE = "https://dataplatform-qa.lumosbot.tech"

# dual_touch_new.move_to_pose_infer 使用的右乘矩阵（policy → 松灵/硬件）
_EEF_ROT_POLICY_TO_ROBOT = np.array([[0, 0, -1], [0, 1, 0], [1, 0, 0]], dtype=float)
# 与上式互逆，对应 dual_touch_new 读状态时的坐标系还原（硬件 → policy）
_EEF_ROT_ROBOT_TO_POLICY = _EEF_ROT_POLICY_TO_ROBOT.T


def _request_priority_close(
    control_endpoint: str,
    task_id: str,
    task_eval_id: str,
    workstation_id: str,
) -> None:
    """通过独立 HTTP 通道请求回零，不能复用可能被 observation 卡住的 control WS。"""
    parsed = urllib.parse.urlsplit(control_endpoint)
    if parsed.scheme not in ("ws", "wss") or not parsed.netloc:
        raise ValueError(f"invalid control endpoint for priority close: {control_endpoint!r}")
    scheme = "https" if parsed.scheme == "wss" else "http"
    close_url = urllib.parse.urlunsplit((scheme, parsed.netloc, "/close", "", ""))
    body = {
        "type": "close",
        "msg_id": f"close-{uuid.uuid4().hex}",
        "payload": {
            "task_id": task_id,
            "task_eval_id": task_eval_id,
            "workstation_id": workstation_id,
            "return_home": True,
        },
    }
    request = urllib.request.Request(
        close_url,
        data=json.dumps(body).encode("utf-8"),
        headers={"Content-Type": "application/json", "Accept": "application/json"},
        method="POST",
    )
    with urllib.request.urlopen(request, timeout=5.0) as response:
        result = json.loads(response.read().decode("utf-8"))
    if result.get("type") != "close_ack" or not result.get("payload", {}).get("return_home_queued"):
        raise RuntimeError(f"priority close rejected: {result}")
    logger.info(
        "priority HTTP close queued: url={} task_eval_id={} workstation_id={}",
        close_url,
        task_eval_id,
        workstation_id,
    )


def _proprio_with_position_bias(proprio: np.ndarray) -> np.ndarray:
    out = np.asarray(proprio, dtype=np.float32).reshape(-1).copy()
    out[0] -= PROPRIO_POS_BIAS
    out[10] -= PROPRIO_POS_BIAS
    return out


def _decode_image_rgb(frame) -> np.ndarray | None:
    """ImageFrame → RGB uint8 (H,W,3)。"""
    data = frame.data
    if data is None:
        return None
    if isinstance(data, str) and data:
        data = base64.b64decode(data)
    if not isinstance(data, (bytes, bytearray)) or not data:
        return None
    arr = np.frombuffer(data, dtype=np.uint8)
    bgr = cv2.imdecode(arr, cv2.IMREAD_COLOR)
    if bgr is None:
        # 部分环境 jpeg 走 PIL
        try:
            from PIL import Image

            return np.array(Image.open(io.BytesIO(bytes(data))).convert("RGB"))
        except Exception:
            return None
    return cv2.cvtColor(bgr, cv2.COLOR_BGR2RGB)


def _pick_images(obs: Observation) -> tuple[np.ndarray, np.ndarray, np.ndarray]:
    """按 nex 实际顺序取图，并输出为 server 约定的 头/左/右。

    当前确认 nex get_observation() 返回：
      - image0 = head
      - image1 = right_wrist
      - image2 = left_wrist

    为了继续对齐 zc / server 输入语义，这里重排为：
      - out image0 = head   <- obs.image0
      - out image1 = left   <- obs.image2
      - out image2 = right  <- obs.image1
    """
    by_id: dict[str, np.ndarray] = {}
    for img in obs.images or []:
        rgb = _decode_image_rgb(img)
        if rgb is None:
            continue
        key = (img.image_id or "").strip() or f"image{len(by_id)}"
        by_id[key] = rgb

    filler = np.full((480, 640, 3), 128, np.uint8)
    ordered = []
    # 输出继续保持 0=head, 1=left, 2=right；因此从 nex 的 0=head,1=right,2=left 重排
    for key in ("image0", "image2", "image1"):
        rgb = by_id.get(key)
        if rgb is None:
            # 回退：按出现顺序
            if by_id:
                rgb = next(iter(by_id.values()))
            else:
                rgb = filler
                logger.warning(f"missing {key}, using gray filler")
        bgr = cv2.cvtColor(np.ascontiguousarray(rgb), cv2.COLOR_RGB2BGR)
        ordered.append(bgr)

    img0 = cv2.resize(ordered[0], OBS_HEAD_SIZE)
    img1 = cv2.resize(ordered[1], OBS_WRIST_SIZE)
    img2 = cv2.resize(ordered[2], OBS_WRIST_SIZE)
    return img0, img1, img2


def _nex_grip_to_policy(grip_01: float) -> float:
    """nexbench [0,1] → 训练 proprio 夹爪（米 / GRIPPER_SCALE）。"""
    grip_m = float(np.clip(grip_01, 0.0, 1.0)) * GRIPPER_DISTANCE_MAX_M
    return grip_m / GRIPPER_SCALE


def _policy_grip_to_nex(grip_policy: float) -> float:
    """训练 action 夹爪 -> nexbench [0,1]，并量化到 0.1。

    nexbench 控制端当前要求 gripper 目标只能是一位小数：
    0.0, 0.1, ..., 1.0。
    """
    grip_m = float(grip_policy) * GRIPPER_SCALE
    if grip_m < 0.035:
        grip_m = 0.0002
    grip_01 = float(np.clip(grip_m / GRIPPER_DISTANCE_MAX_M, 0.0, 1.0))
    return float(np.clip(np.round(grip_01 * 10.0) / 10.0, 0.0, 1.0))


def _matrix_to_interleaved_r6d(mat: np.ndarray) -> np.ndarray:
    """旋转矩阵 -> interleaved R6D，布局与 superbot.rotation_matrix_to_6d 一致。"""
    return np.concatenate([mat[0, :2], mat[1, :2], mat[2, :2]]).astype(np.float32)


def _obs_r6d_to_policy_r6d(r6d: np.ndarray) -> np.ndarray:
    """观测侧配套旋转：硬件/nex 反馈帧 -> policy 帧。"""
    mat = rotation_6d_to_matrix(np.asarray(r6d, dtype=np.float32).reshape(6))
    mat = mat @ _EEF_ROT_ROBOT_TO_POLICY
    return _matrix_to_interleaved_r6d(mat)


def _arm_pose_10d(arm_state, *, apply_eef_rot_obs: bool = False) -> np.ndarray:
    """ArmState -> [xyz(3), r6d(6), grip_policy(1)]，可选做硬件->policy 旋转对齐。"""
    ee = arm_state.ee_pose or {}
    xyz = np.array(
        [float(ee.get("x", 0.0)), float(ee.get("y", 0.0)), float(ee.get("z", 0.0))],
        dtype=np.float32,
    )
    r6d = ee.get("r6d")
    if r6d is None or len(r6d) != 6:
        # 回退：quat → 矩阵 → 连续/interleaved 6D（与 rotation_matrix_to_6d 一致）
        quat = ee.get("quat_xyzw", [0, 0, 0, 1])
        mat = R.from_quat(quat).as_matrix()
        r6d = np.concatenate([mat[0, :2], mat[1, :2], mat[2, :2]])
    r6d = np.asarray(r6d, dtype=np.float32).reshape(6)
    if apply_eef_rot_obs:
        r6d = _obs_r6d_to_policy_r6d(r6d)
    grip = _nex_grip_to_policy(float(getattr(arm_state, "gripper", 1.0)))
    return np.concatenate([xyz, r6d, np.array([grip], dtype=np.float32)])


def _obs_to_proprio20(obs: Observation, *, apply_eef_rot_obs: bool = False) -> np.ndarray:
    left = _arm_pose_10d(obs.robot_state.left_arm, apply_eef_rot_obs=apply_eef_rot_obs)
    right = _arm_pose_10d(obs.robot_state.right_arm, apply_eef_rot_obs=apply_eef_rot_obs)
    return np.concatenate([left, right]).astype(np.float32)


def _build_policy_obs(
    nex_obs: Observation,
    instruction: str,
    steps: int,
    robot_type: str,
    speed: float | None,
    *,
    apply_eef_rot_obs: bool = False,
) -> dict:
    img0, img1, img2 = _pick_images(nex_obs)
    proprio = _obs_to_proprio20(nex_obs, apply_eef_rot_obs=apply_eef_rot_obs)
    # 与 zc 一致：再 /GRIPPER_SCALE 一次？否——_nex_grip_to_policy 已含 /GRIPPER_SCALE
    # zc 是先读米制再 /GRIPPER_SCALE；此处已对齐到同一量纲。
    proprio_biased = _proprio_with_position_bias(proprio)

    os.makedirs("obs", exist_ok=True)
    try:
        stitched = np.concatenate([img0, img1, img2], axis=1)
        cv2.imwrite("obs/img_wrist_nex.jpg", stitched)
        cv2.imwrite("obs/img_head_nex.jpg", img0)
    except Exception:
        pass

    out = {
        "image0": img0,
        "image1": img1,
        "image2": img2,
        "proprio": proprio,
        "proprio_biased": proprio_biased,
        "language_instruction": instruction,
        "robot_type": robot_type,
        "steps": steps,
        "domain_id": 21,
    }
    if speed is not None:
        out["speed"] = float(speed)
    return out


def _apply_eef_rot_to_rpy(rpy: np.ndarray) -> np.ndarray:
    """policy 欧拉 → dual_touch_new.move_to_pose_infer 使用的松灵欧拉。"""
    mat = R.from_euler("xyz", rpy, degrees=False).as_matrix() @ _EEF_ROT_POLICY_TO_ROBOT
    return R.from_matrix(mat).as_euler("xyz", degrees=False)


def _is_valid_policy_pose(pose: np.ndarray) -> bool:
    arr = np.asarray(pose, dtype=np.float32).reshape(-1)
    if arr.shape[0] != 20 or not np.isfinite(arr).all():
        return False
    if np.allclose(arr, 0.0):
        return False
    for slc in (slice(3, 9), slice(13, 19)):
        r6d = arr[slc]
        if np.linalg.norm(r6d) < 1e-6:
            return False
        mat = rotation_6d_to_matrix(r6d)
        if not np.isfinite(mat).all():
            return False
    return True


def _pose20_to_abs14(pose20: np.ndarray, *, apply_eef_rot_exec: bool = False) -> np.ndarray | None:
    """20D policy pose → 双臂 absolute_pose 一行 14D: [Lxyz,Lrpy,Lg, Rxyz,Rrpy,Rg]。"""
    if not _is_valid_policy_pose(pose20):
        logger.warning("skip invalid policy pose (zero/nan/bad r6d)")
        return None
    abs_pose = np.asarray(abs_6d_2_abs_euler(pose20), dtype=float).copy()
    # abs_pose: [L_xyz(3), L_rpy(3), L_g, R_xyz(3), R_rpy(3), R_g]
    l_xyz, l_rpy, l_g = abs_pose[0:3], abs_pose[3:6], abs_pose[6]
    r_xyz, r_rpy, r_g = abs_pose[7:10], abs_pose[10:13], abs_pose[13]
    if apply_eef_rot_exec:
        l_rpy = _apply_eef_rot_to_rpy(l_rpy)
        r_rpy = _apply_eef_rot_to_rpy(r_rpy)
    return np.concatenate(
        [
            l_xyz,
            l_rpy,
            [_policy_grip_to_nex(l_g)],
            r_xyz,
            r_rpy,
            [_policy_grip_to_nex(r_g)],
        ]
    ).astype(np.float64)


def _poses20_to_chunk14(
    poses20,
    *,
    apply_eef_rot_exec: bool = False,
) -> np.ndarray | None:
    """若干 20D policy pose → (N,14) absolute_pose chunk。"""
    rows = []
    for pose in poses20:
        row = _pose20_to_abs14(pose, apply_eef_rot_exec=apply_eef_rot_exec)
        if row is None:
            continue
        rows.append(row)
    if not rows:
        return None
    return np.asarray(rows, dtype=np.float64)


def _build_transition_poses(prev_last_pose, next_first_pose, transition_steps):
    if prev_last_pose is None or next_first_pose is None or transition_steps <= 0:
        return []
    bridge = np.linspace(prev_last_pose, next_first_pose, transition_steps + 2)
    return bridge[1:-1]


def _jsonable(obj: Any) -> Any:
    if obj is None or isinstance(obj, (str, int, float, bool)):
        return obj
    if isinstance(obj, np.ndarray):
        return obj.tolist()
    if isinstance(obj, (np.floating, np.integer)):
        return obj.item()
    if isinstance(obj, dict):
        return {str(k): _jsonable(v) for k, v in obj.items()}
    if isinstance(obj, (list, tuple)):
        return [_jsonable(v) for v in obj]
    return str(obj)


def _arm_state_to_dict(arm) -> dict:
    ee = getattr(arm, "ee_pose", None) or {}
    return {
        "ee_pose": {
            "x": ee.get("x"),
            "y": ee.get("y"),
            "z": ee.get("z"),
            "r6d": list(ee["r6d"]) if ee.get("r6d") is not None else None,
            "quat_xyzw": list(ee["quat_xyzw"]) if ee.get("quat_xyzw") is not None else None,
        },
        "gripper": float(getattr(arm, "gripper", 1.0)),
        "joint_positions": (
            list(arm.joint_positions)
            if getattr(arm, "joint_positions", None) is not None
            else None
        ),
    }


def _nex_obs_state_dict(
    nex_obs: Observation,
    *,
    proprio20: np.ndarray | None = None,
    apply_eef_rot_obs: bool = False,
    last_executed_seq: int | None = None,
    next_seq: int | None = None,
) -> dict:
    rs = nex_obs.robot_state
    out = {
        "timestamp": nex_obs.timestamp,
        "task_eval_id": nex_obs.task_eval_id,
        "status": nex_obs.status,
        "latency_ms": nex_obs.latency_ms,
        # get_observation 响应里的 last_executed_seq：远端已确认执行完的 step 序号
        "last_executed_seq": last_executed_seq,
        # 下一发 step 将使用的 sequence_id（SDK _next_seq）
        "next_seq": next_seq,
        "left_arm": _arm_state_to_dict(rs.left_arm),
        "right_arm": _arm_state_to_dict(rs.right_arm),
        "apply_eef_rot_obs": bool(apply_eef_rot_obs),
    }
    if proprio20 is not None:
        out["proprio20"] = np.asarray(proprio20, dtype=np.float32).reshape(-1).tolist()
    return out


class _DebugDump:
    """每轮记录：收到的远端状态 + 下发的 chunk，便于排查接缝回退等。"""

    def __init__(self, root: str | None):
        self.root = root
        self.cycle = 0
        self._index_fp = None
        if not root:
            return
        os.makedirs(root, exist_ok=True)
        self._index_fp = open(os.path.join(root, "index.jsonl"), "a", encoding="utf-8")
        logger.info(f"debug dump → {os.path.abspath(root)}")

    @property
    def enabled(self) -> bool:
        return bool(self.root)

    def close(self) -> None:
        if self._index_fp is not None:
            self._index_fp.close()
            self._index_fp = None

    def write_meta(self, meta: dict) -> None:
        if not self.enabled:
            return
        path = os.path.join(self.root, "meta.json")
        with open(path, "w", encoding="utf-8") as f:
            json.dump(_jsonable(meta), f, ensure_ascii=False, indent=2)

    def record_cycle(
        self,
        *,
        obs_state: dict,
        policy_chunk20: np.ndarray | None = None,
        exec_poses20: np.ndarray | None = None,
        send_poses20: np.ndarray | None = None,
        send_chunk14: np.ndarray | None = None,
        n_transition: int = 0,
        step_info: dict | None = None,
        policy_rtt_s: float | None = None,
        obs_rtt_ms: float | None = None,
    ) -> None:
        if not self.enabled:
            return
        self.cycle += 1
        cdir = os.path.join(self.root, f"cycle_{self.cycle:05d}")
        os.makedirs(cdir, exist_ok=True)

        with open(os.path.join(cdir, "obs_state.json"), "w", encoding="utf-8") as f:
            json.dump(_jsonable(obs_state), f, ensure_ascii=False, indent=2)

        def _save_array_json(name: str, arr: np.ndarray, cols: list[str] | None = None) -> None:
            a = np.asarray(arr, dtype=np.float64)
            payload: dict[str, Any] = {
                "shape": list(a.shape),
                "dtype": "float64",
                "data": a.tolist(),
            }
            if cols is not None and a.ndim == 2 and a.shape[1] == len(cols):
                payload["columns"] = cols
            with open(os.path.join(cdir, f"{name}.json"), "w", encoding="utf-8") as f:
                json.dump(payload, f, ensure_ascii=False, indent=2)

        cols20 = [
            "L_x", "L_y", "L_z", "L_r6d0", "L_r6d1", "L_r6d2", "L_r6d3", "L_r6d4", "L_r6d5", "L_grip",
            "R_x", "R_y", "R_z", "R_r6d0", "R_r6d1", "R_r6d2", "R_r6d3", "R_r6d4", "R_r6d5", "R_grip",
        ]
        cols14 = [
            "L_x", "L_y", "L_z", "L_rx", "L_ry", "L_rz", "L_grip",
            "R_x", "R_y", "R_z", "R_rx", "R_ry", "R_rz", "R_grip",
        ]
        if policy_chunk20 is not None:
            _save_array_json("policy_chunk20", policy_chunk20, cols20)
        if exec_poses20 is not None:
            _save_array_json("exec_poses20", exec_poses20, cols20)
        if send_poses20 is not None:
            _save_array_json("send_poses20", send_poses20, cols20)
        if send_chunk14 is not None:
            _save_array_json("send_chunk14", send_chunk14, cols14)

        summary = {
            "cycle": self.cycle,
            "wall_time": datetime.now().isoformat(timespec="milliseconds"),
            "obs_rtt_ms": obs_rtt_ms,
            "policy_rtt_s": policy_rtt_s,
            "n_transition": int(n_transition),
            "policy_chunk_n": int(np.asarray(policy_chunk20).shape[0])
            if policy_chunk20 is not None
            else 0,
            "send_n": int(np.asarray(send_chunk14).shape[0])
            if send_chunk14 is not None
            else (int(np.asarray(send_poses20).shape[0]) if send_poses20 is not None else 0),
            "obs_status": obs_state.get("status"),
            "obs_last_executed_seq": obs_state.get("last_executed_seq"),
            "obs_next_seq": obs_state.get("next_seq"),
            "send_sequence_id": (step_info or {}).get("sequence_id"),
            "obs_proprio20": obs_state.get("proprio20"),
            "step": step_info or {},
        }
        with open(os.path.join(cdir, "summary.json"), "w", encoding="utf-8") as f:
            json.dump(_jsonable(summary), f, ensure_ascii=False, indent=2)
        self._index_fp.write(json.dumps(_jsonable(summary), ensure_ascii=False) + "\n")
        self._index_fp.flush()


class _BKeyListener:
    """后台监听 B：标记回零请求（nexbench 无本地 go_home，仅跳过当前 chunk 并用新 obs 重规划）。"""

    def __init__(self):
        self._reset_event = threading.Event()
        self._stop_event = threading.Event()
        self._thread: threading.Thread | None = None

    def start(self) -> bool:
        if not sys.stdin.isatty():
            logger.warning("stdin 非 TTY，键盘控制不可用")
            return False
        self._thread = threading.Thread(target=self._loop, daemon=True)
        self._thread.start()
        logger.info("按 B：丢弃当前 chunk，用最新反馈 obs 重新请求 server")
        return True

    def _loop(self) -> None:
        fd = sys.stdin.fileno()
        old = termios.tcgetattr(fd)
        try:
            tty.setcbreak(fd)
            while not self._stop_event.is_set():
                ready, _, _ = select.select([sys.stdin], [], [], 0.1)
                if not ready:
                    continue
                ch = sys.stdin.read(1)
                if ch in ("b", "B"):
                    self._reset_event.set()
        finally:
            termios.tcsetattr(fd, termios.TCSADRAIN, old)

    def poll_reset(self) -> bool:
        if self._reset_event.is_set():
            self._reset_event.clear()
            return True
        return False

    def stop(self) -> None:
        self._stop_event.set()
        if self._thread is not None:
            self._thread.join(timeout=1.0)


def _advance_nex_seq_after_consumed_step(session, err: LumosError) -> None:
    """SDK 仅在 step 成功时递增 _next_seq；服务端对已收包的故障仍会占用该 sequence_id。

    若不推进，下一轮会以相同 seq 发送不同 chunk → ACTION_DUPLICATE_SEQUENCE。
    """
    # 这些错误通常表示服务端已处理（或已登记）该 sequence_id
    consumed = {
        "ROBOT_FAULT",
        "ACTION_DUPLICATE_SEQUENCE",
        "ACTION_OUT_OF_BOUNDS",
        "ACTION_REJECTED",
        "TIMEOUT",
    }
    if getattr(err, "code", None) not in consumed:
        return
    cur = int(getattr(session, "_next_seq", 1))
    last = int(getattr(session, "_last_executed_seq", 0) or 0)
    new = max(cur + 1, last + 1)
    session._next_seq = new
    logger.warning(
        f"advanced nex sequence_id {cur} → {new} after [{err.code}] "
        f"(SDK 成功路径才自增；故障后需手动推进)"
    )


def _execute_chunk_nex(
    session,
    poses20,
    *,
    apply_eef_rot_exec: bool,
    exec_frequency: float,
) -> tuple[bool, dict]:
    """整段 chunk 一次下发（对齐 absolute_pose_chunk_demo）。

    Action(target_arm="both", chunk=(N,14), exec_frequency=Hz)
    session.step 阻塞到远端按频率执行完整块并返回 StepAck。
    返回 (ok, info)，info 含 chunk14 / seq / 错误等，供 debug dump。
    """
    info: dict[str, Any] = {
        "sequence_id": int(getattr(session, "_next_seq", 0) or 0),
        "seq_before": int(getattr(session, "_next_seq", 0) or 0),
        "last_executed_seq_before": int(getattr(session, "_last_executed_seq", 0) or 0),
        "exec_frequency": float(exec_frequency),
        "apply_eef_rot_exec": bool(apply_eef_rot_exec),
    }
    chunk14 = _poses20_to_chunk14(poses20, apply_eef_rot_exec=apply_eef_rot_exec)
    if chunk14 is None or chunk14.shape[0] == 0:
        logger.warning("empty absolute_pose chunk, skip")
        info.update(ok=False, error="empty_chunk")
        return False, info

    # exec_frequency 要求 N>1；单点时复制一行满足协议，仍只表达同一目标
    if chunk14.shape[0] == 1:
        chunk14 = np.vstack([chunk14, chunk14])

    n = int(chunk14.shape[0])
    freq = float(exec_frequency)
    info["chunk14"] = chunk14
    info["n"] = n
    logger.info(
        f"step chunk N={n} @{freq:.1f}Hz sequence_id={info['sequence_id']} "
        f"（预计执行 {n / max(freq, 1e-6):.1f}s）"
    )
    try:
        ack = session.step(
            Action(
                target_arm="both",
                chunk=chunk14.tolist(),
                exec_frequency=freq,
            )
        )
    except TypeError as e:
        # 旧版 SDK 无 exec_frequency 字段
        logger.error(
            f"Action(exec_frequency=...) 不被当前 lumos_remote 支持，请升级到 V1.3+: {e}"
        )
        info.update(ok=False, error="no_exec_frequency", error_message=str(e))
        return False, info
    except LumosError as e:
        logger.error(f"session.step failed: [{e.code}] {e.message}")
        if e.detail:
            logger.error(f"detail: {e.detail}")
        _advance_nex_seq_after_consumed_step(session, e)
        info.update(
            ok=False,
            error_code=e.code,
            error_message=e.message,
            error_detail=e.detail,
            seq_after=int(getattr(session, "_next_seq", 0) or 0),
        )
        return False, info
    if ack.status in ("completed", "failed", "expired", "terminated"):
        logger.warning(f"session status={ack.status}, stop execution")
        info.update(
            ok=False,
            status=ack.status,
            last_executed_seq=ack.last_executed_seq,
            seq_after=int(getattr(session, "_next_seq", 0) or 0),
        )
        return False, info
    logger.info(f"chunk ack seq={ack.last_executed_seq} status={ack.status}")
    info.update(
        ok=True,
        status=ack.status,
        last_executed_seq=ack.last_executed_seq,
        sequence_id=info["sequence_id"],
        seq_after=int(getattr(session, "_next_seq", 0) or 0),
    )
    return True, info


def ctrl_loop(
    session,
    policy_client: PolicyClient,
    instruction: str,
    steps: int,
    dt: float,
    robot_type: str,
    *,
    speed: float | None = None,
    apply_eef_rot_obs: bool = False,
    apply_eef_rot_exec: bool = False,
    exec_frequency: float | None = None,
    debug_dump: _DebugDump | None = None,
):
    os.makedirs("obs", exist_ok=True)
    prev_chunk_last_pose = None
    transition_steps = 6
    freq = float(exec_frequency) if exec_frequency is not None else (1.0 / max(dt, 1e-6))
    dump = debug_dump if debug_dump is not None else _DebugDump(None)
    key_listener = _BKeyListener()
    key_listener.start()
    logger.info(
        f"eef_rot obs={'on' if apply_eef_rot_obs else 'off'} "
        f"exec={'on' if apply_eef_rot_exec else 'off'}"
    )
    time.sleep(1.0)

    try:
        while True:
            try:
                if key_listener.poll_reset():
                    prev_chunk_last_pose = None
                    logger.info("[Reset] 下一周期用 get_observation() 新拉取的 obs 重规划")
                    continue

                obs_t0 = time.monotonic()
                # time.sleep(0.5)  # disabled: fixed observation polling delay
                nex_obs = session.get_observation()
                obs_rtt_ms = (time.monotonic() - obs_t0) * 1000.0
                obs_last_seq = int(getattr(session, "_last_executed_seq", 0) or 0)
                obs_next_seq = int(getattr(session, "_next_seq", 0) or 0)
                logger.info(
                    f"get_observation RTT {obs_rtt_ms:.1f}ms "
                    f"last_executed_seq={obs_last_seq} next_seq={obs_next_seq}"
                )
                if nex_obs.done:
                    logger.info(f"task done, status={nex_obs.status}")
                    break

                policy_obs = _build_policy_obs(
                    nex_obs,
                    instruction,
                    steps,
                    robot_type,
                    speed,
                    apply_eef_rot_obs=apply_eef_rot_obs,
                )
                obs_state = _nex_obs_state_dict(
                    nex_obs,
                    proprio20=policy_obs["proprio"],
                    apply_eef_rot_obs=apply_eef_rot_obs,
                    last_executed_seq=obs_last_seq,
                    next_seq=obs_next_seq,
                )
                t0 = time.time()
                result = policy_client.call(policy_obs)
                policy_rtt_s = time.time() - t0
                logger.info(f"RTT {policy_rtt_s:.3f}s")

                action = np.asarray(result["action"], dtype=np.float32)
                if action.ndim == 1 and action.shape[-1] == 20:
                    action = action.reshape(1, 1, -1)
                elif action.ndim == 2 and action.shape[-1] == 20:
                    action = action.reshape(1, action.shape[0], 20)
                if action.ndim != 3 or action.shape[0] == 0 or action.shape[1] == 0:
                    logger.warning(f"Unexpected action shape: {action.shape}")
                    dump.record_cycle(
                        obs_state=obs_state,
                        step_info={"ok": False, "error": f"bad_action_shape:{action.shape}"},
                        policy_rtt_s=policy_rtt_s,
                        obs_rtt_ms=obs_rtt_ms,
                    )
                    continue

                chunk = action[0]
                policy_chunk_full = np.asarray(chunk, dtype=np.float32).copy()
                if CHUNK_SKIP > 0 and chunk.shape[0] > CHUNK_SKIP:
                    chunk = chunk[CHUNK_SKIP:]
                if not np.isfinite(chunk).all() or np.allclose(chunk, 0.0):
                    logger.warning("server returned invalid/zero action chunk, skip")
                    dump.record_cycle(
                        obs_state=obs_state,
                        policy_chunk20=policy_chunk_full,
                        step_info={"ok": False, "error": "invalid_zero_chunk"},
                        policy_rtt_s=policy_rtt_s,
                        obs_rtt_ms=obs_rtt_ms,
                    )
                    continue

                exec_poses = chunk[: max(int(steps), 0)]
                if exec_poses.shape[0] == 0:
                    logger.warning("no executable poses after steps truncate, skip")
                    dump.record_cycle(
                        obs_state=obs_state,
                        policy_chunk20=policy_chunk_full,
                        step_info={"ok": False, "error": "empty_after_steps"},
                        policy_rtt_s=policy_rtt_s,
                        obs_rtt_ms=obs_rtt_ms,
                    )
                    continue

                transition_poses = _build_transition_poses(
                    prev_chunk_last_pose, exec_poses[0], transition_steps
                )
                if len(transition_poses) > 0:
                    logger.info(
                        f"Prepend {len(transition_poses)} transition poses into remote chunk"
                    )
                    send_poses = np.concatenate(
                        [np.asarray(transition_poses, dtype=np.float32), exec_poses],
                        axis=0,
                    )
                else:
                    send_poses = exec_poses

                if key_listener.poll_reset():
                    prev_chunk_last_pose = None
                    continue

                step_t0 = time.time()
                ok, step_info = _execute_chunk_nex(
                    session,
                    send_poses,
                    apply_eef_rot_exec=apply_eef_rot_exec,
                    exec_frequency=freq,
                )
                wall_s = time.time() - step_t0
                step_info = dict(step_info)
                step_info["wall_s"] = wall_s
                # chunk14 单独存 npy，summary 里不塞大数组
                send_chunk14 = step_info.pop("chunk14", None)
                logger.info(f"remote chunk wall {wall_s:.3f}s ok={ok}")
                dump.record_cycle(
                    obs_state=obs_state,
                    policy_chunk20=policy_chunk_full,
                    exec_poses20=exec_poses,
                    send_poses20=send_poses,
                    send_chunk14=send_chunk14,
                    n_transition=len(transition_poses),
                    step_info=step_info,
                    policy_rtt_s=policy_rtt_s,
                    obs_rtt_ms=obs_rtt_ms,
                )
                if not ok:
                    # 故障后末端位姿不可信，清过渡起点，避免用失败末帧拼下一段
                    prev_chunk_last_pose = None
                    continue

                prev_chunk_last_pose = exec_poses[-1]

            except KeyboardInterrupt:
                logger.info("KeyboardInterrupt, exiting ctrl loop")
                break
            except Exception as e:
                logger.exception(e)
                time.sleep(0.05)
    finally:
        key_listener.stop()
        dump.close()


def _connect_nex(args) -> tuple[LumosClient, object]:
    mock_mode = not (args.username and args.password)
    config = Config(step_timeout=float(args.step_timeout))
    client_kwargs = {
        "api_key": args.api_key or ("mock" if mock_mode else ""),
        "endpoint": args.endpoint,
        "feedback_endpoint": args.feedback_endpoint,
        "config": config,
    }
    if mock_mode:
        if not (args.endpoint and args.task_eval_id):
            raise SystemExit(
                "mock 模式需要 --endpoint 与 --task-eval-id，或提供 --username/--password/--task-id"
            )
        client = LumosClient(**client_kwargs)
        task_id = "mock"
    else:
        if not args.task_id:
            raise SystemExit("真实后端需要 --username / --password / --task-id")
        client = LumosClient(
            **client_kwargs,
            http_base=args.http_base,
            username=args.username,
            password=args.password,
        )
        task_id = args.task_id

    logger.info(f"连接 nexbench (mock={mock_mode}, arm=both)…")
    session = client.connect(
        task_id=task_id,
        task_eval_id=args.task_eval_id or None,
        control_mode="absolute_pose",
        query_timeout=float(args.query_timeout),
    )
    logger.info(f"已连接 task_eval_id={session.task_eval_id}")

    # 用 get_observation() 验证控制端观测链路可用
    try:
        obs_t0 = time.monotonic()
        first_obs = session.get_observation()
        logger.info(
            f"首帧 get_observation 成功，耗时 {(time.monotonic() - obs_t0) * 1000.0:.1f}ms "
            f"status={first_obs.status}"
        )
    except Exception:
        try:
            _request_priority_close(
                args.endpoint,
                args.task_id,
                getattr(session, "task_eval_id", args.task_eval_id),
                args.workstation_id,
            )
        except Exception as close_exc:
            logger.warning("priority HTTP close after first-observation failure failed: {}", close_exc)
        client.close()
        raise SystemExit("get_observation() 首帧失败")
    return client, session


def main():
    parser = argparse.ArgumentParser(
        description="NexBench Touch 客户端：远程图像/臂控 + 本地 PolicyClient 推理"
    )
    # Policy server
    parser.add_argument("--url", default="tcp://localhost:5562")
    parser.add_argument("--steps", type=int, default=DEFAULT_EXEC_STEPS)
    parser.add_argument("--instruction", default=".")
    parser.add_argument("--robot_type", default="lumos_touch_dualarm")
    parser.add_argument("--frequency", type=float, default=30.0,
                        help="远端 chunk 执行频率 exec_frequency（Hz），默认 30")
    parser.add_argument(
        "--speed",
        type=float,
        default=None,
        help="XFuse 推理 speed（写入 obs['speed']）",
    )
    # NexBench
    parser.add_argument("--endpoint", default=WS_CONTROL_BASE)
    parser.add_argument("--feedback-endpoint", default=None)
    parser.add_argument("--http-base", default=HTTP_BASE)
    parser.add_argument(
        "--workstation-id",
        default=os.environ.get("NEX_WORKSTATION_ID", "1"),
        help="评测服务端 robot-control 的工作站 ID（必须与机械臂客户端连接时一致）",
    )
    parser.add_argument("--username", default=os.environ.get("NEX_USERNAME", ""))
    parser.add_argument("--password", default=os.environ.get("NEX_PASSWORD", ""))
    parser.add_argument("--task-id", default=os.environ.get("NEX_TASK_ID", ""))
    parser.add_argument("--task-eval-id", default=os.environ.get("NEX_TASK_EVAL_ID", ""))
    parser.add_argument("--api-key", default=os.environ.get("NEX_API_KEY", ""))
    parser.add_argument("--query-timeout", type=float, default=10.0)
    parser.add_argument("--step-timeout", type=float, default=20.0)
    parser.add_argument(
        "--apply-eef-rot-obs",
        action="store_true",
        help="观测侧：nex 反馈 r6d 做硬件→policy 旋转后再组 proprio",
    )
    parser.add_argument(
        "--apply-eef-rot-exec",
        action="store_true",
        help="执行侧：policy RPY 做松灵坐标系右乘后再下发 absolute_pose chunk",
    )
    parser.add_argument(
        "--debug-dir",
        default=None,
        help="调试落盘目录（默认 nex_debug/run_<时间戳>）；传空字符串关闭",
    )
    parser.add_argument(
        "--no-debug-dump",
        action="store_true",
        help="关闭 chunk/状态调试落盘",
    )
    args = parser.parse_args()

    if args.no_debug_dump or args.debug_dir == "":
        debug_root = None
    elif args.debug_dir:
        debug_root = args.debug_dir
    else:
        debug_root = os.path.join(
            "nex_debug", f"run_{datetime.now().strftime('%Y%m%d_%H%M%S')}"
        )
    debug_dump = _DebugDump(debug_root)
    if debug_dump.enabled:
        debug_dump.write_meta(
            {
                "instruction": args.instruction,
                "steps": args.steps,
                "frequency": args.frequency,
                "chunk_skip": CHUNK_SKIP,
                "apply_eef_rot_obs": bool(args.apply_eef_rot_obs),
                "apply_eef_rot_exec": bool(args.apply_eef_rot_exec),
                "url": args.url,
                "endpoint": args.endpoint,
                "task_id": args.task_id,
                "task_eval_id": args.task_eval_id,
            }
        )

    policy_client = PolicyClient(url=args.url)
    dt = 1.0 / args.frequency
    nex_client = None
    session = None
    try:
        nex_client, session = _connect_nex(args)
        if debug_dump.enabled:
            debug_dump.write_meta(
                {
                    "instruction": args.instruction,
                    "steps": args.steps,
                    "frequency": args.frequency,
                    "chunk_skip": CHUNK_SKIP,
                    "apply_eef_rot_obs": bool(args.apply_eef_rot_obs),
                    "apply_eef_rot_exec": bool(args.apply_eef_rot_exec),
                    "url": args.url,
                    "endpoint": args.endpoint,
                    "task_id": args.task_id,
                    "task_eval_id": getattr(session, "task_eval_id", args.task_eval_id),
                }
            )
        ctrl_loop(
            session,
            policy_client,
            args.instruction,
            args.steps,
            dt,
            args.robot_type,
            speed=args.speed,
            apply_eef_rot_obs=args.apply_eef_rot_obs,
            apply_eef_rot_exec=args.apply_eef_rot_exec,
            exec_frequency=args.frequency,
            debug_dump=debug_dump,
        )
    except LumosError as e:
        logger.error(f"[{e.code}] {e.message}")
        if e.detail:
            logger.error(f"detail: {e.detail}")
        raise SystemExit(1) from e
    except KeyboardInterrupt:
        pass
    finally:
        debug_dump.close()
        if session is not None:
            try:
                _request_priority_close(
                    args.endpoint,
                    args.task_id,
                    getattr(session, "task_eval_id", args.task_eval_id),
                    args.workstation_id,
                )
            except Exception as exc:
                logger.warning("priority HTTP close failed: {}", exc)
        if nex_client is not None:
            try:
                nex_client.close()
            except Exception:
                pass


if __name__ == "__main__":
    main()
