"""Dora node for eye-in-hand calibration with minimal OpenCV UI."""

from __future__ import annotations

import argparse
from collections import deque
import json
import os
import time
import uuid
from dataclasses import dataclass, field
from typing import Any, Dict, List, Optional, Tuple

import cv2
import numpy as np
import pyarrow as pa
import yaml
from dora import Node

WINDOW_NAME = "Dora Eye-in-Hand Calibration"
DEFAULT_DICT = cv2.aruco.DICT_6X6_1000


@dataclass
class CameraIntrinsics:
    fx: float
    fy: float
    cx: float
    cy: float
    distortion: np.ndarray

    def camera_matrix(self) -> np.ndarray:
        return np.array(
            [[self.fx, 0.0, self.cx], [0.0, self.fy, self.cy], [0.0, 0.0, 1.0]],
            dtype=np.float64,
        )


@dataclass
class CalibrationObservation:
    base_T_gripper: np.ndarray
    cam_T_target: np.ndarray


@dataclass
class CalibrationState:
    intrinsics: Optional[CameraIntrinsics] = None
    latest_tcp_pose: Optional[List[float]] = None
    latest_tcp_pose_at: Optional[float] = None
    observations: List[CalibrationObservation] = field(default_factory=list)
    last_board_pose: Optional[Tuple[np.ndarray, np.ndarray]] = None
    pose_list: Optional[List[Dict[str, Any]]] = None
    pose_index: int = 0
    last_commanded_pose_index: Optional[int] = None
    last_move_pose_index: Optional[int] = None
    awaiting_capture_pose_index: Optional[int] = None
    last_target_pose: Optional[Dict[str, Any]] = None
    status_text: str = ""
    pending_command: Optional[Dict[str, Any]] = None
    last_move_done_at: Optional[float] = None
    awaiting_capture: bool = False
    last_log_time: float = 0.0
    auto_done: bool = False
    last_image_at: Optional[float] = None
    last_status_at: Optional[float] = None
    last_robot_state: Optional[int] = None
    last_robot_idle_at: Optional[float] = None
    pose_history: deque[Tuple[float, List[float]]] = field(
        default_factory=lambda: deque(maxlen=10)
    )
    intrinsics_logged: bool = False
    capture_gate: bool = False
    last_capture_done_at: Optional[float] = None
    waiting_for_stop: bool = False


def _rotation_matrix_xyz(
    roll_deg: float, pitch_deg: float, yaw_deg: float
) -> np.ndarray:
    roll = np.deg2rad(roll_deg)
    pitch = np.deg2rad(pitch_deg)
    yaw = np.deg2rad(yaw_deg)
    cx, sx = np.cos(roll), np.sin(roll)
    cy, sy = np.cos(pitch), np.sin(pitch)
    cz, sz = np.cos(yaw), np.sin(yaw)
    rot_x = np.array([[1.0, 0.0, 0.0], [0.0, cx, -sx], [0.0, sx, cx]])
    rot_y = np.array([[cy, 0.0, sy], [0.0, 1.0, 0.0], [-sy, 0.0, cy]])
    rot_z = np.array([[cz, -sz, 0.0], [sz, cz, 0.0], [0.0, 0.0, 1.0]])
    return rot_z @ rot_y @ rot_x


def _pose_to_matrix(tcp_pose: List[float]) -> np.ndarray:
    x, y, z, roll, pitch, yaw = tcp_pose
    rot = _rotation_matrix_xyz(roll, pitch, yaw)
    mat = np.eye(4)
    mat[:3, :3] = rot
    mat[:3, 3] = np.array([x, y, z], dtype=np.float64) / 1000.0
    return mat


def _try_get_charuco_board(
    squares_x: int, squares_y: int, square_length: float, marker_length: float
) -> Any:
    dictionary = cv2.aruco.getPredefinedDictionary(DEFAULT_DICT)
    try:
        board = cv2.aruco.CharucoBoard(
            (squares_x, squares_y), square_length, marker_length, dictionary
        )
    except AttributeError:
        board = cv2.aruco.CharucoBoard_create(
            squares_x, squares_y, square_length, marker_length, dictionary
        )
    if hasattr(board, "setLegacyPattern"):
        board.setLegacyPattern(True)
    return board


def _create_charuco_detector(board: Any) -> Optional[Any]:
    if not hasattr(cv2.aruco, "CharucoDetector"):
        return None
    aruco_params = cv2.aruco.DetectorParameters()
    charuco_params = cv2.aruco.CharucoParameters()
    charuco_params.minMarkers = 2
    return cv2.aruco.CharucoDetector(board, charuco_params, aruco_params)


