"""Change a Feetech STS servo's ID.

Usage: python set_motor_id.py <old_id> <new_id>

IMPORTANT: only one servo should be on the bus when running this.
If multiple servos share <old_id>, they will all be changed. If any other
servo already has <new_id>, you'll end up with an ID collision.
"""
import sys, time

from scservo_sdk import PortHandler
import scservo_patch
from scservo_sdk.sms_sts import sms_sts

USB_PORT = "/dev/tty.usbserial-0001"
BAUD_RATE = 115200

ADDR_ID = 5
ADDR_LOCK = 55


def main():
    if len(sys.argv) != 3:
        sys.exit("Usage: python set_motor_id.py <old_id> <new_id>")

    old_id = int(sys.argv[1])
    new_id = int(sys.argv[2])

    if not (0 <= new_id <= 253):
        sys.exit("new_id must be in 0..253 (254 is broadcast)")
    if old_id == new_id:
        sys.exit("old_id and new_id are the same")

    port = PortHandler(USB_PORT)
    if not port.openPort():
        sys.exit("Failed to open port")
    if not port.setBaudRate(BAUD_RATE):
        sys.exit("Failed to set baud rate")

    sts = sms_sts(port)

    try:
        _, comm, err = sts.ping(old_id)
        if comm != 0 or err != 0:
            sys.exit(f"Motor {old_id} not responding (comm={comm}, err={err})")

        _, comm, _ = sts.ping(new_id)
        if comm == 0:
            sys.exit(f"ID {new_id} is already in use on the bus. Aborting.")

        print(f"About to change motor ID {old_id} -> {new_id}.")
        print("Make sure only ONE servo is connected to the bus.")
        if input("Proceed? [y/N] ").strip().lower() != "y":
            print("Aborted.")
            return

        # Unlock EPROM
        result, error = sts.writeTxRx(old_id, ADDR_LOCK, 1, [0])
        if result != 0:
            sys.exit(f"Unlock failed (comm={result}, err={error})")
        time.sleep(0.05)

        # Write new ID. The servo often changes ID before acking, so the
        # response comes back tagged with the new ID and the SDK reports
        # COMM_RX_TIMEOUT (-6). We treat that as "probably succeeded" and
        # verify by pinging the new ID.
        sts.writeTxRx(old_id, ADDR_ID, 1, [new_id])
        time.sleep(0.05)
        _, comm, _ = sts.ping(new_id)
        if comm != 0:
            sys.exit(f"Write ID failed: motor not responding at new ID {new_id}")

        # Relock EPROM using the NEW id
        result, error = sts.writeTxRx(new_id, ADDR_LOCK, 1, [1])
        if result != 0:
            sys.exit(f"Relock failed (comm={result}, err={error})")
        time.sleep(0.05)

        _, comm, err = sts.ping(new_id)
        if comm != 0 or err != 0:
            sys.exit(f"Verification ping to {new_id} failed (comm={comm}, err={err})")

        print(f"Done. Motor now responds at ID {new_id}.")
    finally:
        port.closePort()


if __name__ == "__main__":
    main()