"""Shared Feetech STS bus helper — refactored from our_feetech_controller so the
calibration + live-mirror tools share one connection/read/zero implementation.

Importing this module monkey-patches scservo_sdk (via scservo_patch) so methods
take (motor_id, ...) without an explicit portHandler.
"""
import math
import time

import scservo_patch  # noqa: F401  (side-effect: patches the SDK on import)
from scservo_sdk import PortHandler
from scservo_sdk.sms_sts import sms_sts

USB_PORT_DEFAULT = "/dev/tty.usbserial-0001"
BAUD_DEFAULT = 115200

CENTER_TICK = 2048
TICKS_PER_REV = 4096
TICKS_TO_RAD = (2.0 * math.pi) / TICKS_PER_REV
RAD_TO_TICKS = TICKS_PER_REV / (2.0 * math.pi)

ADDR_TORQUE_ENABLE = 40
ADDR_TEMP = 63
ADDR_ID = 5                           # EEPROM servo-id register
ADDR_LOCK = 55                        # EEPROM write-lock (0 = unlocked, 1 = locked)
INST_OFSCAL = 11                      # offset-calibration instruction (sets current pos = 2048)
_PKT_ID, _PKT_LENGTH, _PKT_INSTRUCTION = 2, 3, 4


def ticks_to_rad(ticks, sign=1):
    """Signed servo tick -> URDF radians about the joint zero (tick 2048)."""
    return sign * (ticks - CENTER_TICK) * TICKS_TO_RAD


class Bus:
    def __init__(self, port=USB_PORT_DEFAULT, baud=BAUD_DEFAULT):
        self.port = PortHandler(port)
        if not self.port.openPort():
            raise RuntimeError(f"Failed to open serial port {port}")
        if not self.port.setBaudRate(baud):
            raise RuntimeError(f"Failed to set baud {baud}")
        self.sts = sms_sts(self.port)

    def ping(self, mid):
        _, comm, err = self.sts.ping(mid)
        return comm == 0 and err == 0

    def read_pos(self, mid):
        pos, comm, _ = self.sts.ReadPos(mid)
        return pos if comm == 0 else None

    def read_temp(self, mid):
        val, comm, _ = self.sts.read1ByteTxRx(mid, ADDR_TEMP)
        return val if comm == 0 else None

    def torque(self, mid, on):
        """Enable/disable holding torque. Off => the joint is back-drivable by hand."""
        self.sts.writeTxRx(mid, ADDR_TORQUE_ENABLE, 1, [1 if on else 0])

    def set_zero_here(self, mid, retries=3):
        """INST_OFSCAL: make the servo's CURRENT physical position read as 2048.
        Persists to EEPROM, so the zero survives power cycles."""
        for _ in range(retries):
            tx = [0] * 6
            tx[_PKT_ID] = mid
            tx[_PKT_LENGTH] = 2
            tx[_PKT_INSTRUCTION] = INST_OFSCAL
            _, result, _ = self.sts.txRxPacket(self.sts.portHandler, tx)
            if result == 0:
                return True
            time.sleep(0.1)
        return False

    def set_id(self, old_id, new_id):
        """Change a servo's EEPROM id. Only ONE servo should share old_id on the bus.
        Unlock -> write id -> relock (via the new id) -> verify. The servo often changes
        its id before acking the write (SDK reports RX timeout), so we verify by pinging
        the new id rather than trusting the write's return code. Returns True on success."""
        self.sts.writeTxRx(old_id, ADDR_LOCK, 1, [0])   # unlock EEPROM
        time.sleep(0.05)
        self.sts.writeTxRx(old_id, ADDR_ID, 1, [new_id])
        time.sleep(0.05)
        if not self.ping(new_id):
            return False
        self.sts.writeTxRx(new_id, ADDR_LOCK, 1, [1])   # relock using the new id
        time.sleep(0.05)
        return self.ping(new_id)

    def read_pos_settle(self, mid, tries=6):
        """Read position tolerantly right after INST_OFSCAL (reply can be slow/truncated)."""
        for _ in range(tries):
            time.sleep(0.1)
            try:
                p, comm, err = self.sts.ReadPos(mid)
                if comm == 0 and err == 0:
                    return p
            except IndexError:
                pass
        return None

    def close(self):
        self.port.closePort()