#!/usr/bin/env python3
"""로봄 연결기 — 내 PC의 ROS 2 노드를 로봄 도시 지도에 잇는다.

쓰는 법 (Ubuntu 24.04 + ROS 2 Jazzy):
    source /opt/ros/jazzy/setup.bash
    source ~/robom_ws/install/setup.bash   # 형식 패키지 robom_interfaces를 빌드한 작업 공간
    python3 robom_link.py --key <사이트에서 받은 연결 열쇠>

- 드론: /drone/pose(geometry_msgs/msg/PoseStamped) · /drone/battery(sensor_msgs/msg/BatteryState)를 받아 로봄 서버로 보내고,
  사이트에서 누른 「출발 · 멈춤」을 /drone/command(std_msgs/msg/String, start · stop)로 내보낸다.
  받은 frame_id는 비어 있어도 그대로 올린다(채우지 않는다).
- 판 4: 형식을 이름으로 찾는다(rosidl_runtime_py). 셔틀 정류장 서비스와 바리스타 로봇 액션은 우리 형식 패키지
  robom_interfaces(ShuttleEta · Brew)를 쓴다. 패키지를 찾지 못하면 연결기는 켜지되 그 요청에 「찾지 못했습니다」 한 줄을 돌려준다.
- 사이트가 부탁한 것만 한다. 허용 목록(아래 SERVICES · ACTIONS · PUBLISH_TOPICS · WATCH_TOPICS · PARAM_NODES) 밖은 거절한다.
  보낼 토픽의 퍼블리셔는 켤 때 미리 만든다. /crossroad/signal은 다음 값이 올 때까지 같은 값을 0.5초마다 다시 보낸다.
  지켜보는 토픽은 켤 때 모두 구독해 두고, 받은 것을 1초에 한 번 모아 올린다.
- --demo: ROS 2 없이 가짜 노드가 답한다(가짜 드론 · 셔틀 · 바리스타 · 교차로 셔틀 · 청소 로봇 · 재배 로봇).
  네트워크와 열쇠만 먼저 확인할 때 쓴다. 연습 모드라서 사이트의 확인받기는 되지 않는다.
- --ns <이름>: 토픽 · 서비스 · 액션 · 노드 이름 앞에 이름 공간을 붙인다(/<이름>/drone/pose). 한 PC에서 여럿 돌릴 때.

파이썬 표준 라이브러리와 ROS 2에 들어 있는 것(rclpy · rosidl_runtime_py)만 쓴다.
서버와는 웹 주소(HTTPS 443)로만 주고받는다: 보내기는 POST, 받기는 기다려 받기(최대 25초).
열쇠 값은 화면과 기록에 찍지 않는다.
"""

import argparse
import collections
import json
import math
import queue
import re
import sys
import threading
import time
import urllib.error
import urllib.request

VERSION = "4"
DEFAULT_SERVER = "https://city.robom.dev"  # 사이트에서 내려받으면 이 사이트 주소로 채워진다

KEY_EXPIRED = "열쇠가 만료되었습니다. 사이트에서 새 열쇠를 받으세요"
SEND_PER_SECOND = 5  # 서버 제한(열쇠 하나에 초당 10번) 안쪽
WAIT_SECONDS = 25
BACKOFF = (1, 2, 4, 8)  # 네트워크 오류 뒤 다시 붙기까지 기다리는 초
EXIT_KEY = 3
DEPOT = (12.0, 48.0)  # 하늘 배달 지도의 창고 · 도착지(map 좌표, y가 위로 커진다)
DROPOFF = (88.0, 14.0)
FIND_SECONDS = 4.5  # 서비스 · 액션 서버를 찾는 시간. 처음 만든 클라이언트는 ROS 2 발견(discovery)에 몇 초 걸린다(사이트는 5초까지 기다린다)
NAME = re.compile(r"[A-Za-z][A-Za-z0-9_]{0,31}(/[A-Za-z][A-Za-z0-9_]{0,31}){0,3}")  # 서버가 보낸 이름(앞 / 없이)

# 둘러보기: 허용 목록
# ── 판 4 허용 목록 (바로잡기 명세 1-2절) ── 서버(app/link/state.py)의 목록과 같아야 한다. 목록 밖은 연결기가 거절한다.
INTERFACES = "robom_interfaces"
SERVICES = {"/shuttle/arrival": "robom_interfaces/srv/ShuttleEta"}  # 셔틀 정류장
ACTIONS = {"/cafe/brew": "robom_interfaces/action/Brew"}  # 바리스타 로봇
PUBLISH_TOPICS = {  # 사이트가 보내 달라고 할 수 있는 토픽
    "/crossroad/signal": "std_msgs/msg/String",
    "/farm/moisture": "std_msgs/msg/Float64",
}
REPEAT_TOPICS = ("/crossroad/signal",)  # 다음 값이 올 때까지 같은 값을 다시 보낸다(교차로가 신호를 계속 방송하는 장면)
REPEAT_EVERY = 0.5
WATCH_TOPICS = {  # 켜질 때 모두 구독해 두고, 받은 것을 1초에 한 번 모아 올린다
    "/shuttle/position": "std_msgs/msg/Float64",
    "/crossroad/cmd_vel": "geometry_msgs/msg/Twist",
    "/energy/robot_status": "std_msgs/msg/Header",
    "/farm/water": "std_msgs/msg/Bool",
}
PARAM_NODES = ("/robom_farm",)  # 파라미터를 읽고 바꿀 수 있는 노드
DRONE_TOPICS = {  # 드론(3차부터 그대로)
    "/drone/pose": "geometry_msgs/msg/PoseStamped",
    "/drone/battery": "sensor_msgs/msg/BatteryState",
    "/drone/command": "std_msgs/msg/String",
}
# 둘러보기 끝
PARAM_TYPES = {  # 파라미터 읽기 · 바꾸기에 쓰는 ROS 2 기본 형식
    "get": "rcl_interfaces/srv/GetParameters",
    "set": "rcl_interfaces/srv/SetParameters",
    "parameter": "rcl_interfaces/msg/Parameter",
    "value": "rcl_interfaces/msg/ParameterValue",
}
PARAM_NAME = re.compile(r"[A-Za-z_][A-Za-z0-9_.]{0,63}")
PARAM_FIND_SECONDS = 2.0  # 파라미터 서비스를 이 안에 찾지 못하거나, 부른 뒤 이 안에 답이 없으면 「노드를 찾지 못했습니다」
WATCH_EVERY = 1.0  # 지켜본 메시지를 올리는 간격(초)
WATCH_BATCH = 16  # 한 번에 올리는 메시지 수(넘치면 오래된 것부터 버린다. 서버의 4KB 제한 안쪽)
TEXT_MAX = 40  # 올리는 글자 칸(String data · Header frame_id)의 길이
FRAME_MAX = 64  # 드론 위치의 frame_id 길이(서버와 같다)
NOT_ALLOWED_TOPIC = "허용되지 않은 토픽입니다"
NOT_ALLOWED_NODE = "허용되지 않은 노드입니다"
NOT_ALLOWED_SERVICE = "허용되지 않은 서비스입니다"
NOT_ALLOWED_ACTION = "허용되지 않은 액션입니다"
NO_NODE = "노드를 찾지 못했습니다"
NO_INTERFACES = "robom_interfaces를 찾지 못했습니다. ~/robom_ws/install/setup.bash를 source했는지 보세요"
REQUEST_KINDS = ("service_call", "action_goal", "action_cancel", "topic_publish", "param_get", "param_set")