def _decode_image(storage: pa.Array, metadata: Dict[str, Any]) -> np.ndarray:
    encoding = str(metadata.get("encoding", "bgr8")).lower()
    width = metadata.get("width")
    height = metadata.get("height")
    if (width is None or height is None) and "shape" in metadata:
        shape = metadata.get("shape")
        if isinstance(shape, (list, tuple)) and len(shape) >= 2:
            height = height if height is not None else int(shape[0])
            width = width if width is not None else int(shape[1])
    if width is None or height is None:
        raise KeyError("width/height (or shape) missing from metadata")

    if encoding == "bgr8":
        frame = storage.to_numpy().astype(np.uint8).reshape((height, width, 3))
        return frame.copy()
    if encoding == "rgb8":
        frame = storage.to_numpy().astype(np.uint8).reshape((height, width, 3))
        return cv2.cvtColor(frame, cv2.COLOR_RGB2BGR)
    if encoding in {"jpeg", "jpg", "jpe", "bmp", "webp", "png"}:
        frame = storage.to_numpy().astype(np.uint8)
        return cv2.imdecode(frame, cv2.IMREAD_COLOR)
    if encoding == "yuv420":
        yuv = storage.to_numpy().astype(np.uint8)
        yuv = yuv[: width * height * 3 // 2].reshape((height * 3 // 2, width))
        return cv2.cvtColor(yuv, cv2.COLOR_YUV420p2BGR)

    raise RuntimeError(f"Unsupported image encoding: {encoding}")


def _intrinsics_from_metadata(metadata: Dict[str, Any]) -> Optional[CameraIntrinsics]:
    if not all(k in metadata for k in ("fx", "fy", "cx", "cy")):
        return None
    distortion = metadata.get("distortion", [])
    if distortion is None:
        distortion = []
    dist = np.array(distortion, dtype=np.float64).reshape(-1)
    if dist.size == 0:
        dist = np.zeros(5, dtype=np.float64)
    return CameraIntrinsics(
        fx=float(metadata["fx"]),
        fy=float(metadata["fy"]),
        cx=float(metadata["cx"]),
        cy=float(metadata["cy"]),
        distortion=dist,
    )


def _intrinsics_from_camera_info(
    values: np.ndarray, metadata: Dict[str, Any]
) -> Optional[CameraIntrinsics]:
    if values.size < 4:
        return None
    fx, fy, cx, cy = [float(v) for v in values[:4]]
    distortion = metadata.get("distortion", [])
    if distortion is None:
        distortion = []
    dist = np.array(distortion, dtype=np.float64).reshape(-1)
    if dist.size == 0:
        dist = np.zeros(5, dtype=np.float64)
    return CameraIntrinsics(fx=fx, fy=fy, cx=cx, cy=cy, distortion=dist)


def _load_calibration_poses(path: str) -> List[Dict[str, Any]]:
    with open(path) as handle:
        data = yaml.safe_load(handle)
    return data.get("calibration_poses", [])


def _send_dora_command(
    node: Node, output_name: str, action: str, payload: Dict[str, Any]
) -> str:
    command_id = str(uuid.uuid4())
    message = {"id": command_id, "action": action, "payload": payload}
    node.send_output(
        output_name,
        pa.array([json.dumps(message)]),
        metadata={"encoding": "json", "timestamp_ns": time.time_ns()},
    )
    return command_id


def _parse_status_payload(value: pa.Array) -> Optional[Dict[str, Any]]:
    if len(value) == 0:
        return None
    raw = value[0].as_py()
    if not raw:
        return None
    try:
        return json.loads(raw)
    except Exception:
        return None


def _parse_idle_states(raw: str) -> List[int]:
    values: List[int] = []
    for part in raw.split(","):
        part = part.strip()
        if not part:
            continue
        try:
            values.append(int(part))
        except ValueError:
            continue
    return values


def _pose_is_stable(
    pose_history: deque[Tuple[float, List[float]]],
    window_s: float,
    eps_mm: float,
    eps_deg: float,
) -> bool:
    if not pose_history:
        return False
    latest_t = pose_history[-1][0]
    window = [p for (t, p) in pose_history if latest_t - t <= window_s]
    if len(window) < 2:
        return False
    positions = np.array([p[:3] for p in window], dtype=np.float64)
    rotations = np.array([p[3:6] for p in window], dtype=np.float64)
    pos_span = positions.max(axis=0) - positions.min(axis=0)
    rot_span = rotations.max(axis=0) - rotations.min(axis=0)
    return bool(np.all(pos_span <= eps_mm) and np.all(rot_span <= eps_deg))


def _select_pose_for_capture(
    pose_history: deque[Tuple[float, List[float]]],
    image_time: Optional[float],
    sync_warn_threshold_ms: float = 100.0,
) -> Optional[List[float]]:
    if not pose_history:
        return None
    if image_time is None:
        return pose_history[-1][1]
    # Choose the newest pose at or before the image time.
    candidates = [(t, p) for (t, p) in pose_history if t <= image_time]
    if candidates:
        pose_time, pose = candidates[-1]
        sync_delta_ms = (image_time - pose_time) * 1000
        if sync_delta_ms > sync_warn_threshold_ms:
            print(
                f"WARN: pose-image sync delta {sync_delta_ms:.0f}ms > {sync_warn_threshold_ms:.0f}ms threshold",
                flush=True,
            )
        return pose
    return pose_history[-1][1]


def _estimate_charuco_pose(
    frame: np.ndarray,
    board: Any,
    intrinsics: CameraIntrinsics,
    detector: Optional[Any],
    min_corners: int,
) -> Tuple[
    Optional[np.ndarray],
    Optional[np.ndarray],
    np.ndarray,
    bool,
    int,
    int,
    bool,
]:
    gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
    if detector is not None:
        charuco_corners, charuco_ids, marker_corners, marker_ids = detector.detectBoard(
            frame
        )
    else:
        dictionary = (
            board.dictionary if hasattr(board, "dictionary") else board.getDictionary()
        )
        marker_corners, marker_ids, _ = cv2.aruco.detectMarkers(gray, dictionary)
        charuco_corners, charuco_ids = None, None
        if marker_ids is not None and len(marker_ids) > 0:
            _, charuco_corners, charuco_ids = cv2.aruco.interpolateCornersCharuco(
                marker_corners, marker_ids, gray, board
            )
    debug = frame.copy()
    marker_count = 0 if marker_ids is None else len(marker_ids)
    if marker_count == 0:
        return None, None, debug, False, 0, 0, False

    cv2.aruco.drawDetectedMarkers(debug, marker_corners, marker_ids)
    charuco_count = 0 if charuco_ids is None else len(charuco_ids)
    if charuco_count < min_corners:
        return None, None, debug, True, marker_count, charuco_count, False

    cv2.aruco.drawDetectedCornersCharuco(debug, charuco_corners, charuco_ids)
    obj_points = board.getChessboardCorners()[charuco_ids.flatten()]
    ok, rvec, tvec = cv2.solvePnP(
        obj_points,
        charuco_corners,
        intrinsics.camera_matrix(),
        intrinsics.distortion,
        flags=cv2.SOLVEPNP_IPPE,
    )
    if not ok:
        return None, None, debug, True, marker_count, charuco_count, False

    rvec, tvec = cv2.solvePnPRefineVVS(
        obj_points,
        charuco_corners,
        intrinsics.camera_matrix(),
        intrinsics.distortion,
        rvec,
        tvec,
    )

    cv2.drawFrameAxes(
        debug,
        intrinsics.camera_matrix(),
        intrinsics.distortion,
        rvec,
        tvec,
        0.05,
    )
    return rvec, tvec, debug, True, marker_count, charuco_count, True


def _calibrate_hand_eye(
    observations: List[CalibrationObservation], method: int
) -> Tuple[np.ndarray, Dict[str, float]]:
    r_gripper2base: List[np.ndarray] = []
    t_gripper2base: List[np.ndarray] = []
    r_target2cam: List[np.ndarray] = []
    t_target2cam: List[np.ndarray] = []

    for obs in observations:
        r_gripper2base.append(obs.base_T_gripper[:3, :3])
        t_gripper2base.append(obs.base_T_gripper[:3, 3].reshape(3, 1))
        r_target2cam.append(obs.cam_T_target[:3, :3])
        t_target2cam.append(obs.cam_T_target[:3, 3].reshape(3, 1))

    r_cam2gripper, t_cam2gripper = cv2.calibrateHandEye(
        r_gripper2base, t_gripper2base, r_target2cam, t_target2cam, method=method
    )

    cam2gripper = np.eye(4)
    cam2gripper[:3, :3] = r_cam2gripper
    cam2gripper[:3, 3] = t_cam2gripper.reshape(3)

    base_targets = []
    for obs in observations:
        base_T_target = obs.base_T_gripper @ cam2gripper @ obs.cam_T_target
        base_targets.append(base_T_target[:3, 3])
    base_targets = np.array(base_targets)
    mean_target = base_targets.mean(axis=0)
    errors = np.linalg.norm(base_targets - mean_target[None, :], axis=1)
    info = {
        "mean_translation_error_m": float(errors.mean()),
        "max_translation_error_m": float(errors.max()),
    }
    return cam2gripper, info


def _save_calibration(
    filename: str,
    cam2gripper: np.ndarray,
    method_name: str,
    observations: List[CalibrationObservation],
    intrinsics: CameraIntrinsics,
) -> None:
    save_data = {
        "T_cam2gripper": cam2gripper,
        "calibration_type": "eye_in_hand",
        "method": method_name,
        "num_observations": len(observations),
        "camera_matrix": intrinsics.camera_matrix(),
        "distortion": intrinsics.distortion,
    }
    np.savez(filename, **save_data)

    obs_file = os.path.join(
        os.path.dirname(filename),
        f"{os.path.splitext(os.path.basename(filename))[0]}_observations.npz",
    )
    r_gripper2base = [obs.base_T_gripper[:3, :3] for obs in observations]
    t_gripper2base = [obs.base_T_gripper[:3, 3:4] for obs in observations]
    r_target2cam = [obs.cam_T_target[:3, :3] for obs in observations]
    t_target2cam = [obs.cam_T_target[:3, 3:4] for obs in observations]
    np.savez(
        obs_file,
        R_gripper2base=r_gripper2base,
        t_gripper2base=t_gripper2base,
        R_target2cam=r_target2cam,
        t_target2cam=t_target2cam,
    )


def _method_from_name(name: str) -> int:
    name = name.upper()
    methods = {
        "TSAI": cv2.CALIB_HAND_EYE_TSAI,
        "PARK": cv2.CALIB_HAND_EYE_PARK,
        "HORAUD": cv2.CALIB_HAND_EYE_HORAUD,
        "ANDREFF": cv2.CALIB_HAND_EYE_ANDREFF,
        "DANIILIDIS": cv2.CALIB_HAND_EYE_DANIILIDIS,
    }
    return methods.get(name, cv2.CALIB_HAND_EYE_TSAI)


def main() -> None:
    parser = argparse.ArgumentParser(
        description="Eye-in-hand calibration node with minimal OpenCV UI.",
    )
    parser.add_argument("--name", type=str, default="dora-calibration")
    args = parser.parse_args()

    image_input = os.getenv("IMAGE_INPUT", "image")
    camera_info_input = os.getenv("CAMERA_INFO_INPUT", "camera_info")
    pose_input = os.getenv("POSE_INPUT", "tcp_pose")
    status_input = os.getenv("STATUS_INPUT", "status")
    command_output = os.getenv("COMMAND_OUTPUT", "robot_cmd")
    poses_file = os.getenv("POSES_FILE", "config/calibration_poses.yml")
    pose_units = os.getenv("POSE_UNITS", "m")
    move_speed = os.getenv("MOVE_SPEED")
    output_file = os.getenv("OUTPUT_FILE", "calibration.npz")
    method_name = os.getenv("METHOD", "TSAI")
    auto_run = os.getenv("AUTO_RUN", "true").lower() in ("true", "1", "yes")
    min_corners = int(os.getenv("MIN_CORNERS", "6"))
    log_interval = float(os.getenv("LOG_INTERVAL", "2.0"))
    log_after_done = os.getenv("LOG_AFTER_DONE", "false").lower() in ("true", "1", "yes")
    log_markers = os.getenv("LOG_MARKERS", "false").lower() in ("true", "1", "yes")
    auto_solve = os.getenv("AUTO_SOLVE", "true").lower() in ("true", "1", "yes")
    wait_after_move = float(os.getenv("WAIT_AFTER_MOVE", "2.0"))
    max_pose_age = float(os.getenv("MAX_POSE_AGE", "0.5"))
    pose_stability_window = float(os.getenv("POSE_STABILITY_WINDOW", "0.5"))
    pose_stability_eps_mm = float(os.getenv("POSE_STABILITY_EPS_MM", "0.5"))
    pose_stability_eps_deg = float(os.getenv("POSE_STABILITY_EPS_DEG", "0.5"))
    require_status_idle = os.getenv("REQUIRE_STATUS_IDLE", "false").lower() in (
        "true",
        "1",
        "yes",
    )
    idle_states = _parse_idle_states(os.getenv("IDLE_STATES", ""))
    wait_between_poses = float(os.getenv("WAIT_BETWEEN_POSES", "0.0"))
    move_home_on_start = os.getenv("MOVE_HOME_ON_START", "true").lower() in (
        "true",
        "1",
        "yes",
    )
    home_x = float(os.getenv("HOME_X", "250"))
    home_y = float(os.getenv("HOME_Y", "0"))
    home_z = float(os.getenv("HOME_Z", "400"))
    home_roll = float(os.getenv("HOME_ROLL", "180"))
    home_pitch = float(os.getenv("HOME_PITCH", "0"))
    home_yaw = float(os.getenv("HOME_YAW", "0"))

    squares_x = int(os.getenv("SQUARES_X", "4"))
    squares_y = int(os.getenv("SQUARES_Y", "6"))
    square_length = float(os.getenv("SQUARE_LENGTH", "0.04"))
    marker_length = float(os.getenv("MARKER_LENGTH", "0.03"))

    move_speed_val = float(move_speed) if move_speed else None

    state = CalibrationState()
    if poses_file and not os.path.isabs(poses_file):
        config_path = os.path.join("config", poses_file)
        poses_file = config_path if os.path.exists(config_path) else poses_file

    if poses_file:
        if os.path.exists(poses_file):
            state.pose_list = _load_calibration_poses(poses_file)
            state.status_text = f"Loaded {len(state.pose_list)} poses"
        else:
            state.status_text = f"Poses file not found: {poses_file}"

    board = _try_get_charuco_board(squares_x, squares_y, square_length, marker_length)
    detector = _create_charuco_detector(board)
    method = _method_from_name(method_name)

    node = Node(args.name)
    cv2.namedWindow(WINDOW_NAME, cv2.WINDOW_NORMAL)
    started = False

    for event in node:
        if event["type"] != "INPUT":
            continue

        now = time.monotonic()
        if not started:
            started = True
            if auto_run and move_home_on_start:
                payload = {
                    "x": home_x,
                    "y": home_y,
                    "z": home_z,
                    "roll": home_roll,
                    "pitch": home_pitch,
                    "yaw": home_yaw,
                }
                cmd_id = _send_dora_command(
                    node, command_output, "move_to_pose", payload
                )
                state.pending_command = {
                    "id": cmd_id,
                    "action": "move_to_pose",
                    "sent_at": now,
                }
                state.status_text = "Sent calibration home"
        if state.pending_command:
            elapsed = now - state.pending_command["sent_at"]
            if elapsed > 30.0:
                state.status_text = (
                    f"Command timeout ({state.pending_command['action']})"
                )
                state.pending_command = None

        if event["id"] == status_input:
            payload = _parse_status_payload(event["value"])
            state.last_status_at = now
            if payload and state.pending_command:
                if payload.get("command_id") == state.pending_command["id"]:
                    ok = bool(payload.get("ok", False))
                    msg = payload.get("message", "")
                    state.status_text = "Command ok" if ok else f"Command failed: {msg}"
                    state.pending_command = None
                    if ok:
                        state.last_move_done_at = None
                        if state.last_commanded_pose_index is not None:
                            state.awaiting_capture = True
                            state.last_move_pose_index = state.last_commanded_pose_index
                            state.awaiting_capture_pose_index = (
                                state.last_commanded_pose_index
                            )
                            state.last_commanded_pose_index = None
                            state.capture_gate = False
                            state.waiting_for_stop = True
            if payload and "state" in payload:
                try:
                    state.last_robot_state = int(payload["state"])
                except (TypeError, ValueError):
                    state.last_robot_state = None
                if state.last_robot_state is not None and state.last_robot_state in idle_states:
                    state.last_robot_idle_at = now
            continue

        if event["id"] == pose_input:
            tcp_pose = event["value"].to_numpy().astype(np.float64).reshape(-1)
            if tcp_pose.size >= 6:
                state.latest_tcp_pose = tcp_pose[:6].tolist()
                state.latest_tcp_pose_at = now
                state.pose_history.append((now, state.latest_tcp_pose))
            continue

        if event["id"] == camera_info_input:
            raw_bytes = event["value"].to_numpy().astype(np.uint8).tobytes()
            dtype_str = str(event["metadata"].get("dtype", "float32"))
            dtype_map = {"float32": np.float32, "float64": np.float64}
            dtype = dtype_map.get(dtype_str, np.float32)
            values = np.frombuffer(raw_bytes, dtype=dtype).astype(np.float64)
            intrinsics = _intrinsics_from_camera_info(values, event["metadata"])
            if intrinsics:
                state.intrinsics = intrinsics
                if not state.intrinsics_logged:
                    state.intrinsics_logged = True
                    print(
                        f"intrinsics fx={intrinsics.fx:.4f} fy={intrinsics.fy:.4f} "
                        f"cx={intrinsics.cx:.4f} cy={intrinsics.cy:.4f} "
                        f"dist={intrinsics.distortion.tolist()}",
                        flush=True,
                    )
            continue

        if event["id"] != image_input:
            # Non-image events drive the move + capture gate state machine.
            if (
                auto_run
                and state.awaiting_capture
                and not state.pending_command
            ):
                if state.awaiting_capture_pose_index is None:
                    state.awaiting_capture = False
                    continue
                if (
                    state.last_move_pose_index is None
                    or state.last_move_pose_index != state.awaiting_capture_pose_index
                ):
                    continue
                if state.last_move_done_at is None:
                    if state.waiting_for_stop and _pose_is_stable(
                        state.pose_history,
                        pose_stability_window,
                        pose_stability_eps_mm,
                        pose_stability_eps_deg,
                    ):
                        state.last_move_done_at = now
                        state.waiting_for_stop = False
                        print(
                            f"pose_stable pose={state.awaiting_capture_pose_index} at={now:.3f}",
                            flush=True,
                        )
                    else:
                        state.status_text = "Waiting for pose to stabilize"
                        continue
                if state.last_move_done_at is None:
                    state.awaiting_capture = False
                elif now - state.last_move_done_at < wait_after_move:
                    state.status_text = "Waiting after move"
                    continue
                elif require_status_idle and idle_states and state.last_status_at is not None:
                    if (
                        state.last_robot_state is None
                        or state.last_robot_state not in idle_states
                    ):
                        state.status_text = "Waiting for robot idle state"
                    elif (
                        state.last_robot_idle_at is None
                        or state.last_robot_idle_at < state.last_move_done_at
                    ):
                        state.status_text = "Waiting for idle after move"
                    else:
                        state.capture_gate = True
                        state.status_text = "Capture gate armed"
                        print("capture_gate armed (idle)", flush=True)
                else:
                    state.capture_gate = True
                    state.status_text = "Capture gate armed"
                    print("capture_gate armed", flush=True)

            if auto_run and state.pose_list and not state.awaiting_capture:
                if (
                    state.last_capture_done_at is not None
                    and now - state.last_capture_done_at < wait_between_poses
                ):
                    state.status_text = "Waiting between poses"
                    continue
                if state.pending_command:
                    continue
                if state.pose_index < len(state.pose_list):
                    pose = state.pose_list[state.pose_index]
                    pos = pose.get("position", {})
                    ori = pose.get("orientation_rpy_deg", {})
                    x, y, z = (
                        float(pos.get("x", 0.0)),
                        float(pos.get("y", 0.0)),
                        float(pos.get("z", 0.0)),
                    )
                    if pose_units == "m":
                        x *= 1000.0
                        y *= 1000.0
                        z *= 1000.0
                    payload = {
                        "x": x,
                        "y": y,
                        "z": z,
                        "roll": float(ori.get("roll", 0.0)),
                        "pitch": float(ori.get("pitch", 0.0)),
                        "yaw": float(ori.get("yaw", 0.0)),
                    }
                    if move_speed_val is not None:
                        payload["speed"] = move_speed_val
                    cmd_id = _send_dora_command(
                        node, command_output, "move_to_pose", payload
                    )
                    state.pending_command = {
                        "id": cmd_id,
                        "action": "move_to_pose",
                        "sent_at": now,
                    }
                    state.status_text = f"Auto move {state.pose_index + 1}"
                    state.last_commanded_pose_index = state.pose_index + 1
                    state.last_target_pose = pose
                    state.pose_index += 1
                elif auto_solve and not state.auto_done:
                    if len(state.observations) >= 3:
                        cam2gripper, info = _calibrate_hand_eye(
                            state.observations, method
                        )
                        _save_calibration(
                            output_file,
                            cam2gripper,
                            method_name,
                            state.observations,
                            state.intrinsics,
                        )
                        state.status_text = (
                            "Saved calibration | "
                            f"mean err {info['mean_translation_error_m'] * 1000:.2f}mm "
                            f"max {info['max_translation_error_m'] * 1000:.2f}mm"
                        )
                        print("Calibration Result (T_cam2gripper):")
                        print(cam2gripper)
                        print(
                            f"Mean translation error: {info['mean_translation_error_m']*1000:.2f} mm"
                        )
                        print(
                            f"Max translation error: {info['max_translation_error_m']*1000:.2f} mm"
                        )
                    else:
                        state.status_text = "Need at least 3 observations"
                    state.auto_done = True
            continue

        storage = event["value"]
        metadata = event.get("metadata", {})
        if state.intrinsics is None:
            intrinsics = _intrinsics_from_metadata(metadata)
            if intrinsics:
                state.intrinsics = intrinsics
                if not state.intrinsics_logged:
                    state.intrinsics_logged = True
                    print(
                        f"intrinsics fx={intrinsics.fx:.4f} fy={intrinsics.fy:.4f} "
                        f"cx={intrinsics.cx:.4f} cy={intrinsics.cy:.4f} "
                        f"dist={intrinsics.distortion.tolist()}",
                        flush=True,
                    )

        frame = _decode_image(storage, metadata)
        state.last_image_at = now
        image_frame_id = metadata.get("frame_id")
        image_ts_ns = metadata.get("timestamp_ns")
        debug = frame.copy()

        if state.intrinsics is None:
            cv2.putText(
                debug,
                "Missing camera intrinsics (fx,fy,cx,cy)",
                (10, 30),
                cv2.FONT_HERSHEY_SIMPLEX,
                0.7,
                (0, 0, 255),
                2,
            )
            cv2.imshow(WINDOW_NAME, debug)
            if cv2.waitKey(1) & 0xFF == ord("q"):
                break
            continue

        rvec, tvec, debug, detected, marker_count, charuco_count, pose_ok = (
            _estimate_charuco_pose(
                frame, board, state.intrinsics, detector, min_corners
            )
        )
        if (
            log_markers
            and now - state.last_log_time >= log_interval
            and (log_after_done or not state.auto_done)
        ):
            state.last_log_time = now
            print(
                f"markers={marker_count} charuco={charuco_count} pose_ok={pose_ok}",
                flush=True,
            )
        state.last_board_pose = (rvec, tvec) if pose_ok else None

        if marker_count == 0:
            status = "NO MARKERS"
            color = (0, 0, 255)
        elif not pose_ok:
            status = "MARKERS ONLY"
            color = (0, 165, 255)
        else:
            status = "DETECTED"
            color = (0, 255, 0)
        cv2.putText(
            debug,
            f"ChArUco: {status} | Obs: {len(state.observations)}",
            (10, 30),
            cv2.FONT_HERSHEY_SIMPLEX,
            0.7,
            color,
            2,
        )
        cv2.putText(
            debug,
            f"Markers: {marker_count} | Charuco: {charuco_count}",
            (10, 55),
            cv2.FONT_HERSHEY_SIMPLEX,
            0.6,
            (255, 255, 255),
            1,
        )
        cv2.putText(
            debug,
            f"Auto: {'ON' if auto_run else 'OFF'}",
            (10, 60),
            cv2.FONT_HERSHEY_SIMPLEX,
            0.6,
            (255, 255, 255),
            1,
        )
        if state.pose_list:
            total_poses = len(state.pose_list)
            next_pose = (
                state.pose_index + 1 if state.pose_index < total_poses else None
            )
            last_pose = state.last_move_pose_index
            pose_text = (
                f"Last pose {last_pose}/{total_poses} | Next pose {next_pose}/{total_poses}"
                if next_pose is not None
                else f"Last pose {last_pose}/{total_poses} | Next pose --/{total_poses}"
            )
            cv2.putText(
                debug,
                pose_text,
                (10, 85),
                cv2.FONT_HERSHEY_SIMPLEX,
                0.6,
                (255, 255, 255),
                1,
            )
            cv2.putText(
                debug,
                f"Obs: {len(state.observations)}",
                (10, 110),
                cv2.FONT_HERSHEY_SIMPLEX,
                0.6,
                (255, 255, 255),
                1,
            )
        if state.status_text:
            cv2.putText(
                debug,
                state.status_text,
                (10, 135),
                cv2.FONT_HERSHEY_SIMPLEX,
                0.6,
                (255, 255, 255),
                1,
            )
        if state.capture_gate:
            cv2.putText(
                debug,
                "Capture gate armed",
                (10, 160),
                cv2.FONT_HERSHEY_SIMPLEX,
                0.6,
                (255, 255, 0),
                1,
            )

        cv2.putText(
            debug,
            "Keys: r=restart, q=quit",
            (10, debug.shape[0] - 10),
            cv2.FONT_HERSHEY_SIMPLEX,
            0.5,
            (255, 255, 255),
            1,
        )

        cv2.imshow(WINDOW_NAME, debug)
        key = cv2.waitKey(1) & 0xFF

        if key == ord("q"):
            break
        if key == ord("r"):
            state.observations.clear()
            state.pose_index = 0
            state.pending_command = None
            state.awaiting_capture = False
            state.capture_gate = False
            state.last_capture_done_at = None
            state.waiting_for_stop = False
            state.auto_done = False
            state.status_text = "Calibration restarted"

        if auto_run and state.pose_list:
            if state.pending_command:
                continue
            if state.awaiting_capture:
                if not state.capture_gate:
                    continue
                if state.awaiting_capture_pose_index is None:
                    state.awaiting_capture = False
                    continue
                if state.last_move_done_at is None:
                    state.awaiting_capture = False
                elif now - state.last_move_done_at < wait_after_move:
                    continue
                elif require_status_idle and idle_states and state.last_status_at is not None:
                    if (
                        state.last_robot_state is None
                        or state.last_robot_state not in idle_states
                    ):
                        state.status_text = "Waiting for robot idle state"
                        continue
                    if (
                        state.last_robot_idle_at is None
                        or state.last_robot_idle_at < state.last_move_done_at
                    ):
                        state.status_text = "Waiting for idle after move"
                        continue
                if (
                    state.latest_tcp_pose_at is None
                    or now - state.latest_tcp_pose_at > max_pose_age
                ):
                    state.status_text = "Waiting for fresh tcp_pose"
                    continue
                if not _pose_is_stable(
                    state.pose_history,
                    pose_stability_window,
                    pose_stability_eps_mm,
                    pose_stability_eps_deg,
                ):
                    state.status_text = "Waiting for stable pose"
                    continue
                elif pose_ok and state.last_board_pose and state.latest_tcp_pose:
                    rvec, tvec = state.last_board_pose
                    rot, _ = cv2.Rodrigues(rvec)
                    cam_T_target = np.eye(4)
                    cam_T_target[:3, :3] = rot
                    cam_T_target[:3, 3] = tvec.reshape(3)
                    pose_for_capture = _select_pose_for_capture(
                        state.pose_history, state.last_image_at
                    )
                    if pose_for_capture is None:
                        state.status_text = "No pose for capture"
                        continue
                    base_T_gripper = _pose_to_matrix(pose_for_capture)
                    state.observations.append(
                        CalibrationObservation(
                            base_T_gripper=base_T_gripper,
                            cam_T_target=cam_T_target,
                        )
                    )
                    state.status_text = f"Auto captured {len(state.observations)}"
                    print(
                        f"capture_done pose={state.last_move_pose_index} at={now:.3f}",
                        flush=True,
                    )
                    tvec_norm = float(np.linalg.norm(tvec))
                    print(
                        f"auto_capture pose={state.awaiting_capture_pose_index} "
                        f"obs={len(state.observations)} markers={marker_count} "
                        f"charuco={charuco_count} pose_ok={pose_ok} "
                        f"board_tvec_norm_m={tvec_norm:.4f}",
                        flush=True,
                    )
                    if image_ts_ns is not None:
                        age_ms = (time.monotonic_ns() - int(image_ts_ns)) / 1e6
                        print(
                            f"image_frame_id={image_frame_id} image_age_ms={age_ms:.1f}",
                            flush=True,
                        )
                    if state.last_target_pose:
                        print(
                            f"desired_pose={state.last_target_pose}",
                            flush=True,
                        )
                    print(
                        f"robot_pose={pose_for_capture}",
                        flush=True,
                    )
                    state.awaiting_capture = False
                    state.awaiting_capture_pose_index = None
                    state.capture_gate = False
                    state.last_capture_done_at = now
                continue

            if (
                state.last_capture_done_at is not None
                and now - state.last_capture_done_at < wait_between_poses
            ):
                continue

            if state.pose_index < len(state.pose_list):
                pose = state.pose_list[state.pose_index]
                pos = pose.get("position", {})
                ori = pose.get("orientation_rpy_deg", {})
                x, y, z = (
                    float(pos.get("x", 0.0)),
                    float(pos.get("y", 0.0)),
                    float(pos.get("z", 0.0)),
                )
                if pose_units == "m":
                    x *= 1000.0
                    y *= 1000.0
                    z *= 1000.0
                payload = {
                    "x": x,
                    "y": y,
                    "z": z,
                    "roll": float(ori.get("roll", 0.0)),
                    "pitch": float(ori.get("pitch", 0.0)),
                    "yaw": float(ori.get("yaw", 0.0)),
                }
                if move_speed_val is not None:
                    payload["speed"] = move_speed_val
                cmd_id = _send_dora_command(
                    node, command_output, "move_to_pose", payload
                )
                state.pending_command = {
                    "id": cmd_id,
                    "action": "move_to_pose",
                    "sent_at": now,
                }
                state.status_text = f"Auto move {state.pose_index + 1}"
                state.last_commanded_pose_index = state.pose_index + 1
                state.last_target_pose = pose
                state.pose_index += 1
            elif auto_solve and not state.auto_done:
                if len(state.observations) >= 3:
                    cam2gripper, info = _calibrate_hand_eye(
                        state.observations, method
                    )
                    _save_calibration(
                        output_file,
                        cam2gripper,
                        method_name,
                        state.observations,
                        state.intrinsics,
                    )
                    state.status_text = (
                        "Saved calibration | "
                        f"mean err {info['mean_translation_error_m'] * 1000:.2f}mm "
                        f"max {info['max_translation_error_m'] * 1000:.2f}mm"
                    )
                    print("Calibration Result (T_cam2gripper):")
                    print(cam2gripper)
                    print(
                        f"Mean translation error: {info['mean_translation_error_m']*1000:.2f} mm"
                    )
                    print(
                        f"Max translation error: {info['max_translation_error_m']*1000:.2f} mm"
                    )
                else:
                    state.status_text = "Need at least 3 observations"
                state.auto_done = True

    cv2.destroyAllWindows()


if __name__ == "__main__":
    main()
