G1_Lootah/Lidar/.bak-before-prod-sync/SLAM_GUI.py

# SLAM_GUI.py
from __future__ import annotations

import json
import os
import sys
import signal
import time
from pathlib import Path

import numpy as np

from PyQt6.QtWidgets import (
    QApplication, QMainWindow, QVBoxLayout, QHBoxLayout,
    QLabel, QLineEdit, QPushButton, QFileDialog, QMessageBox,
    QFrame, QSplitter, QCheckBox, QGraphicsDropShadowEffect, QScrollArea
)
from PyQt6.QtCore import Qt, QTimer, QThread, pyqtSignal

import pyqtgraph.opengl as gl

from SLAM_engine import SlamEngineClient, load_slam_config


def apply_card_shadow(widget: QFrame):
    shadow = QGraphicsDropShadowEffect(widget)
    shadow.setBlurRadius(22)
    shadow.setOffset(0, 6)
    shadow.setColor(Qt.GlobalColor.black)
    widget.setGraphicsEffect(shadow)


def pill_button(bg: str, fg: str = "white") -> str:
    return f"""
    QPushButton {{
        background-color: {bg};
        color: {fg};
        font-weight: 800;
        border: 1px solid rgba(255,255,255,0.08);
        border-radius: 10px;
        padding: 10px 12px;
    }}
    QPushButton:hover {{ border: 1px solid rgba(255,255,255,0.18); }}
    QPushButton:disabled {{
        background-color: rgba(255,255,255,0.08);
        color: rgba(255,255,255,0.35);
        border: 1px solid rgba(255,255,255,0.05);
    }}
    """


def card_style() -> str:
    return """
    QFrame#Card {
        background-color: rgba(255,255,255,0.04);
        border: 1px solid rgba(255,255,255,0.06);
        border-radius: 14px;
    }
    QLabel#CardTitle { color: rgba(255,255,255,0.75); font-weight: 800; }
    QLineEdit {
        background-color: rgba(0,0,0,0.30);
        border: 1px solid rgba(255,255,255,0.10);
        border-radius: 10px;
        padding: 10px 10px;
        color: white;
        font-weight: 600;
    }
    QLineEdit:disabled {
        color: rgba(255,255,255,0.40);
        background-color: rgba(255,255,255,0.05);
        border: 1px solid rgba(255,255,255,0.05);
    }
    QCheckBox { color: white; font-weight: 700; spacing: 10px; }
    QCheckBox:disabled { color: rgba(255,255,255,0.40); }
    """


class SLAMGLViewWidget(gl.GLViewWidget):
    mapClicked = pyqtSignal(float, float, float)
    mapPickDrag = pyqtSignal(float, float, float, bool)

    def __init__(self, *args, **kwargs):
        super().__init__(*args, **kwargs)
        self.pick_mode = False
        self.pick_plane_z = 0.0
        self._pick_dragging = False
        self.pick_points = None
        self.pick_screen_max_px = 140.0

    @staticmethod
    def _qmat_to_np_candidates(m) -> list[np.ndarray]:
        try:
            data = np.array(m.copyDataTo(), dtype=np.float64)
            if data.size != 16:
                return [np.eye(4, dtype=np.float64)]
            mats = [
                data.reshape((4, 4), order="F"),
                data.reshape((4, 4), order="C"),
            ]
            out: list[np.ndarray] = []
            for mm in mats:
                if np.isfinite(mm).all():
                    out.append(np.asarray(mm, dtype=np.float64))
            return out if out else [np.eye(4, dtype=np.float64)]
        except Exception:
            return [np.eye(4, dtype=np.float64)]

    def _screen_to_world_on_plane(self, sx: float, sy: float, plane_z: float = 0.0):
        w = max(2, int(self.width()))
        h = max(2, int(self.height()))
        x_ndc = (2.0 * float(sx) / float(w)) - 1.0
        y_ndc = 1.0 - (2.0 * float(sy) / float(h))

        try:
            cam = self.cameraPosition()
            ctr = self.opts["center"]
            cam_p = np.asarray([float(cam.x()), float(cam.y()), float(cam.z())], dtype=np.float64)
            ctr_p = np.asarray([float(ctr.x()), float(ctr.y()), float(ctr.z())], dtype=np.float64)
            fwd = ctr_p - cam_p
            nf = float(np.linalg.norm(fwd))
            if nf < 1e-9:
                raise ValueError("degenerate camera forward")
            fwd = fwd / nf
            up_world = np.asarray([0.0, 0.0, 1.0], dtype=np.float64)
            right = np.cross(fwd, up_world)
            nr = float(np.linalg.norm(right))
            if nr < 1e-6:
                up_world = np.asarray([0.0, 1.0, 0.0], dtype=np.float64)
                right = np.cross(fwd, up_world)
                nr = float(np.linalg.norm(right))
            if nr < 1e-9:
                raise ValueError("degenerate camera right")
            right = right / nr
            up = np.cross(right, fwd)
            nu = float(np.linalg.norm(up))
            if nu < 1e-9:
                raise ValueError("degenerate camera up")
            up = up / nu
            fov_rad = float(np.deg2rad(float(self.opts.get("fov", 60.0))))
            sx_scale = float(np.tan(0.5 * fov_rad))
            sy_scale = sx_scale * (float(h) / float(w))
            ray = fwd + (x_ndc * sx_scale * right) + (y_ndc * sy_scale * up)
            nray = float(np.linalg.norm(ray))
            if nray < 1e-9:
                raise ValueError("degenerate pick ray")
            ray = ray / nray
            if abs(float(ray[2])) < 1e-9:
                return None
            t = (float(plane_z) - float(cam_p[2])) / float(ray[2])
            if t < 0.0:
                return None
            pt = cam_p + (t * ray)
            if np.isfinite(pt).all():
                return np.asarray(pt, dtype=np.float64)
        except Exception:
            pass

        # Fallback: matrix unproject solver
        viewport = self.getViewport()
        region = viewport
        proj_cands = self._qmat_to_np_candidates(self.projectionMatrix(region, viewport))
        view_cands = self._qmat_to_np_candidates(self.viewMatrix())
        near = np.array([x_ndc, y_ndc, -1.0, 1.0], dtype=np.float64)
        far = np.array([x_ndc, y_ndc, 1.0, 1.0], dtype=np.float64)
        best_pt = None
        best_err = 1e18
        for proj in proj_cands:
            for view in view_cands:
                for m in (proj @ view, view @ proj):
                    try:
                        inv = np.linalg.inv(m)
                    except Exception:
                        continue
                    p0 = inv @ near
                    p1 = inv @ far
                    if abs(float(p0[3])) < 1e-9 or abs(float(p1[3])) < 1e-9:
                        continue
                    p0 = p0[:3] / p0[3]
                    p1 = p1[:3] / p1[3]
                    ray = p1 - p0
                    if abs(float(ray[2])) < 1e-9:
                        continue
                    t = (float(plane_z) - float(p0[2])) / float(ray[2])
                    if t < 0.0:
                        continue
                    pt = p0 + (t * ray)
                    if not np.isfinite(pt).all():
                        continue
                    clip = m @ np.array([float(pt[0]), float(pt[1]), float(pt[2]), 1.0], dtype=np.float64)
                    if abs(float(clip[3])) < 1e-9:
                        continue
                    ndc = clip[:3] / clip[3]
                    sx2 = ((float(ndc[0]) + 1.0) * 0.5) * float(w)
                    sy2 = ((1.0 - float(ndc[1])) * 0.5) * float(h)
                    err = (sx2 - float(sx)) ** 2 + (sy2 - float(sy)) ** 2
                    if err < best_err:
                        best_err = float(err)
                        best_pt = np.asarray(pt, dtype=np.float64)
        return best_pt

    def _screen_pick_from_points(self, sx: float, sy: float):
        pts = self.pick_points
        if pts is None:
            return None
        arr = np.asarray(pts, dtype=np.float64)
        if arr.ndim != 2 or arr.shape[1] != 3 or len(arr) == 0:
            return None
        w = max(2, int(self.width()))
        h = max(2, int(self.height()))
        try:
            cam = self.cameraPosition()
            ctr = self.opts["center"]
            cam_p = np.asarray([float(cam.x()), float(cam.y()), float(cam.z())], dtype=np.float64)
            ctr_p = np.asarray([float(ctr.x()), float(ctr.y()), float(ctr.z())], dtype=np.float64)
            fwd = ctr_p - cam_p
            nf = float(np.linalg.norm(fwd))
            if nf < 1e-9:
                return None
            fwd = fwd / nf
            up_world = np.asarray([0.0, 0.0, 1.0], dtype=np.float64)
            right = np.cross(fwd, up_world)
            nr = float(np.linalg.norm(right))
            if nr < 1e-6:
                up_world = np.asarray([0.0, 1.0, 0.0], dtype=np.float64)
                right = np.cross(fwd, up_world)
                nr = float(np.linalg.norm(right))
            if nr < 1e-9:
                return None
            right = right / nr
            up = np.cross(right, fwd)
            nu = float(np.linalg.norm(up))
            if nu < 1e-9:
                return None
            up = up / nu
            fov_rad = float(np.deg2rad(float(self.opts.get("fov", 60.0))))
            sx_scale = float(np.tan(0.5 * fov_rad))
            sy_scale = sx_scale * (float(h) / float(w))
            rel = arr - cam_p.reshape((1, 3))
            zc = rel @ fwd
            keep = zc > 1e-6
            if not np.any(keep):
                return None
            rel = rel[keep]
            zc = zc[keep]
            pts_k = arr[keep]
            xc = rel @ right
            yc = rel @ up
            x_ndc = xc / (zc * sx_scale)
            y_ndc = yc / (zc * sy_scale)
            sxv = ((x_ndc + 1.0) * 0.5) * float(w)
            syv = ((1.0 - y_ndc) * 0.5) * float(h)
            d2 = (sxv - float(sx)) ** 2 + (syv - float(sy)) ** 2
            idx = int(np.argmin(d2))
            if float(d2[idx]) > float(self.pick_screen_max_px) * float(self.pick_screen_max_px):
                return None
            p = pts_k[idx]
            if np.isfinite(p).all():
                return p.astype(np.float64)
            return None
        except Exception:
            return None

    def mousePressEvent(self, ev):
        lpos = ev.position() if hasattr(ev, "position") else ev.localPos()
        self.mousePos = lpos
        if self.pick_mode and ev.button() == Qt.MouseButton.LeftButton:
            pt = self._screen_pick_from_points(float(lpos.x()), float(lpos.y()))
            if pt is None:
                pt = self._screen_to_world_on_plane(float(lpos.x()), float(lpos.y()), float(self.pick_plane_z))
            if pt is not None:
                self._pick_dragging = False
                self.mapPickDrag.emit(float(pt[0]), float(pt[1]), float(pt[2]), True)
                self.mapClicked.emit(float(pt[0]), float(pt[1]), float(pt[2]))
                ev.accept()
                return
        super().mousePressEvent(ev)

    def mouseMoveEvent(self, ev):
        lpos = ev.position() if hasattr(ev, "position") else ev.localPos()
        if not hasattr(self, "mousePos"):
            self.mousePos = lpos
        diff = lpos - self.mousePos
        self.mousePos = lpos

        buttons = ev.buttons()
        mods = ev.modifiers()

        if self.pick_mode and self._pick_dragging and (buttons & Qt.MouseButton.LeftButton):
            pt = self._screen_to_world_on_plane(float(lpos.x()), float(lpos.y()), float(self.pick_plane_z))
            if pt is not None:
                self.mapPickDrag.emit(float(pt[0]), float(pt[1]), float(pt[2]), False)
            ev.accept()
            return

        # Keep default left-drag behavior.
        if buttons & Qt.MouseButton.LeftButton:
            if mods & Qt.KeyboardModifier.ControlModifier:
                self.pan(diff.x(), diff.y(), 0, relative="view")
            else:
                self.orbit(-diff.x(), diff.y())
            return

        # Add right-drag panning so touchpads/mice without middle-click can move map.
        if buttons & Qt.MouseButton.RightButton:
            if mods & Qt.KeyboardModifier.ControlModifier:
                self.pan(diff.x(), 0, diff.y(), relative="view-upright")
            else:
                self.pan(diff.x(), diff.y(), 0, relative="view-upright")
            return

        # Keep default middle-drag panning behavior.
        if buttons & Qt.MouseButton.MiddleButton:
            if mods & Qt.KeyboardModifier.ControlModifier:
                self.pan(diff.x(), 0, diff.y(), relative="view-upright")
            else:
                self.pan(diff.x(), diff.y(), 0, relative="view-upright")

    def mouseReleaseEvent(self, ev):
        lpos = ev.position() if hasattr(ev, "position") else ev.localPos()
        if self.pick_mode and ev.button() == Qt.MouseButton.LeftButton and self._pick_dragging:
            self._pick_dragging = False
            pt = self._screen_to_world_on_plane(float(lpos.x()), float(lpos.y()), float(self.pick_plane_z))
            if pt is not None:
                self.mapPickDrag.emit(float(pt[0]), float(pt[1]), float(pt[2]), True)
                self.mapClicked.emit(float(pt[0]), float(pt[1]), float(pt[2]))
            ev.accept()
            return
        super().mouseReleaseEvent(ev)


class RefLoaderThread(QThread):
    loaded = pyqtSignal(object, object, str)  # pts, colors, filename
    failed = pyqtSignal(str)

    def __init__(self, filename: str, display_voxel: float, color_mode: str = "GRAY"):
        super().__init__()
        self.filename = filename
        self.display_voxel = float(display_voxel)
        self.color_mode = str(color_mode).upper().strip()

    @staticmethod
    def _height_colors_fast(pts: np.ndarray, alpha: float = 0.40) -> np.ndarray:
        if pts is None or len(pts) == 0:
            return np.zeros((0, 4), dtype=np.float32)
        z = pts[:, 2]
        zmin = float(np.min(z))
        zmax = float(np.max(z))
        span = max(1e-6, zmax - zmin)
        norm = np.clip((z - zmin) / span, 0.0, 1.0).astype(np.float32)
        colors = np.zeros((len(pts), 4), dtype=np.float32)
        colors[:, 0] = norm
        colors[:, 1] = 1.0 - np.abs(norm - 0.5) * 2.0
        colors[:, 2] = 1.0 - norm
        colors[:, 3] = alpha
        return colors

    def run(self):
        try:
            import open3d as o3d
            pcd = o3d.io.read_point_cloud(self.filename)
            if self.display_voxel > 0:
                pcd = pcd.voxel_down_sample(self.display_voxel)
            pts = np.asarray(pcd.points).astype(np.float32)
            if len(pts) < 2:
                raise RuntimeError("Ref map is empty.")

            mode = self.color_mode if self.color_mode in ("GRAY", "HEIGHT", "ORIGINAL") else "GRAY"
            colors = np.zeros((len(pts), 4), dtype=np.float32)
            colors[:, 3] = 0.40
            if mode == "HEIGHT":
                colors = self._height_colors_fast(pts, alpha=0.40)
            elif mode == "ORIGINAL":
                src = np.asarray(pcd.colors)
                if src is not None and len(src) == len(pts) and len(src) > 0:
                    src_rgb = np.asarray(src, dtype=np.float32)
                    colors[:, :3] = np.clip(src_rgb[:, :3], 0.0, 1.0)
                else:
                    colors[:, :3] = 0.7
            else:
                colors[:, :3] = 0.7
            self.loaded.emit(pts, colors, self.filename)
        except Exception as e:
            self.failed.emit(str(e))


