#!/usr/bin/env python3
"""로봄 연습 교차로 셔틀 노드 — 신호를 받아 두고 자기 주기로 주행 명령을 내는 ROS 2 노드(뼈대).

쓰는 법 (Ubuntu 24.04 + ROS 2 Jazzy):
    source /opt/ros/jazzy/setup.bash
    python3 robom_crossroad_sim.py
    # 다른 터미널에서 연결기를 켠다: python3 robom_link.py --key <연결 열쇠>

장면: 교차로의 신호 제어기가 지금 신호를 계속 방송한다(연결기가 0.5초마다 다시 보낸다). 셔틀은 받은 신호를 기억해 두고,
자기 타이머로 1초에 5번 주행 명령을 낸다.

- 구독자: /crossroad/signal(std_msgs/msg/String, red · yellow · green)을 받아 마지막 신호를 저장해 둔다.
- 퍼블리셔: 타이머가 1초에 5번 /crossroad/cmd_vel(geometry_msgs/msg/Twist)을 낸다. linear.x는 decide()가 고른다.
- --ns <이름>: 토픽 이름 앞에 이름 공간을 붙인다(/<이름>/crossroad/signal). 연결기의 --ns와 같은 이름을 쓴다.

연결 · 구독 · 타이머 · 발행 뼈대는 다 되어 있습니다. 「# 할 일」 자리를 채워 완성하세요. 이 파일을 고쳐 가며 배우세요.
그대로 켜면 셔틀이 신호와 상관없이 늘 서 있어(linear.x 0.0) 교차로 과제를 통과하지 못합니다.

파이썬 표준 라이브러리와 ROS 2에 들어 있는 rclpy · std_msgs · geometry_msgs만 쓴다(따로 설치할 것이 없다).
"""

import argparse
import re
import sys

VERSION = "2"
SEND_PER_SECOND = 5  # 주행 명령을 내는 횟수(1초에)


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


# 둘러보기: 속도 고르기
def decide(signal: str | None) -> float:
    """받아 둔 신호(아직 받은 신호가 없으면 None)로 셔틀 속도(linear.x)를 고른다(ROS 2와 상관없는 계산)."""
    # 할 일: 신호별 속도. 초록(green)이면 0.5, 노랑(yellow)이면 0.2, 빨강(red)이면 0.0을 돌려주세요.
    #        모르는 신호이거나 아직 받은 신호가 없으면 서 있어야 합니다(0.0).
    return 0.0
# 둘러보기 끝


def run(namespace: str) -> None:
    # rclpy와 메시지 형식은 여기서만 불러온다.
    import rclpy
    from geometry_msgs.msg import Twist
    from rclpy.node import Node
    from std_msgs.msg import String

    # 둘러보기: 노드와 퍼블리셔
    prefix = f"/{namespace}/crossroad" if namespace else "/crossroad"
    rclpy.init()
    node = Node("robom_crossroad_sim")
    state = {"signal": None}  # 마지막으로 받은 신호(구독 콜백이 저장하고 타이머 콜백이 읽는다)
    command = node.create_publisher(Twist, prefix + "/cmd_vel", 10)  # 주행 명령을 낼 토픽
    # 둘러보기 끝

    # 둘러보기: 신호 받아 두기
    def on_signal(message) -> None:
        # 구독 콜백: 신호 메시지가 올 때마다 불린다(신호 제어기가 0.5초마다 다시 보낸다). 여기서는 저장만 한다.
        if message.data != state["signal"]:
            say(f"신호 바뀜: {state['signal']} → {message.data}")
        state["signal"] = message.data
    # 둘러보기 끝

    # 둘러보기: 주행 명령 내기
    def drive() -> None:
        # 타이머 콜백: 1초에 SEND_PER_SECOND번 불린다. 저장해 둔 신호로 속도를 골라 Twist를 낸다(다른 칸은 0).
        twist = Twist()
        twist.linear.x = float(decide(state["signal"]))
        command.publish(twist)
    # 둘러보기 끝

    # 둘러보기: 구독 · 타이머 걸고 돌리기
    node.create_subscription(String, prefix + "/signal", on_signal, 10)
    node.create_timer(1.0 / SEND_PER_SECOND, drive)
    say(f"로봄 연습 교차로 셔틀 노드 판 {VERSION} · 받음 {prefix}/signal · 냄 {prefix}/cmd_vel(1초에 {SEND_PER_SECOND}번)")
    try:
        rclpy.spin(node)  # 신호가 오면 on_signal(), 주기마다 drive()가 불린다
    except KeyboardInterrupt:
        say("연습 교차로 셔틀 노드를 끕니다")
    finally:
        node.destroy_node()
        if rclpy.ok():
            rclpy.shutdown()
    # 둘러보기 끝


def main(argv=None) -> int:
    parser = argparse.ArgumentParser(prog="robom_crossroad_sim.py", description="로봄 연습 교차로 셔틀 노드 판 " + VERSION)
    parser.add_argument("--ns", default="", help="토픽 이름 공간(예: car1 → /car1/crossroad/signal)")
    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)를 찾지 못했습니다. source /opt/ros/jazzy/setup.bash 를 먼저 하세요")
        return 2
    return 0


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