def say(text: str) -> None:
    print(text, flush=True)


def missing_type(type_name: str) -> str:
    """형식을 찾지 못했을 때 학생에게 보일 한 줄."""
    if type_name.startswith(INTERFACES + "/"):
        return NO_INTERFACES
    return f"형식 {type_name}을(를) 찾지 못했습니다. source /opt/ros/jazzy/setup.bash를 했는지 보세요"


def load_type(type_name: str):
    """「패키지/msg|srv|action/이름」 형식을 이름으로 불러온다. 못 찾으면 None."""
    from rosidl_runtime_py.utilities import get_action, get_message, get_service

    getter = {"msg": get_message, "srv": get_service, "action": get_action}.get(type_name.split("/")[1] if type_name.count("/") == 2 else "")
    if getter is None:
        return None
    try:
        return getter(type_name)
    except (ImportError, AttributeError, ValueError, LookupError):
        return None


class KeyExpired(Exception):
    pass


class ServerSaid(Exception):
    """서버가 한 줄 안내로 거절했다(400 · 413 · 429)."""

    def __init__(self, status: int, message: str):
        super().__init__(message)
        self.status = status


class MissingType(ImportError):
    """켜는 데 꼭 필요한 ROS 2 기본 형식(드론 · std_msgs · rcl_interfaces)을 찾지 못했다."""


class Server:
    def __init__(self, base: str, key: str, demo: bool = False):
        self.base = base.rstrip("/")
        self._key = key
        self.demo = demo  # 연습 모드(--demo)면 모든 요청에 그렇다고 알린다. 사이트는 연습 모드로는 확인받기를 하지 않는다

    def _call(self, method: str, path: str, payload=None, timeout: float = 10.0):
        data = None if payload is None else json.dumps(payload, separators=(",", ":")).encode("utf-8")
        request = urllib.request.Request(self.base + path, data=data, method=method)
        request.add_header("Authorization", "Bearer " + self._key)
        request.add_header("User-Agent", "robom-link/" + VERSION)
        request.add_header("X-Robom-Link", VERSION)  # 판 머리: 서버가 이 판이 아는 명령만 준다
        if self.demo:
            request.add_header("X-Robom-Link-Mode", "demo")
        if data is not None:
            request.add_header("Content-Type", "application/json")
        try:
            with urllib.request.urlopen(request, timeout=timeout) as response:
                body = response.read()
                return json.loads(body) if body else None
        except urllib.error.HTTPError as error:
            if error.code == 401:
                raise KeyExpired from None
            try:
                message = json.loads(error.read()).get("error", "")
            except ValueError:
                message = ""
            raise ServerSaid(error.code, message or "서버가 요청을 받지 않았습니다 (" + str(error.code) + ")") from None

    def send(self, payload: dict) -> None:
        self._call("POST", "/api/link/telemetry", payload)

    def report(self, payload: dict) -> None:
        self._call("POST", "/api/link/results", payload)

    def wait_commands(self) -> list:
        data = self._call("GET", "/api/link/commands?wait=" + str(WAIT_SECONDS), timeout=WAIT_SECONDS + 10)
        return list((data or {}).get("commands", []))


