#!/usr/bin/env python3
"""로봄 연습 드론 노드 — 하늘 배달 드론. 뼈대는 되어 있고, 「# 할 일」을 채워 완성하는 노드다.

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

이 드론 노드가 하는 일 (퍼블리셔 둘 · 구독자 하나):
- /drone/pose(geometry_msgs/msg/PoseStamped)와 /drone/battery(sensor_msgs/msg/BatteryState)를 초당 5번 보낸다. (완성됨)
- 위치의 header.frame_id는 지도 좌표의 이름이어야 한다. 지금은 odom이다. (「# 할 일」)
- /drone/command(std_msgs/msg/String)를 받는다. start면 창고(12, 48)와 도착지(88, 14) 사이 배달 길을 오가며 날고(끝에 닿으면
  돌아 나온다), stop이면 그 자리 그 높이에서 떠 있는다. 지금은 받은 글자를 찍기만 한다. (「# 할 일」)
- --ns <이름>: 토픽 앞에 이름 공간을 붙인다(/<이름>/drone/pose). 연결기의 --ns와 같은 이름을 쓴다.

지도 좌표는 ROS 2 map 좌표처럼 x는 오른쪽, y는 위로 커진다(가로 100 · 세로 60). 사이트 화면이 뒤집어 그린다.
그대로 켜면 과제 1(연결하기)은 되지만, 과제 2 · 3은 「무엇이 안 됐는지」 한 줄로 떨어진다.
파이썬 표준 라이브러리와 ROS 2에 들어 있는 rclpy만 쓴다(따로 설치할 것이 없다).
"""

import argparse
import re
import sys

VERSION = "2"
RATE = 5  # 초당 보내기 횟수
DEPOT, DROPOFF = (12.0, 48.0), (88.0, 14.0)  # 창고 · 도착지(map 좌표)
SPEED = 4.0  # 지도 단위/초
ALTITUDE = 10.0
# 둘러보기: 지도 좌표 이름
FRAME_ID = "odom"  # 할 일: 하늘 배달 지도의 좌표 이름으로 바꾸세요(과제 2)
# 둘러보기 끝
DRAIN_FLYING = 0.001  # 날 때 초당 배터리 감소(0~1): 다 차 있으면 16분 남짓 난다
DRAIN_HOVER = 0.0  # 서 있거나 떠 있을 때는 닳지 않는다(수업 내내 켜 두어도 날 수 있게)
FULL_VOLTAGE = 12.6


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


class SimDrone:
    """ROS 2와 상관없는 드론 상태. 처음에는 창고 바닥에 서 있다."""

    def __init__(self):
        self.x, self.y, self.z = DEPOT[0], DEPOT[1], 0.0
        self.flying = False  # 배달 길을 날고 있는가
        self.goal = DROPOFF  # 지금 날아가는 끝(닿으면 다른 끝으로 바꿔 돌아 나온다)
        self.battery = 1.0

    # 둘러보기: 명령 알아듣기
    def command(self, command: str) -> bool:
        """/drone/command로 받은 글자. 알아들은 명령이면 True."""
        if command not in ("start", "stop"):
            return False
        # 할 일: start면 날기 시작하고(self.flying), stop이면 그 자리 그 높이에서 떠 있게 하세요
        return True
    # 둘러보기 끝

    # 둘러보기: 나는 걸음
    def step(self, dt: float) -> None:
        """타이머마다 dt초만큼 상태를 고친다."""
        if self.flying and self.battery > 0:
            pass  # 할 일: 날고 있으면 높이를 맞추고, 지금 자리에서 가는 끝(self.goal) 쪽으로 이번 주기에 갈 만큼 옮기세요. 끝에 닿으면 다른 끝으로 바꿔 돌아 나옵니다
        drain = DRAIN_FLYING if self.flying else DRAIN_HOVER
        self.battery = max(0.0, self.battery - drain * dt)
    # 둘러보기 끝

    def position(self) -> tuple[float, float, float]:
        return self.x, self.y, self.z


def run(namespace: str) -> int:
    # rclpy는 여기서만 불러온다.
    import rclpy
    from geometry_msgs.msg import PoseStamped
    from rclpy.node import Node
    from sensor_msgs.msg import BatteryState
    from std_msgs.msg import String

    prefix = f"/{namespace}/drone" if namespace else "/drone"
    rclpy.init()
    node = Node("robom_drone_sim")
    drone = SimDrone()
    # 둘러보기: 퍼블리셔 · 구독자 만들기
    pose_pub = node.create_publisher(PoseStamped, prefix + "/pose", 10)
    battery_pub = node.create_publisher(BatteryState, prefix + "/battery", 10)

    def on_command(message) -> None:  # 구독자 콜백: 연결기가 사이트의 「출발 · 멈춤」을 이 토픽으로 넘긴다
        if drone.command(message.data):
            say(f"명령 받음: {message.data}")

    node.create_subscription(String, prefix + "/command", on_command, 10)
    # 둘러보기 끝

    # 둘러보기: 위치 · 배터리 보내기
    def tick() -> None:
        drone.step(1.0 / RATE)
        x, y, z = drone.position()
        pose = PoseStamped()
        pose.header.stamp = node.get_clock().now().to_msg()
        pose.header.frame_id = FRAME_ID
        pose.pose.position.x, pose.pose.position.y, pose.pose.position.z = float(x), float(y), float(z)
        pose.pose.orientation.w = 1.0
        pose_pub.publish(pose)
        battery = BatteryState()
        battery.header.stamp = pose.header.stamp
        battery.percentage = float(drone.battery)
        battery.voltage = float(FULL_VOLTAGE * (0.8 + 0.2 * drone.battery))
        battery.present = True
        battery_pub.publish(battery)

    node.create_timer(1.0 / RATE, tick)
    # 둘러보기 끝
    say(f"로봄 연습 드론 노드 판 {VERSION} · 토픽 {prefix}/pose · {prefix}/battery 보냄 · {prefix}/command 받음")
    say(f"지금은 창고({DEPOT[0]:.0f}, {DEPOT[1]:.0f})에 서 있습니다. 「# 할 일」을 채우면 「출발」에 배달 길을 오가며 납니다")
    # 둘러보기: 노드 돌리기
    try:
        rclpy.spin(node)
    except KeyboardInterrupt:
        say("연습 드론 노드를 끕니다")
    finally:
        node.destroy_node()
        if rclpy.ok():
            rclpy.shutdown()
    # 둘러보기 끝
    return 0


def main(argv=None) -> int:
    parser = argparse.ArgumentParser(prog="robom_drone_sim.py", description="로봄 연습 드론 노드 판 " + VERSION)
    parser.add_argument("--ns", default="", help="토픽 이름 공간(예: drone1 → /drone1/drone/pose)")
    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:
        return run(args.ns)
    except ImportError:
        say("ROS 2(rclpy)를 찾지 못했습니다. source /opt/ros/jazzy/setup.bash 를 먼저 하세요")
        return 2


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