from gerri import SubControllerCommon
from synapse import Capability, Result

class TemplateSub(SubControllerCommon):
    def __init__(self, info, sdk=None):
        super().__init__(info)
        self._sdk = sdk or _FakeSDK()
        self.capability = [
            Capability.NAVIGATE_TO_POSE,
            Capability.VELOCITY_CTRL,
            Capability.DOCK, Capability.UNDOCK,
            Capability.STOP,            # ← 필수
        ]

    def navigate_to_pose(self, x, y, theta=0.0, timeout_s=30.0) -> Result:
        """좌표로 이동. x,y(미터, map frame) · theta(도, deg) · timeout_s(초)."""
        try:
            if self._sdk.move_to(x, y, theta):
                return Result.ok(message="도착", reached=[x, y, theta])
            return Result.fail("not_reached", message="도달 실패")
        except TimeoutError:
            return Result.fail("timeout", message=f"{timeout_s}초 초과")

    def velocity_ctrl(self, linear, angular) -> Result:
        """직접 속도 제어. linear(m/s) · angular(deg/s)."""
        self._sdk.set_velocity(linear, angular)
        return Result.ok()

    def dock(self, charger_id=None) -> Result: return Result.ok(message="도킹")
    def undock(self) -> Result: return Result.ok()

    def stop(self) -> Result:
        """필수 — 로봇을 IDLE 로."""
        self._sdk.halt()
        return Result.ok(message="정지")