#!/usr/bin/env python3
"""
control_panel_node

description in progress
"""

import json
import math
import os
import signal
import subprocess
import sys
import threading
import time

import rclpy
from rclpy.node import Node
from rclpy.qos import DurabilityPolicy, HistoryPolicy, QoSProfile, ReliabilityPolicy
from rclpy.time import Time
from std_msgs.msg import Bool, Empty, String

BUTTONS = ("start", "stop", "reset")
STOP_UNIT_DEFAULT = "apollo-project.service"
SYSTEMCTL = "/usr/bin/systemctl"

# exit codes — non-zero makes the launch shut the whole run down (OnProcessExit handler)
EXIT_NO_DIO = 2          # button/LED topics never appeared: panel not in params or dio failed
EXIT_STOP_FAILED = 3     # sudo systemctl stop was refused: sudoers rule missing


RESET_ACTIONS = ("none", "nav_return_to_start")

# how closely the return has to match the start heading before it counts as "reached".
# nav itself is heading-agnostic (yaw_goal_tolerance is pi in the route params and the BT
# ends the goal when the route completes), so the robot arrives at the start XY pointing
# wherever the last edge left it. that matters: the program's set_initial_pose block then
# re-declares the start heading on the next Start, and if the robot is not actually facing
# that way AMCL is forced wrong and the next drive goes into the furniture.
RESET_YAW_TOL_RAD = 0.035         # ~2 degrees, same as the navigate_to block's alignment
RESET_SPIN_TIME_S = 20.0
RESET_SPIN_PASSES = 3             # measure, spin, re-measure, spin the residual, report
# before every heading read: wait for the robot to be still (odom heading steady, capped), then
# for fresh amcl estimates at rest. a fixed 0.4 s wait read a half-updated heading under load
# (.162, 2026-10-01: read +11, spun 11 deg, read +14), the same fault as the navigate_to block
RESET_STILL_S = 0.5
RESET_STILL_MAX_S = 3.0
RESET_AMCL_UPDATES = 2
RESET_AMCL_WAIT_S = 1.5


def _yaw_of(q):
    return math.atan2(2.0 * (q.w * q.z + q.x * q.y), 1.0 - 2.0 * (q.y * q.y + q.z * q.z))


def _wrap(a):
    return (a + math.pi) % (2.0 * math.pi) - math.pi


