runtime.py

# -*- coding: utf-8 -*-
"""TaskRuntime：單一包裹任務的 FSM 實例。

PICK → REORIENT → PUSH → DONE
            │
            └── (任一階段失敗) ──► FAULT

對應架構教學文件 §5.3 / §10.3、interfaces.md §附錄 A 的 fsm_state 列舉。
"""
from __future__ import annotations

import time
from dataclasses import dataclass, field
from enum import Enum
from typing import Optional

from .events import SkillEvent, VisionObservation


class TaskState(str, Enum):
    PICK = "PICK"
    REORIENT = "REORIENT"
    PUSH = "PUSH"
    DONE = "DONE"
    FAULT = "FAULT"


# 進度條映射（給 UI 用）
PROGRESS_MAP = {
    TaskState.PICK: 0.0,
    TaskState.REORIENT: 0.33,
    TaskState.PUSH: 0.66,
    TaskState.DONE: 1.0,
    TaskState.FAULT: 0.0,
}


@dataclass
class Transition:
    prev: TaskState
    curr: TaskState
    reason: str = ""
    fault_code: Optional[str] = None
    ts: float = field(default_factory=time.time)


@dataclass
class TaskRuntime:
    """單一包裹任務的 FSM。

    `evaluate()` 是純函式（不訪問外部）— 只接受事件、回傳 Transition or None。
    狀態跳轉的副作用（廣播、寫上下文）由 TaskMonitor 在外面做。
    """

    task_id: str
    package_id: str
    subtask_id: str = ""
    state: TaskState = TaskState.PICK
    retry_count: int = 0
    reorient_max_retry: int = 3
    push_retry_max: int = 1
    grasp_timeout_sec: float = 8.0
    push_timeout_sec: float = 6.0
    # 連續 N 幀視覺確認後才跳轉
    confirm_frames: int = 3
    _state_entered_at: float = field(default_factory=time.time)
    _consec_face_up: int = 0
    _consec_face_down_or_side: int = 0
    _consec_zone_right: int = 0
    _grasp_started: bool = False
    _grasp_ok: bool = False

    # --- 對外接口 ---------------------------------------------------------
    def evaluate(self, evt) -> Optional[Transition]:
        """根據 vision / skill 事件決定是否跳轉。

        Args:
            evt: VisionObservation 或 SkillEvent。
        Returns:
            Transition（已套用到 self.state）或 None（不跳轉）。
        """
        if isinstance(evt, SkillEvent):
            return self._on_skill(evt)
        if isinstance(evt, VisionObservation):
            return self._on_vision(evt)
        return None

    def force(self, target: TaskState, reason: str = "force") -> Transition:
        prev, self.state = self.state, target
        self._reset_counters()
        self._state_entered_at = time.time()
        return Transition(prev=prev, curr=target, reason=reason)

    def is_terminal(self) -> bool:
        return self.state in (TaskState.DONE, TaskState.FAULT)

    @property
    def progress(self) -> float:
        return PROGRESS_MAP.get(self.state, 0.0)

    @property
    def time_in_state(self) -> float:
        return time.time() - self._state_entered_at

    # --- 內部：技能事件 ---------------------------------------------------
    def _on_skill(self, evt: SkillEvent) -> Optional[Transition]:
        if not evt.is_terminal:
            if evt.skill_name.startswith(("G1", "G2")) and evt.phase == "begin":
                self._grasp_started = True
            return None

        # 任何技能失敗 → FAULT（除非已經 terminal）
        if evt.status in ("fail", "timeout") and not self.is_terminal():
            return self._goto(
                TaskState.FAULT,
                reason=f"skill_fault:{evt.skill_name}",
                fault_code=evt.fault_code or "SK-UNKNOWN",
            )

        # PICK 階段：等抓取類技能（G1/G2）回傳 ok
        if (
            self.state == TaskState.PICK
            and evt.status == "ok"
            and (evt.skill_name.startswith("G1") or evt.skill_name.startswith("G2"))
        ):
            self._grasp_ok = True
            return self._goto(TaskState.REORIENT, reason=f"grasped:{evt.skill_name}")

        return None

    # --- 內部：視覺事件 ---------------------------------------------------
    def _on_vision(self, obs: VisionObservation) -> Optional[Transition]:
        # 確認是我的包裹
        if obs.package_id != self.package_id:
            return None

        # PICK 超時 → FAULT (GRASP_TIMEOUT)
        if (
            self.state == TaskState.PICK
            and self.time_in_state > self.grasp_timeout_sec
        ):
            return self._goto(
                TaskState.FAULT, reason="grasp_timeout", fault_code="TM-003"
            )

        # REORIENT：等條碼朝上連續 N 幀
        if self.state == TaskState.REORIENT:
            if obs.barcode_face == "up":
                self._consec_face_up += 1
                self._consec_face_down_or_side = 0
                if self._consec_face_up >= self.confirm_frames:
                    return self._goto(
                        TaskState.PUSH, reason="barcode_face_up_confirmed"
                    )
            elif obs.barcode_face in ("down", "side"):
                self._consec_face_down_or_side += 1
                self._consec_face_up = 0

        # PUSH：等包裹進入 right_conveyor 連續 N 幀
        if self.state == TaskState.PUSH:
            if obs.zone == "right_conveyor":
                self._consec_zone_right += 1
                if self._consec_zone_right >= self.confirm_frames:
                    return self._goto(TaskState.DONE, reason="placed_on_right_conveyor")
            else:
                self._consec_zone_right = 0

            if self.time_in_state > self.push_timeout_sec:
                if self.retry_count >= self.push_retry_max:
                    return self._goto(
                        TaskState.FAULT,
                        reason="place_out_of_zone",
                        fault_code="TM-005",
                    )

        return None

    # --- 內部：state 跳轉 -------------------------------------------------
    def _goto(
        self, target: TaskState, reason: str, fault_code: Optional[str] = None
    ) -> Transition:
        prev = self.state
        # REORIENT 失敗超過上限 → FAULT
        if (
            target == TaskState.PUSH
            and prev == TaskState.REORIENT
            and self._consec_face_down_or_side > self.reorient_max_retry
        ):
            self.state = TaskState.FAULT
            return Transition(
                prev=prev, curr=self.state, reason="reorient_fail", fault_code="TM-004"
            )

        self.state = target
        self._state_entered_at = time.time()
        self._reset_counters()
        return Transition(prev=prev, curr=target, reason=reason, fault_code=fault_code)

    def _reset_counters(self) -> None:
        self._consec_face_up = 0
        self._consec_face_down_or_side = 0
        self._consec_zone_right = 0


__all__ = ["TaskRuntime", "TaskState", "Transition", "PROGRESS_MAP"]