#!/usr/bin/env python3
"""로봄 연습 셔틀 노드 — 셔틀 호출 구역의 순환 노선을 도는 셔틀. 뼈대는 되어 있고, 「# 할 일」을 채워 완성하는 노드다.

쓰는 법 (Ubuntu 24.04 + ROS 2 Jazzy, 형식 패키지 robom_interfaces를 빌드해 둔 뒤):
    source /opt/ros/jazzy/setup.bash
    source ~/robom_ws/install/setup.bash
    python3 robom_shuttle_sim.py
    # 다른 터미널에서도 같은 두 줄을 source하고 연결기를 켠다: python3 robom_link.py --key <연결 열쇠>

이 셔틀 노드가 하는 일 (한 노드가 두 역할):
- 퍼블리셔: 노선(한 바퀴 12칸, 1초에 1칸) 위의 자기 위치를 /shuttle/position(std_msgs/msg/Float64)으로 1초에 5번 보낸다. (완성됨)
- 서비스 서버: /shuttle/arrival(robom_interfaces/srv/ShuttleEta)을 열고, 「n번 정류장까지 몇 분?」(request.stop)에
  남은 분(response.minutes)으로 답한다. (서버를 여는 데까지는 완성됨, 계산은 「# 할 일」)
- --ns <이름>: 토픽 · 서비스 이름 앞에 이름 공간을 붙인다(/<이름>/shuttle/...). 연결기의 --ns와 같은 이름을 쓴다.

정류장 1 · 2 · 3은 노선의 0 · 4 · 8칸째에 있다. 1칸 = 1분으로 본다.
그대로 켜면 과제 1(서비스로 답하기)은 통과하지만, 과제 2(셔틀 위치로 답하기)는 「셔틀 위치로 보면 n분입니다」 한 줄로 떨어진다.
파이썬 표준 라이브러리와 ROS 2에 들어 있는 rclpy · std_msgs, 그리고 내려받아 빌드한 robom_interfaces만 쓴다.
이 파일을 고쳐 가며 배우세요. 「# 할 일」을 채운 뒤 노드를 다시 켜고 같은 정류장을 두 번 물어 보면 답이 바뀌는 것이 보입니다.
"""

import argparse
import re
import sys

VERSION = "2"
LAP = 12.0  # 노선 한 바퀴 칸 수
SPEED = 1.0  # 1초에 가는 칸 수
SEND_PER_SECOND = 5  # 위치를 보내는 횟수(1초에)
STOPS = {1: 0.0, 2: 4.0, 3: 8.0}  # 정류장 번호 → 노선의 몇 칸째


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


def make_node(namespace: str):
    """셔틀 노드를 만든다. rclpy와 메시지 형식은 여기서만 불러온다."""
    from rclpy.node import Node
    from robom_interfaces.srv import ShuttleEta
    from std_msgs.msg import Float64

    prefix = f"/{namespace}" if namespace else ""

    # 둘러보기: 셔틀 노드 만들기
    class Shuttle(Node):
        def __init__(self):
            super().__init__("robom_shuttle_sim")
            self.position = 0.0  # 지금 노선의 몇 칸째에 있나(0 이상 12 미만)
            self.publisher = self.create_publisher(Float64, prefix + "/shuttle/position", 10)
            self.create_timer(1.0 / SEND_PER_SECOND, self.drive)
            self.create_service(ShuttleEta, prefix + "/shuttle/arrival", self.arrival)
    # 둘러보기 끝

        # 둘러보기: 위치 보내기
        def drive(self):
            # 타이머 콜백: 셔틀을 조금 움직이고 지금 위치를 보낸다. 연결기가 이 토픽을 구독해 지도에 셔틀을 그린다.
            self.position = (self.position + SPEED / SEND_PER_SECOND) % LAP
            message = Float64()
            message.data = self.position
            self.publisher.publish(message)
        # 둘러보기 끝

        # 둘러보기: 정류장 답하기
        def arrival(self, request, response):
            # 서비스 콜백: 요청(request.stop)을 읽고 답(response.minutes)을 채워 돌려준다.
            # 연결기는 로봄 서버가 보낸 요청을 이 서비스에 넘기고, 돌려준 답을 서버로 보낸다.
            stop = request.stop
            # 할 일: 셔틀이 지금 어디 있는지 보지 않습니다. self.position으로 계산하세요
            #        (STOPS[stop]이 정류장 자리, 노선은 한 바퀴 LAP칸, 정류장이 셔틀 뒤에 있으면 한 바퀴를 돌아야 닿습니다)
            response.minutes = stop * 2
            say(f"요청 받음: 정류장 {stop}번 → {response.minutes}분 뒤 도착 (셔틀은 {self.position:.1f}칸째)")
            return response
        # 둘러보기 끝

    return Shuttle()


# 둘러보기: 노드 돌리기
def run(namespace: str) -> None:
    import rclpy

    rclpy.init()
    node = make_node(namespace)
    say(f"로봄 연습 셔틀 노드 판 {VERSION} · /shuttle/position을 보내고 /shuttle/arrival(robom_interfaces/srv/ShuttleEta)에 답합니다")
    try:
        rclpy.spin(node)  # 타이머마다 drive(), 요청이 올 때마다 arrival()이 불린다
    except KeyboardInterrupt:
        say("연습 셔틀 노드를 끕니다")
    finally:
        node.destroy_node()
        if rclpy.ok():
            rclpy.shutdown()
# 둘러보기 끝


def main(argv=None) -> int:
    parser = argparse.ArgumentParser(prog="robom_shuttle_sim.py", description="로봄 연습 셔틀 노드 판 " + VERSION)
    parser.add_argument("--ns", default="", help="이름 공간(예: bus1 → /bus1/shuttle/arrival)")
    args = parser.parse_args(argv)
    if args.ns and not re.fullmatch(r"[A-Za-z][A-Za-z0-9_]{0,31}", args.ns):
        say("--ns는 영문자로 시작하는 32자 안의 영문 · 숫자 · _만 됩니다")
        return 2
    try:
        run(args.ns)
    except ImportError:
        say("ROS 2(rclpy)나 robom_interfaces를 찾지 못했습니다. source /opt/ros/jazzy/setup.bash 와 "
            "source ~/robom_ws/install/setup.bash 를 먼저 했는지 보세요")
        return 2
    return 0


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