class NavReturnToStart:
    """Reset action; cancel whatever nav is doing, then drive back to the start pose.

    The start pose is where the robot was when Start was pressed; if the program then
    publishes its own /initialpose (the set_initial_pose block does), that wins, because it
    is what the program declared the start to be. Before any Start, the first localized pose
    after boot is used, so Reset also works in runs without a user program.

    Two steps: NavigateToPose to the start XY, then the behavior server's spin action to
    the start heading (nav does not do headings, see RESET_YAW_TOL_RAD).

    eveything is async so buttons work while completing task
    """

    def __init__(self, node, timeout_s):
        self.node = node
        self.timeout_s = timeout_s
        self.available = False
        try:
            from action_msgs.msg import GoalStatus
            from action_msgs.srv import CancelGoal
            from geometry_msgs.msg import PoseStamped, PoseWithCovarianceStamped, Twist
            from nav2_msgs.action import NavigateToPose, Spin
            from rclpy.action import ActionClient
            from rclpy.duration import Duration
            from rclpy.executors import SingleThreadedExecutor
            from std_srvs.srv import Empty as EmptySrv
            from tf2_ros import Buffer, TransformListener
        except ImportError as exc:  # robot without nav2: the action is simply unavailable
            node.get_logger().warn(f"[control_panel] nav reset unavailable: {exc}")
            return
        self._GoalStatus = GoalStatus
        self._CancelGoal = CancelGoal
        self._PoseStamped = PoseStamped
        self._Twist = Twist
        self._NavigateToPose = NavigateToPose
        self._Spin = Spin
        self._Duration = Duration

        self._client = ActionClient(node, NavigateToPose, "navigate_to_pose")
        self._spin_client = ActionClient(node, Spin, "spin")
        self._cancel_client = node.create_client(CancelGoal, "/navigate_to_pose/_action/cancel_goal")
        self._cmd_vel_pub = node.create_publisher(Twist, "cmd_vel", 10)
        self._amcl_sub = node.create_subscription(PoseWithCovarianceStamped, "/amcl_pose", self._on_amcl, 10)
        self._initialpose_sub = node.create_subscription(PoseWithCovarianceStamped, "/initialpose",
                                                         self._on_initialpose, 10)
        # map -> base_footprint straight from TF for the final heading check; /amcl_pose
        # only updates when the robot has moved enough, so it can be a little stale.
        # the listener gets its own node and thread: on this node's single spin loop it fell
        # seconds behind /tf under load, so the reset measured stale headings
        self._tf_buffer = Buffer()
        self._tf_node = rclpy.create_node(f"{node.get_name()}_tf_listener")
        self._tf_listener = TransformListener(self._tf_buffer, self._tf_node, spin_thread=False)
        self._tf_exec = SingleThreadedExecutor()
        self._tf_exec.add_node(self._tf_node)
        threading.Thread(target=self._tf_spin, daemon=True).start()
        self._EmptySrv = EmptySrv
        self._nomotion_client = node.create_client(EmptySrv, "request_nomotion_update")
        self._amcl_count = 0

        self._boot_pose = None       # first localized pose after boot
        self._last_amcl = None       # most recent /amcl_pose, fallback when TF is unavailable
        self._start_pose = None      # pose captured for the current run of the program
        self._capture_next = False   # take the next /amcl_pose as the start pose
        self._done_cb = None
        self._goal_handle = None
        self._spin_handle = None
        self._spin_passes = 0
        self._zero_timer = None
        self._zero_ticks = 0
        self._settle_timer = None
        self._deadline_timer = None
        self.available = True

    def _tf_spin(self):
        try:
            self._tf_exec.spin()
        except Exception:
            pass   # rclpy shutting down

    # ---- start pose ------------------------------------------------------------------
    def _on_amcl(self, msg):
        self._amcl_count += 1
        self._last_amcl = msg
        if self._boot_pose is None:
            self._boot_pose = msg
        if self._capture_next:
            self._capture_next = False
            self._start_pose = msg

    def _on_initialpose(self, msg):
        # the program (or the operator) put where start was so use that
        self._start_pose = msg
        self._capture_next = False

    def note_started(self):
        self._start_pose = None
        self._capture_next = True

    def start_pose(self):
        return self._start_pose or self._boot_pose

    # ---- the action ------------------------------------------------
    def begin(self, done_cb):
        if not self.available:
            done_cb(False, "nav2 not available on this robot")
            return
        if self._done_cb is not None:
            done_cb(False, "already returning to start")
            return
        self._done_cb = done_cb
        self._cancel_all_goals()
        # zero the velocity command a few times over half a second so the drive settles
        # before the return goal is sent; the interrupted program may have left it non-zero
        self._zero_ticks = 5
        self._zero_timer = self.node.create_timer(0.1, self._zero_tick)

    def _cancel_all_goals(self):
        if not self._cancel_client.service_is_ready():
            return
        try:
            # an all-zero goal id + zero stamp means "cancel every goal" for this action server
            self._cancel_client.call_async(self._CancelGoal.Request())
        except Exception as exc:
            self.node.get_logger().warn(f"[control_panel] nav reset: cancel failed: {exc}")

    def _zero_tick(self):
        try:
            self._cmd_vel_pub.publish(self._Twist())
            self._zero_ticks -= 1
            if self._zero_ticks > 0:
                return
            self._zero_timer.cancel()
            self._zero_timer = None
            self._send_goal()
        except Exception as exc:
            self._finish(False, f"error while stopping: {exc}")

    def _send_goal(self):
        if not self._client.server_is_ready():
            # a run without navigation (touch, plain drive program): there is nowhere to
            # return to, so Reset just means "stop the program and re-arm", which has
            # already happened by now. not a failure.
            self._finish(True, "no navigation in this run — program stopped, re-armed")
            return
        pose = self.start_pose()
        if pose is None:
            self._finish(False, "no start pose known — no /amcl_pose received yet")
            return
        goal = self._NavigateToPose.Goal()
        goal.pose = self._PoseStamped()
        goal.pose.header.frame_id = "map"
        goal.pose.header.stamp = self.node.get_clock().now().to_msg()
        goal.pose.pose = pose.pose.pose
        p = goal.pose.pose.position
        self.node.get_logger().info(f"[control_panel] nav reset: returning to start x={p.x:.2f} y={p.y:.2f}")
        self._client.send_goal_async(goal).add_done_callback(self._on_goal_response)
        self._deadline_timer = self.node.create_timer(self.timeout_s, self._on_deadline)

    def _on_goal_response(self, future):
        try:
            handle = future.result()
            if not handle.accepted:
                self._finish(False, "navigation rejected the return goal")
                return
            self._goal_handle = handle
            handle.get_result_async().add_done_callback(self._on_result)
        except Exception as exc:
            self._finish(False, f"goal error: {exc}")

    def _on_result(self, future):
        try:
            self._goal_handle = None
            status = future.result().status
            if status != self._GoalStatus.STATUS_SUCCEEDED:
                self._finish(False, f"navigation ended with status={status}")
                return
            if self._done_cb is None:      # cancelled while the result was in flight
                return
            self._spin_passes = 0
            self._settle_then(self._align_heading)
        except Exception as exc:
            self._finish(False, f"result error: {exc}")

    def _settle_then(self, fn):
        """Run fn once the robot is still and AMCL has re-fitted at rest. The heading is read
        from AMCL, which only corrects while the robot moves and under load trails a spin, so
        a reading taken the instant motion stops can be stale. Polled every 0.1 s: wait for
        the odom heading to hold for RESET_STILL_S (capped at RESET_STILL_MAX_S), then ask
        AMCL for RESET_AMCL_UPDATES no-motion updates, waiting for each fresh /amcl_pose
        (capped at RESET_AMCL_WAIT_S), then one more tick for the new map->odom."""
        st = {"t0": time.monotonic(), "last": None, "still": 0.0, "phase": "still",
              "asked": 0, "count": 0, "t1": 0.0}

        def _ask():
            st["asked"] += 1
            st["count"], st["t1"] = self._amcl_count, time.monotonic()
            if self._nomotion_client.service_is_ready():
                self._nomotion_client.call_async(self._EmptySrv.Request())

        def _tick():
            now = time.monotonic()
            if st["phase"] == "still":
                yaw = self._odom_yaw()
                if yaw is not None and st["last"] is not None and abs(_wrap(yaw - st["last"])) < math.radians(0.2):
                    st["still"] += 0.1
                else:
                    st["still"] = 0.0
                st["last"] = yaw
                if yaw is None or st["still"] >= RESET_STILL_S or now - st["t0"] > RESET_STILL_MAX_S:
                    st["phase"] = "amcl"
                    _ask()
                return
            if st["phase"] == "amcl":
                if self._amcl_count == st["count"] and now - st["t1"] < RESET_AMCL_WAIT_S:
                    return
                if st["asked"] < RESET_AMCL_UPDATES:
                    _ask()
                    return
                st["phase"] = "done"
                return
            self._settle_timer.cancel()
            self._settle_timer = None
            if self._done_cb is not None:
                fn()
        self._settle_timer = self.node.create_timer(0.1, _tick)

    def _odom_yaw(self):
        try:
            t = self._tf_buffer.lookup_transform("odom", "base_footprint", Time())
            return _yaw_of(t.transform.rotation)
        except Exception:
            return None

    # ---- heading ---------------------------------------------------------------------
    def _current_pose(self):
        """(x, y, yaw) of base_footprint in map, from TF; falls back to the last /amcl_pose."""
        try:
            t = self._tf_buffer.lookup_transform("map", "base_footprint", Time())
            tr, q = t.transform.translation, t.transform.rotation
            return tr.x, tr.y, _yaw_of(q)
        except Exception:
            pass
        last = self._last_amcl
        if last is None:
            return None
        p = last.pose.pose
        return p.position.x, p.position.y, _yaw_of(p.orientation)

    def _align_heading(self):
        start = self.start_pose().pose.pose
        cur = self._current_pose()
        if cur is None:
            self._finish(True, "reached start (heading unknown — no pose available)")
            return
        x, y, yaw = cur
        dist = math.hypot(start.position.x - x, start.position.y - y)
        diff = _wrap(_yaw_of(start.orientation) - yaw)
        where = f"{dist:.2f} m, {math.degrees(diff):+.0f}° from start"
        if abs(diff) <= RESET_YAW_TOL_RAD:
            self._finish(True, f"reached start ({where})")
            return
        if not self._spin_client.server_is_ready():
            self._finish(True, f"reached start XY, spin unavailable ({where})")
            return
        self.node.get_logger().info(f"[control_panel] nav reset: at {where}, turning to start heading")
        goal = self._Spin.Goal()
        goal.target_yaw = float(diff)
        goal.time_allowance = self._Duration(seconds=RESET_SPIN_TIME_S).to_msg()
        self._spin_client.send_goal_async(goal).add_done_callback(self._on_spin_response)

    def _on_spin_response(self, future):
        try:
            handle = future.result()
            if not handle.accepted:
                self._finish(True, "reached start XY, spin rejected")
                return
            self._spin_handle = handle
            handle.get_result_async().add_done_callback(self._on_spin_result)
        except Exception as exc:
            self._finish(False, f"spin error: {exc}")

    def _on_spin_result(self, future):
        try:
            self._spin_handle = None
            status = future.result().status
            if status != self._GoalStatus.STATUS_SUCCEEDED:
                cur = self._current_pose()
                where = "pose unknown"
                if cur is not None:
                    start = self.start_pose().pose.pose
                    diff = _wrap(_yaw_of(start.orientation) - cur[2])
                    where = f"{math.degrees(diff):+.0f}° from start"
                self._finish(False, f"spin ended with status={status} ({where})")
                return
            self._spin_passes += 1
            if self._spin_passes < RESET_SPIN_PASSES:
                # re-measure once the estimate has caught up; _align_heading finishes if
                # the residual is inside tolerance, otherwise spins the remainder
                self._settle_then(self._align_heading)
                return
            self._settle_then(self._report_final)
        except Exception as exc:
            self._finish(False, f"spin result error: {exc}")

    def _report_final(self):
        cur = self._current_pose()
        if cur is None:
            self._finish(True, "reached start (pose unknown)")
            return
        start = self.start_pose().pose.pose
        dist = math.hypot(start.position.x - cur[0], start.position.y - cur[1])
        diff = _wrap(_yaw_of(start.orientation) - cur[2])
        self._finish(True, f"reached start ({dist:.2f} m, {math.degrees(diff):+.0f}° from start)")

    def _on_deadline(self):
        self.cancel()
        self._finish(False, f"timed out after {self.timeout_s:.0f}s")

    def cancel(self):
        """Abort a return in progress (Stop pressed, or the deadline hit)."""
        for timer_name in ("_zero_timer", "_settle_timer", "_deadline_timer"):
            timer = getattr(self, timer_name)
            if timer is not None:
                timer.cancel()
                setattr(self, timer_name, None)
        for handle_name in ("_goal_handle", "_spin_handle"):
            handle = getattr(self, handle_name)
            setattr(self, handle_name, None)
            if handle is not None:
                try:
                    handle.cancel_goal_async()
                except Exception:
                    pass

    def _finish(self, ok, detail):
        cb = self._done_cb
        if cb is None:
            return
        self.cancel()
        self._done_cb = None
        cb(ok, detail)


