import sys
import time
from collections import deque
from pathlib import Path
from typing import Deque, Dict, Optional, Tuple

import grpc
import numpy as np
from PyQt5 import QtCore, QtWidgets
from matplotlib.backends.backend_qt5agg import FigureCanvasQTAgg as FigureCanvas
from matplotlib.backends.backend_qt5agg import NavigationToolbar2QT as NavigationToolbar
from matplotlib.figure import Figure
try:
    from yourdfpy import URDF
except Exception:  # pragma: no cover - fallback if yourdfpy not available
    from urdfpy import URDF

from clients._path_setup import ensure_paths

ensure_paths()

from google.protobuf import timestamp_pb2

from cmvr.api import humanoid_robot_command_pb2 as pb
from cmvr.api import humanoid_robot_service_pb2_grpc as rpc

DEFAULT_JOINT_ORDER = [
    "L_SHOULDER_P", "L_SHOULDER_R", "L_SHOULDER_Y",
    "L_ELBOW_R", "L_WRIST_P", "L_WRIST_Y", "L_WRIST_R",
    "R_SHOULDER_P", "R_SHOULDER_R", "R_SHOULDER_Y",
    "R_ELBOW_R", "R_WRIST_P", "R_WRIST_Y", "R_WRIST_R",
    "HEAD_P", "HEAD_Y", "HEAD_R", "WAIST_Y", "WAIST_R",
]

URDF_PATH = Path(__file__).resolve().parents[1] / "robot_model" / "xiaoyan_description" / "dual_arm.urdf"


class JointStateWorker(QtCore.QThread):
    updated = QtCore.pyqtSignal(dict)
    log = QtCore.pyqtSignal(str)

    def __init__(
        self,
        address: str,
        device_id: str,
        interval: float,
        joint_names: list[str],
        timeout: float = 2.0,
    ) -> None:
        super().__init__()
        self.address = address
        self.device_id = device_id
        self.interval = interval
        self.joint_names = joint_names
        self.timeout = timeout
        self._stop = False
        self._channel: Optional[grpc.Channel] = None

    def stop(self) -> None:
        self._stop = True

    def run(self) -> None:
        try:
            self._channel = grpc.insecure_channel(self.address)
            stub = rpc.HumanoidRobotServiceStub(self._channel)
        except Exception as exc:
            self.log.emit(f"[error] connect failed: {exc}")
            return

        while not self._stop:
            req = pb.JointRequest()
            req.header.device_id = self.device_id
            ts = timestamp_pb2.Timestamp()
            ts.GetCurrentTime()
            req.header.timestamp.CopyFrom(ts)
            try:
                resp = stub.getJointState(req, timeout=10)
            except Exception as exc:
                self.log.emit(f"[error] getJointState failed: {exc}")
                time.sleep(self.interval)
                continue

            joint_pos: Dict[str, float] = {}
            joint_vel: Dict[str, float] = {}
            for state in resp.state:
                for name, pos, vel in zip(state.name, state.position, state.velocity):
                    joint_pos[name] = pos
                    joint_vel[name] = vel

            payload: Dict[str, Tuple[float, float]] = {}
            for name in self.joint_names:
                payload[name] = (joint_pos.get(name, 0.0), joint_vel.get(name, 0.0))

            self.updated.emit(payload)
            time.sleep(self.interval)
        if self._channel:
            self._channel.close()


class PoseWorker(QtCore.QThread):
    updated = QtCore.pyqtSignal(dict)
    log = QtCore.pyqtSignal(str)

    def __init__(
        self,
        address: str,
        device_id: str,
        interval: float,
        base_link: str,
        ee_link: str,
        timeout: float = 2.0,
    ) -> None:
        super().__init__()
        self.address = address
        self.device_id = device_id
        self.interval = interval
        self.base_link = base_link
        self.ee_link = ee_link
        self.timeout = timeout
        self._stop = False
        self._channel: Optional[grpc.Channel] = None

    def stop(self) -> None:
        self._stop = True

    def run(self) -> None:
        try:
            self._channel = grpc.insecure_channel(self.address)
            stub = rpc.HumanoidRobotServiceStub(self._channel)
        except Exception as exc:
            self.log.emit(f"[error] connect failed: {exc}")
            return

        while not self._stop:
            req = pb.GetPose.Request()
            req.header.device_id = self.device_id
            req.base_link = self.base_link
            req.ee_link = self.ee_link
            ts = timestamp_pb2.Timestamp()
            ts.GetCurrentTime()
            req.header.timestamp.CopyFrom(ts)
            try:
                resp = stub.getPose(req, timeout=10)
            except Exception as exc:
                self.log.emit(f"[error] getPose failed: {exc}")
                time.sleep(self.interval)
                continue

            pose = resp.pose
            payload = {
                "x": pose.x,
                "y": pose.y,
                "z": pose.z,
                "rx": pose.rx,
                "ry": pose.ry,
                "rz": pose.rz,
            }
            self.updated.emit(payload)
            time.sleep(self.interval)
        if self._channel:
            self._channel.close()