class Link:
    """보내기 · 받기 · 결과 돌려주기 세 줄기를 돌린다.

    드론 쪽(ROS 2 또는 가짜 드론)은 update()로 값을 넣고 on_command로 명령을 받는다.
    서비스 · 액션 쪽은 on_request로 요청({id, kind, ...})을 받고, 결과는 report()로 넣는다(넣은 차례대로 서버에 간다).
    """

    def __init__(self, server: Server):
        self.server = server
        self.stop = threading.Event()
        self.exit_code = 0
        self.on_command = lambda command: None
        self.on_request = lambda line: self.report({"id": line["id"], "kind": "error", "error": "이 연결기는 요청을 받을 준비가 되지 않았습니다"})
        self._results: queue.Queue = queue.Queue()
        self._lock = threading.Lock()
        self._pose = None
        self._battery = None
        self._fresh = False
        self._watched = collections.deque(maxlen=WATCH_BATCH)  # 지켜보는 토픽에서 받은 것(판 3)

    def update(self, pose=None, battery=None) -> None:
        with self._lock:
            if pose is not None:
                self._pose = pose
            if battery is not None:
                self._battery = battery
            self._fresh = True

    def watch(self, topic: str, data: dict) -> None:
        """지켜보는 토픽에서 받은 메시지 하나를 올릴 묶음에 넣는다. 받은 시각은 이 PC의 시계(유닉스 초)다."""
        with self._lock:
            self._watched.append({"topic": topic, "data": data, "received_at": round(time.time(), 3)})

    def _put_back(self, batch: list) -> None:
        """보내지 못한 묶음을 앞에 되돌린다. 넘치면 오래된 것부터 버린다(WATCH_BATCH개까지)."""
        with self._lock:
            kept = (batch + list(self._watched))[-WATCH_BATCH:]
            self._watched.clear()
            self._watched.extend(kept)

    def _take_watched(self) -> list:
        with self._lock:
            batch = list(self._watched)
            self._watched.clear()
            return batch

    def _take(self):
        with self._lock:
            if not self._fresh:
                return None
            self._fresh = False
            payload = {}
            if self._pose is not None:
                payload["pose"] = self._pose
            if self._battery is not None:
                payload["battery"] = self._battery
            return payload or None

    def _expired(self) -> None:
        if not self.stop.is_set():
            say(KEY_EXPIRED)
            self.exit_code = EXIT_KEY
            self.stop.set()

    def _retry(self, tries: int, what: str) -> int:
        delay = BACKOFF[min(tries, len(BACKOFF) - 1)]
        say(f"서버에 닿지 않습니다({what}). {delay}초 뒤 다시 붙습니다. 인터넷 연결과 --server 주소를 확인하세요")
        self.stop.wait(delay)
        return tries + 1

    # 둘러보기: 올리기 줄기
    def send_loop(self) -> None:
        connected, tries = False, 0
        while not self.stop.is_set():
            payload = self._take()
            if payload is not None:
                try:
                    self.server.send(payload)
                    if not connected:
                        say(f"연결됨 · {self.server.base}")
                        connected = True
                    tries = 0
                except KeyExpired:
                    self._expired()
                    return
                except ServerSaid as error:
                    say(error.args[0])
                    self.stop.wait(1.0 if error.status == 429 else 5.0)
                    continue
                except (urllib.error.URLError, OSError):
                    connected = False
                    tries = self._retry(tries, "보내기")
                    continue
            self.stop.wait(1.0 / SEND_PER_SECOND)
    # 둘러보기 끝

    # 둘러보기: 명령 받기
    def command_loop(self) -> None:
        tries = 0
        while not self.stop.is_set():
            try:
                commands = self.server.wait_commands()
                tries = 0
            except KeyExpired:
                self._expired()
                return
            except ServerSaid as error:
                say(error.args[0])
                self.stop.wait(1.0)
                continue
            except (urllib.error.URLError, OSError):
                tries = self._retry(tries, "받기")
                continue
            for line in commands:
                if isinstance(line, str):  # 판 1 모양(서버가 판 머리를 못 본 때)
                    line = {"kind": "drone", "command": line}
                if not isinstance(line, dict):
                    continue
                kind = line.get("kind")
                if kind == "drone" and line.get("command") in ("start", "stop"):
                    say(f"명령 받음: {line['command']}")
                    self.on_command(line["command"])
                elif kind in REQUEST_KINDS and isinstance(line.get("id"), str):
                    what = line.get("name") or line.get("topic") or ""
                    say(f"요청 받음: {kind} {what}".rstrip())
                    self.on_request(line)
    # 둘러보기 끝

    # 둘러보기: 지켜본 메시지 올리기
    def watch_loop(self) -> None:
        """지켜본 메시지를 WATCH_EVERY초에 한 번 모아 올린다(판 3). 받은 것이 없으면 보내지 않는다."""
        tries = 0
        while not self.stop.wait(WATCH_EVERY):
            batch = self._take_watched()
            if not batch:
                continue
            try:
                self.server.report({"kind": "topic_watch", "messages": batch})
                tries = 0
            except KeyExpired:
                self._expired()
                return
            except ServerSaid as error:
                say(error.args[0])
                if error.status == 429:  # 속도 제한: 묶음을 되돌려 두고 다음 차례에 함께 보낸다(모양 오류는 버린다)
                    self._put_back(batch)
            except (urllib.error.URLError, OSError):
                tries = self._retry(tries, "지켜본 메시지 올리기")
    # 둘러보기 끝

    def report(self, payload: dict) -> None:
        """결과 하나를 보낼 줄에 넣는다(답 · 수락 · 진행률 · 결과 · 실패)."""
        self._results.put(payload)

    def result_loop(self) -> None:
        tries = 0
        while not self.stop.is_set():
            try:
                payload = self._results.get(timeout=0.2)
            except queue.Empty:
                continue
            while not self.stop.is_set():
                try:
                    self.server.report(payload)
                    say(f"결과 보냄: {payload['kind']}")
                    tries = 0
                    break
                except KeyExpired:
                    self._expired()
                    return
                except ServerSaid as error:
                    say(error.args[0])  # 닫힌 요청 · 모양 오류는 다시 보내지 않는다
                    if error.status != 429:
                        break
                    self.stop.wait(1.0)
                except (urllib.error.URLError, OSError):
                    tries = self._retry(tries, "결과 보내기")


# ── 허용 목록 확인과 값 바꾸기 (ROS 2 없이도 쓰는 부분) ──────────────────


def refuse(line: dict):
    """사이트가 보낸 요청을 허용 목록에 맞춰 본다. 거절할 까닭 한 줄, 받아도 되면 None."""
    kind = line.get("kind")
    if kind in ("service_call", "action_goal"):
        allowed, why, body = (SERVICES, NOT_ALLOWED_SERVICE, "request") if kind == "service_call" else (ACTIONS, NOT_ALLOWED_ACTION, "goal")
        name = line.get("name")
        if not isinstance(name, str) or not NAME.fullmatch(name) or allowed.get("/" + name) != line.get("type"):
            return why
        if not isinstance(line.get(body), dict):
            return "요청 값이 형식에 맞지 않습니다"
        return None
    if kind == "topic_publish":
        topic = line.get("topic")
        if topic not in PUBLISH_TOPICS or line.get("type") != PUBLISH_TOPICS[topic]:
            return NOT_ALLOWED_TOPIC
        data = line.get("data")
        if not isinstance(data, dict) or set(data) != {"data"}:
            return "보낼 값의 형식이 맞지 않습니다"
        value = data["data"]
        if PUBLISH_TOPICS[topic] == "std_msgs/msg/String":
            ok = isinstance(value, str) and len(value) <= TEXT_MAX
        else:  # std_msgs/msg/Float64
            ok = isinstance(value, (int, float)) and not isinstance(value, bool) and math.isfinite(value)
        return None if ok else "보낼 값의 형식이 맞지 않습니다"
    if kind in ("param_get", "param_set"):
        if line.get("node") not in PARAM_NODES:
            return NOT_ALLOWED_NODE
        if not isinstance(line.get("name"), str) or not PARAM_NAME.fullmatch(line["name"]):
            return "파라미터 이름이 맞지 않습니다"
        if kind == "param_set":
            value = line.get("value")
            if not isinstance(value, (bool, int, float, str)) or (isinstance(value, float) and not math.isfinite(value)):
                return "파라미터 값의 형식이 맞지 않습니다"
    return None