class ControlPanelNode(Node):
    def __init__(self):
        super().__init__("control_panel")

        self.declare_parameter("user_code", "")
        # json list: the command that runs user_code as the student user (intermediate/advanced).
        # a string, not a list, because an empty list parameter has no type and rclpy rejects it
        self.declare_parameter("user_command_json", "")
        self.declare_parameter("project_dir", "")
        self.declare_parameter("start_timeout_s", 60.0)
        self.declare_parameter("blink_hz", 2.0)
        self.declare_parameter("press_lockout_ms", 300)
        self.declare_parameter("dio_ready_timeout_s", 20.0)
        self.declare_parameter("reset_action", "none")
        self.declare_parameter("reset_nav_timeout_s", 600.0)   # a 25 m return with reroutes is easily 3 min

        self.user_code = self.get_parameter("user_code").value or ""
        try:
            self.user_command = json.loads(self.get_parameter("user_command_json").value or "[]") or []
        except ValueError:
            self.user_command = []
        self.project_dir = self.get_parameter("project_dir").value or ""
        self.start_timeout_s = float(self.get_parameter("start_timeout_s").value)
        self.blink_hz = max(0.1, float(self.get_parameter("blink_hz").value))
        self.press_lockout_ns = int(max(0, self.get_parameter("press_lockout_ms").value) * 1_000_000)
        self.dio_ready_timeout_s = float(self.get_parameter("dio_ready_timeout_s").value)
        self.reset_action = str(self.get_parameter("reset_action").value or "none")
        reset_nav_timeout_s = float(self.get_parameter("reset_nav_timeout_s").value)
        self.unit = os.environ.get("APOLLO_PROJECT_UNIT") or STOP_UNIT_DEFAULT
        if self.reset_action not in RESET_ACTIONS:
            self.get_logger().warn(f"[control_panel] unknown reset_action '{self.reset_action}' — treating as none")
            self.reset_action = "none"

        # --- reset action ----------------------------------------------------------------
        # what the Reset button does is chosen in Apollo (control_panel.reset_action). the
        # panel always publishes /control_panel/reset for external consumers; the actions
        # implemented here are dispatched by name so new ones slot in beside this one.
        self._nav_reset = (NavReturnToStart(self, reset_nav_timeout_s)
                           if self.reset_action == "nav_return_to_start" else None)
        self._reset_pending = False      # program interrupted for a reset; continue when it exits
        self._reset_kill_timer = None

        self.exit_code = None          # set to make main() exit with that code
        self.state = "booting"

        # --- buttons -------------------------------------------------------
        # last level seen per button; none until the first sample arrives. the first sample
        # only sets the baseline, so a button held during boot can never count as a press.
        self._level = {b: None for b in BUTTONS}
        self._last_press_ns = {b: 0 for b in BUTTONS}
        # press timestamps are kept (not just edges) so hold / double-press patterns can be
        # layered on later without rewriting the edge detection.
        self._pressed_since_ns = {b: None for b in BUTTONS}
        self._button_subs = [
            self.create_subscription(Bool, f"/button_{b}/state",
                                     lambda msg, b=b: self._on_button(b, msg), 10)
            for b in BUTTONS
        ]

        # --- LEDs --------------------------------------------------------------------
        # each LED is "off" | "solid" | "blink". the driver re-publishes every LED's level on
        # every tick, solid ones included: the dio component's /cmd subscriber is volatile,
        # so a single command sent before discovery matched would be lost for good.
        self._led_mode = {b: "off" for b in BUTTONS}
        self._blink_phase = False
        self._led_pubs = {b: self.create_publisher(Bool, f"/led_{b}/cmd", 10) for b in BUTTONS}
        self._led_timer = self.create_timer(1.0 / (2.0 * self.blink_hz), self._led_tick)

        # --- status ---------------------------------------------------------------------
        latched = QoSProfile(depth=1, reliability=ReliabilityPolicy.RELIABLE,
                             durability=DurabilityPolicy.TRANSIENT_LOCAL,
                             history=HistoryPolicy.KEEP_LAST)
        self._state_pub = self.create_publisher(String, "/control_panel/state", latched)
        self._reset_pub = self.create_publisher(Empty, "/control_panel/reset", 10)

        # --- virtual presses ----------------------------------------
        # the Apollo toolbar mirrors the panel with its own Start/Stop/Reset buttons and
        # publishes the button name here (over rosbridge). a separate entry point from the
        # physical edge detection, but it lands in the same _on_press, so both sources
        # drive one state machine and the LEDs react identically.
        self._press_sub = self.create_subscription(String, "/control_panel/press",
                                                   self._on_virtual_press, 10)

        self._stop_proc = None
        self._stop_timer = None

        # --- user program ---------------------------------------------------------------
        # launched by this node on Start (the launch file no longer runs it when the panel
        # is enabled); polled for exit; interrupted on the way out.
        self._program = None
        self._program_timer = None
        self._start_timeout_timer = None

        self._boot_started = time.monotonic()
        self._boot_timer = self.create_timer(0.25, self._boot_tick)
        self._log(f"node up unit={self.unit} lockout_ms={self.press_lockout_ns // 1_000_000} "
                  f"user_code={'yes' if self.user_code else 'no'}")
        self._publish_state()

    # --------------------------------------------------------- helpers
    def _log(self, text):
        self.get_logger().info(f"[control_panel] {text}")

    def _set_state(self, state, detail=""):
        self.state = state
        self._log(f"state={state}{(' ' + detail) if detail else ''}")
        self._publish_state()

    def _publish_state(self):
        try:
            self._state_pub.publish(String(data=self.state))
        except Exception:
            pass

    def _set_leds(self, start=None, stop=None, reset=None):
        for name, mode in (("start", start), ("stop", stop), ("reset", reset)):
            if mode is not None:
                self._led_mode[name] = mode

    def _fail(self, code, text):
        self.get_logger().error(f"[control_panel] ERROR {text}")
        self.exit_code = code

    # ------------------------------------------------------------------ boot
    def _boot_tick(self):
        try:
            have_buttons = all(v is not None for v in self._level.values())
            have_leds = all(p.get_subscription_count() > 0 for p in self._led_pubs.values())
            if have_buttons and have_leds:
                self._boot_timer.cancel()
                self._on_ready()
                return
            if time.monotonic() - self._boot_started > self.dio_ready_timeout_s:
                self._boot_timer.cancel()
                missing = [f"/button_{b}/state" for b, v in self._level.items() if v is None]
                missing += [f"/led_{b}/cmd" for b, p in self._led_pubs.items()
                            if p.get_subscription_count() == 0]
                self._fail(EXIT_NO_DIO, "no DIO samples after "
                           f"{self.dio_ready_timeout_s:.0f}s — is the control panel in params.yaml? "
                           f"missing: {', '.join(missing)}")
        except Exception as exc:  # never let a callback take the run down by accident
            self.get_logger().warn(f"[control_panel] boot tick error: {exc}")

    def _on_ready(self):
        # buttons featuur; red + yellow stay solid for as long as the runtime is up; green is
        # the program indicator: blinking while waiting for Start, solid while it runs
        self._set_leds(stop="solid", reset="solid")
        if self.user_code:
            self._enter_waiting()
        else:
            # touch / localization-only modes have no user program so basically stop works but start will not
            self._set_leds(start="off")
            self._set_state("idle", "(no user program)")

    # -------------------------------------------------------- program
    def _enter_waiting(self):
        self._set_leds(start="blink")
        self._cancel_start_timeout()
        if self.start_timeout_s > 0:
            self._start_timeout_timer = self.create_timer(self.start_timeout_s, self._on_start_timeout)
        self._set_state("waiting", f"timeout_s={self.start_timeout_s:.0f}")

    def _cancel_start_timeout(self):
        if self._start_timeout_timer is not None:
            self._start_timeout_timer.cancel()
            self._start_timeout_timer = None

    def _on_start_timeout(self):
        try:
            self._cancel_start_timeout()
            if self.state != "waiting":
                return
            self._request_stop(f"START timeout after {self.start_timeout_s:.0f}s")
        except Exception as exc:
            self.get_logger().warn(f"[control_panel] start timeout error: {exc}")

    def _launch_program(self, label):
        # same command, cwd and environment the launch file used before the panel took this
        # over, and stdio is inherited so program output still reaches the journal through
        # launch. the child stays in this cgroup: apollo-project-stop's /proc scan finds it by
        # the code.py path and SIGINTs it before the run goes down.
        # a sandboxed program (user_command) starts as `setsid sudo -u apollo-student ...`, which
        # can't carry PATH or LD_LIBRARY_PATH, so they go renamed. blocks programs get exactly the
        # command and environment they always had
        env = None
        if self.user_command:
            env = dict(os.environ,
                       APOLLO_PATH=os.environ.get("PATH", ""),
                       APOLLO_LD_LIBRARY_PATH=os.environ.get("LD_LIBRARY_PATH", ""))
        try:
            self._program = subprocess.Popen(
                self.user_command or ["/usr/bin/python3", self.user_code],
                cwd=self.project_dir or None,
                stdin=subprocess.DEVNULL,
                env=env,
            )
        except Exception as exc:
            self.get_logger().error(f"[control_panel] {label} -> failed to launch {self.user_code}: {exc}")
            return
        self._cancel_start_timeout()
        self._log(f"{label} -> launching {self.user_code} pid={self._program.pid}")
        if self._nav_reset is not None:
            self._nav_reset.note_started()   # where the robot is now is "start" for this run
        self._set_leds(start="solid", reset="solid")
        self._set_state("running", f"pid={self._program.pid}")
        self._program_timer = self.create_timer(0.1, self._program_tick)

    def _program_tick(self):
        try:
            if self._program is None:
                return
            rc = self._program.poll()
            if rc is None:
                return
            self._program_timer.cancel()
            self._program_timer = None
            self._program = None
            if self._reset_pending:
                # we interrupted it ourselves for a Reset — now the robot is ours to move
                self._reset_pending = False
                self._cancel_reset_kill_timer()
                self._log(f"program exited code={rc} (interrupted for reset)")
                self._run_reset_action()
                return
            # one play is one start is one run of the program. when the program ends; meaning it's finished or
            # crashed — the run ends with it, the same as a Stop press; the robot only arms
            # again on the next Play in Apollo.
            if self.state == "running":
                self._request_stop(f"program exited code={rc}")
            else:
                self._log(f"program exited code={rc}")
        except Exception as exc:
            self.get_logger().warn(f"[control_panel] program tick error: {exc}")

    # ------------------------------- reset
    def _begin_reset(self, label):
        if self.reset_action == "none":
            return
        if self.state in ("resetting", "stopping", "booting"):
            self._log(f"{label} ignored (state={self.state})")
            return
        if self.reset_action == "nav_return_to_start":
            if self._nav_reset is None or not self._nav_reset.available:
                self._log(f"{label} ignored (nav2 not available on this robot)")
                return
            self._cancel_start_timeout()
            self._set_leds(start="off", reset="blink")
            self._set_state("resetting", "return to start")
            if self._program is not None and self._program.poll() is None:
                # the program owns the robot's goals; take it down first, then act.
                # _program_tick continues the reset once it has exited.
                self._reset_pending = True
                self._log(f"reset -> interrupting program pid={self._program.pid}")
                self._program.send_signal(signal.SIGINT)
                self._reset_kill_timer = self.create_timer(5.0, self._on_reset_kill_deadline)
            else:
                self._run_reset_action()

    def _on_reset_kill_deadline(self):
        # the program ignored SIGINT for 5 s; do not let it hold the reset hostage
        try:
            self._cancel_reset_kill_timer()
            if self._reset_pending and self._program is not None and self._program.poll() is None:
                self._log(f"reset -> program pid={self._program.pid} did not exit, terminating")
                self._program.terminate()
        except Exception as exc:
            self.get_logger().warn(f"[control_panel] reset kill error: {exc}")

    def _cancel_reset_kill_timer(self):
        if self._reset_kill_timer is not None:
            self._reset_kill_timer.cancel()
            self._reset_kill_timer = None

    def _run_reset_action(self):
        if self.state != "resetting":
            return   # a Stop arrived while the program was shutting down
        if self.reset_action == "nav_return_to_start":
            self._log("reset -> cancelling navigation goals, returning to start")
            self._nav_reset.begin(self._on_reset_done)

    def _on_reset_done(self, ok, detail):
        try:
            if self.state != "resetting":
                return
            self._log(f"RESET {'done' if ok else 'failed'}: {detail}")
            # a failed return leaves yellow blinking alongside green: the robot is armed for
            # Start but is NOT at the start pose. the next Start or Reset press clears it.
            self._set_leds(reset="solid" if ok else "blink")
            # re-arm: the operator can press Start to run the program again from the start pose
            if self.user_code:
                self._enter_waiting()
            else:
                self._set_leds(start="off")
                self._set_state("idle", "(no user program)")
        except Exception as exc:
            self.get_logger().warn(f"[control_panel] reset done error: {exc}")

    def shutdown_program(self):
        """Called from main() on the way out: interrupt a live program and give it a moment
        to run its own stop hooks. Normally safe-stop has already done this."""
        proc = self._program
        self._program = None
        if proc is None or proc.poll() is not None:
            return
        try:
            proc.send_signal(signal.SIGINT)
            self._log(f"SIGINT received, forwarding to program pid={proc.pid}")
            proc.wait(timeout=3.0)
        except subprocess.TimeoutExpired:
            try:
                proc.terminate()
            except Exception:
                pass
        except Exception:
            pass

    # ------------------------------------------------------------- buttons
    def _on_button(self, name, msg):
        try:
            level = bool(msg.data)
            prev = self._level[name]
            self._level[name] = level
            if prev is None or prev == level:
                return
            now = time.monotonic_ns()
            if prev and not level:                    # true -> false: pressed (pull-up)
                self._pressed_since_ns[name] = now
                if now - self._last_press_ns[name] < self.press_lockout_ns:
                    return
                self._last_press_ns[name] = now
                self._on_press(name, "button")
            else:                                        # false -> true: released
                self._pressed_since_ns[name] = None
        except Exception as exc:
            self.get_logger().warn(f"[control_panel] button {name} error: {exc}")

    def _on_virtual_press(self, msg):
        try:
            name = (msg.data or "").strip().lower()
            if name not in BUTTONS:
                self._log(f"ignored unknown virtual press '{msg.data}'")
                return
            # same lockout as the physical button, shared with it, so a UI click right after
            # a physical press (or a double click) does not fire twice
            now = time.monotonic_ns()
            if now - self._last_press_ns[name] < self.press_lockout_ns:
                return
            self._last_press_ns[name] = now
            self._on_press(name, "ui")
        except Exception as exc:
            self.get_logger().warn(f"[control_panel] virtual press error: {exc}")

    def _on_press(self, name, source):
        if self.state == "booting":
            return
        label = f"{name.upper()} pressed ({source})"
        if name == "stop":
            self._request_stop(label)
        elif name == "start":
            if self.state == "waiting":
                self._launch_program(label)
            elif self.state == "running":
                self._log(f"{label} ignored (program running)")
            elif self.state == "idle":
                self._log(f"{label} ignored (no user program in this run mode)")
            else:
                self._log(f"{label} ignored (state={self.state})")
        elif name == "reset":
            self._log(label)
            self._reset_pub.publish(Empty())   # hook for external consumers, whatever reset_action is
            self._begin_reset(label)

    # ------------------------------------------------------------------ stop
    def _request_stop(self, reason):
        if self.state == "stopping":
            self._log(f"{reason} ignored (already stopping)")
            return
        self._log(f"{reason} -> systemctl stop {self.unit}")
        self._cancel_start_timeout()
        # Stop always wins: abandon a return-to-start in progress. control_server's
        # destructors zero the motors on the way down regardless.
        self._reset_pending = False
        self._cancel_reset_kill_timer()
        if self._nav_reset is not None:
            self._nav_reset.cancel()
        self._set_leds(start="off", stop="solid", reset="solid")
        self._set_state("stopping")
        # byte-for-byte the command the Apollo server runs, under the same sudoers rule.
        # --no-block: return immediately; ExecStop then interrupts the program first and
        # systemd SIGINTs the whole run, this node included.
        try:
            self._stop_proc = subprocess.Popen(
                ["sudo", "-n", SYSTEMCTL, "stop", "--no-block", self.unit],
                stdin=subprocess.DEVNULL, stdout=subprocess.DEVNULL, stderr=subprocess.PIPE)
        except Exception as exc:
            self._fail(EXIT_STOP_FAILED, f"could not run systemctl stop: {exc}")
            return
        self._stop_timer = self.create_timer(0.2, self._stop_tick)

    def _stop_tick(self):
        try:
            rc = self._stop_proc.poll()
            if rc is None:
                return
            self._stop_timer.cancel()
            if rc != 0:
                err = (self._stop_proc.stderr.read() or b"").decode("utf-8", "replace").strip()
                # exiting non-zero makes the launch shut the run down anyway — slower, and
                # without ExecStop's early SIGINT of the program, but the robot still stops.
                self._fail(EXIT_STOP_FAILED, f"systemctl stop failed rc={rc} {err}")
        except Exception as exc:
            self.get_logger().warn(f"[control_panel] stop tick error: {exc}")

    # -------------------------------------------------- LEDs
    def _led_tick(self):
        try:
            self._blink_phase = not self._blink_phase
            for name, pub in self._led_pubs.items():
                mode = self._led_mode[name]
                level = mode == "solid" or (mode == "blink" and self._blink_phase)
                pub.publish(Bool(data=level))
        except Exception as exc:
            self.get_logger().warn(f"[control_panel] led tick error: {exc}")


def main(args=None):
    rclpy.init(args=args)
    node = ControlPanelNode()
    try:
        while rclpy.ok() and node.exit_code is None:
            rclpy.spin_once(node, timeout_sec=0.1)
    except KeyboardInterrupt:
        pass
    finally:
        code = node.exit_code or 0
        # systemd sends a second SIGINT to the whole cgroup shortly after launch's; each step
        # is guarded so that second one cannot skip the ones after it.
        for step in (node.shutdown_program, node.destroy_node):
            try:
                step()
            except (KeyboardInterrupt, Exception):
                pass
        try:
            if rclpy.ok():
                rclpy.shutdown()
        except (KeyboardInterrupt, Exception):
            pass
    sys.exit(code)


if __name__ == "__main__":
    main()
