#!/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_cafe_sim.py
    # 다른 터미널에서도 같은 두 줄을 source하고 연결기를 켠다: python3 robom_link.py --key <연결 열쇠>

이 바리스타 노드가 하는 일 (액션 서버):
- /cafe/brew 액션(robom_interfaces/action/Brew)을 연다. 목표는 메뉴(menu: americano · latte)와 잔 수(cups: 1 ~ 3)다. (완성됨)
- 목표를 받으면 메뉴의 단계를 잔마다 밟으며, 단계마다 진행률(step · cups_done · percent)을 보낸다. (「# 할 일」)
- 다 만들면 결과(다 만든 잔 수 cups_done, 걸린 초 seconds)를 돌려준다. (「# 할 일」)
- 「주문 취소」가 오면 받아들이고, 그때까지 다 만든 잔 수만 돌려주며 취소(canceled)로 끝낸다. (「# 할 일」)
- --ns <이름>: 액션 이름 앞에 이름 공간을 붙인다(/<이름>/cafe/brew). 연결기의 --ns와 같은 이름을 쓴다.

메뉴의 단계(잔마다): 아메리카노 = grind → extract → pour_water, 라떼 = grind → extract → steam_milk → pour. 단계 하나는 약 1초.
그대로 켜면 주문은 받지만 곧바로 「0잔 완성」으로 끝나고 진행률도 취소도 없어서, 과제 셋 모두 한 줄로 떨어진다.
파이썬 표준 라이브러리와 ROS 2에 들어 있는 rclpy, 그리고 내려받아 빌드한 robom_interfaces만 쓴다.
이 파일을 고쳐 가며 배우세요. 「# 할 일」을 채운 뒤 노드를 다시 켜고 카운터에서 주문하면 진행 줄이 단계마다 바뀌는 것이 보입니다.
"""

import argparse
import re
import sys
import time

VERSION = "2"
STEP_SECONDS = 1.0  # 단계 하나에 걸리는 시간
MENUS = {"americano": ("grind", "extract", "pour_water"), "latte": ("grind", "extract", "steam_milk", "pour")}


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


def make_node(namespace: str):
    """바리스타 노드를 만든다. rclpy와 메시지 형식은 여기서만 불러온다."""
    from rclpy.action import ActionServer, CancelResponse, GoalResponse
    from rclpy.callback_groups import ReentrantCallbackGroup
    from rclpy.node import Node
    from robom_interfaces.action import Brew

    name = f"/{namespace}/cafe/brew" if namespace else "/cafe/brew"

    # 둘러보기: 액션 서버 열기
    class Barista(Node):
        def __init__(self):
            super().__init__("robom_cafe_sim")
            # 취소가 실행 중에 닿으려면 콜백이 동시에 돌 수 있어야 한다(ReentrantCallbackGroup + MultiThreadedExecutor).
            group = ReentrantCallbackGroup()
            self.server = ActionServer(self, Brew, name, self.brew, goal_callback=self.on_goal, cancel_callback=self.on_cancel, callback_group=group)
    # 둘러보기 끝

        # 둘러보기: 주문 받을지 정하기
        def on_goal(self, goal_request):
            # 목표를 받을지 정한다. 아는 메뉴이고 잔 수가 1 ~ 3이면 받는다.
            ok = goal_request.menu in MENUS and 1 <= goal_request.cups <= 3
            say(f"주문 받음: {goal_request.menu} {goal_request.cups}잔" + ("" if ok else " → 받지 않음"))
            return GoalResponse.ACCEPT if ok else GoalResponse.REJECT
        # 둘러보기 끝

        # 둘러보기: 취소 받을지 정하기
        def on_cancel(self, goal_handle):
            # 「주문 취소」를 받을지 정한다.
            # 할 일: 지금은 취소를 늘 거절합니다. 받아들이는 답을 돌려주세요. 실제로 멈추는 일은 brew()의 단계마다 합니다
            say("취소 요청 받음 → 거절")
            return CancelResponse.REJECT
        # 둘러보기 끝

        # 둘러보기: 만들기
        def brew(self, goal_handle):
            # 실행 콜백: 목표(goal_handle.request.menu · cups)를 읽고 만든다. 연결기는 서버가 보낸 주문을 이 액션에 넘기고,
            # 여기서 보내는 진행률과 결과를 서버로 보낸다.
            menu, cups = goal_handle.request.menu, goal_handle.request.cups
            steps = MENUS[menu]
            started = time.monotonic()
            # 할 일: 메뉴의 단계마다 feedback을 보내세요(잔마다 steps를 밟고, 단계를 마칠 때마다 지금 단계 · 다 만든 잔 수 ·
            #        전체 진행률을 담아 보냅니다. 단계마다 취소가 요청됐는지 보고, 요청됐으면 그때까지 만든 잔 수로 취소로 끝냅니다)
            result = Brew.Result()
            result.cups_done = 0  # 할 일: 다 만든 잔 수를 넣으세요
            result.seconds = float(time.monotonic() - started)
            goal_handle.succeed()  # 성공으로 끝낸다
            say(f"{menu} {result.cups_done}잔 완성 · {result.seconds:.1f}초 (단계 {len(steps)}개 × {cups}잔을 밟지 않았습니다)")
            return result
        # 둘러보기 끝

    return Barista()


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

    rclpy.init()
    node = make_node(namespace)
    executor = MultiThreadedExecutor()
    executor.add_node(node)
    say(f"로봄 연습 바리스타 노드 판 {VERSION} · 액션 /cafe/brew(robom_interfaces/action/Brew)를 엽니다")
    try:
        executor.spin()  # 목표가 올 때마다 on_goal() → brew(), 취소가 오면 on_cancel()이 불린다
    except KeyboardInterrupt:
        say("연습 바리스타 노드를 끕니다")
    finally:
        node.destroy_node()
        if rclpy.ok():
            rclpy.shutdown()
# 둘러보기 끝


def main(argv=None) -> int:
    parser = argparse.ArgumentParser(prog="robom_cafe_sim.py", description="로봄 연습 바리스타 노드 판 " + VERSION)
    parser.add_argument("--ns", default="", help="액션 이름 공간(예: cafe1 → /cafe1/cafe/brew)")
    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())