class NotSendable(ValueError):
    """JSON으로 보낼 수 없는 값(NaN · 무한대)."""


def _clean(value):
    """보낼 모양으로 다듬는다: 글자는 TEXT_MAX자까지, 실수는 유한한 값만."""
    if isinstance(value, dict):
        return {name: _clean(item) for name, item in value.items()}
    if isinstance(value, list):
        return [_clean(item) for item in value]
    if isinstance(value, str):
        return value[:TEXT_MAX]
    if isinstance(value, float) and not math.isfinite(value):
        raise NotSendable(value)
    return value


def to_plain(type_name: str, message):
    """지켜보는 토픽의 메시지를 칸 이름 그대로의 dict로 바꾼다(Twist · 사용자 정의 형식도 같은 방법).
    보낼 수 없는 값(NaN 등)이 있으면 None."""
    try:
        data = _clean(plain(message))
    except NotSendable:
        return None
    return data if isinstance(data, dict) else None


# rcl_interfaces/msg/ParameterType 값
PARAM_NOT_SET, PARAM_BOOL, PARAM_INTEGER, PARAM_DOUBLE, PARAM_STRING = 0, 1, 2, 3, 4


def param_python(value):
    """rcl_interfaces/msg/ParameterValue → 파이썬 값. 선언되지 않았거나 이 연결기가 모르는 형식(배열 등)이면 None."""
    return {
        PARAM_BOOL: lambda: bool(value.bool_value),
        PARAM_INTEGER: lambda: int(value.integer_value),
        PARAM_DOUBLE: lambda: float(value.double_value),
        PARAM_STRING: lambda: str(value.string_value),
    }.get(value.type, lambda: None)()


def param_message(parameter_value, value):
    """파이썬 값 → rcl_interfaces/msg/ParameterValue(parameter_value는 그 형식). bool을 int보다 먼저 본다."""
    message = parameter_value()
    if isinstance(value, bool):
        message.type, message.bool_value = PARAM_BOOL, value
    elif isinstance(value, int):
        message.type, message.integer_value = PARAM_INTEGER, value
    elif isinstance(value, float):
        message.type, message.double_value = PARAM_DOUBLE, value
    else:
        message.type, message.string_value = PARAM_STRING, str(value)
    return message


def fill(message, values):
    """사이트가 보낸 dict를 ROS 2 메시지 칸에 채운다. 모르는 칸은 버린다."""
    if not isinstance(values, dict):
        raise ValueError("dict가 아닙니다")
    fields = message.get_fields_and_field_types()
    for name, value in values.items():
        if name in fields:
            setattr(message, name, value)
    return message


def plain(message):
    """ROS 2 메시지를 JSON으로 보낼 수 있는 dict · list · 수로 바꾼다(칸 이름 그대로)."""
    if hasattr(message, "get_fields_and_field_types"):
        return {name: plain(getattr(message, name)) for name in message.get_fields_and_field_types()}
    if isinstance(message, (list, tuple)) or type(message).__name__ in ("array", "ndarray"):
        return [plain(item) for item in message]
    if hasattr(message, "item"):  # numpy 수
        return message.item()
    return message


# ── 가짜 노드(--demo) ─────────────────────────────────────────


class DemoDrone:
    """ROS 2 없이 창고와 도착지 사이를 오가는 가짜 드론. 화면이 움직이는 것을 보일 만큼만 답한다(모양만 맞는 답).

    「출발 · 멈춤」과 상관없이 늘 오가고, frame_id는 지도 좌표(map)가 아니다. 그래서 과제 2 · 3은 통과하지 않는다.
    사이트 화면은 연습 모드면 frame_id와 상관없이 그린다.
    """

    SPEED = 8.0  # 지도 단위/초
    FRAME = "demo_odom"

    def __init__(self):
        self.x, self.y = DEPOT
        self.target = DROPOFF
        self.battery = 1.0
        self._lock = threading.Lock()

    def command(self, command: str) -> None:
        """가짜 드론은 명령을 듣지 않는다(받은 줄은 연결기가 찍는다)."""

    def step(self, dt: float) -> None:
        with self._lock:
            dx, dy = self.target[0] - self.x, self.target[1] - self.y
            distance = math.hypot(dx, dy)
            move = self.SPEED * dt
            if distance <= move:
                self.x, self.y = self.target
                self.target = DEPOT if self.target == DROPOFF else DROPOFF
            else:
                self.x += dx / distance * move
                self.y += dy / distance * move
            self.battery = max(0.0, self.battery - 0.002 * dt)

    def message(self):
        with self._lock:
            pose = {
                "frame_id": self.FRAME,
                "position": {"x": round(self.x, 3), "y": round(self.y, 3), "z": 10.0},
                "orientation": {"x": 0.0, "y": 0.0, "z": 0.0, "w": 1.0},
            }
            return pose, {"percentage": round(self.battery, 4)}