class MainWindow(QtWidgets.QMainWindow):
    def __init__(self) -> None:
        super().__init__()
        self.setWindowTitle("gRPC Client UI")
        self.resize(900, 600)

        self._pose_process: Optional[QtCore.QProcess] = None
        self._joint_worker: Optional[JointStateWorker] = None
        self._pose_worker: Optional[PoseWorker] = None
        self._plot_selected: set[str] = set()
        self._last_data: Dict[str, Tuple[float, float]] = {}
        self._updating_movej_table = False
        self.urdf: Optional[URDF] = None
        self.urdf_error: Optional[str] = None
        self.joint_names: list[str] = DEFAULT_JOINT_ORDER.copy()
        self.link_names: list[str] = []
        self._warned_no_joint_match = False

        self._load_urdf()

        central = self._build_center_panel()
        self.setCentralWidget(central)
        self._build_docks()
        self.statusBar().showMessage("Ready")

    def _load_urdf(self) -> None:
        try:
            self.urdf = URDF.load(str(URDF_PATH))
            self.link_names = [link.name for link in self._urdf_links()]
            self.joint_names = [
                joint.name for joint in self._urdf_joints() if getattr(joint, "joint_type", "") != "fixed"
            ]
        except Exception as exc:
            self.urdf_error = str(exc)
            self.urdf = None
            self.link_names = []
            self.joint_names = DEFAULT_JOINT_ORDER.copy()

    def _urdf_links(self) -> list:
        if not self.urdf:
            return []
        if hasattr(self.urdf, "links"):
            return list(self.urdf.links)
        if hasattr(self.urdf, "link_map"):
            return list(self.urdf.link_map.values())
        return []

    def _urdf_joints(self) -> list:
        if not self.urdf:
            return []
        if hasattr(self.urdf, "joints"):
            return list(self.urdf.joints)
        if hasattr(self.urdf, "joint_map"):
            return list(self.urdf.joint_map.values())
        return []

    def _build_center_panel(self) -> QtWidgets.QWidget:
        panel = QtWidgets.QWidget()
        layout = QtWidgets.QVBoxLayout(panel)
        layout.setContentsMargins(0, 0, 0, 0)
        layout.setSpacing(0)
        self.center_tabs = QtWidgets.QTabWidget()
        self.center_tabs.addTab(self._build_live_panel(), "Plot")
        self.center_tabs.setCurrentIndex(0)
        self.center_tabs.currentChanged.connect(self._on_center_tab_changed)
        layout.addWidget(self.center_tabs)
        return panel

    def _build_docks(self) -> None:
        control = QtWidgets.QDockWidget("Control")
        control.setWidget(self._build_control_panel())
        control.setMinimumWidth(550)
        control.setFeatures(QtWidgets.QDockWidget.AllDockWidgetFeatures)
        self.addDockWidget(QtCore.Qt.LeftDockWidgetArea, control)

        status = QtWidgets.QDockWidget("Joint Status")
        status.setWidget(self._build_status_panel())
        status.setMinimumWidth(650)
        status.setFeatures(QtWidgets.QDockWidget.AllDockWidgetFeatures)
        self.addDockWidget(QtCore.Qt.RightDockWidgetArea, status)

        log_dock = QtWidgets.QDockWidget("Log")
        log_dock.setWidget(self._build_log_panel())
        log_dock.setMinimumHeight(140)
        log_dock.setMaximumHeight(220)
        log_dock.setFeatures(QtWidgets.QDockWidget.AllDockWidgetFeatures)
        self.addDockWidget(QtCore.Qt.BottomDockWidgetArea, log_dock)

    def _build_status_panel(self) -> QtWidgets.QWidget:
        panel = QtWidgets.QWidget()
        layout = QtWidgets.QVBoxLayout(panel)
        splitter = QtWidgets.QSplitter()
        splitter.setOrientation(QtCore.Qt.Vertical)
        splitter.addWidget(self._build_table())
        splitter.addWidget(self._build_pose_panel())
        splitter.setStretchFactor(0, 3)
        splitter.setStretchFactor(1, 1)
        layout.addWidget(splitter)
        return panel

    def _build_control_panel(self) -> QtWidgets.QWidget:
        panel = QtWidgets.QWidget()
        layout = QtWidgets.QVBoxLayout(panel)
        layout.addWidget(self._build_settings_group())
        layout.addWidget(self._build_movej_group())
        layout.addWidget(self._build_actions_group())
        layout.addStretch(1)
        return panel

    def _build_settings_group(self) -> QtWidgets.QGroupBox:
        group = QtWidgets.QGroupBox("Settings")
        layout = QtWidgets.QVBoxLayout(group)
        layout.addLayout(self._build_form())
        return group

    def _build_movej_group(self) -> QtWidgets.QGroupBox:
        group = QtWidgets.QGroupBox("MoveJ")
        layout = QtWidgets.QVBoxLayout(group)
        params = QtWidgets.QFormLayout()
        params.addRow("MoveJ Vel", self.vel_edit)
        params.addRow("MoveJ Acc", self.acc_edit)
        layout.addLayout(params)
        layout.addWidget(self._build_movej_table())
        self.movej_btn = QtWidgets.QPushButton("Send MoveJ")
        self.movej_btn.clicked.connect(self._on_movej)
        layout.addWidget(self.movej_btn)
        return group

    def _build_actions_group(self) -> QtWidgets.QGroupBox:
        group = QtWidgets.QGroupBox("Actions")
        layout = QtWidgets.QVBoxLayout(group)
        self.jointstate_toggle = self._build_switch_row("JointState")
        self.torque_on_btn = QtWidgets.QPushButton("Torque On")
        self.torque_off_btn = QtWidgets.QPushButton("Torque Off")
        self.jointstate_toggle.toggled.connect(self._on_jointstate_toggle)
        self.torque_on_btn.clicked.connect(self._on_torque_on)
        self.torque_off_btn.clicked.connect(self._on_torque_off)
        layout.addLayout(self._wrap_switch_row(self.jointstate_toggle, "Start JointState"))
        layout.addWidget(self.torque_on_btn)
        layout.addWidget(self.torque_off_btn)
        return group

    def _build_switch_row(self, name: str) -> QtWidgets.QCheckBox:
        toggle = QtWidgets.QCheckBox()
        toggle.setChecked(False)
        toggle.setFixedSize(56, 28)
        toggle.setStyleSheet(
            """
            QCheckBox {
                background-color: #bfc5cf;
                border-radius: 14px;
                padding: 0px;
            }
            QCheckBox:checked {
                background-color: #2f6fed;
            }
            QCheckBox::indicator {
                width: 22px;
                height: 22px;
                border-radius: 11px;
                background: #ffffff;
                margin-left: 3px;
            }
            QCheckBox::indicator:checked {
                margin-left: 31px;
            }
            """
        )
        toggle.setTristate(False)
        toggle.setObjectName(f"{name}_toggle")
        return toggle

    def _wrap_switch_row(self, toggle: QtWidgets.QCheckBox, label: str) -> QtWidgets.QHBoxLayout:
        row = QtWidgets.QHBoxLayout()
        row.addWidget(toggle)
        row.addWidget(QtWidgets.QLabel(label))
        row.addStretch(1)
        return row
    def _build_form(self) -> QtWidgets.QLayout:
        form = QtWidgets.QFormLayout()

        self.host_edit = QtWidgets.QLineEdit("192.168.0.222")
        self.port_edit = QtWidgets.QSpinBox()
        self.port_edit.setRange(1, 65535)
        self.port_edit.setValue(50052)
        self.device_edit = QtWidgets.QLineEdit("hc01")
        self.interval_edit = QtWidgets.QDoubleSpinBox()
        self.interval_edit.setDecimals(3)
        self.interval_edit.setRange(0.001, 10.0)
        self.interval_edit.setValue(0.05)
        self.vel_edit = QtWidgets.QDoubleSpinBox()
        self.vel_edit.setRange(0.1, 10.0)
        self.vel_edit.setValue(1.0)
        self.acc_edit = QtWidgets.QDoubleSpinBox()
        self.acc_edit.setRange(0.1, 10.0)
        self.acc_edit.setValue(0.5)

        form.addRow("Host", self.host_edit)
        form.addRow("Port", self.port_edit)
        form.addRow("Device ID", self.device_edit)
        form.addRow("Interval (s)", self.interval_edit)

        return form

    def _build_log(self) -> QtWidgets.QWidget:
        self.log = QtWidgets.QTextEdit()
        self.log.setReadOnly(True)
        return self.log

    def _build_log_panel(self) -> QtWidgets.QWidget:
        panel = QtWidgets.QWidget()
        layout = QtWidgets.QVBoxLayout(panel)
        header = QtWidgets.QHBoxLayout()
        header.addWidget(QtWidgets.QLabel("Runtime logs"))
        header.addStretch(1)
        log_widget = self._build_log()
        self.clear_log_btn = QtWidgets.QPushButton("Clear")
        self.clear_log_btn.clicked.connect(log_widget.clear)
        header.addWidget(self.clear_log_btn)
        layout.addLayout(header)
        layout.addWidget(log_widget)
        return panel

    def _build_pose_panel(self) -> QtWidgets.QGroupBox:
        group = QtWidgets.QGroupBox("GetPose")
        layout = QtWidgets.QVBoxLayout(group)

        self.getpose_toggle = self._build_switch_row("GetPose")
        self.getpose_toggle.toggled.connect(self._on_getpose_toggle)
        layout.addLayout(self._wrap_switch_row(self.getpose_toggle, "Start GetPose"))

        form = QtWidgets.QFormLayout()
        self.base_link_edit = QtWidgets.QComboBox()
        self.ee_link_edit = QtWidgets.QComboBox()
        if self.link_names:
            self.base_link_edit.addItems(self.link_names)
            self.ee_link_edit.addItems(self.link_names)
            if "PELVIS_S" in self.link_names:
                self.base_link_edit.setCurrentText("PELVIS_S")
            if "R_FINGER_TIP" in self.link_names:
                self.ee_link_edit.setCurrentText("R_FINGER_TIP")
        else:
            self.base_link_edit.addItem("PELVIS_S")
            self.ee_link_edit.addItem("R_FINGER_TIP")
        form.addRow("Base Link", self.base_link_edit)
        form.addRow("EE Link", self.ee_link_edit)
        layout.addLayout(form)

        self.pose_labels = {}
        grid = QtWidgets.QGridLayout()
        for i, key in enumerate(["x", "y", "z", "rx", "ry", "rz"]):
            label = QtWidgets.QLabel(f"{key}: 0.000000")
            self.pose_labels[key] = label
            grid.addWidget(label, i // 3, i % 3)
        layout.addLayout(grid)
        return group

    def _build_live_panel(self) -> QtWidgets.QWidget:
        tab = QtWidgets.QWidget()
        layout = QtWidgets.QVBoxLayout(tab)

        layout.setContentsMargins(0, 0, 0, 0)
        layout.setSpacing(4)

        self.figure = Figure(figsize=(5, 4))
        self.canvas = FigureCanvas(self.figure)
        self.ax_pos = self.figure.add_subplot(2, 1, 1)
        self.ax_vel = self.figure.add_subplot(2, 1, 2)
        self.ax_pos.set_title("Position (rad)")
        self.ax_vel.set_title("Velocity (rad/s)")
        self.ax_pos.grid(True, alpha=0.3)
        self.ax_vel.grid(True, alpha=0.3)
        self.pos_lines: Dict[str, object] = {}
        self.vel_lines: Dict[str, object] = {}

        self.time_hist: Deque[float] = deque(maxlen=300)
        self._last_plot_time: Optional[float] = None
        self.pos_hist: Dict[str, Deque[float]] = {name: deque(maxlen=300) for name in self.joint_names}
        self.vel_hist: Dict[str, Deque[float]] = {name: deque(maxlen=300) for name in self.joint_names}
        self._plot_start = time.time()

        self.canvas.setSizePolicy(
            QtWidgets.QSizePolicy.Expanding, QtWidgets.QSizePolicy.Expanding
        )
        self.toolbar = NavigationToolbar(self.canvas, self)
        layout.addWidget(self.toolbar)
        layout.addWidget(self.canvas)
        return tab

    def _build_table(self) -> QtWidgets.QWidget:
        self.table = QtWidgets.QTableWidget(len(self.joint_names), 4)
        self.table.setHorizontalHeaderLabels(
            ["Joint", "Position (rad)", "Position (deg)", "Velocity (rad/s)"]
        )
        self.table.verticalHeader().setVisible(False)
        self.table.setEditTriggers(QtWidgets.QAbstractItemView.NoEditTriggers)
        self.table.setSelectionMode(QtWidgets.QAbstractItemView.ExtendedSelection)
        self.table.setSelectionBehavior(QtWidgets.QAbstractItemView.SelectRows)
        for row, name in enumerate(self.joint_names):
            self.table.setItem(row, 0, QtWidgets.QTableWidgetItem(name))
            self.table.setItem(row, 1, QtWidgets.QTableWidgetItem("0.0"))
            self.table.setItem(row, 2, QtWidgets.QTableWidgetItem("0.0"))
            self.table.setItem(row, 3, QtWidgets.QTableWidgetItem("0.0"))
        self.table.horizontalHeader().setStretchLastSection(True)
        self.table.itemSelectionChanged.connect(self._on_status_selection_changed)
        return self.table

    def _build_movej_table(self) -> QtWidgets.QWidget:
        self.movej_table = QtWidgets.QTableWidget(len(self.joint_names), 4)
        self.movej_table.setHorizontalHeaderLabels(["Use", "Joint", "Deg", "Rad"])
        self.movej_table.verticalHeader().setVisible(False)
        self.movej_table.setSizePolicy(
            QtWidgets.QSizePolicy.Expanding, QtWidgets.QSizePolicy.Expanding
        )
        for row, name in enumerate(self.joint_names):
            use_item = QtWidgets.QTableWidgetItem()
            use_item.setCheckState(QtCore.Qt.Unchecked)
            self.movej_table.setItem(row, 0, use_item)
            self.movej_table.setItem(row, 1, QtWidgets.QTableWidgetItem(name))
            self.movej_table.setItem(row, 2, QtWidgets.QTableWidgetItem("0.0"))
            self.movej_table.setItem(row, 3, QtWidgets.QTableWidgetItem("0.0"))
        self.movej_table.horizontalHeader().setStretchLastSection(True)
        self._fit_table_height(self.movej_table)
        self.movej_table.itemChanged.connect(self._on_movej_angle_changed)
        return self.movej_table

    def _fit_table_height(self, table: QtWidgets.QTableWidget) -> None:
        header_height = table.horizontalHeader().height()
        row_height = table.verticalHeader().defaultSectionSize()
        total = header_height + row_height * table.rowCount() + 2
        table.setMinimumHeight(total)

    def _append_log(self, text: str) -> None:
        if hasattr(self, "log") and self.log is not None:
            self.log.append(text.rstrip())

    def _base_args(self) -> list[str]:
        return [
            "-m",
            "ui.run_action",
            "--host",
            self.host_edit.text().strip(),
            "--port",
            str(self.port_edit.value()),
            "--device-id",
            self.device_edit.text().strip(),
        ]

    def _start_process(self, args: list[str], keep: bool = False) -> QtCore.QProcess:
        proc = QtCore.QProcess(self)
        proc.setProgram(sys.executable)
        proc.setArguments(args)
        proc.setWorkingDirectory(str(Path(__file__).resolve().parents[1]))
        proc.setProcessChannelMode(QtCore.QProcess.MergedChannels)
        proc.readyReadStandardOutput.connect(
            lambda p=proc: self._append_log(p.readAllStandardOutput().data().decode("utf-8", "ignore"))
        )
        proc.finished.connect(
            lambda code, status: self._append_log(f"[exit] code={code} status={int(status)}")
        )
        proc.start()
        if keep:
            return proc
        proc.finished.connect(proc.deleteLater)
        return proc

    def _on_movej(self) -> None:
        joint_list = self._get_selected_movej_joints()

        if not joint_list:
            self._append_log("[warn] No joints selected for MoveJ.")
            return

        args = self._base_args() + [
            "movej",
            "--vel",
            str(self.vel_edit.value()),
            "--acc",
            str(self.acc_edit.value()),
        ]
        self._start_process(args + ["--joint-list", self._encode_joint_list(joint_list)])

    def _on_get_pose(self) -> None:
        if self._joint_worker and self._joint_worker.isRunning():
            self._append_log("[info] JointState already running.")
            return
        address = f"{self.host_edit.text().strip()}:{self.port_edit.value()}"
        self._joint_worker = JointStateWorker(
            address=address,
            device_id=self.device_edit.text().strip(),
            interval=float(self.interval_edit.value()),
            joint_names=self.joint_names,
        )
        self._joint_worker.updated.connect(self._update_joint_table)
        self._joint_worker.log.connect(self._append_log)
        self._joint_worker.start()

    def _on_stop_pose(self) -> None:
        if self._joint_worker:
            self._append_log("[info] Stopping JointState...")
            self._joint_worker.stop()
            self._joint_worker = None

    def _on_torque_on(self) -> None:
        args = self._base_args() + ["torque_on"]
        self._start_process(args)

    def _on_torque_off(self) -> None:
        args = self._base_args() + ["torque_off"]
        self._start_process(args)

    def _on_jointstate_toggle(self, checked: bool) -> None:
        if checked:
            self._on_get_pose()
        else:
            self._on_stop_pose()

    def _on_getpose_toggle(self, checked: bool) -> None:
        if checked:
            self._on_start_get_pose()
        else:
            self._on_stop_get_pose()

    def _update_joint_table(self, data: Dict[str, Tuple[float, float]]) -> None:
        self._last_data = data
        self._render_joint_table_and_plot()

    def _append_plot(self, name: str, pos: float, vel: float) -> None:
        t = time.time() - self._plot_start
        if self._last_plot_time is None or t > self._last_plot_time:
            self.time_hist.append(t)
            self._last_plot_time = t
        self.pos_hist[name].append(pos)
        self.vel_hist[name].append(vel)

    def _refresh_plot(self, selected: set) -> None:
        for name in list(self.pos_lines.keys()):
            if name not in selected:
                self.pos_lines[name].remove()
                self.vel_lines[name].remove()
                del self.pos_lines[name]
                del self.vel_lines[name]

        for name in selected:
            if name not in self.pos_lines:
                (pos_line,) = self.ax_pos.plot([], [], label=name)
                (vel_line,) = self.ax_vel.plot([], [], label=name)
                self.pos_lines[name] = pos_line
                self.vel_lines[name] = vel_line
            x_data = list(self.time_hist)
            y_pos_full = list(self.pos_hist[name])
            y_vel_full = list(self.vel_hist[name])
            n_pos = min(len(x_data), len(y_pos_full))
            n_vel = min(len(x_data), len(y_vel_full))
            if n_pos:
                self.pos_lines[name].set_data(x_data[-n_pos:], y_pos_full[-n_pos:])
            else:
                self.pos_lines[name].set_data([], [])
            if n_vel:
                self.vel_lines[name].set_data(x_data[-n_vel:], y_vel_full[-n_vel:])
            else:
                self.vel_lines[name].set_data([], [])

        if len(self.time_hist) > 1 and selected:
            self.ax_pos.set_xlim(self.time_hist[0], self.time_hist[-1])
            self.ax_vel.set_xlim(self.time_hist[0], self.time_hist[-1])
            self.ax_pos.relim()
            self.ax_pos.autoscale_view(scalex=False, scaley=True)
            self.ax_vel.relim()
            self.ax_vel.autoscale_view(scalex=False, scaley=True)
        if selected:
            self.ax_pos.legend(loc="upper right", fontsize=8, ncol=2)
            self.ax_vel.legend(loc="upper right", fontsize=8, ncol=2)
        self.canvas.draw_idle()

    def _reset_plot_data(self) -> None:
        self.time_hist.clear()
        for name in self.joint_names:
            self.pos_hist[name].clear()
            self.vel_hist[name].clear()
        self._plot_start = time.time()
        self._last_plot_time = None
        for line in self.pos_lines.values():
            line.set_data([], [])
        for line in self.vel_lines.values():
            line.set_data([], [])
        self.canvas.draw_idle()

    def _encode_joint_list(self, joint_list: list[dict]) -> str:
        return ";".join(f"{j['joint_name']}:{j['rad']}" for j in joint_list)

    def _get_selected_movej_joints(self) -> list[dict]:
        joint_list = []
        for row, name in enumerate(self.joint_names):
            use_item = self.movej_table.item(row, 0)
            if use_item and use_item.checkState() == QtCore.Qt.Checked:
                rad_item = self.movej_table.item(row, 3)
                try:
                    rad = float(rad_item.text()) if rad_item else 0.0
                except ValueError:
                    rad = 0.0
                joint_list.append({"joint_name": name, "rad": rad})
        return joint_list

    def _on_movej_angle_changed(self, item: QtWidgets.QTableWidgetItem) -> None:
        if self._updating_movej_table:
            return
        col = item.column()
        if col not in (2, 3):
            return
        row = item.row()
        self._updating_movej_table = True
        try:
            if col == 2:
                try:
                    deg = float(item.text())
                except ValueError:
                    deg = 0.0
                rad = deg * 3.141592653589793 / 180.0
                rad_item = self.movej_table.item(row, 3)
                if rad_item is None:
                    rad_item = QtWidgets.QTableWidgetItem("0.0")
                    self.movej_table.setItem(row, 3, rad_item)
                rad_item.setText(f"{rad:.6f}")
            elif col == 3:
                try:
                    rad = float(item.text())
                except ValueError:
                    rad = 0.0
                deg = rad * 180.0 / 3.141592653589793
                deg_item = self.movej_table.item(row, 2)
                if deg_item is None:
                    deg_item = QtWidgets.QTableWidgetItem("0.0")
                    self.movej_table.setItem(row, 2, deg_item)
                deg_item.setText(f"{deg:.3f}")
        finally:
            self._updating_movej_table = False

    def _apply_plot_selection(self) -> None:
        # Deprecated: plot selection now follows right-side Joint Status selection.
        pass

    def _on_status_selection_changed(self) -> None:
        selected_rows = {idx.row() for idx in self.table.selectionModel().selectedRows()}
        self._plot_selected = {self.joint_names[row] for row in selected_rows}
        self.statusBar().showMessage(f"Selected joints: {len(self._plot_selected)}")
        self._render_joint_table_and_plot()

    def _on_start_get_pose(self) -> None:
        if self._pose_worker and self._pose_worker.isRunning():
            self._append_log("[info] GetPose already running.")
            return
        address = f"{self.host_edit.text().strip()}:{self.port_edit.value()}"
        self._pose_worker = PoseWorker(
            address=address,
            device_id=self.device_edit.text().strip(),
            interval=float(self.interval_edit.value()),
            base_link=self.base_link_edit.currentText().strip(),
            ee_link=self.ee_link_edit.currentText().strip(),
        )
        self._pose_worker.updated.connect(self._update_pose)
        self._pose_worker.log.connect(self._append_log)
        self._pose_worker.start()

    def _on_stop_get_pose(self) -> None:
        if self._pose_worker:
            self._append_log("[info] Stopping GetPose...")
            self._pose_worker.stop()
            self._pose_worker = None

    def _update_pose(self, data: Dict[str, float]) -> None:
        for key, value in data.items():
            if key in self.pose_labels:
                self.pose_labels[key].setText(f"{key}: {value:.6f}")
        return

    def _render_joint_table_and_plot(self) -> None:
        selected = set(self._plot_selected)
        if not self._last_data:
            self._last_data = {name: (0.0, 0.0) for name in self.joint_names}

        selected_lines = []
        for row, name in enumerate(self.joint_names):
            pos, vel = self._last_data.get(name, (0.0, 0.0))
            self.table.item(row, 1).setText(f"{pos:.6f}")
            self.table.item(row, 2).setText(f"{pos * 180.0 / 3.141592653589793:.3f}")
            self.table.item(row, 3).setText(f"{vel:.6f}")

            if name in selected:
                selected_lines.append(f"{name}: {pos:.4f} rad, {vel:.4f} rad/s")
                self._append_plot(name, pos, vel)

        if selected_lines:
            self.statusBar().showMessage(
                f"Selected joints: {len(selected)} | " + " | ".join(selected_lines)
            )
        else:
            self.statusBar().showMessage(f"Selected joints: {len(selected)}")

        if self._is_plot_visible():
            self._refresh_plot(selected)
        return

    def _is_plot_visible(self) -> bool:
        return hasattr(self, "center_tabs") and self.center_tabs.currentIndex() == 0

    def _on_center_tab_changed(self, index: int) -> None:
        if index == 0:
            self._render_joint_table_and_plot()


def main() -> int:
    app = QtWidgets.QApplication(sys.argv)
    window = MainWindow()
    window.show()
    return app.exec_()


if __name__ == "__main__":
    raise SystemExit(main())
