"""Read and print current values for Feetech motors in a loop.

Usage: python feetech_read_motor.py 0 1 2 3 4 5
"""
import sys, time

from scservo_sdk import PortHandler
import scservo_patch
from scservo_sdk.sms_sts import sms_sts
import numpy as np

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

if len(sys.argv) < 2:
    sys.exit("Usage: python feetech_read_motor.py <motor_id> [motor_id ...]")

motor_ids = [int(x) for x in sys.argv[1:]]

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)

# Verify all motors are reachable
for mid in motor_ids:
    model, comm, err = sts.ping(mid)
    if comm != 0 or err != 0:
        sys.exit(f"Motor {mid} not responding (comm={comm}, err={err})")
    print(f"Motor {mid} connected (model={model})")
print()

# Print header
header = "  ".join(f"[{mid:>2d}] pos   spd   load  V     T   mA   mov" for mid in motor_ids)
print(header)
print("-" * len(header))

try:
    while True:
        parts = []
        pos_list = []
        for mid in motor_ids:
            pos, _, _ = sts.ReadPos(mid)
            speed, _, _ = sts.ReadSpeed(mid)
            moving, _, _ = sts.ReadMoving(mid)
            load, _, _ = sts.read2ByteTxRx(mid, 60)
            voltage, _, _ = sts.read1ByteTxRx(mid, 62)
            temp, _, _ = sts.read1ByteTxRx(mid, 63)
            current, _, _ = sts.read2ByteTxRx(mid, 69)

            speed_val = -(speed & 0x7FFF) if speed & 0x8000 else speed
            load_val = -(load & 0x3FF) if load & 0x400 else load & 0x3FF

            parts.append(
                f"[{mid:>2d}] {pos:5d} {speed_val:+5d} {load_val:+5d} "
                f"{voltage*0.1:4.1f}V {temp:3d}C {current*6.5:5.0f} {'Y' if moving else 'N'}"
            )
            pos_list.append(int(pos))
        #print(np.array(pos_list))# print all pos as np array as radians
        import math
        print(np.array(pos_list) * (2.0 * math.pi) / 4096)# print all pos as np array as radians

        #print("  ".join(parts), end="\r")
        time.sleep(0.05)
except KeyboardInterrupt:
    print("\nStopped.")
finally:
    port.closePort()