"""Scan the bus and list which Feetech motor IDs respond to ping.

Usage: python which_motors.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
SCAN_RANGE = range(0, 15)

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)
while True:

    print(f"Scanning IDs {SCAN_RANGE.start}..{SCAN_RANGE.stop - 1}...")
    found = []
    for mid in SCAN_RANGE:
        model, comm, err = sts.ping(mid)
        if comm == 0 and err == 0:
            found.append((mid, model))
            print(f"  found id={mid} model={model}")

    print()
    if found:
        print(f"{len(found)} motor(s) found: {[mid for mid, _ in found]}")
    else:
        print("No motors found.")
port.closePort()