DEMO_STOPS = (1, 2, 3)  # 가짜 셔틀이 받는 정류장 번호
DEMO_MINUTES = 5  # 가짜 셔틀의 답: 위치와 상관없이 늘 같다(모양만 맞는 답, 바로잡기 명세 0-7)
DEMO_MENUS = ("americano", "latte")  # 가짜 바리스타가 받는 메뉴 이름
DEMO_BREW_STEPS = 4  # 가짜 바리스타는 메뉴와 상관없이 잔마다 이만큼 퍼센트를 올린다(단계 이름 없음)
DEMO_CUPS = 1  # 가짜 바리스타의 cups_done: 주문과 상관없이 늘 같다(모양만 맞는 답, 바로잡기 명세 0-7)
DEMO_CRUISE = 0.3  # 가짜 교차로 셔틀의 linear.x: 신호와 상관없이 늘 같다(모양만 맞는 답, 바로잡기 명세 0-7)
DEMO_VALVE_BELOW = 40.0  # 가짜 재배 로봇은 기준값과 상관없이 수분이 이보다 낮으면 밸브를 연다(모양만 맞는 답, 바로잡기 명세 0-7)
DEMO_STATUS_TICKS = 5  # 가짜 청소 로봇은 TICK 5번(2.5초)마다 보고한다: 주기 2초를 일부러 어긴다(모양만 맞는 답, 바로잡기 명세 0-7)


