"""Constants for Feetech STS protocol and arm defaults."""

from __future__ import annotations

HEADER = 0xFF
BROADCAST_ID = 0xFE


class Instruction:
    """STS/SCS protocol instruction set."""

    PING = 0x01
    READ_DATA = 0x02
    WRITE_DATA = 0x03
    REG_WRITE = 0x04
    ACTION = 0x05
    RESET = 0x0A
    POSITION_CALIBRATE = 0x0B
    SYNC_READ = 0x82
    SYNC_WRITE = 0x83


class MemAddr:
    """Common STS register addresses."""

    ID = 0x05
    BAUD_RATE = 0x06
    PHASE = 0x12
    OFFSET_POSITION = 0x1F
    OPERATING_MODE = 0x21

    TORQUE_ENABLE = 0x28
    ACCELERATION = 0x29
    GOAL_POSITION = 0x2A
    GOAL_SPEED = 0x2E
    LOCK_FLAG = 0x37

    PRESENT_POSITION = 0x38
    PRESENT_SPEED = 0x3A
    PRESENT_LOAD = 0x3C
    PRESENT_VOLTAGE = 0x3E
    PRESENT_TEMPERATURE = 0x3F
    STATUS = 0x41
    MOVING_FLAG = 0x42
    PRESENT_CURRENT = 0x45


class TorqueEnable:
    """Torque enable values."""

    OFF = 0
    ON = 1
    DAMPING = 2
    CALIBRATE_CENTER = 128


class StatusBit:
    """STS status bits."""

    VOLTAGE_ERROR = 0x01
    ENCODER_ERROR = 0x02
    TEMPERATURE_ERROR = 0x04
    CURRENT_ERROR = 0x08
    OVERLOAD_ERROR = 0x20

    # Errors that invalidate position read data.
    # Only ENCODER_ERROR means the encoder value itself is unreliable.
    # Other errors (voltage, temperature, current, overload) are status
    # warnings — the encoder hardware still returns valid readings.
    READ_INVALIDATING_ERRORS = ENCODER_ERROR


class ServoParams:
    """STS conversion constants."""

    POSITION_MIN = 0
    POSITION_MAX = 4095
    POSITION_CENTER = 2048
    POSITION_RESOLUTION_DEG = 0.087

    SPEED_RESOLUTION_RPM = 0.732
    VOLTAGE_RESOLUTION_V = 0.1
    CURRENT_RESOLUTION_MA = 6.5


DEFAULT_BAUDRATE = 1_000_000
DEFAULT_TIMEOUT = 0.1
DEFAULT_RETRIES = 2
SYNC_READ_FRAME_BUDGET = 16
