"""STS register-level helper class."""

from __future__ import annotations

from .constants import MemAddr, ServoParams, TorqueEnable
from .exceptions import InvalidParameterError
from .protocol import build_position_calibrate_frame, bytes_to_int16, int16_to_bytes
from .servo_controller import ServoController


def _validate_position_mode_non_negative(name: str, value: int) -> None:
    """Reject negative values until STS signed-magnitude mode is implemented."""
    if value < 0:
        raise InvalidParameterError(
            f"{name} must be non-negative in current STS position mode; "
            "negative values require STS signed-magnitude encoding support."
        )


class STS:
    """Convenience wrapper around ServoController for STS semantics."""

    def __init__(self, controller: ServoController) -> None:
        self._ctrl = controller

    def set_torque_enabled(self, servo_id: int, enabled: bool) -> bool:
        """Enable/disable servo torque."""
        value = TorqueEnable.ON if enabled else TorqueEnable.OFF
        return self._ctrl.write_register(servo_id, MemAddr.TORQUE_ENABLE, [value])

    def calibrate_current_as_center(self, servo_id: int) -> bool:
        """Calibrate current position to encoder center 2048."""
        return self._ctrl.write_register(
            servo_id,
            MemAddr.TORQUE_ENABLE,
            [TorqueEnable.CALIBRATE_CENTER],
        )

    def read_position(self, servo_id: int) -> int:
        """Read one servo position."""
        data = self._ctrl.read_register(servo_id, MemAddr.PRESENT_POSITION, 2)
        return bytes_to_int16(data[0], data[1])

    def read_positions(self, servo_ids: list[int]) -> dict[int, int]:
        """Read multiple positions."""
        return self._ctrl.read_positions(servo_ids)

    def write_position(
        self, servo_id: int, encoder: int, speed: int, acceleration: int = 0
    ) -> bool:
        """Write one servo position via ACC..GOAL_SPEED register block."""
        _validate_position_mode_non_negative("encoder", encoder)
        _validate_position_mode_non_negative("speed", speed)
        payload = (
            [acceleration & 0xFF]
            + int16_to_bytes(encoder)
            + [0, 0]
            + int16_to_bytes(speed)
        )
        return self._ctrl.write_register(servo_id, MemAddr.ACCELERATION, payload)

    def write_positions_sync(
        self, servo_positions: list[tuple[int, int]], speed: int, acceleration: int = 0
    ) -> bool:
        """Sync-write positions for multiple servos."""
        return self._ctrl.write_positions_sync(
            servo_positions, speed=speed, acceleration=acceleration
        )

    # ---------------- offset calibration ----------------

    def calibrate_current_as_position(
        self, servo_id: int, target_position: int
    ) -> bool:
        """Send POSITION_CALIBRATE (0x0B): map current position to *target_position*.

        The servo computes and saves the offset to EEPROM automatically.
        No LOCK_FLAG management needed. Requires STS firmware >= 3.10.

        Args:
            target_position: Encoder value (0-4095) to assign to the current
                physical position.
        """
        if (
            target_position < ServoParams.POSITION_MIN
            or target_position > ServoParams.POSITION_MAX
        ):
            raise InvalidParameterError(
                f"target_position must be {ServoParams.POSITION_MIN}-"
                f"{ServoParams.POSITION_MAX}, got {target_position}"
            )
        frame = build_position_calibrate_frame(servo_id, target_position)
        return self._ctrl._send_instruction(frame)

    def read_offset_position(self, servo_id: int) -> int:
        """Read offset register (0x1F, 2 bytes) raw value.

        Returns the raw 16-bit value from EEPROM. STS uses sign-magnitude
        encoding (BIT13 = direction), range 0-8191.
        """
        data = self._ctrl.read_register(servo_id, MemAddr.OFFSET_POSITION, 2)
        return bytes_to_int16(data[0], data[1])

    # ---------------- batch read helpers ----------------

    def read_registers_sync(
        self,
        servo_ids: list[int],
        addr: int,
        data_len: int,
    ) -> tuple[dict[int, list[int] | None], dict[int, int | None]]:
        """SyncRead arbitrary register for multiple servos.

        Returns (parsed_data, error_codes) — delegates to ServoController.
        """
        return self._ctrl.read_registers_sync(servo_ids, addr, data_len)

    def get_cached_error_code(self, servo_id: int) -> int | None:
        """Get last cached response-frame error code for one servo."""
        return self._ctrl.get_cached_error_code(servo_id)