class DemoNodes:
    """ROS 2 없이 판 4 모양으로 답하는 가짜 노드들.

    - 셔틀: 한 바퀴 12칸 노선을 1초에 1칸 돌며 /shuttle/position을 올린다. ShuttleEta에는 위치와 상관없이 늘 DEMO_MINUTES분으로
      답한다. 화면 흐름만 보이는 「모양만 맞는 답」이라 셔틀 과제 2는 통과하지 못한다(바로잡기 명세 0-7). 계산은 학생 노드가 한다.
    - 바리스타: Brew 목표를 받아 STEP초마다 percent만 올린다(단계 이름은 비우고, cups_done은 주문과 상관없이 늘 DEMO_CUPS).
      진행 막대와 결과 줄만 움직이는 「모양만 맞는 답」이라 카페 과제는 통과하지 못한다(바로잡기 명세 0-7). 단계는 학생 노드가 밟는다.
    - 교차로 셔틀: 신호를 한 번 받은 뒤부터 0.5초마다 /crossroad/cmd_vel을 올린다. linear.x는 신호와 상관없이 늘 DEMO_CRUISE다.
      지도의 셔틀만 움직이는 「모양만 맞는 답」이라 교차로 과제는 통과하지 못한다(바로잡기 명세 0-7). 신호별 속도는 학생 노드가 고른다.
    - 청소 로봇: 2.5초마다 /energy/robot_status(Header, frame_id는 패널 좌표계 이름)를 올린다. stamp는 켤 때 시각에 멈춰 있다.
      패널 그림만 움직이는 「모양만 맞는 답」이라 에너지 과제는 통과하지 못한다(바로잡기 명세 0-7). 주기와 시각은 학생 노드가 맞춘다.
    - 재배 로봇(/robom_farm): moisture_threshold(기본 40.0)를 갖고, /farm/moisture를 받으면 /farm/water로 답한다. 결정은 기준값과
      상관없이 늘 DEMO_VALVE_BELOW로 하고, 기준값은 범위를 보지 않고 무엇이든 받아들인다. 밸브만 움직이는 「모양만 맞는 답」이라
      농장 과제 2 · 3은 통과하지 못한다(바로잡기 명세 0-7). 기준값을 따르고 틀린 값을 거절하는 것은 학생 노드가 한다.
    """

    LAP = 12.0
    STEP = 0.3
    PARAM = "moisture_threshold"
    TICK = 0.5

    def __init__(self, link: Link):
        self.link = link
        self.started = time.monotonic()
        self.signal = None
        self.threshold = 40.0
        self.cancel: dict[str, threading.Event] = {}
        self.ticks = 0
        self.stamp_at = time.time()  # 가짜 청소 로봇의 stamp: 켤 때 시각에서 바뀌지 않는다

    def position(self, now: float) -> float:
        return (now - self.started) % self.LAP

    def handle(self, line: dict) -> None:
        kind, rid = line["kind"], line["id"]
        why = refuse(line)
        if why is not None:
            self.link.report({"id": rid, "kind": "error", "error": why})
        elif kind == "action_cancel":
            if rid in self.cancel:
                self.cancel[rid].set()
        elif kind == "service_call":
            stop = line["request"].get("stop")
            if not isinstance(stop, int) or isinstance(stop, bool) or stop not in DEMO_STOPS:
                self.link.report({"id": rid, "kind": "error", "error": "요청 값이 형식에 맞지 않습니다"})
                return
            self.link.report({"id": rid, "kind": "service_reply", "response": {"minutes": DEMO_MINUTES}})
        elif kind == "action_goal":
            goal = line["goal"]
            menu, cups = goal.get("menu"), goal.get("cups")
            if menu not in DEMO_MENUS or not isinstance(cups, int) or isinstance(cups, bool) or not 1 <= cups <= 3:
                self.link.report({"id": rid, "kind": "error", "error": "목표 값이 형식에 맞지 않습니다"})
                return
            self.cancel[rid] = threading.Event()
            threading.Thread(target=self._brew, args=(rid, menu, cups), daemon=True).start()
        elif kind == "topic_publish":
            self.link.report({"id": rid, "kind": "topic_published"})
            value = line["data"]["data"]
            if line["topic"] == "/crossroad/signal":
                self.signal = value
            else:  # /farm/moisture: 가짜 재배 로봇은 기준값을 보지 않고 늘 같은 선으로 정한다(모양만 맞는 답)
                self.link.watch("/farm/water", {"data": float(value) < DEMO_VALVE_BELOW})
        else:
            self.link.report({"id": rid, **self.param(line)})

    def _brew(self, rid: str, menu: str, cups: int) -> None:
        self.link.report({"id": rid, "kind": "action_accepted"})
        started, total = time.monotonic(), DEMO_BREW_STEPS * cups
        for done in range(1, total + 1):
            if self.cancel[rid].wait(self.STEP) or self.link.stop.is_set():
                result = {"cups_done": DEMO_CUPS, "seconds": round(time.monotonic() - started, 2)}
                self.link.report({"id": rid, "kind": "action_result", "status": "canceled", "result": result})
                return
            feedback = {"step": "", "cups_done": DEMO_CUPS, "percent": round(done * 100.0 / total, 1)}
            self.link.report({"id": rid, "kind": "action_feedback", "feedback": feedback})
        result = {"cups_done": DEMO_CUPS, "seconds": round(time.monotonic() - started, 2)}
        self.link.report({"id": rid, "kind": "action_result", "status": "succeeded", "result": result})

    def param(self, line: dict) -> dict:
        """param_get · param_set 하나에 대한 결과(요청 번호 없이)."""
        name = line["name"]
        if line["kind"] == "param_get":
            return {"kind": "param_values", "values": {name: self.threshold if name == self.PARAM else None}}
        value = line["value"]
        if name != self.PARAM:
            return {"kind": "param_set_result", "successful": False, "reason": f"선언되지 않은 파라미터입니다: {name}"}
        self.threshold = value  # 범위를 보지 않고 무엇이든 받아들인다(모양만 맞는 답, 바로잡기 명세 0-7)
        return {"kind": "param_set_result", "successful": True, "reason": ""}

    def once(self) -> None:
        """TICK초마다: 셔틀 위치, (신호를 받았으면) 교차로 셔틀 주행 명령, DEMO_STATUS_TICKS번에 한 번 청소 로봇 상태."""
        now = time.monotonic()
        self.link.watch("/shuttle/position", {"data": round(self.position(now), 3)})
        if self.signal is not None:
            zero = {"x": 0.0, "y": 0.0, "z": 0.0}
            self.link.watch("/crossroad/cmd_vel", {"linear": {"x": DEMO_CRUISE, "y": 0.0, "z": 0.0}, "angular": dict(zero)})
        if self.ticks % DEMO_STATUS_TICKS == 0:
            sec = int(self.stamp_at)
            panel = (self.ticks // DEMO_STATUS_TICKS) % 4 + 1
            stamp = {"sec": sec, "nanosec": int((self.stamp_at - sec) * 1e9)}
            self.link.watch("/energy/robot_status", {"stamp": stamp, "frame_id": f"panel_{panel}"})
        self.ticks += 1

    def loop(self) -> None:
        due = time.monotonic()
        while not self.link.stop.wait(max(0.0, due - time.monotonic())):
            self.once()
            due += self.TICK


def run_demo(link: Link) -> None:
    drone = DemoDrone()
    nodes = DemoNodes(link)
    link.on_command = drone.command
    link.on_request = nodes.handle
    threading.Thread(target=nodes.loop, name="demo-nodes", daemon=True).start()
    say("가짜 노드(--demo)로 답합니다. ROS 2는 쓰지 않습니다. 연습 모드라서 사이트의 확인받기는 되지 않습니다")
    last = time.monotonic()
    while not link.stop.is_set():
        now = time.monotonic()
        drone.step(now - last)
        last = now
        pose, battery = drone.message()
        link.update(pose=pose, battery=battery)
        link.stop.wait(1.0 / SEND_PER_SECOND)


# ── ROS 2 ──────────────────────────────────────────────────────


class RosSide:
    """판 4의 ROS 2 쪽 전부: 드론 토픽, 서비스 · 액션 부르기, 토픽 보내기 · 지켜보기, 파라미터 읽기 · 바꾸기.
    모두 run_ros 줄기(spin_once 도는 곳)에서 부른다. 요청은 받기 줄기에서 오므로 inbox로 넘겨받는다.

    node, load(형식 이름 → 형식, 못 찾으면 None), action_client(node, 형식, 이름)를 받는다(시험은 가짜를 넣는다).
    """

    def __init__(self, link: Link, node, namespace: str, load, action_client):
        self.link, self.node, self.load, self.action_client = link, node, load, action_client
        self.prefix = f"/{namespace}" if namespace else ""
        self.inbox: queue.Queue = queue.Queue()
        link.on_request = self.inbox.put
        self._types = {}
        self.waiting = []  # 서버 · 노드를 찾는 중인 (마감 monotonic, 요청)
        self.calling = []  # 파라미터를 부른 뒤 답을 기다리는 (마감 monotonic, 요청 번호, future)
        self.answered = set()  # 파라미터 결과를 이미 올린 요청 번호(늦게 온 답은 버린다)
        self.goals = {}  # 요청 번호 → 목표 손잡이(goal handle)
        self.cancel_later = set()  # 수락 전에 취소가 온 요청 번호
        self.repeat = {}  # 다시 보낼 토픽 → [메시지, 다음 때 monotonic]

        # 둘러보기: 드론 토픽
        # 드론(3차부터 그대로). 받은 frame_id는 비어 있어도 그대로 올린다.
        string = self._need("std_msgs/msg/String")
        node.create_subscription(self._need(DRONE_TOPICS["/drone/pose"]), self.prefix + "/drone/pose", self.on_pose, 10)
        node.create_subscription(self._need(DRONE_TOPICS["/drone/battery"]), self.prefix + "/drone/battery", self.on_battery, 10)
        command = node.create_publisher(string, self.prefix + "/drone/command", 10)
        link.on_command = lambda text: command.publish(string(data=text))
        # 둘러보기 끝

        # 둘러보기: 보낼 토픽 퍼블리셔
        # 보낼 토픽의 퍼블리셔는 켤 때 미리 만든다(처음 보낸 메시지를 구독자가 놓치지 않게)
        self.publishers = {topic: node.create_publisher(self._need(t), self.prefix + topic, 10) for topic, t in PUBLISH_TOPICS.items()}
        # 둘러보기 끝
        # 둘러보기: 지켜볼 토픽 구독
        for topic, type_name in WATCH_TOPICS.items():
            node.create_subscription(self._need(type_name), self.prefix + topic, self._watcher(topic, type_name), 10)
        # 둘러보기 끝

        # 둘러보기: 파라미터 클라이언트
        # 파라미터 클라이언트도 미리 만든다(처음 만든 클라이언트는 ROS 2 발견에 몇 초 걸린다)
        self.param = {name: self._need(t) for name, t in PARAM_TYPES.items()}
        self.param_clients = {}
        for target in PARAM_NODES:
            self.param_clients[(target, "param_get")] = node.create_client(self.param["get"], self.prefix + target + "/get_parameters")
            self.param_clients[(target, "param_set")] = node.create_client(self.param["set"], self.prefix + target + "/set_parameters")
        # 둘러보기 끝

        # 둘러보기: 서비스 · 액션 클라이언트
        # 서비스 · 액션 클라이언트: 형식 패키지(robom_interfaces)가 있으면 미리 만든다
        self.clients, self.actions = {}, {}
        for name, type_name in SERVICES.items():
            if self._type(type_name) is not None:
                self.clients[name] = node.create_client(self._type(type_name), self.prefix + name)
        for name, type_name in ACTIONS.items():
            if self._type(type_name) is not None:
                self.actions[name] = action_client(node, self._type(type_name), self.prefix + name)
        # 둘러보기 끝
        self.missing = sorted(t for t in (*SERVICES.values(), *ACTIONS.values()) if self._type(t) is None)
        if self.missing:
            say(missing_type(self.missing[0]))

    def _type(self, type_name: str):
        if type_name not in self._types:
            self._types[type_name] = self.load(type_name)
        return self._types[type_name]

    def _need(self, type_name: str):
        found = self._type(type_name)
        if found is None:
            raise MissingType(missing_type(type_name))
        return found

    # ── 받은 것 올리기 ──

    # 둘러보기: 위치 올리기
    def on_pose(self, message) -> None:
        p, q = message.pose.position, message.pose.orientation
        self.link.update(
            pose={
                "frame_id": str(message.header.frame_id)[:FRAME_MAX],
                "position": {"x": p.x, "y": p.y, "z": p.z},
                "orientation": {"x": q.x, "y": q.y, "z": q.z, "w": q.w},
            }
        )

    def on_battery(self, message) -> None:
        if math.isfinite(message.percentage):
            battery = {"percentage": min(1.0, max(0.0, message.percentage))}
            if math.isfinite(message.voltage) and message.voltage >= 0:
                battery["voltage"] = message.voltage
            self.link.update(battery=battery)
    # 둘러보기 끝

    # 둘러보기: 받은 메시지 담기
    def _watcher(self, topic: str, type_name: str):
        def on_message(message) -> None:
            data = to_plain(type_name, message)
            if data is not None:
                self.link.watch(topic, data)

        return on_message
    # 둘러보기 끝

    # ── 사이트가 보낸 요청 ──

    def _fail(self, rid: str, text: str) -> None:
        self.link.report({"id": rid, "kind": "error", "error": text})

    def start(self, line: dict) -> None:
        rid, kind = line["id"], line["kind"]
        why = refuse(line)
        if why is not None:
            self._fail(rid, why)
            return
        # 둘러보기: 취소 넘기기
        if kind == "action_cancel":
            if rid in self.goals:
                self.goals[rid].cancel_goal_async()
            else:
                self.cancel_later.add(rid)
            return
        # 둘러보기 끝
        if kind == "topic_publish":
            self._publish(line)
            return
        if kind in ("service_call", "action_goal"):
            type_name = line["type"]
            if self._type(type_name) is None:
                say(missing_type(type_name))
                self._fail(rid, missing_type(type_name))
                return
        self.waiting.append((time.monotonic() + (PARAM_FIND_SECONDS if kind.startswith("param_") else FIND_SECONDS), line))

    # 둘러보기: 토픽 보내기
    def _publish(self, line: dict) -> None:
        topic = line["topic"]
        value = line["data"]["data"]
        message = self._type(PUBLISH_TOPICS[topic])(data=float(value) if PUBLISH_TOPICS[topic].endswith("Float64") else value)
        self.publishers[topic].publish(message)
        if topic in REPEAT_TOPICS:
            self.repeat[topic] = [message, time.monotonic() + REPEAT_EVERY]
        self.link.report({"id": line["id"], "kind": "topic_published"})
    # 둘러보기 끝

    def poll(self) -> None:
        """inbox의 요청을 시작하고, 기다리는 요청 · 답 · 다시 보내기를 한 번씩 본다."""
        while not self.inbox.empty():
            self.start(self.inbox.get())
        self.waiting[:] = [item for item in self.waiting if not self._try(*item)]
        still = []
        for deadline, rid, future in self.calling:
            if future.done() or rid in self.answered:
                continue
            if time.monotonic() < deadline:
                still.append((deadline, rid, future))
            else:
                self._answer(rid, {"kind": "error", "error": NO_NODE})
        self.calling[:] = still
        # 둘러보기: 신호 다시 보내기
        now = time.monotonic()
        for topic, item in self.repeat.items():
            if now >= item[1]:
                self.publishers[topic].publish(item[0])
                item[1] = max(item[1] + REPEAT_EVERY, now)
        # 둘러보기 끝

    def _answer(self, rid: str, result: dict) -> None:
        """파라미터 요청 하나에 결과를 한 번만 올린다."""
        if rid not in self.answered:
            self.answered.add(rid)
            self.link.report({"id": rid, **result})

    def _try(self, deadline: float, line: dict) -> bool:
        """서버 · 노드가 보이면 부르고 True. 찾는 시간이 지나면 실패를 알리고 True. 아직이면 False."""
        rid, kind = line["id"], line["kind"]
        if kind.startswith("param_"):
            client = self.param_clients[(line["node"], kind)]
            if not client.service_is_ready():
                if time.monotonic() < deadline:
                    return False
                self._answer(rid, {"kind": "error", "error": NO_NODE})
                return True
            self._call_param(line, client)
            return True
        name = "/" + line["name"]
        type_ = self._type(line["type"])
        if kind == "service_call":
            if name not in self.clients:
                self.clients[name] = self.node.create_client(type_, self.prefix + name)
            ready = self.clients[name].service_is_ready()
        else:
            if name not in self.actions:
                self.actions[name] = self.action_client(self.node, type_, self.prefix + name)
            ready = self.actions[name].server_is_ready()
        if not ready:
            if time.monotonic() < deadline:
                return False
            what = "서비스" if kind == "service_call" else "액션"
            self._fail(rid, f"{what} 서버 {self.prefix + name}을(를) 찾지 못했습니다. 내 노드가 켜져 있는지 보세요")
            return True
        # 둘러보기: 서비스 · 액션 부르기
        try:
            if kind == "service_call":
                future = self.clients[name].call_async(fill(type_.Request(), line["request"]))
                future.add_done_callback(lambda f: self._replied(rid, f.result()))
            else:
                future = self.actions[name].send_goal_async(
                    fill(type_.Goal(), line["goal"]),
                    feedback_callback=lambda f: self.link.report({"id": rid, "kind": "action_feedback", "feedback": plain(f.feedback)}),
                )
                future.add_done_callback(lambda f: self._accepted(rid, f.result()))
        except (TypeError, ValueError, AssertionError, AttributeError):
            self._fail(rid, "요청 값이 형식에 맞지 않습니다")
        # 둘러보기 끝
        return True

    # 둘러보기: 서비스 답 올리기
    def _replied(self, rid: str, response) -> None:
        if response is None:
            self._fail(rid, "서비스 서버가 답을 주지 않았습니다. 내 노드의 터미널에 오류가 있는지 보세요")
        else:
            self.link.report({"id": rid, "kind": "service_reply", "response": plain(response)})
    # 둘러보기 끝

    # 둘러보기: 액션 결과 올리기
    def _accepted(self, rid: str, handle) -> None:
        statuses = {4: "succeeded", 5: "canceled", 6: "aborted"}  # action_msgs/msg/GoalStatus 값
        if handle is None or not handle.accepted:
            self._fail(rid, "학생 노드가 목표를 받지 않았습니다(거절)")
            return
        self.goals[rid] = handle
        self.link.report({"id": rid, "kind": "action_accepted"})
        if rid in self.cancel_later:
            handle.cancel_goal_async()
        handle.get_result_async().add_done_callback(
            lambda f: self.link.report(
                {"id": rid, "kind": "action_result", "status": statuses.get(f.result().status, "aborted"), "result": plain(f.result().result)}
            )
        )
    # 둘러보기 끝

    # 둘러보기: 파라미터 부르기
    def _call_param(self, line: dict, client) -> None:
        rid, name = line["id"], line["name"]
        if line["kind"] == "param_get":
            request = self.param["get"].Request()
            request.names = [name]

            def got(future) -> None:
                values = future.result().values
                self._answer(rid, {"kind": "param_values", "values": {name: param_python(values[0]) if values else None}})

            callback = got
        else:
            request = self.param["set"].Request()
            request.parameters = [self.param["parameter"](name=name, value=param_message(self.param["value"], line["value"]))]

            def done(future) -> None:
                results = future.result().results
                ok = bool(results and results[0].successful)
                reason = str(results[0].reason)[:200] if results else "노드가 답을 주지 않았습니다"
                self._answer(rid, {"kind": "param_set_result", "successful": ok, "reason": reason})

            callback = done
        future = client.call_async(request)
        future.add_done_callback(callback)
        self.calling.append((time.monotonic() + PARAM_FIND_SECONDS, rid, future))
    # 둘러보기 끝


def run_ros(link: Link, namespace: str) -> None:
    # rclpy와 형식은 여기서만 불러온다. --demo에서는 ROS 2가 없어도 돈다.
    import rclpy
    from rclpy.action import ActionClient
    from rclpy.node import Node

    rclpy.init()
    node = Node("robom_link")
    try:
        side = RosSide(link, node, namespace, load_type, ActionClient)
        prefix = side.prefix
        say(f"드론: {prefix}/drone/pose · {prefix}/drone/battery → 로봄 · 로봄 → {prefix}/drone/command")
        say(f"서비스 · 액션: {' · '.join(prefix + n for n in (*SERVICES, *ACTIONS))} ({INTERFACES})")
        say(f"토픽 지켜보기: {' · '.join(prefix + t for t in WATCH_TOPICS)} · 보내기: {' · '.join(prefix + t for t in PUBLISH_TOPICS)}")
        while not link.stop.is_set() and rclpy.ok():
            rclpy.spin_once(node, timeout_sec=0.05)
            side.poll()
    finally:
        node.destroy_node()
        if rclpy.ok():
            rclpy.shutdown()


# ── 시작 ──────────────────────────────────────────────────────


def parse(argv):
    parser = argparse.ArgumentParser(prog="robom_link.py", description="로봄 연결기 판 " + VERSION)
    parser.add_argument("--key", required=True, help="사이트 「연결 열쇠 받기」에서 받은 열쇠")
    parser.add_argument("--server", default=None, help="로봄 사이트 주소(기본: 내려받은 사이트)")
    parser.add_argument("--ns", default="", help="이름 공간(예: drone1 → /drone1/drone/pose)")
    parser.add_argument("--demo", action="store_true", help="ROS 2 없이 가짜 노드가 답한다(연습 모드: 확인받기는 되지 않는다)")
    return parser.parse_args(argv)


def main(argv=None) -> int:
    args = parse(argv)
    server = args.server or DEFAULT_SERVER
    if server.startswith("__"):
        say("서버 주소가 없습니다. --server <로봄 사이트 주소>를 붙여 다시 켜세요")
        return 2
    if not server.startswith(("https://", "http://")):
        say("서버 주소는 https://로 시작해야 합니다. --server 값을 다시 보세요")
        return 2
    if args.ns and not re.fullmatch(r"[A-Za-z][A-Za-z0-9_]{0,31}", args.ns):
        say("--ns는 영문자로 시작하는 32자 안의 영문 · 숫자 · _만 됩니다")
        return 2
    say(f"로봄 연결기 판 {VERSION} · 서버 {server.rstrip('/')}")
    link = Link(Server(server, args.key, demo=args.demo))
    loops = (link.send_loop, link.command_loop, link.result_loop, link.watch_loop)
    threads = [threading.Thread(target=loop, daemon=True) for loop in loops]
    for thread in threads:
        thread.start()
    try:
        if args.demo:
            run_demo(link)
        else:
            run_ros(link, args.ns)
    except KeyboardInterrupt:
        say("연결기를 끕니다")
    except MissingType as error:
        say(str(error))
        return 2
    except ImportError:
        say("ROS 2(rclpy)를 찾지 못했습니다. source /opt/ros/jazzy/setup.bash 를 먼저 하거나 --demo로 켜 보세요")
        return 2
    finally:
        link.stop.set()
    return link.exit_code


if __name__ == "__main__":
    sys.exit(main())