class UltimateDashboard(QMainWindow):
    def __init__(self):
        super().__init__()

        self.cfg = load_slam_config()
        title = self.cfg["app"]["title"]
        accent = self.cfg["gui"]["colors"]["accent"]
        maps_dir_cfg = str(self.cfg["app"]["maps_dir"])
        maps_dir_path = Path(maps_dir_cfg).expanduser()
        if not maps_dir_path.is_absolute():
            cfg_path = os.environ.get("SLAM_CONFIG", "").strip()
            base_dir = Path(cfg_path).expanduser().resolve().parent if cfg_path else Path(__file__).resolve().parent
            maps_dir_path = (base_dir / maps_dir_path).resolve()
        maps_dir = str(maps_dir_path)

        self.setWindowTitle(title)
        self.resize(1600, 900)

        Path(maps_dir).mkdir(parents=True, exist_ok=True)

        self._self_check_popup_shown = False
        self.client = SlamEngineClient()
        self.client.start_process()
        self.show_self_check_popup(self.client.get_self_check(), source="engine")

        self.save_armed = False
        self.ref_map_path = None
        self._ref_thread = None
        self.ref_color_mode = "GRAY"
        self.density_mode = "MEDIUM"
        self.min_stable_points = int(self.cfg["map"]["min_points_to_save"])
        self.loop_closure_enabled = bool(self.cfg.get("loop_closure", {}).get("enabled", False))
        self.loc_machine_enabled = bool(self.cfg.get("state_machine", {}).get("enabled", True))
        self.submap_mode_enabled = bool(self.cfg.get("submap_mapping", {}).get("enabled", True))
        self.autosave_enabled = bool(self.cfg.get("autosave", {}).get("enabled", False))
        self.autosave_interval_sec = float(self.cfg.get("autosave", {}).get("interval_sec", 90.0))
        self.nav_cfg = dict(self.cfg.get("navigation_export", {}))
        self.map_quality_cfg = dict(self.cfg.get("map_quality", {}))
        self._last_error_popup_text = ""
        self._last_error_popup_t = 0.0
        self._error_popup_cooldown_sec = 2.0
        self._worker_dead_notified = False
        self._last_saved_popup_text = ""
        self._last_saved_popup_t = 0.0
        self._saved_popup_cooldown_sec = 1.0
        self.current_stable_points = 0
        self.worker_mode = "IDLE"
        self.is_connected = False
        self.recording_enabled = False
        self.recording_frames = 0
        self.recording_dropped = 0
        self.workflow_profile = "BALANCED"
        self.workflow_selected = False
        self.last_replay_report = None
        self.replay_baseline_path = str(Path(maps_dir) / "SLAM_replay_baseline.json")
        self.approx_pick_armed = False
        self.approx_guess_z = float(self.cfg.get("localization", {}).get("approx_guess_default_z_m", 0.0))
        self.last_loc_confidence = 0.0
        self.last_loc_match_ratio = 0.0
        self.require_start_pick = True
        self.start_pick_ready = False
        self.start_pick_xyz = None
        self._pick_help_shown_for_ref = None
        self.ref_pick_points = None
        self.ref_pick_snap_radius_m = float(
            self.cfg.get("localization", {}).get("pick_snap_radius_m", 1.2)
        )
        self.ref_pick_max_points = int(
            self.cfg.get("localization", {}).get("pick_snap_max_points", 60000)
        )

        root = QSplitter(Qt.Orientation.Horizontal)
        self.setCentralWidget(root)

        # Sidebar
        sidebar = QFrame()
        sidebar.setMinimumWidth(320)
        sidebar.setMaximumWidth(760)
        sidebar.setStyleSheet("background-color: rgba(0,0,0,0.35);")
        side_layout = QVBoxLayout(sidebar)
        side_layout.setContentsMargins(18, 18, 18, 18)
        side_layout.setSpacing(14)

        side_scroll = QScrollArea()
        side_scroll.setWidgetResizable(True)
        side_scroll.setFrameShape(QFrame.Shape.NoFrame)
        side_scroll.setHorizontalScrollBarPolicy(Qt.ScrollBarPolicy.ScrollBarAlwaysOff)
        side_scroll.setWidget(sidebar)
        side_scroll.setMinimumWidth(330)
        side_scroll.setMaximumWidth(780)

        lbl = QLabel("SLAM COMMANDER")
        lbl.setStyleSheet(f"color:{accent}; font-size:20px; font-weight:900;")
        side_layout.addWidget(lbl)

        # Network card
        net_card = QFrame(); net_card.setObjectName("Card"); net_card.setStyleSheet(card_style()); apply_card_shadow(net_card)
        net_layout = QVBoxLayout(net_card); net_layout.setContentsMargins(14,14,14,14); net_layout.setSpacing(10)
        net_title = QLabel("NETWORK SETTINGS"); net_title.setObjectName("CardTitle"); net_layout.addWidget(net_title)

        self.txt_iface = QLineEdit(self.cfg["network"]["default_interface"])
        self.txt_ip = QLineEdit(self.cfg["network"]["default_host_ip"])
        net_layout.addWidget(self.txt_iface); net_layout.addWidget(self.txt_ip)

        row1 = QHBoxLayout()
        self.btn_connect = QPushButton("🔌  CONNECT"); self.btn_connect.setStyleSheet(pill_button("#1677ff")); self.btn_connect.clicked.connect(self.on_connect)
        self.btn_connect.setEnabled(False)
        self.btn_connect.setToolTip("Choose a workflow first.")
        self.btn_start = QPushButton("▶  START"); self.btn_start.setStyleSheet(pill_button("#21a453")); self.btn_start.clicked.connect(self.on_start)
        self.btn_localize_only = QPushButton("🎯  LOCALIZE-ONLY")
        self.btn_localize_only.setStyleSheet(pill_button("#7a4cff"))
        self.btn_localize_only.clicked.connect(self.on_start_localize_only)
        row1.addWidget(self.btn_connect); row1.addWidget(self.btn_start); row1.addWidget(self.btn_localize_only); net_layout.addLayout(row1)

        row2 = QHBoxLayout()
        self.btn_pause = QPushButton("⏸  PAUSE"); self.btn_pause.setStyleSheet(pill_button("#c78a12", fg="black")); self.btn_pause.clicked.connect(self.on_pause)
        self.btn_stop = QPushButton("⛔  STOP"); self.btn_stop.setStyleSheet(pill_button("#d0302f")); self.btn_stop.clicked.connect(self.on_stop)
        row2.addWidget(self.btn_pause); row2.addWidget(self.btn_stop); net_layout.addLayout(row2)

        self.btn_reset = QPushButton("🧹  RESET (NO SAVE)"); self.btn_reset.setStyleSheet(pill_button("#3a3a3a")); self.btn_reset.clicked.connect(self.on_reset)
        net_layout.addWidget(self.btn_reset)

        side_layout.addWidget(net_card)

        # Workflow card
        wf_card = QFrame(); wf_card.setObjectName("Card"); wf_card.setStyleSheet(card_style()); apply_card_shadow(wf_card)
        wf_layout = QVBoxLayout(wf_card); wf_layout.setContentsMargins(14,14,14,14); wf_layout.setSpacing(10)
        wf_title = QLabel("WORKFLOW (BEFORE RUN)"); wf_title.setObjectName("CardTitle"); wf_layout.addWidget(wf_title)

        self.btn_wf_map_new = QPushButton("🧭 MAP NEW PLACE")
        self.btn_wf_map_new.setStyleSheet(pill_button("#2b2b2b"))
        self.btn_wf_map_new.clicked.connect(self.on_workflow_map_new)
        wf_layout.addWidget(self.btn_wf_map_new)

        self.btn_wf_extend = QPushButton("📌 EXTEND SAVED MAP")
        self.btn_wf_extend.setStyleSheet(pill_button("#2b2b2b"))
        self.btn_wf_extend.clicked.connect(self.on_workflow_extend_map)
        wf_layout.addWidget(self.btn_wf_extend)

        self.btn_wf_localize = QPushButton("📍 NAVIGATE IN MAPPED PLACE")
        self.btn_wf_localize.setStyleSheet(pill_button("#2b2b2b"))
        self.btn_wf_localize.clicked.connect(self.on_workflow_localize_existing)
        wf_layout.addWidget(self.btn_wf_localize)

        self.btn_wf_live_nav = QPushButton("🤖 LIVE NAV WITH MAP")
        self.btn_wf_live_nav.setStyleSheet(pill_button("#2b2b2b"))
        self.btn_wf_live_nav.clicked.connect(self.on_workflow_live_nav)
        wf_layout.addWidget(self.btn_wf_live_nav)

        self.btn_wf_live_nav_nomap = QPushButton("🛰 LIVE NAV (NO MAP)")
        self.btn_wf_live_nav_nomap.setStyleSheet(pill_button("#2b2b2b"))
        self.btn_wf_live_nav_nomap.clicked.connect(self.on_workflow_live_nav_nomap)
        wf_layout.addWidget(self.btn_wf_live_nav_nomap)

        self.btn_wf_quick = QPushButton("🧪 QUICK DEMO")
        self.btn_wf_quick.setStyleSheet(pill_button("#2b2b2b"))
        self.btn_wf_quick.clicked.connect(self.on_workflow_quick_demo)
        wf_layout.addWidget(self.btn_wf_quick)

        self.lbl_workflow = QLabel("Workflow: NOT_SELECTED")
        self.lbl_workflow.setStyleSheet("color:white; font-family:monospace; font-weight:800;")
        wf_layout.addWidget(self.lbl_workflow)

        side_layout.addWidget(wf_card)

        # Map card
        map_card = QFrame(); map_card.setObjectName("Card"); map_card.setStyleSheet(card_style()); apply_card_shadow(map_card)
        map_layout = QVBoxLayout(map_card); map_layout.setContentsMargins(14,14,14,14); map_layout.setSpacing(10)
        map_title = QLabel("MAP CONTROLS"); map_title.setObjectName("CardTitle"); map_layout.addWidget(map_title)

        self.txt_name = QLineEdit("Work")
        map_layout.addWidget(self.txt_name)

        self.chk_save = QCheckBox("✅  SAVE ARMED (WILL SAVE ON STOP)")
        self.chk_save.stateChanged.connect(self.on_toggle_save)
        map_layout.addWidget(self.chk_save)

        self.btn_save_now = QPushButton("💾  SAVE NOW")
        self.btn_save_now.setStyleSheet(pill_button("#2b2b2b"))
        self.btn_save_now.clicked.connect(self.on_save_now)
        map_layout.addWidget(self.btn_save_now)

        auto_title = QLabel("AUTOSAVE")
        auto_title.setObjectName("CardTitle")
        map_layout.addWidget(auto_title)

        auto_row = QHBoxLayout()
        self.chk_autosave = QCheckBox("ON")
        self.chk_autosave.setChecked(self.autosave_enabled)
        self.txt_autosave_sec = QLineEdit(str(int(self.autosave_interval_sec)))
        self.txt_autosave_sec.setMaximumWidth(90)
        self.btn_apply_autosave = QPushButton("APPLY")
        self.btn_apply_autosave.setStyleSheet(pill_button("#2b2b2b"))
        self.btn_apply_autosave.clicked.connect(self.on_apply_autosave)
        auto_row.addWidget(self.chk_autosave)
        auto_row.addWidget(self.txt_autosave_sec)
        auto_row.addWidget(self.btn_apply_autosave)
        map_layout.addLayout(auto_row)

        rec_title = QLabel("RECORD / REPLAY")
        rec_title.setObjectName("CardTitle")
        map_layout.addWidget(rec_title)

        rec_row1 = QHBoxLayout()
        self.btn_record_start = QPushButton("⏺ START REC")
        self.btn_record_start.setStyleSheet(pill_button("#2b2b2b"))
        self.btn_record_start.clicked.connect(self.on_record_start)
        self.btn_record_stop = QPushButton("⏹ STOP REC")
        self.btn_record_stop.setStyleSheet(pill_button("#2b2b2b"))
        self.btn_record_stop.clicked.connect(self.on_record_stop)
        rec_row1.addWidget(self.btn_record_start)
        rec_row1.addWidget(self.btn_record_stop)
        map_layout.addLayout(rec_row1)

        rec_row2 = QHBoxLayout()
        self.btn_replay_run = QPushButton("▶ RUN REPLAY")
        self.btn_replay_run.setStyleSheet(pill_button("#2b2b2b"))
        self.btn_replay_run.clicked.connect(self.on_replay_run)
        self.btn_replay_set_base = QPushButton("📌 SET BASELINE")
        self.btn_replay_set_base.setStyleSheet(pill_button("#2b2b2b"))
        self.btn_replay_set_base.clicked.connect(self.on_replay_set_baseline)
        rec_row2.addWidget(self.btn_replay_run)
        rec_row2.addWidget(self.btn_replay_set_base)
        map_layout.addLayout(rec_row2)

        min_pts_title = QLabel("MIN STABLE POINTS")
        min_pts_title.setObjectName("CardTitle")
        map_layout.addWidget(min_pts_title)

        min_pts_row = QHBoxLayout()
        self.txt_min_stable = QLineEdit(str(self.min_stable_points))
        self.txt_min_stable.returnPressed.connect(self.on_apply_min_stable_points)
        self.btn_apply_min_stable = QPushButton("APPLY")
        self.btn_apply_min_stable.setStyleSheet(pill_button("#2b2b2b"))
        self.btn_apply_min_stable.clicked.connect(self.on_apply_min_stable_points)
        min_pts_row.addWidget(self.txt_min_stable)
        min_pts_row.addWidget(self.btn_apply_min_stable)
        map_layout.addLayout(min_pts_row)

        self.btn_export_nav = QPushButton("🧭  EXPORT NAV")
        self.btn_export_nav.setStyleSheet(pill_button("#2b2b2b"))
        self.btn_export_nav.clicked.connect(self.on_export_nav)
        map_layout.addWidget(self.btn_export_nav)

        nav_title = QLabel("NAV TUNING")
        nav_title.setObjectName("CardTitle")
        map_layout.addWidget(nav_title)

        nav_row1 = QHBoxLayout()
        self.txt_nav_zmin = QLineEdit(str(self.nav_cfg.get("z_min_m", -0.4)))
        self.txt_nav_zmax = QLineEdit(str(self.nav_cfg.get("z_max_m", 1.2)))
        self.txt_nav_zmin.setPlaceholderText("z min")
        self.txt_nav_zmax.setPlaceholderText("z max")
        nav_row1.addWidget(self.txt_nav_zmin)
        nav_row1.addWidget(self.txt_nav_zmax)
        map_layout.addLayout(nav_row1)

        nav_row2 = QHBoxLayout()
        self.txt_nav_res = QLineEdit(str(self.nav_cfg.get("resolution_m", 0.05)))
        self.txt_nav_inf = QLineEdit(str(self.nav_cfg.get("inflation_radius_m", 0.2)))
        self.txt_nav_res.setPlaceholderText("res")
        self.txt_nav_inf.setPlaceholderText("inflate")
        self.btn_apply_nav = QPushButton("APPLY NAV")
        self.btn_apply_nav.setStyleSheet(pill_button("#2b2b2b"))
        self.btn_apply_nav.clicked.connect(self.on_apply_nav_config)
        nav_row2.addWidget(self.txt_nav_res)
        nav_row2.addWidget(self.txt_nav_inf)
        nav_row2.addWidget(self.btn_apply_nav)
        map_layout.addLayout(nav_row2)

        goal_row = QHBoxLayout()
        self.txt_goal_x = QLineEdit("0.0")
        self.txt_goal_y = QLineEdit("0.0")
        self.txt_goal_x.setPlaceholderText("goal x")
        self.txt_goal_y.setPlaceholderText("goal y")
        self.btn_goal_apply = QPushButton("SET GOAL")
        self.btn_goal_apply.setStyleSheet(pill_button("#2b2b2b"))
        self.btn_goal_apply.clicked.connect(self.on_set_nav_goal)
        self.btn_goal_clear = QPushButton("CLEAR")
        self.btn_goal_clear.setStyleSheet(pill_button("#2b2b2b"))
        self.btn_goal_clear.clicked.connect(self.on_clear_nav_goal)
        goal_row.addWidget(self.txt_goal_x)
        goal_row.addWidget(self.txt_goal_y)
        goal_row.addWidget(self.btn_goal_apply)
        goal_row.addWidget(self.btn_goal_clear)
        map_layout.addLayout(goal_row)

        mission_row1 = QHBoxLayout()
        self.txt_mission = QLineEdit("1.0,0.0; 2.0,0.0")
        self.txt_mission.setPlaceholderText("x,y; x,y; ...")
        self.btn_mission_start = QPushButton("MISSION START")
        self.btn_mission_start.setStyleSheet(pill_button("#2b2b2b"))
        self.btn_mission_start.clicked.connect(self.on_mission_start)
        mission_row1.addWidget(self.txt_mission)
        mission_row1.addWidget(self.btn_mission_start)
        map_layout.addLayout(mission_row1)

        mission_row2 = QHBoxLayout()
        self.btn_mission_pause = QPushButton("PAUSE M")
        self.btn_mission_pause.setStyleSheet(pill_button("#2b2b2b"))
        self.btn_mission_pause.clicked.connect(lambda: self._send_mission_cmd("pause"))
        self.btn_mission_resume = QPushButton("RESUME M")
        self.btn_mission_resume.setStyleSheet(pill_button("#2b2b2b"))
        self.btn_mission_resume.clicked.connect(lambda: self._send_mission_cmd("resume"))
        self.btn_mission_stop = QPushButton("STOP M")
        self.btn_mission_stop.setStyleSheet(pill_button("#2b2b2b"))
        self.btn_mission_stop.clicked.connect(lambda: self._send_mission_cmd("stop"))
        mission_row2.addWidget(self.btn_mission_pause)
        mission_row2.addWidget(self.btn_mission_resume)
        mission_row2.addWidget(self.btn_mission_stop)
        map_layout.addLayout(mission_row2)

        den_title = QLabel("POINT DENSITY")
        den_title.setObjectName("CardTitle")
        map_layout.addWidget(den_title)

        den_row = QHBoxLayout()
        self.btn_den_low = QPushButton("LOW")
        self.btn_den_med = QPushButton("MEDIUM")
        self.btn_den_high = QPushButton("HIGH")
        self.btn_den_low.clicked.connect(lambda: self.set_density_mode("LOW"))
        self.btn_den_med.clicked.connect(lambda: self.set_density_mode("MEDIUM"))
        self.btn_den_high.clicked.connect(lambda: self.set_density_mode("HIGH"))
        den_row.addWidget(self.btn_den_low)
        den_row.addWidget(self.btn_den_med)
        den_row.addWidget(self.btn_den_high)
        map_layout.addLayout(den_row)
        self.refresh_density_buttons()

        self.chk_loop_closure = QCheckBox("🔁 LOOP CLOSURE SAFE")
        self.chk_loop_closure.setChecked(self.loop_closure_enabled)
        self.chk_loop_closure.stateChanged.connect(self.on_toggle_loop_closure)
        map_layout.addWidget(self.chk_loop_closure)

        self.chk_loc_machine = QCheckBox("🧠 LOC STATE MACHINE")
        self.chk_loc_machine.setChecked(self.loc_machine_enabled)
        self.chk_loc_machine.stateChanged.connect(self.on_toggle_loc_machine)
        map_layout.addWidget(self.chk_loc_machine)

        self.chk_submap_mode = QCheckBox("🗺️ SUBMAP (LOCAL+GLOBAL)")
        self.chk_submap_mode.setChecked(self.submap_mode_enabled)
        self.chk_submap_mode.stateChanged.connect(self.on_toggle_submap_mode)
        map_layout.addWidget(self.chk_submap_mode)

        quality_title = QLabel("MAP QUALITY")
        quality_title.setObjectName("CardTitle")
        map_layout.addWidget(quality_title)

        q_row1 = QHBoxLayout()
        self.txt_near_range = QLineEdit(str(self.map_quality_cfg.get("near_min_range_m", 0.15)))
        self.chk_outlier = QCheckBox("Outlier")
        self.chk_outlier.setChecked(bool(self.map_quality_cfg.get("outlier_filter_enabled", False)))
        q_row1.addWidget(self.txt_near_range)
        q_row1.addWidget(self.chk_outlier)
        map_layout.addLayout(q_row1)

        q_row2 = QHBoxLayout()
        self.chk_clip_z = QCheckBox("Clip Z")
        self.chk_clip_z.setChecked(bool(self.map_quality_cfg.get("world_z_clip_enabled", False)))
        self.txt_world_zmin = QLineEdit(str(self.map_quality_cfg.get("world_z_min_m", -2.0)))
        self.txt_world_zmax = QLineEdit(str(self.map_quality_cfg.get("world_z_max_m", 3.0)))
        q_row2.addWidget(self.chk_clip_z)
        q_row2.addWidget(self.txt_world_zmin)
        q_row2.addWidget(self.txt_world_zmax)
        map_layout.addLayout(q_row2)

        q_row3 = QHBoxLayout()
        self.txt_outlier_voxel = QLineEdit(str(self.map_quality_cfg.get("outlier_voxel_m", 0.12)))
        self.txt_outlier_min = QLineEdit(str(self.map_quality_cfg.get("outlier_min_points", 2)))
        self.btn_apply_quality = QPushButton("APPLY Q")
        self.btn_apply_quality.setStyleSheet(pill_button("#2b2b2b"))
        self.btn_apply_quality.clicked.connect(self.on_apply_map_quality)
        q_row3.addWidget(self.txt_outlier_voxel)
        q_row3.addWidget(self.txt_outlier_min)
        q_row3.addWidget(self.btn_apply_quality)
        map_layout.addLayout(q_row3)

        side_layout.addWidget(map_card)

        # Localization card
        loc_card = QFrame(); loc_card.setObjectName("Card"); loc_card.setStyleSheet(card_style()); apply_card_shadow(loc_card)
        loc_layout = QVBoxLayout(loc_card); loc_layout.setContentsMargins(14,14,14,14); loc_layout.setSpacing(10)
        loc_title = QLabel("LOCALIZATION"); loc_title.setObjectName("CardTitle"); loc_layout.addWidget(loc_title)

        rowl = QHBoxLayout()
        self.btn_load_ref = QPushButton("🗺️  LOAD REF MAP"); self.btn_load_ref.setStyleSheet(pill_button("#2b2b2b")); self.btn_load_ref.clicked.connect(self.on_load_ref)
        self.btn_localize = QPushButton("📍  LOCALIZE NOW"); self.btn_localize.setStyleSheet(pill_button("#2b2b2b")); self.btn_localize.clicked.connect(self.on_localize)
        rowl.addWidget(self.btn_load_ref); rowl.addWidget(self.btn_localize)
        loc_layout.addLayout(rowl)

        self.btn_clear_ref = QPushButton("🧽  CLEAR REF"); self.btn_clear_ref.setStyleSheet(pill_button("#2b2b2b")); self.btn_clear_ref.clicked.connect(self.on_clear_ref)
        loc_layout.addWidget(self.btn_clear_ref)

        ref_color_title = QLabel("REF MAP COLOR")
        ref_color_title.setObjectName("CardTitle")
        loc_layout.addWidget(ref_color_title)

        ref_color_row = QHBoxLayout()
        self.btn_ref_gray = QPushButton("GRAY")
        self.btn_ref_height = QPushButton("HEIGHT")
        self.btn_ref_orig = QPushButton("ORIGINAL")
        self.btn_ref_gray.clicked.connect(lambda: self.set_ref_color_mode("GRAY"))
        self.btn_ref_height.clicked.connect(lambda: self.set_ref_color_mode("HEIGHT"))
        self.btn_ref_orig.clicked.connect(lambda: self.set_ref_color_mode("ORIGINAL"))
        ref_color_row.addWidget(self.btn_ref_gray)
        ref_color_row.addWidget(self.btn_ref_height)
        ref_color_row.addWidget(self.btn_ref_orig)
        loc_layout.addLayout(ref_color_row)
        self.refresh_ref_color_buttons()

        approx_title = QLabel("START POINTER")
        approx_title.setObjectName("CardTitle")
        loc_layout.addWidget(approx_title)

        approx_row1 = QHBoxLayout()
        self.txt_approx_z = QLineEdit(f"{self.approx_guess_z:.2f}")
        self.txt_approx_z.setPlaceholderText("z")
        self.txt_approx_z.setVisible(False)
        self.btn_pick_approx = QPushButton("🧭 CLICK START POINTER")
        self.btn_pick_approx.setStyleSheet(pill_button("#2b2b2b"))
        self.btn_pick_approx.clicked.connect(self.on_set_approx_pick)
        approx_row1.addWidget(self.btn_pick_approx)
        loc_layout.addLayout(approx_row1)

        self.btn_clear_approx = QPushButton("✖ CLEAR APPROX")
        self.btn_clear_approx.setStyleSheet(pill_button("#2b2b2b"))
        self.btn_clear_approx.clicked.connect(self.on_clear_approx)
        loc_layout.addWidget(self.btn_clear_approx)

        self.chk_require_start_pick = QCheckBox("Require start-point pick before START")
        self.chk_require_start_pick.setChecked(True)
        self.chk_require_start_pick.stateChanged.connect(self.on_toggle_require_start_pick)
        loc_layout.addWidget(self.chk_require_start_pick)

        self.lbl_pick_status = QLabel("Start pick: pending")
        self.lbl_pick_status.setStyleSheet("color:#ffcc00; font-family:monospace; font-weight:800;")
        loc_layout.addWidget(self.lbl_pick_status)

        side_layout.addWidget(loc_card)

        perf_card = QFrame(); perf_card.setObjectName("Card"); perf_card.setStyleSheet(card_style()); apply_card_shadow(perf_card)
        perf_layout = QVBoxLayout(perf_card); perf_layout.setContentsMargins(14,14,14,14); perf_layout.setSpacing(6)
        perf_title = QLabel("PERFORMANCE"); perf_title.setObjectName("CardTitle"); perf_layout.addWidget(perf_title)
        self.lbl_perf_fps = QLabel("FPS in/out: 0.0 / 0.0")
        self.lbl_perf_fps.setStyleSheet("color:white; font-family:monospace; font-weight:700;")
        self.lbl_perf_icp = QLabel("ICP: 0.0 ms  CPU: 0.0%")
        self.lbl_perf_icp.setStyleSheet("color:white; font-family:monospace; font-weight:700;")
        self.lbl_perf_queue = QLabel("Queue lag: 0  Growth: 0.0 pts/s")
        self.lbl_perf_queue.setStyleSheet("color:white; font-family:monospace; font-weight:700;")
        self.lbl_mode = QLabel("Mode: IDLE  Rec: OFF")
        self.lbl_mode.setStyleSheet("color:white; font-family:monospace; font-weight:700;")
        self.lbl_nav = QLabel("Nav: goal none  cmd 0.00/0.00")
        self.lbl_nav.setStyleSheet("color:white; font-family:monospace; font-weight:700;")
        self.lbl_loc_match = QLabel("Match: n/a")
        self.lbl_loc_match.setStyleSheet("color:white; font-family:monospace; font-weight:700;")
        perf_layout.addWidget(self.lbl_perf_fps)
        perf_layout.addWidget(self.lbl_perf_icp)
        perf_layout.addWidget(self.lbl_perf_queue)
        perf_layout.addWidget(self.lbl_mode)
        perf_layout.addWidget(self.lbl_nav)
        perf_layout.addWidget(self.lbl_loc_match)
        side_layout.addWidget(perf_card)

        side_layout.addStretch(1)

        # Status labels
        self.lbl_status = QLabel("Status: IDLE")
        self.lbl_status.setStyleSheet(f"color:{accent}; font-family:monospace; font-weight:900;")

        self.lbl_points = QLabel("Stable points: 0")
        self.lbl_points.setStyleSheet("color:white; font-family:monospace; font-weight:800;")

        self.lbl_pose = QLabel("Pos: 0.00, 0.00, 0.00")
        self.lbl_pose.setStyleSheet("color:white; font-family:monospace; font-weight:800;")

        self.lbl_loc = QLabel("Localization: NO_REF")
        self.lbl_loc.setStyleSheet("color:white; font-family:monospace; font-weight:800;")

        self.lbl_loc_state = QLabel("Loc health: TRACKING")
        self.lbl_loc_state.setStyleSheet("color:white; font-family:monospace; font-weight:800;")

        self.lbl_density = QLabel(f"Density: {self.density_mode}")
        self.lbl_density.setStyleSheet("color:white; font-family:monospace; font-weight:800;")

        self.lbl_min_stable = QLabel(f"Min stable: {self.min_stable_points}")
        self.lbl_min_stable.setStyleSheet("color:white; font-family:monospace; font-weight:800;")

        side_layout.addWidget(self.lbl_status)
        side_layout.addWidget(self.lbl_points)
        side_layout.addWidget(self.lbl_pose)
        side_layout.addWidget(self.lbl_loc)
        side_layout.addWidget(self.lbl_loc_state)
        side_layout.addWidget(self.lbl_density)
        side_layout.addWidget(self.lbl_min_stable)

        root.addWidget(side_scroll)

        # 3D view
        self.view = SLAMGLViewWidget()
        self.view.opts["distance"] = float(self.cfg["gui"]["view"]["distance"])
        self.view.setBackgroundColor("#0a0c10")
        root.addWidget(self.view)
        root.setChildrenCollapsible(False)
        root.setHandleWidth(10)
        root.setStretchFactor(0, 0)
        root.setStretchFactor(1, 1)
        root.setSizes([380, 1220])

        grid = gl.GLGridItem()
        grid.setSize(x=float(self.cfg["gui"]["view"]["grid_size"]), y=float(self.cfg["gui"]["view"]["grid_size"]), z=0)
        self.view.addItem(grid)

        self.axis = gl.GLAxisItem()
        self.axis.setSize(2,2,2)
        self.view.addItem(self.axis)

        self.scatter = gl.GLScatterPlotItem(pos=np.zeros((1,3),dtype=np.float32), size=2)
        self.scatter.setGLOptions("opaque")
        self.view.addItem(self.scatter)

        # Reference map overlay (shown after user loads a map)
        self.ref_scatter = gl.GLScatterPlotItem(pos=np.zeros((1,3),dtype=np.float32), size=1.2)
        self.ref_scatter.setGLOptions("translucent")
        self.view.addItem(self.ref_scatter)
        self.ref_match_scatter = gl.GLScatterPlotItem(pos=np.zeros((1,3),dtype=np.float32), size=1.4)
        self.ref_match_scatter.setGLOptions("translucent")
        self.view.addItem(self.ref_match_scatter)
        self.start_ptr_scatter = gl.GLScatterPlotItem(pos=np.zeros((1,3),dtype=np.float32), size=1.8)
        self.start_ptr_scatter.setGLOptions("translucent")
        self.view.addItem(self.start_ptr_scatter)
        self.view.mapPickDrag.connect(self.on_view_map_pick_drag)

        self.timer = QTimer()
        self.timer.timeout.connect(self.update_loop)
        self.timer.start(50)

        self.max_draw = int(self.cfg["gui"]["render"]["max_draw_points"])
        self.max_ref_draw = max(50000, self.max_draw // 2)
        self.decimate_step = int(self.cfg["gui"]["render"]["decimate_step"])
        self.maps_dir = maps_dir
        self.ref_display_voxel = float(self.cfg["localization"]["ref_display_voxel"])

        # Workflow UI control groups: non-relevant controls are disabled/greyed.
        self._workflow_widget_groups = {
            "runtime": [
                self.txt_min_stable, self.btn_apply_min_stable,
                self.btn_den_low, self.btn_den_med, self.btn_den_high,
            ],
            "mapping": [
                self.txt_name, self.chk_save, self.btn_save_now,
                self.chk_autosave, self.txt_autosave_sec, self.btn_apply_autosave,
                self.chk_loop_closure,
                self.txt_near_range, self.chk_outlier, self.chk_clip_z,
                self.txt_world_zmin, self.txt_world_zmax,
                self.txt_outlier_voxel, self.txt_outlier_min, self.btn_apply_quality,
            ],
            "recording": [
                self.btn_record_start, self.btn_record_stop,
                self.btn_replay_run, self.btn_replay_set_base,
            ],
            "localization": [
                self.btn_localize_only, self.btn_load_ref, self.btn_localize, self.btn_clear_ref,
                self.chk_loc_machine,
                self.btn_ref_gray, self.btn_ref_height, self.btn_ref_orig,
                self.txt_approx_z, self.btn_pick_approx, self.btn_clear_approx,
                self.chk_require_start_pick, self.lbl_pick_status,
            ],
            "submap_nav": [
                self.chk_submap_mode,
            ],
            "nav_export": [
                self.btn_export_nav, self.txt_nav_zmin, self.txt_nav_zmax,
                self.txt_nav_res, self.txt_nav_inf, self.btn_apply_nav,
            ],
            "nav_live": [
                self.txt_goal_x, self.txt_goal_y, self.btn_goal_apply, self.btn_goal_clear,
                self.txt_mission, self.btn_mission_start,
                self.btn_mission_pause, self.btn_mission_resume, self.btn_mission_stop,
            ],
        }
        self._workflow_managed_widgets = []
        seen = set()
        for group_widgets in self._workflow_widget_groups.values():
            for widget in group_widgets:
                k = id(widget)
                if k in seen:
                    continue
                seen.add(k)
                self._workflow_managed_widgets.append(widget)
        self._apply_workflow_ui_rules(self.workflow_profile, update_label=True)
        self._refresh_pick_status()
        self._hide_start_pointer_visual()

    def set_status(self, text: str, color: str):
        self.lbl_status.setText(text)
        self.lbl_status.setStyleSheet(f"color:{color}; font-family:monospace; font-weight:900;")

    def show_error_popup(self, message: str):
        msg = str(message).strip() or "Unknown error"
        now = time.monotonic()
        if msg == self._last_error_popup_text and (now - self._last_error_popup_t) < self._error_popup_cooldown_sec:
            return
        self._last_error_popup_text = msg
        self._last_error_popup_t = now
        QMessageBox.critical(self, "SLAM Error", msg)

    def show_saved_popup(self, message: str):
        msg = str(message).strip()
        if not msg:
            return
        now = time.monotonic()
        if msg == self._last_saved_popup_text and (now - self._last_saved_popup_t) < self._saved_popup_cooldown_sec:
            return
        self._last_saved_popup_text = msg
        self._last_saved_popup_t = now
        QMessageBox.information(self, "Map Saved", msg)

    def show_self_check_popup(self, report: dict | None, source: str = "startup"):
        if self._self_check_popup_shown and source != "worker":
            return
        rep = report or {}
        errs = list(rep.get("errors", []) or [])
        warns = list(rep.get("warnings", []) or [])
        info = list(rep.get("info", []) or [])
        if not errs and not warns:
            if source == "engine":
                self._self_check_popup_shown = True
            return
        msg_lines = []
        if errs:
            msg_lines.append("Errors:")
            msg_lines.extend(f"- {x}" for x in errs)
        if warns:
            msg_lines.append("Warnings:")
            msg_lines.extend(f"- {x}" for x in warns)
        if info:
            msg_lines.append("Info:")
            msg_lines.extend(f"- {x}" for x in info[:3])
        text = "\n".join(msg_lines)
        if errs:
            QMessageBox.critical(self, "SLAM Startup Check", text)
        else:
            QMessageBox.warning(self, "SLAM Startup Check", text)
        if source == "engine":
            self._self_check_popup_shown = True

    @staticmethod
    def _read_float(line: QLineEdit, default: float) -> float:
        txt = line.text().strip()
        try:
            return float(txt)
        except Exception:
            line.setText(str(default))
            return float(default)

    def request_map_export_if_armed(self) -> bool:
        if not self.save_armed:
            return False
        base = self.txt_name.text().strip() or "map_robot"
        self.client.export_map(base)
        return True

    def _read_min_stable_input(self, show_error: bool = False):
        txt = self.txt_min_stable.text().strip()
        try:
            value = int(txt)
        except Exception:
            if show_error:
                self.show_error_popup("Minimum stable points must be an integer.")
            return None
        return int(np.clip(value, 50, 5_000_000))

    def _apply_min_stable_value(self, value: int, set_status_msg: bool = True):
        self.min_stable_points = int(value)
        self.txt_min_stable.setText(str(self.min_stable_points))
        self.lbl_min_stable.setText(f"Min stable: {self.min_stable_points}")
        try:
            self.client.set_min_stable_points(self.min_stable_points)
        except Exception:
            pass
        if set_status_msg:
            self.set_status(f"Min stable set: {self.min_stable_points}", "#ffcc00")

    def ensure_min_stable_synced(self, show_error: bool = False, set_status_msg: bool = False) -> bool:
        value = self._read_min_stable_input(show_error=show_error)
        if value is None:
            return False
        if int(value) != int(self.min_stable_points):
            self._apply_min_stable_value(int(value), set_status_msg=set_status_msg)
        return True

    def _set_widgets_enabled(self, widgets, enabled: bool):
        for w in widgets:
            if w is None:
                continue
            w.setEnabled(bool(enabled))

    def _apply_workflow_ui_rules(self, profile: str | None = None, update_label: bool = False):
        p = str(profile if profile is not None else self.workflow_profile).upper().strip()
        if p not in ("MAP_NEW", "EXTEND_MAP", "LOCALIZE_MAP", "LIVE_NAV_MAP", "LIVE_NAV_NO_MAP", "QUICK_DEMO", "BALANCED"):
            p = "BALANCED"
        self.workflow_profile = p
        if update_label:
            if self.workflow_selected:
                self.lbl_workflow.setText(f"Workflow: {self.workflow_profile}")
            else:
                self.lbl_workflow.setText("Workflow: NOT_SELECTED")

        # Start from fully enabled, then gray-out non-relevant controls per workflow.
        self._set_widgets_enabled(self._workflow_managed_widgets, True)
        # Submap LOCAL+GLOBAL is intentionally limited to map-based localization/nav workflows.
        self._set_widgets_enabled(
            self._workflow_widget_groups["submap_nav"],
            p in ("LOCALIZE_MAP", "LIVE_NAV_MAP"),
        )

        if p == "MAP_NEW":
            self._set_widgets_enabled(self._workflow_widget_groups["localization"], False)
            self._set_widgets_enabled(self._workflow_widget_groups["nav_live"], False)
        elif p == "EXTEND_MAP":
            # Extending an existing map needs localization (to anchor incoming
            # scans to the loaded frame) AND mapping (to add the new points).
            self._set_widgets_enabled(self._workflow_widget_groups["nav_live"], False)
        elif p == "LOCALIZE_MAP":
            self._set_widgets_enabled(self._workflow_widget_groups["mapping"], False)
            self._set_widgets_enabled(self._workflow_widget_groups["recording"], False)
            self._set_widgets_enabled(self._workflow_widget_groups["nav_export"], False)
            self._set_widgets_enabled(self._workflow_widget_groups["nav_live"], False)
        elif p == "LIVE_NAV_MAP":
            self._set_widgets_enabled(self._workflow_widget_groups["mapping"], False)
            self._set_widgets_enabled(self._workflow_widget_groups["recording"], False)
        elif p == "LIVE_NAV_NO_MAP":
            self._set_widgets_enabled(self._workflow_widget_groups["localization"], False)
        elif p == "QUICK_DEMO":
            # Quick demo keeps most options active, but submap remains map-workflow only.
            pass
        else:
            # BALANCED keeps broad controls active; submap remains map-workflow only.
            pass
        if p not in ("LOCALIZE_MAP", "LIVE_NAV_MAP") and self.approx_pick_armed:
            self._set_approx_pick_mode(False)
        self._refresh_pick_status()

    def apply_runtime_settings(self):
        try:
            self.client.set_density(self.density_mode)
        except Exception:
            pass
        if not self.ensure_min_stable_synced(show_error=False, set_status_msg=False):
            try:
                self.client.set_min_stable_points(self.min_stable_points)
            except Exception:
                pass
        try:
            self.client.set_loop_closure(bool(self.loop_closure_enabled))
        except Exception:
            pass
        try:
            self.client.set_loc_state_machine(bool(self.loc_machine_enabled))
        except Exception:
            pass
        try:
            self.client.set_submap_mode(
                bool(self.submap_mode_enabled),
                {"apply_profiles": ["LOCALIZE_MAP", "LIVE_NAV_MAP"]},
            )
        except Exception:
            pass
        try:
            self.client.set_stability_profile(str(self.workflow_profile))
        except Exception:
            pass
        self.on_apply_autosave(set_status_msg=False, quiet=True)
        self.on_apply_nav_config(set_status_msg=False, quiet=True)
        self.on_apply_map_quality(set_status_msg=False, quiet=True)

    def _apply_workflow_profile(self, profile: str, set_status_msg: bool = True):
        p = str(profile).upper().strip()
        self.workflow_selected = True
        self._apply_workflow_ui_rules(p, update_label=True)
        self.btn_connect.setEnabled(True)
        self.btn_connect.setToolTip("")
        try:
            self.client.set_stability_profile(self.workflow_profile)
        except Exception:
            pass
        if set_status_msg:
            self.set_status(f"Workflow selected: {self.workflow_profile}", "#ffcc00")

    def _pick_and_load_ref_map(self) -> bool:
        fname, _ = QFileDialog.getOpenFileName(self, "Load Reference Map", self.maps_dir, "Point Clouds (*.ply *.pcd)")
        if not fname:
            return False
        self.ref_map_path = fname
        self.start_pick_ready = False
        self.start_pick_xyz = None
        self._pick_help_shown_for_ref = None
        self._refresh_pick_status()
        self.client.load_ref_map(fname)
        self.lbl_loc.setText(f"Localization: ref={Path(fname).name}")
        self._start_ref_loader(fname, show_status=False)
        return True

    def _workflow_requires_ref_map(self) -> bool:
        return str(self.workflow_profile).upper().strip() in ("LOCALIZE_MAP", "LIVE_NAV_MAP")

    def on_workflow_map_new(self):
        self._apply_workflow_profile("MAP_NEW", set_status_msg=False)
        self._set_approx_pick_mode(False)
        self.start_pick_ready = False
        self.start_pick_xyz = None
        self._refresh_pick_status()
        self.set_density_mode("MEDIUM")
        # Keep loop closure off by default for stability; user can enable manually later.
        self.chk_loop_closure.setChecked(False)
        self.chk_submap_mode.setChecked(False)
        # Mapping-new should run in pure SLAM frame (no stale reference alignment).
        if self.ref_map_path:
            self.on_clear_ref()
        self.set_status("Workflow MAP_NEW ready: CONNECT -> START mapping (loop closure OFF by default).", "#ffcc00")

    def on_workflow_extend_map(self):
        """Extend an existing saved map: load .ply, seed stable map, continue mapping."""
        fname, _ = QFileDialog.getOpenFileName(
            self, "Extend Saved Map", self.maps_dir, "Point Clouds (*.ply *.pcd)"
        )
        if not fname:
            return
        self._apply_workflow_profile("EXTEND_MAP", set_status_msg=False)
        self._set_approx_pick_mode(False)
        self.start_pick_ready = False
        self.start_pick_xyz = None
        self._refresh_pick_status()
        self.set_density_mode("MEDIUM")
        # Loop closure on by default for extend — drift accumulated during the
        # original scan plus drift in the new segment both benefit from it.
        self.chk_loop_closure.setChecked(True)
        self.chk_submap_mode.setChecked(False)
        # Wire ref_map_path so the GUI shows the loaded map in the viewer.
        self.ref_map_path = fname
        self.lbl_loc.setText(f"Localization: ref={Path(fname).name}")
        self._start_ref_loader(fname, show_status=False)
        # Send the extend command (also sets ref internally and seeds stable/filter).
        try:
            self.client.load_for_extend(fname)
        except Exception as e:
            self.show_error_popup(f"Extend load failed: {e}")
            return
        self.set_status(
            f"Workflow EXTEND_MAP ready (loaded {Path(fname).name}): CONNECT -> START.",
            "#ffcc00",
        )
        QMessageBox.information(
            self,
            "Extend Saved Map",
            "1) Click CONNECT\n2) Click START\n\n"
            "New scans will be added to the loaded map. Save as usual on STOP — "
            "the merged map is written to a new file (auto-incremented name).",
        )

    def on_workflow_localize_existing(self):
        self._apply_workflow_profile("LOCALIZE_MAP", set_status_msg=False)
        self._set_approx_pick_mode(False)
        self.start_pick_ready = False
        self.start_pick_xyz = None
        self._refresh_pick_status()
        self.set_density_mode("MEDIUM")
        self.chk_loop_closure.setChecked(False)
        self.chk_loc_machine.setChecked(True)
        # Reference-anchored default: keep uploaded map static, use submap only if user enables it.
        self.chk_submap_mode.setChecked(False)
        self.set_status(
            "Workflow LOCALIZE_MAP ready: LOAD REF -> click start pointer -> CONNECT -> START.",
            "#ffcc00",
        )
        QMessageBox.information(
            self,
            "Navigate In Mapped Place",
            "1) Click LOAD REF MAP\n2) Click CLICK START POINTER, then click current location on map\n3) Click CONNECT\n4) Click START\n\nLocalization will start automatically from your picked start location.",
        )

    def on_workflow_live_nav(self):
        self._apply_workflow_profile("LIVE_NAV_MAP", set_status_msg=False)
        self._set_approx_pick_mode(False)
        self.start_pick_ready = False
        self.start_pick_xyz = None
        self._refresh_pick_status()
        self.set_density_mode("HIGH")
        self.chk_loop_closure.setChecked(False)
        self.chk_loc_machine.setChecked(True)
        # Reference-anchored default: keep uploaded map static, use submap only if user enables it.
        self.chk_submap_mode.setChecked(False)
        try:
            self.client.set_nav_runtime_cfg({"enabled": True, "dynamic_decay_sec": 1.6, "dynamic_min_hits": 2})
        except Exception:
            pass
        self.set_status(
            "Workflow LIVE_NAV_MAP ready: LOAD REF -> click start pointer -> CONNECT -> START.",
            "#ffcc00",
        )
        QMessageBox.information(
            self,
            "Live Nav With Map",
            "1) Click LOAD REF MAP\n2) Click CLICK START POINTER, then click current location on map\n3) Click CONNECT\n4) Click START\n\nLocalization will start automatically from your picked start location.",
        )

    def on_workflow_live_nav_nomap(self):
        self._apply_workflow_profile("LIVE_NAV_NO_MAP", set_status_msg=False)
        self._set_approx_pick_mode(False)
        self.start_pick_ready = False
        self.start_pick_xyz = None
        self._refresh_pick_status()
        self.set_density_mode("HIGH")
        self.chk_loop_closure.setChecked(False)
        self.chk_submap_mode.setChecked(False)
        if self.ref_map_path:
            # Force no-reference mode for live mapless navigation.
            self.on_clear_ref()
        try:
            self.client.set_nav_runtime_cfg({"enabled": True, "dynamic_decay_sec": 1.4, "dynamic_min_hits": 2})
        except Exception:
            pass
        self.set_status("Workflow LIVE_NAV_NO_MAP ready: CONNECT -> START -> set GOAL/MISSION.", "#ffcc00")

    def on_workflow_quick_demo(self):
        self._apply_workflow_profile("QUICK_DEMO", set_status_msg=False)
        self._set_approx_pick_mode(False)
        self.start_pick_ready = False
        self.start_pick_xyz = None
        self._refresh_pick_status()
        self.set_density_mode("HIGH")
        self.set_status("Workflow QUICK_DEMO ready (fast build, lower persistence).", "#ffcc00")

    # actions
    def on_connect(self):
        if not self.workflow_selected:
            self.show_error_popup("Select a workflow first, then CONNECT.")
            self.set_status("Status: select workflow first", "#ffcc00")
            return
        if self._workflow_requires_ref_map() and not self.ref_map_path:
            self.show_error_popup("Load a reference map first for this workflow.")
            self.set_status("Status: load ref map first", "#ffcc00")
            return
        # Update config-derived IP at runtime by environment is not needed; just re-create engine process
        ip = self.txt_ip.text().strip()
        if not ip:
            self.set_status("Status: IP required", "#ffcc00")
            return

        # write into process config by setting env and recreating client config file? keep behavior: just connect
        # engine reads config ip; for runtime typed IP, we pass command CONNECT and worker uses eng_cfg.host_ip.
        # easiest: set env override and restart client
        self.client.stop_process()
        # update env config override is not a numeric param; it’s user input.
        # We rebuild a new client, then mutate host_ip for this session:
        self.client = SlamEngineClient()
        self.client.eng_cfg.host_ip = ip
        self.client.start_process()
        self.show_self_check_popup(self.client.get_self_check(), source="engine")
        self._worker_dead_notified = False
        self.worker_mode = "IDLE"
        self.is_connected = False
        self.recording_enabled = False
        self.recording_frames = 0
        self.recording_dropped = 0
        self.lbl_mode.setText(f"Mode: {self.worker_mode}  Rec: {'ON' if self.recording_enabled else 'OFF'}")
        self.client.connect()
        self.apply_runtime_settings()
        if self._workflow_requires_ref_map() and self.ref_map_path:
            self.client.load_ref_map(self.ref_map_path)
            if self.start_pick_ready and isinstance(self.start_pick_xyz, tuple) and len(self.start_pick_xyz) == 3:
                px, py, pz = self.start_pick_xyz
                try:
                    self.client.set_approx_pose(float(px), float(py), float(pz))
                except Exception:
                    pass
            else:
                self._prompt_pick_before_start_if_needed()
        self.set_status("Status: CONNECTING...", self.cfg["gui"]["colors"]["accent"])

    def on_start(self):
        if not self.is_connected:
            self.show_error_popup("Click CONNECT first.")
            return
        self._set_approx_pick_mode(False)
        self.apply_runtime_settings()
        if self._workflow_requires_ref_map():
            if not self.ref_map_path:
                self.show_error_popup("Load a reference map first, then CONNECT and START.")
                self.set_status("Status: load ref map first", "#ffcc00")
                return
            if self.require_start_pick and not self.start_pick_ready:
                self.show_error_popup("Set approximate start location on the map first.")
                self._prompt_pick_before_start_if_needed()
                self.set_status("Status: click map to set start location", "#ffcc00")
                return
            if self.start_pick_ready and isinstance(self.start_pick_xyz, tuple) and len(self.start_pick_xyz) == 3:
                px, py, pz = self.start_pick_xyz
                try:
                    self.client.set_approx_pose(float(px), float(py), float(pz))
                except Exception:
                    pass
            self.client.start_localize_only()
            self.worker_mode = "LOCALIZE_ONLY"
            self.lbl_mode.setText(f"Mode: {self.worker_mode}  Rec: {'ON' if self.recording_enabled else 'OFF'}")
            self.set_status("Status: LOCALIZE_ONLY (AUTO LOCALIZING FROM LIVE SCAN)", "#ffcc00")
            return
        self.client.start_mapping()
        self.worker_mode = "MAPPING"
        self.lbl_mode.setText(f"Mode: {self.worker_mode}  Rec: {'ON' if self.recording_enabled else 'OFF'}")
        self.set_status("Status: MAPPING", self.cfg["gui"]["colors"]["accent"])

    def on_start_localize_only(self):
        if not self.ref_map_path:
            self.show_error_popup("Load a reference map first for LOCALIZE-ONLY mode.")
            return
        if not self.is_connected:
            self.show_error_popup("Click CONNECT first.")
            return
        if self.require_start_pick and not self.start_pick_ready:
            self.show_error_popup("Set approximate start location on the map first.")
            self._prompt_pick_before_start_if_needed()
            return
        self.apply_runtime_settings()
        if self.start_pick_ready and isinstance(self.start_pick_xyz, tuple) and len(self.start_pick_xyz) == 3:
            px, py, pz = self.start_pick_xyz
            try:
                self.client.set_approx_pose(float(px), float(py), float(pz))
            except Exception:
                pass
        self.client.start_localize_only()
        self.worker_mode = "LOCALIZE_ONLY"
        self.lbl_mode.setText(f"Mode: {self.worker_mode}  Rec: {'ON' if self.recording_enabled else 'OFF'}")
        self.set_status("Status: LOCALIZE_ONLY", "#ffcc00")

    def on_pause(self):
        self.client.pause_mapping()
        self.worker_mode = "PAUSED"
        self.lbl_mode.setText(f"Mode: {self.worker_mode}  Rec: {'ON' if self.recording_enabled else 'OFF'}")
        self.set_status("Status: PAUSED", "#ffcc00")

    def on_stop(self):
        if not self.ensure_min_stable_synced(show_error=True, set_status_msg=False):
            return
        self.client.stop_mapping()
        self.worker_mode = "STOPPED"
        self.lbl_mode.setText(f"Mode: {self.worker_mode}  Rec: {'ON' if self.recording_enabled else 'OFF'}")
        if self.request_map_export_if_armed():
            self.set_status("Status: STOPPED (SAVING MAP...)", "#ffcc00")
        else:
            self.set_status("Status: STOPPED", "white")

    def on_record_start(self):
        base = self.txt_name.text().strip() or "slam_recording"
        self.client.record_start(base)
        self.recording_enabled = True
        self.lbl_mode.setText(f"Mode: {self.worker_mode}  Rec: ON")
        self.set_status("Recording started.", "#ffcc00")

    def on_record_stop(self):
        base = self.txt_name.text().strip() or "slam_recording"
        self.client.record_stop(True, base)
        self.recording_enabled = False
        self.lbl_mode.setText(f"Mode: {self.worker_mode}  Rec: OFF")
        self.set_status("Recording stopped.", "#ffcc00")

    def on_replay_run(self):
        fname, _ = QFileDialog.getOpenFileName(self, "Run Replay", self.maps_dir, "Replay Files (*.npz)")
        if not fname:
            return
        try:
            from SLAM_Replay import load_replay_npz, run_replay, compare_reports

            frames, poses = load_replay_npz(fname)
            report = run_replay(frames, poses=poses, export_nav=False)
            self.last_replay_report = report
            out = {"report": report}
            passed = None
            base_path = Path(self.replay_baseline_path)
            if base_path.exists():
                baseline = json.loads(base_path.read_text(encoding="utf-8"))
                base_rep = baseline.get("report", baseline) if isinstance(baseline, dict) else {}
                out["comparison"] = compare_reports(report, base_rep, tol_ratio=0.15)
                passed = bool(out["comparison"].get("passed", False))
            msg = json.dumps(out, indent=2)
            QMessageBox.information(self, "Replay Report", msg)
            if passed is None:
                self.set_status("Replay complete (no baseline).", self.cfg["gui"]["colors"]["accent"])
            elif passed:
                self.set_status("Replay regression: PASS", self.cfg["gui"]["colors"]["accent"])
            else:
                self.set_status("Replay regression: FAIL", "#ff4444")
        except Exception as e:
            self.show_error_popup(f"Replay failed: {e}")

    def on_replay_set_baseline(self):
        if not isinstance(self.last_replay_report, dict):
            self.show_error_popup("Run replay first, then set baseline.")
            return
        try:
            base = {"report": self.last_replay_report}
            Path(self.replay_baseline_path).write_text(json.dumps(base, indent=2), encoding="utf-8")
            self.set_status(f"Baseline saved: {Path(self.replay_baseline_path).name}", "#ffcc00")
        except Exception as e:
            self.show_error_popup(f"Failed to write baseline: {e}")

    def on_save_now(self):
        if not self.ensure_min_stable_synced(show_error=True, set_status_msg=False):
            return
        base = self.txt_name.text().strip() or "map_robot"
        self.client.export_map(base)
        self.set_status("Saving map now...", "#ffcc00")

    def on_reset(self):
        self.save_armed = False
        self.chk_save.blockSignals(True); self.chk_save.setChecked(False); self.chk_save.blockSignals(False)
        self.client.reset_mapping()
        self.worker_mode = "IDLE"
        self.recording_enabled = False
        self.recording_frames = 0
        self.recording_dropped = 0
        self.lbl_mode.setText("Mode: IDLE  Rec: OFF")
        self.scatter.setData(pos=np.zeros((1,3),dtype=np.float32))
        self.set_status("Status: RESET", "#ffcc00")
        self.lbl_points.setText("Stable points: 0")
        self.current_stable_points = 0

    def on_toggle_save(self, state: int):
        self.save_armed = bool(state)
        self.set_status("Save armed" if self.save_armed else "Save disarmed",
                        self.cfg["gui"]["colors"]["accent"] if self.save_armed else "white")

    def on_apply_autosave(self, _checked: bool = False, set_status_msg: bool = True, quiet: bool = False):
        sec = self._read_float(self.txt_autosave_sec, self.autosave_interval_sec)
        sec = max(5.0, sec)
        self.autosave_interval_sec = sec
        self.autosave_enabled = bool(self.chk_autosave.isChecked())
        self.txt_autosave_sec.setText(str(int(sec)))
        base = self.txt_name.text().strip() or "autosave_map"
        try:
            self.client.set_autosave(self.autosave_enabled, self.autosave_interval_sec, base)
        except Exception:
            if not quiet:
                self.show_error_popup("Failed to send autosave settings to worker.")
            return
        if set_status_msg:
            mode = "ON" if self.autosave_enabled else "OFF"
            self.set_status(f"Autosave {mode} ({int(self.autosave_interval_sec)}s)", "#ffcc00")

    def on_export_nav(self):
        if not self.ensure_min_stable_synced(show_error=True, set_status_msg=False):
            return
        self.on_apply_nav_config(set_status_msg=False, quiet=True)
        base = self.txt_name.text().strip() or "map_robot"
        self.client.export_nav(base)
        self.set_status("Exporting nav map...", "#ffcc00")

    def on_apply_min_stable_points(self):
        value = self._read_min_stable_input(show_error=True)
        if value is None:
            return
        self._apply_min_stable_value(int(value), set_status_msg=True)

    def on_toggle_loop_closure(self, state: int):
        self.loop_closure_enabled = bool(state)
        try:
            self.client.set_loop_closure(self.loop_closure_enabled)
        except Exception:
            pass
        self.set_status(
            "Loop closure SAFE ON" if self.loop_closure_enabled else "Loop closure OFF",
            "#ffcc00",
        )

    def on_toggle_loc_machine(self, state: int):
        self.loc_machine_enabled = bool(state)
        try:
            self.client.set_loc_state_machine(self.loc_machine_enabled)
        except Exception:
            pass
        self.set_status(
            "Localization state machine ON" if self.loc_machine_enabled else "Localization state machine OFF",
            "#ffcc00",
        )

    def on_toggle_submap_mode(self, state: int):
        self.submap_mode_enabled = bool(state)
        try:
            self.client.set_submap_mode(
                self.submap_mode_enabled,
                {"apply_profiles": ["LOCALIZE_MAP", "LIVE_NAV_MAP"]},
            )
        except Exception:
            pass
        self.set_status(
            "Submap LOCAL+GLOBAL ON" if self.submap_mode_enabled else "Submap LOCAL+GLOBAL OFF",
            "#ffcc00",
        )

    def on_apply_nav_config(self, _checked: bool = False, set_status_msg: bool = True, quiet: bool = False):
        zmin = self._read_float(self.txt_nav_zmin, float(self.nav_cfg.get("z_min_m", -0.4)))
        zmax = self._read_float(self.txt_nav_zmax, float(self.nav_cfg.get("z_max_m", 1.2)))
        res = max(0.01, self._read_float(self.txt_nav_res, float(self.nav_cfg.get("resolution_m", 0.05))))
        inf = max(0.0, self._read_float(self.txt_nav_inf, float(self.nav_cfg.get("inflation_radius_m", 0.2))))
        patch = {
            "z_min_m": zmin,
            "z_max_m": zmax,
            "resolution_m": res,
            "inflation_radius_m": inf,
            "min_points": int(self.min_stable_points),
        }
        self.nav_cfg.update(patch)
        try:
            self.client.set_nav_export_cfg(patch)
        except Exception:
            if not quiet:
                self.show_error_popup("Failed to send nav tuning to worker.")
            return
        if set_status_msg:
            self.set_status("Nav tuning applied.", "#ffcc00")

    def on_set_nav_goal(self):
        x = self._read_float(self.txt_goal_x, 0.0)
        y = self._read_float(self.txt_goal_y, 0.0)
        try:
            self.client.set_nav_goal(x, y)
            self.set_status(f"Nav goal set ({x:.2f}, {y:.2f})", "#ffcc00")
        except Exception:
            self.show_error_popup("Failed to send nav goal.")

    def on_clear_nav_goal(self):
        try:
            self.client.clear_nav_goal()
            self.set_status("Nav goal cleared.", "white")
        except Exception:
            self.show_error_popup("Failed to clear nav goal.")

    def on_mission_start(self):
        raw = self.txt_mission.text().strip()
        if not raw:
            self.show_error_popup("Mission waypoints are empty.")
            return
        waypoints = []
        try:
            chunks = [x.strip() for x in raw.split(";") if x.strip()]
            for ch in chunks:
                parts = [p.strip() for p in ch.split(",")]
                if len(parts) < 2:
                    continue
                waypoints.append({"x": float(parts[0]), "y": float(parts[1])})
        except Exception:
            self.show_error_popup("Mission format should be: x,y; x,y; ...")
            return
        if not waypoints:
            self.show_error_popup("No valid mission waypoints parsed.")
            return
        try:
            self.client.mission_start(waypoints)
            self.set_status(f"Mission started ({len(waypoints)} wp)", "#ffcc00")
        except Exception:
            self.show_error_popup("Failed to start mission.")

    def _send_mission_cmd(self, mode: str):
        m = str(mode).lower().strip()
        try:
            if m == "pause":
                self.client.mission_pause()
                self.set_status("Mission paused.", "#ffcc00")
            elif m == "resume":
                self.client.mission_resume()
                self.set_status("Mission resumed.", "#ffcc00")
            elif m == "stop":
                self.client.mission_stop()
                self.set_status("Mission stopped.", "white")
        except Exception:
            self.show_error_popup("Failed to send mission command.")

    def on_apply_map_quality(self, _checked: bool = False, set_status_msg: bool = True, quiet: bool = False):
        patch = {
            "enabled": True,
            "near_min_range_m": max(0.0, self._read_float(self.txt_near_range, 0.15)),
            "world_z_clip_enabled": bool(self.chk_clip_z.isChecked()),
            "world_z_min_m": self._read_float(self.txt_world_zmin, -2.0),
            "world_z_max_m": self._read_float(self.txt_world_zmax, 3.0),
            "outlier_filter_enabled": bool(self.chk_outlier.isChecked()),
            "outlier_voxel_m": max(0.02, self._read_float(self.txt_outlier_voxel, 0.12)),
            "outlier_min_points": max(1, int(self._read_float(self.txt_outlier_min, 2))),
        }
        self.map_quality_cfg.update(patch)
        try:
            self.client.set_map_quality_cfg(patch)
        except Exception:
            if not quiet:
                self.show_error_popup("Failed to send map-quality settings to worker.")
            return
        if set_status_msg:
            self.set_status("Map-quality filter applied.", "#ffcc00")

    def refresh_density_buttons(self):
        for mode, btn in (
            ("LOW", self.btn_den_low),
            ("MEDIUM", self.btn_den_med),
            ("HIGH", self.btn_den_high),
        ):
            if mode == self.density_mode:
                btn.setStyleSheet(pill_button("#0f8f4b"))
            else:
                btn.setStyleSheet(pill_button("#2b2b2b"))

    def set_density_mode(self, mode: str):
        self.density_mode = str(mode).upper().strip()
        if self.density_mode not in ("LOW", "MEDIUM", "HIGH"):
            self.density_mode = "MEDIUM"
        self.refresh_density_buttons()
        self.lbl_density.setText(f"Density: {self.density_mode}")
        try:
            self.client.set_density(self.density_mode)
        except Exception:
            pass
        self.set_status(f"Density set: {self.density_mode}", "#ffcc00")

    def refresh_ref_color_buttons(self):
        for mode, btn in (
            ("GRAY", self.btn_ref_gray),
            ("HEIGHT", self.btn_ref_height),
            ("ORIGINAL", self.btn_ref_orig),
        ):
            if mode == self.ref_color_mode:
                btn.setStyleSheet(pill_button("#0f8f4b"))
            else:
                btn.setStyleSheet(pill_button("#2b2b2b"))

    def _start_ref_loader(self, filename: str, show_status: bool = True):
        if self._ref_thread is not None and self._ref_thread.isRunning():
            self._ref_thread.quit()
            self._ref_thread.wait(200)
        self._ref_thread = RefLoaderThread(
            filename,
            display_voxel=self.ref_display_voxel,
            color_mode=self.ref_color_mode,
        )
        self._ref_thread.loaded.connect(self.on_ref_loaded)
        self._ref_thread.failed.connect(lambda e: QMessageBox.critical(self, "REF load failed", e))
        self._ref_thread.start()
        if show_status:
            self.set_status(f"Ref loading ({self.ref_color_mode})...", "#ffcc00")

    def set_ref_color_mode(self, mode: str):
        m = str(mode).upper().strip()
        if m not in ("GRAY", "HEIGHT", "ORIGINAL"):
            m = "GRAY"
        self.ref_color_mode = m
        self.refresh_ref_color_buttons()
        if self.ref_map_path:
            self._start_ref_loader(self.ref_map_path, show_status=False)
        self.set_status(f"Ref color: {self.ref_color_mode}", "#ffcc00")

    def on_load_ref(self):
        if not self._pick_and_load_ref_map():
            return
        self.set_status("Ref loaded.", "#ffcc00")

    def on_ref_loaded(self, pts, cols, filename: str):
        draw_pts = pts
        draw_cols = cols
        if len(draw_pts) > self.max_ref_draw:
            step = max(2, len(draw_pts) // self.max_ref_draw)
            draw_pts = draw_pts[::step]
            if draw_cols is not None and len(draw_cols) == len(pts):
                draw_cols = draw_cols[::step]

        if draw_cols is not None:
            self.ref_scatter.setData(pos=draw_pts, color=draw_cols, size=1.2)
        else:
            self.ref_scatter.setData(pos=draw_pts, size=1.2)
        try:
            pick_pts = np.asarray(pts, dtype=np.float32)
            if pick_pts.ndim == 2 and pick_pts.shape[1] == 3 and len(pick_pts) > 0:
                max_n = max(2000, int(self.ref_pick_max_points))
                if len(pick_pts) > max_n:
                    step = max(1, int(np.ceil(float(len(pick_pts)) / float(max_n))))
                    pick_pts = pick_pts[::step][:max_n]
                self.ref_pick_points = np.asarray(pick_pts, dtype=np.float32)
                self.view.pick_points = self.ref_pick_points
            else:
                self.ref_pick_points = None
                self.view.pick_points = None
        except Exception:
            self.ref_pick_points = None
            self.view.pick_points = None

        # Auto-localize only when stable map is ready; otherwise avoid error popups.
        if self.worker_mode == "LOCALIZE_ONLY":
            self.client.start_localize_only()
            self.set_status("Ref shown. Localize-only running...", "#ffcc00")
        elif self._workflow_requires_ref_map():
            if self.require_start_pick and not self.start_pick_ready:
                self._set_approx_pick_mode(True)
                if self._pick_help_shown_for_ref != str(filename):
                    self._pick_help_shown_for_ref = str(filename)
                    QMessageBox.information(
                        self,
                        "Set Start Location",
                        "Reference map loaded.\n\nClick CLICK START POINTER, then click your current robot location on the map, then press START.",
                    )
                self.set_status("Ref shown. Click start pointer, then click map to set start location.", "#ffcc00")
            else:
                self.set_status("Ref shown. Click CONNECT then START (auto localize).", "#ffcc00")
        elif int(self.current_stable_points) >= int(self.min_stable_points):
            self.client.localize_now()
            self.set_status("Ref shown. Localizing...", "#ffcc00")
        else:
            self.set_status(
                f"Ref shown. Need stable points {self.current_stable_points}/{self.min_stable_points} before localize.",
                "#ffcc00",
            )
        self.lbl_loc.setText(
            f"Localization: ref={Path(filename).name} ({len(pts)} pts, {self.ref_color_mode})"
        )
        self._refresh_pick_status()

    def on_localize(self):
        if not self.ref_map_path:
            self.show_error_popup("Load a reference map first.")
            return
        if not self._workflow_requires_ref_map() and self.worker_mode != "LOCALIZE_ONLY":
            if not self.ensure_min_stable_synced(show_error=True, set_status_msg=False):
                return
        self.client.localize_now()
        self.set_status("Localization forced...", "#ffcc00")

    def _refresh_pick_status(self):
        if self.start_pick_ready and isinstance(self.start_pick_xyz, tuple) and len(self.start_pick_xyz) == 3:
            x, y, z = self.start_pick_xyz
            self.lbl_pick_status.setText(
                f"Start pick: set ({self._fmt_coord(x)}, {self._fmt_coord(y)}, {self._fmt_coord(z)})"
            )
            self.lbl_pick_status.setStyleSheet("color:#00ff88; font-family:monospace; font-weight:800;")
        else:
            txt = "Start pick: optional (not required)" if not self.require_start_pick else "Start pick: pending"
            self.lbl_pick_status.setText(txt)
            self.lbl_pick_status.setStyleSheet("color:#ffcc00; font-family:monospace; font-weight:800;")

    @staticmethod
    def _fmt_coord(v: float, ndigits: int = 3) -> str:
        vv = float(v)
        if abs(vv) < (10 ** (-(ndigits + 1))):
            vv = 0.0
        return f"{vv:.{int(ndigits)}f}"

    def _hide_start_pointer_visual(self):
        self.start_ptr_scatter.setData(
            pos=np.zeros((1, 3), dtype=np.float32),
            color=np.zeros((1, 4), dtype=np.float32),
            size=1.0,
        )

    def _set_start_pointer_visual(self, x: float, y: float, z: float):
        pos = np.asarray([[float(x), float(y), float(z)]], dtype=np.float32)
        col = np.asarray([[1.00, 0.82, 0.08, 0.96]], dtype=np.float32)
        self.start_ptr_scatter.setData(pos=pos, color=col, size=19.0)

    def _snap_pick_to_ref(self, x: float, y: float, z: float) -> tuple[float, float, float]:
        pts = self.ref_pick_points
        if pts is None:
            return float(x), float(y), float(z)
        arr = np.asarray(pts, dtype=np.float32)
        if arr.ndim != 2 or arr.shape[1] != 3 or len(arr) == 0:
            return float(x), float(y), float(z)
        q = np.asarray([float(x), float(y)], dtype=np.float32)
        rel = arr[:, :2] - q.reshape((1, 2))
        d2 = np.einsum("ij,ij->i", rel, rel)
        idx = int(np.argmin(d2))
        if float(d2[idx]) <= float(self.ref_pick_snap_radius_m) * float(self.ref_pick_snap_radius_m):
            p = arr[idx]
            return float(p[0]), float(p[1]), float(p[2])
        return float(x), float(y), float(z)

    def on_toggle_require_start_pick(self, state: int):
        self.require_start_pick = bool(state)
        if not self.require_start_pick:
            self._set_approx_pick_mode(False)
        elif self._workflow_requires_ref_map() and self.ref_map_path and (not self.start_pick_ready):
            self._set_approx_pick_mode(True)
        self._refresh_pick_status()
        self.set_status(
            "Start-point pick required." if self.require_start_pick else "Start-point pick optional.",
            "#ffcc00",
        )

    def _prompt_pick_before_start_if_needed(self):
        if (not self._workflow_requires_ref_map()) or (not self.require_start_pick):
            return
        if self.start_pick_ready:
            return
        self._set_approx_pick_mode(True)
        QMessageBox.information(
            self,
            "Set Start Location",
            "Before START, click CLICK START POINTER, then click the reference map at robot current location.",
        )

    def _set_approx_pick_mode(self, armed: bool):
        self.approx_pick_armed = bool(armed)
        self.view.pick_mode = bool(armed)
        if bool(armed):
            self.btn_pick_approx.setStyleSheet(pill_button("#0f8f4b"))
            self.btn_pick_approx.setText("🧭 CLICK START POINTER (ON)")
            self.set_status("Click on map to set start pointer. Click again to move it. Press button to stop.", "#ffcc00")
        else:
            self.btn_pick_approx.setStyleSheet(pill_button("#2b2b2b"))
            self.btn_pick_approx.setText("🧭 CLICK START POINTER")

    def on_set_approx_pick(self):
        if not self.ref_map_path:
            self.show_error_popup("Load a reference map first.")
            return
        if self.approx_pick_armed:
            self._set_approx_pick_mode(False)
            self.set_status("Start pointer pick disabled.", "#ffcc00")
            return
        self.view.pick_plane_z = float(self.approx_guess_z)
        self._set_approx_pick_mode(True)
        if self.start_pick_ready and isinstance(self.start_pick_xyz, tuple) and len(self.start_pick_xyz) == 3:
            sx, sy, _ = self.start_pick_xyz
            self._set_start_pointer_visual(float(sx), float(sy), float(self.approx_guess_z))

    def on_clear_approx(self):
        self._set_approx_pick_mode(False)
        self.start_pick_ready = False
        self.start_pick_xyz = None
        try:
            self.client.clear_approx_pose()
        except Exception:
            pass
        self._refresh_pick_status()
        self._hide_start_pointer_visual()
        self.set_status("Approximate position cleared.", "#ffcc00")

    def on_view_map_pick_drag(self, x: float, y: float, z: float, final: bool):
        if not self.approx_pick_armed:
            return
        if not self.ref_map_path:
            return
        z_guess = float(z) if np.isfinite(float(z)) else float(self.approx_guess_z)
        sx, sy, sz = self._snap_pick_to_ref(float(x), float(y), float(z_guess))
        self.approx_guess_z = float(sz)
        self.txt_approx_z.setText(f"{self.approx_guess_z:.2f}")
        self._set_start_pointer_visual(float(sx), float(sy), float(sz))
        self.start_pick_ready = True
        self.start_pick_xyz = (float(sx), float(sy), float(sz))
        self._refresh_pick_status()
        if not bool(final):
            return
        try:
            self.client.set_approx_pose(float(sx), float(sy), float(sz))
            self.set_status(
                f"Start pointer set at ({self._fmt_coord(sx)}, {self._fmt_coord(sy)}, {self._fmt_coord(sz)}). Click map again to adjust.",
                "#ffcc00",
            )
        except Exception:
            self.show_error_popup("Failed to send approximate position to worker.")

    def on_clear_ref(self):
        if self._ref_thread is not None and self._ref_thread.isRunning():
            self._ref_thread.quit()
            self._ref_thread.wait(200)
        self.ref_map_path = None
        self.ref_pick_points = None
        self.view.pick_points = None
        self._set_approx_pick_mode(False)
        self.start_pick_ready = False
        self.start_pick_xyz = None
        self._pick_help_shown_for_ref = None
        self.client.clear_ref()
        try:
            self.client.clear_approx_pose()
        except Exception:
            pass
        self.ref_scatter.setData(
            pos=np.zeros((1, 3), dtype=np.float32),
            color=np.zeros((1, 4), dtype=np.float32),
            size=1.0,
        )
        self.ref_match_scatter.setData(
            pos=np.zeros((1, 3), dtype=np.float32),
            color=np.zeros((1, 4), dtype=np.float32),
            size=1.0,
        )
        self.lbl_loc.setText("Localization: NO_REF")
        self._refresh_pick_status()
        self.lbl_loc_match.setText("Match: n/a")
        self.set_status("Ref cleared.", "white")

    def update_loop(self):
        # handle status msgs
        try:
            while True:
                level, msg = self.client.status_q.get_nowait()
                if level == "ERROR":
                    self.set_status(f"ERR: {msg}", "#ff4444")
                    if "CONNECT" in str(msg).upper() or "LIVOX" in str(msg).upper():
                        self.is_connected = False
                    self.show_error_popup(str(msg))
                elif level == "WARN":
                    self.set_status(f"WARN: {msg}", "#ffcc00")
                elif level == "INFO" and isinstance(msg, str):
                    up = msg.upper()
                    if "CONNECTED TO LIVOX" in up:
                        self.is_connected = True
                        self.set_status("Status: CONNECTED", self.cfg["gui"]["colors"]["accent"])
                    elif "MAPPING STARTED" in up:
                        self.set_status("Status: MAPPING", self.cfg["gui"]["colors"]["accent"])
                    elif "LOCALIZE_ONLY STARTED" in up:
                        self.set_status("Status: LOCALIZE_ONLY", "#ffcc00")
                    elif "MAPPING PAUSED" in up:
                        self.set_status("Status: PAUSED", "#ffcc00")
                    elif "MAPPING STOPPED" in up:
                        self.set_status("Status: STOPPED", "white")
                    elif "SAVED:" in up:
                        saved_path = str(msg).split("SAVED:", 1)[1].strip() if "SAVED:" in str(msg) else ""
                        if saved_path:
                            self.set_status(f"Status: MAP SAVED ({Path(saved_path).name})", self.cfg["gui"]["colors"]["accent"])
                            self.show_saved_popup(f"Map saved:\n{saved_path}")
                        else:
                            self.set_status("Status: MAP SAVED", self.cfg["gui"]["colors"]["accent"])
                elif isinstance(msg, dict) and "NAV_EXPORTED" in msg:
                    nav = msg.get("NAV_EXPORTED", {})
                    pgm = str(nav.get("pgm", ""))
                    self.set_status("Status: NAV EXPORTED", self.cfg["gui"]["colors"]["accent"])
                    if pgm:
                        QMessageBox.information(self, "Navigation Export", f"Saved navigation files:\n{pgm}")
                elif isinstance(msg, dict) and "SESSION" in msg:
                    sess = msg.get("SESSION", {})
                    if bool(sess.get("prior_loaded", False)):
                        ref = str(sess.get("ref", "reference"))
                        self.set_status(f"Session prior loaded: {ref}", "#ffcc00")
                elif isinstance(msg, dict) and "SELF_CHECK" in msg:
                    rep = msg.get("SELF_CHECK", {})
                    src = str(msg.get("source", "worker"))
                    self.show_self_check_popup(rep if isinstance(rep, dict) else {}, source=src)
                elif isinstance(msg, dict) and "LOOP" in msg:
                    loop = msg.get("LOOP", {})
                    if bool(loop.get("optimized", False)):
                        self.set_status("Status: LOOP CLOSED", self.cfg["gui"]["colors"]["accent"])
                    rej = loop.get("rejected", {})
                    if isinstance(rej, dict) and rej:
                        tr = float(rej.get("translation_m", 0.0))
                        ang = float(rej.get("rotation_deg", 0.0))
                        self.set_status(f"Loop reject safe guard ({tr:.2f}m, {ang:.1f}deg)", "#ffcc00")
                elif isinstance(msg, dict) and "LOOP_MODE" in msg:
                    info = msg.get("LOOP_MODE", {})
                    enabled = bool(info.get("enabled", self.loop_closure_enabled))
                    self.loop_closure_enabled = enabled
                    self.chk_loop_closure.blockSignals(True)
                    self.chk_loop_closure.setChecked(enabled)
                    self.chk_loop_closure.blockSignals(False)
                elif isinstance(msg, dict) and "LOC_MACHINE" in msg:
                    info = msg.get("LOC_MACHINE", {})
                    enabled = bool(info.get("enabled", self.loc_machine_enabled))
                    self.loc_machine_enabled = enabled
                    self.chk_loc_machine.blockSignals(True)
                    self.chk_loc_machine.setChecked(enabled)
                    self.chk_loc_machine.blockSignals(False)
                elif isinstance(msg, dict) and "SUBMAP_MODE" in msg:
                    info = msg.get("SUBMAP_MODE", {})
                    enabled = bool(info.get("enabled", self.submap_mode_enabled))
                    self.submap_mode_enabled = enabled
                    self.chk_submap_mode.blockSignals(True)
                    self.chk_submap_mode.setChecked(enabled)
                    self.chk_submap_mode.blockSignals(False)
                elif isinstance(msg, dict) and "LOC_STATE" in msg:
                    loc_info = msg.get("LOC_STATE", {})
                    state = str(loc_info.get("state", "TRACKING")).upper().strip()
                    enabled = bool(loc_info.get("enabled", self.loc_machine_enabled))
                    self.loc_machine_enabled = enabled
                    self.chk_loc_machine.blockSignals(True)
                    self.chk_loc_machine.setChecked(enabled)
                    self.chk_loc_machine.blockSignals(False)
                    self.lbl_loc_state.setText(f"Loc health: {state}")
                    if state == "LOST":
                        self.set_status("Status: LOST (RECOVERING)", "#ff4444")
                    elif state == "RECOVERY":
                        self.set_status("Status: RECOVERY (AUTO RELOCALIZE)", "#ffcc00")
                    elif state == "DEGRADED":
                        self.set_status("Status: DEGRADED", "#ffcc00")
                elif isinstance(msg, dict) and "DENSITY" in msg:
                    den = msg.get("DENSITY", {})
                    mode = str(den.get("mode", self.density_mode)).upper().strip()
                    stride = int(den.get("stride", 1))
                    self.density_mode = mode if mode in ("LOW", "MEDIUM", "HIGH") else self.density_mode
                    self.refresh_density_buttons()
                    self.lbl_density.setText(f"Density: {self.density_mode} (stride {stride})")
                elif isinstance(msg, dict) and "MIN_STABLE_POINTS" in msg:
                    info = msg.get("MIN_STABLE_POINTS", {})
                    try:
                        value = int(info.get("value", self.min_stable_points))
                    except Exception:
                        value = self.min_stable_points
                    self.min_stable_points = value
                    self.txt_min_stable.setText(str(value))
                    self.lbl_min_stable.setText(f"Min stable: {value}")
                elif isinstance(msg, dict) and "AUTOSAVE" in msg:
                    info = msg.get("AUTOSAVE", {})
                    self.autosave_enabled = bool(info.get("enabled", self.autosave_enabled))
                    self.autosave_interval_sec = float(info.get("interval_sec", self.autosave_interval_sec))
                    self.chk_autosave.blockSignals(True)
                    self.chk_autosave.setChecked(self.autosave_enabled)
                    self.chk_autosave.blockSignals(False)
                    self.txt_autosave_sec.setText(str(int(self.autosave_interval_sec)))
                elif isinstance(msg, dict) and "AUTOSAVED" in msg:
                    auto = msg.get("AUTOSAVED", {})
                    path = str(auto.get("path", ""))
                    pts = int(auto.get("points", 0))
                    if path:
                        self.set_status(f"Autosaved: {Path(path).name} ({pts} pts)", self.cfg["gui"]["colors"]["accent"])
                elif isinstance(msg, dict) and "MODE" in msg:
                    info = msg.get("MODE", {})
                    self.worker_mode = str(info.get("mode", self.worker_mode)).upper().strip()
                    self.lbl_mode.setText(
                        f"Mode: {self.worker_mode}  Rec: {'ON' if self.recording_enabled else 'OFF'}"
                    )
                elif isinstance(msg, dict) and "RECORDING" in msg:
                    info = msg.get("RECORDING", {})
                    self.recording_enabled = bool(info.get("enabled", self.recording_enabled))
                    self.recording_frames = int(info.get("frames", self.recording_frames))
                    self.recording_dropped = int(info.get("dropped", self.recording_dropped))
                    self.lbl_mode.setText(
                        f"Mode: {self.worker_mode}  Rec: {'ON' if self.recording_enabled else 'OFF'}"
                    )
                elif isinstance(msg, dict) and "RECORD_SAVED" in msg:
                    rec = msg.get("RECORD_SAVED", {})
                    path = str(rec.get("path", ""))
                    frames = int(rec.get("frames", 0))
                    dropped = int(rec.get("dropped", 0))
                    if path:
                        self.set_status(f"Recording saved ({frames} frames)", self.cfg["gui"]["colors"]["accent"])
                        QMessageBox.information(
                            self,
                            "Recording Saved",
                            f"Saved replay file:\n{path}\n\nFrames: {frames}\nDropped: {dropped}",
                        )
                elif isinstance(msg, dict) and "NAV_CONFIG" in msg:
                    info = msg.get("NAV_CONFIG", {})
                    self.txt_nav_zmin.setText(str(info.get("z_min_m", self.txt_nav_zmin.text())))
                    self.txt_nav_zmax.setText(str(info.get("z_max_m", self.txt_nav_zmax.text())))
                    self.txt_nav_res.setText(str(info.get("resolution_m", self.txt_nav_res.text())))
                    self.txt_nav_inf.setText(str(info.get("inflation_radius_m", self.txt_nav_inf.text())))
                elif isinstance(msg, dict) and "NAV_GOAL" in msg:
                    info = msg.get("NAV_GOAL", None)
                    if isinstance(info, dict):
                        self.set_status(
                            f"Nav goal set: ({float(info.get('x', 0.0)):.2f}, {float(info.get('y', 0.0)):.2f})",
                            "#ffcc00",
                        )
                    else:
                        self.set_status("Nav goal cleared.", "white")
                elif isinstance(msg, dict) and "MISSION" in msg:
                    info = msg.get("MISSION", {})
                    if bool(info.get("completed", False)):
                        self.set_status("Mission completed.", self.cfg["gui"]["colors"]["accent"])
                elif isinstance(msg, dict) and "MAP_QUALITY" in msg:
                    info = msg.get("MAP_QUALITY", {})
                    self.txt_near_range.setText(str(info.get("near_min_range_m", self.txt_near_range.text())))
                    self.chk_outlier.setChecked(bool(info.get("outlier_filter_enabled", self.chk_outlier.isChecked())))
                    self.chk_clip_z.setChecked(bool(info.get("world_z_clip_enabled", self.chk_clip_z.isChecked())))
                    self.txt_world_zmin.setText(str(info.get("world_z_min_m", self.txt_world_zmin.text())))
                    self.txt_world_zmax.setText(str(info.get("world_z_max_m", self.txt_world_zmax.text())))
                    self.txt_outlier_voxel.setText(str(info.get("outlier_voxel_m", self.txt_outlier_voxel.text())))
                    self.txt_outlier_min.setText(str(info.get("outlier_min_points", self.txt_outlier_min.text())))
                elif isinstance(msg, dict) and "FILTER_TUNING" in msg:
                    info = msg.get("FILTER_TUNING", {})
                    prof = str(info.get("profile", self.workflow_profile)).upper().strip()
                    self._apply_workflow_ui_rules(prof, update_label=True)
                elif isinstance(msg, dict) and "WORKFLOW_PROFILE" in msg:
                    info = msg.get("WORKFLOW_PROFILE", {})
                    prof = str(info.get("name", self.workflow_profile)).upper().strip()
                    self._apply_workflow_ui_rules(prof, update_label=True)
                elif isinstance(msg, dict) and "LOCALIZE" in msg:
                    loc = msg["LOCALIZE"]
                    fit = float(loc.get("fitness", 0.0))
                    rmse = float(loc.get("rmse", 0.0))
                    conf = float(loc.get("confidence", 0.0))
                    self.last_loc_confidence = float(conf)
                    ok = bool(loc.get("accepted", False))
                    src = str(loc.get("source", "")).upper().strip()
                    state = str(loc.get("state", self.lbl_loc_state.text().split(":")[-1].strip())).upper().strip()
                    bi = float(loc.get("inlier_bidir", 0.0))
                    prefix = f"{src} " if src else ""
                    self.lbl_loc.setText(
                        f"Localization: {prefix}fit={fit:.3f} rmse={rmse:.3f} bi={bi:.2f} conf={conf:.2f} {'OK' if ok else 'NO'}"
                    )
                    self.lbl_loc_state.setText(f"Loc health: {state}")
                    if ok:
                        self.set_status(f"Status: LOCALIZED ({src or 'RUN'})", self.cfg["gui"]["colors"]["accent"])
                    else:
                        self.set_status(f"Status: LOCALIZE WEAK ({src or 'RUN'})", "#ffcc00")
                elif isinstance(msg, dict) and "APPROX_POSE" in msg:
                    info = msg.get("APPROX_POSE", {})
                    if bool(info.get("set", False)):
                        ax = float(info.get("x", 0.0))
                        ay = float(info.get("y", 0.0))
                        az = float(info.get("z", self.approx_guess_z))
                        self.approx_guess_z = az
                        self.txt_approx_z.setText(f"{az:.2f}")
                        self.start_pick_ready = True
                        self.start_pick_xyz = (ax, ay, az)
                        self._set_start_pointer_visual(ax, ay, az)
                        self._refresh_pick_status()
                        self.set_status(
                            f"Approx pose armed at ({self._fmt_coord(ax)}, {self._fmt_coord(ay)}, {self._fmt_coord(az)})",
                            "#ffcc00",
                        )
                    else:
                        self.start_pick_ready = False
                        self.start_pick_xyz = None
                        self._hide_start_pointer_visual()
                        self._refresh_pick_status()
                        self.set_status("Approx pose cleared.", "#ffcc00")
        except Exception:
            pass

        if self.client.proc is not None and not self.client.proc.is_alive():
            if not self._worker_dead_notified:
                self._worker_dead_notified = True
                code = self.client.proc.exitcode
                code_txt = "unknown" if code is None else str(code)
                self.worker_mode = "DEAD"
                self.is_connected = False
                self.lbl_mode.setText(f"Mode: {self.worker_mode}  Rec: OFF")
                self.set_status(f"ERR: Worker stopped (exit {code_txt})", "#ff4444")
                if int(code or 0) == -11:
                    # Native crash fallback: keep advanced loop closure off on next restart.
                    was_loop_on = bool(self.loop_closure_enabled)
                    self.loop_closure_enabled = False
                    self.chk_loop_closure.blockSignals(True)
                    self.chk_loop_closure.setChecked(False)
                    self.chk_loop_closure.blockSignals(False)
                    extra = "Advanced loop closure was disabled for safety.\n" if was_loop_on else ""
                    self.show_error_popup(
                        "SLAM worker process stopped (exit code -11).\n"
                        "Native ICP/localization path crashed.\n"
                        f"{extra}"
                        "Click CONNECT to restart."
                    )
                else:
                    self.show_error_popup(f"SLAM worker process stopped (exit code {code_txt}). Click CONNECT to restart.")
        else:
            self._worker_dead_notified = False

        latest = None
        try:
            while True:
                latest = self.client.data_q.get_nowait()
        except Exception:
            pass
        if not latest:
            return

        tag, payload = latest
        if tag != "FRAME":
            return

        pts = payload.get("stable_points", None)
        cols = payload.get("stable_colors", None)
        pose = payload.get("pose", None)
        perf = payload.get("perf", {}) if isinstance(payload, dict) else {}
        map_lock = payload.get("map_lock", {}) if isinstance(payload, dict) else {}
        continuity = payload.get("continuity", {}) if isinstance(payload, dict) else {}
        submap = payload.get("submap", {}) if isinstance(payload, dict) else {}
        mode = str(payload.get("mode", self.worker_mode)).upper().strip() if isinstance(payload, dict) else self.worker_mode
        nav = payload.get("nav", {}) if isinstance(payload, dict) else {}
        safety = payload.get("safety", {}) if isinstance(payload, dict) else {}
        rec = payload.get("recording", {}) if isinstance(payload, dict) else {}
        wf = str(payload.get("workflow_profile", self.workflow_profile)).upper().strip() if isinstance(payload, dict) else self.workflow_profile
        ref_match_points = payload.get("ref_match_points", None) if isinstance(payload, dict) else None
        ref_match_colors = payload.get("ref_match_colors", None) if isinstance(payload, dict) else None
        ref_match_stats = payload.get("ref_match", {}) if isinstance(payload, dict) else {}
        self.last_loc_confidence = float(payload.get("loc_confidence", self.last_loc_confidence)) if isinstance(payload, dict) else self.last_loc_confidence

        if pts is not None and len(pts) > 1:
            draw_pts = pts
            draw_cols = cols
            if len(draw_pts) > self.max_draw:
                step = max(2, len(draw_pts) // self.max_draw)
                if self.decimate_step > 1:
                    step = max(step, self.decimate_step)
                draw_pts = draw_pts[::step]
                if draw_cols is not None and len(draw_cols) == len(pts):
                    draw_cols = draw_cols[::step]

            if draw_cols is not None:
                self.scatter.setData(pos=draw_pts, color=draw_cols)
            else:
                self.scatter.setData(pos=draw_pts)

            self.lbl_points.setText(f"Stable points: {len(pts)}")
            self.current_stable_points = int(len(pts))

        if ref_match_points is not None:
            try:
                mpts = np.asarray(ref_match_points, dtype=np.float32)
                if mpts.ndim == 2 and mpts.shape[1] == 3 and len(mpts) > 1:
                    mcols = None
                    if ref_match_colors is not None:
                        mcols_arr = np.asarray(ref_match_colors, dtype=np.float32)
                        if mcols_arr.ndim == 2 and mcols_arr.shape[1] == 4 and len(mcols_arr) == len(mpts):
                            mcols = mcols_arr
                    if len(mpts) > self.max_ref_draw:
                        mstep = max(2, len(mpts) // self.max_ref_draw)
                        mpts = mpts[::mstep]
                        if mcols is not None:
                            mcols = mcols[::mstep]
                    if mcols is not None:
                        self.ref_match_scatter.setData(pos=mpts, color=mcols, size=1.5)
                    else:
                        self.ref_match_scatter.setData(pos=mpts, size=1.5)
                else:
                    self.ref_match_scatter.setData(
                        pos=np.zeros((1, 3), dtype=np.float32),
                        color=np.zeros((1, 4), dtype=np.float32),
                        size=1.0,
                    )
            except Exception:
                pass
        elif str(mode).upper().strip() != "LOCALIZE_ONLY":
            self.ref_match_scatter.setData(
                pos=np.zeros((1, 3), dtype=np.float32),
                color=np.zeros((1, 4), dtype=np.float32),
                size=1.0,
            )

        if pose is not None:
            try:
                x, y, z = float(pose[0, 3]), float(pose[1, 3]), float(pose[2, 3])
                self.lbl_pose.setText(f"Pos: {x:.2f}, {y:.2f}, {z:.2f}")
                self.axis.resetTransform()
                self.axis.translate(x, y, z)
            except Exception:
                pass

        if isinstance(perf, dict) and perf:
            try:
                in_fps = float(perf.get("input_fps", 0.0))
                out_fps = float(perf.get("publish_fps", 0.0))
                icp_ms = float(perf.get("icp_ms", 0.0))
                cpu_pct = float(perf.get("cpu_percent", 0.0))
                qlag = int(perf.get("queue_lag", 0))
                growth = float(perf.get("stable_growth_per_sec", 0.0))
                freeze_count = int(map_lock.get("freeze_count", 0)) if isinstance(map_lock, dict) else 0
                c_rej_total = int(continuity.get("reject_count", 0)) if isinstance(continuity, dict) else 0
                c_rej_streak = int(continuity.get("reject_streak", 0)) if isinstance(continuity, dict) else 0
                self.lbl_perf_fps.setText(f"FPS in/out: {in_fps:.1f} / {out_fps:.1f}")
                self.lbl_perf_icp.setText(f"ICP: {icp_ms:.1f} ms  CPU: {cpu_pct:.1f}%")
                hold_txt = f"  Hold: {freeze_count}" if freeze_count > 0 else ""
                self.lbl_perf_queue.setText(
                    f"Queue lag: {qlag}  Growth: {growth:.1f} pts/s{hold_txt}  Cont: {c_rej_streak}/{c_rej_total}"
                )
            except Exception:
                pass

        if isinstance(rec, dict):
            self.recording_enabled = bool(rec.get("enabled", self.recording_enabled))
            self.recording_frames = int(rec.get("frames", self.recording_frames))
            self.recording_dropped = int(rec.get("dropped", self.recording_dropped))

        if mode:
            self.worker_mode = mode
        if wf:
            self._apply_workflow_ui_rules(wf, update_label=True)
        mode_suffix = ""
        if isinstance(map_lock, dict) and bool(map_lock.get("freeze_active", False)):
            mode_suffix = "  [ROTATE HOLD]"
        submap_suffix = ""
        if isinstance(submap, dict):
            if bool(submap.get("enabled", False)) and bool(submap.get("active", False)):
                lg = int(submap.get("local_points", 0))
                gg = int(submap.get("global_points", 0))
                submap_suffix = f"  Submap L/G:{lg}/{gg}"
            elif bool(submap.get("enabled", False)):
                submap_suffix = "  Submap ON"
        self.lbl_mode.setText(
            f"Mode: {self.worker_mode}  Rec: {'ON' if self.recording_enabled else 'OFF'} ({self.recording_frames}){mode_suffix}{submap_suffix}"
        )

        if isinstance(nav, dict):
            goal = nav.get("goal", None)
            cmd = nav.get("cmd", {}) if isinstance(nav.get("cmd", {}), dict) else {}
            lin = float(cmd.get("linear_mps", 0.0))
            ang = float(cmd.get("angular_rps", 0.0))
            if goal and isinstance(goal, (list, tuple)) and len(goal) >= 2:
                gx = float(goal[0]); gy = float(goal[1])
                self.lbl_nav.setText(f"Nav: goal {gx:.2f},{gy:.2f}  cmd {lin:.2f}/{ang:.2f}")
            else:
                self.lbl_nav.setText(f"Nav: goal none  cmd {lin:.2f}/{ang:.2f}")

        if isinstance(ref_match_stats, dict) and ref_match_stats:
            matched_ratio = float(ref_match_stats.get("matched_ratio", self.last_loc_match_ratio))
            self.last_loc_match_ratio = matched_ratio
            matched = int(ref_match_stats.get("matched", 0))
            near = int(ref_match_stats.get("near", 0))
            unmatched = int(ref_match_stats.get("unmatched", 0))
            confirmed = int(ref_match_stats.get("confirmed", 0))
            self.lbl_loc_match.setText(
                f"Match: {matched_ratio * 100.0:.0f}%  C:{confirmed} M:{matched} N:{near} U:{unmatched}  Conf:{self.last_loc_confidence:.2f}"
            )
        elif not self._workflow_requires_ref_map():
            self.lbl_loc_match.setText("Match: n/a")

        if isinstance(safety, dict) and bool(safety.get("emergency", False)):
            reasons = safety.get("reasons", [])
            rs = ",".join(str(x) for x in reasons[:2]) if isinstance(reasons, list) else "safety"
            self.set_status(f"SAFETY STOP: {rs}", "#ff4444")

    def closeEvent(self, event):
        try:
            if self.recording_enabled:
                self.client.record_stop(True, self.txt_name.text().strip() or "slam_recording")
            # If save is armed, request one final export before shutting down worker.
            self.client.stop_mapping()
            if self.request_map_export_if_armed():
                end_t = time.monotonic() + 1.2
                while time.monotonic() < end_t:
                    QApplication.processEvents()
                    time.sleep(0.05)
            self.client.stop_process()
        except Exception:
            pass
        event.accept()


def main():
    signal.signal(signal.SIGINT, signal.SIG_DFL)
    app = QApplication(sys.argv)
    app.setStyle("Fusion")
    w = UltimateDashboard()
    w.show()
    sys.exit(app.exec())


if __name__ == "__main__":
    main()