"""Two claw control for Keyestudio-style USB servo boards."""

from __future__ import annotations

import logging

from .config import AgentConfig

LOG = logging.getLogger(__name__)


class ClawController:
    def __init__(self, config: AgentConfig):
        self.config = config
        self.hardware_enabled = config.hardware_enabled
        self._ports = {}
        if self.hardware_enabled:
            try:
                import serial  # type: ignore

                self._ports["left"] = serial.Serial(config.left_claw_device, 115200, timeout=0.2)
                self._ports["right"] = serial.Serial(config.right_claw_device, 115200, timeout=0.2)
            except Exception:
                LOG.exception("Claw hardware setup failed; disabling claw serial output")
                self.hardware_enabled = False

    def open(self, side: str = "both") -> None:
        self._pose(side, "open")

    def close(self, side: str = "both") -> None:
        self._pose(side, "close")

    def home(self, side: str = "both") -> None:
        self._pose(side, "home")

    def _pose(self, side: str, pose: str) -> None:
        sides = ["left", "right"] if side == "both" else [side]
        LOG.info("claw pose side=%s pose=%s", side, pose)
        if not self.hardware_enabled:
            return
        # Placeholder protocol. Replace with the exact Keyestudio serial command
        # format once the board firmware command set is confirmed.
        for name in sides:
            port = self._ports.get(name)
            if port:
                port.write(f"{pose}\n".encode("ascii"))

    def shutdown(self) -> None:
        for port in self._ports.values():
            port.close()
