"""Disable torque on every Feetech servo with ID in 0..10.

Missing IDs are skipped silently. Safe to run at any time.

Usage: python release_torques.py
"""
import sys

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_TORQUE_ENABLE = 40
MAX_ID = 10


def main():
    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:
        for mid in range(MAX_ID + 1):
            _, comm, err = sts.ping(mid)
            if comm != 0 or err != 0:
                continue
            result, error = sts.writeTxRx(mid, ADDR_TORQUE_ENABLE, 1, [0])
            if result == 0 and error == 0:
                print(f"  motor {mid}: torque released")
            else:
                print(f"  motor {mid}: FAILED (comm={result}, err={error})")
    finally:
        port.closePort()


if __name__ == "__main__":
    main()