"""Live-read motors 1 and 2 (double servo joint test).

Prints both servo positions and their difference so you can verify
they're tracking together. No MuJoCo model needed — just two servos
clamped together.

Usage: python read_double_servo_test.py
"""
import math
import sys
import 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

MOTOR_IDS = [1, 2]
SIGNS = [1, 1]

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

RATE_HZ = 30
ADDR_TEMP = 63
TEMP_WARN_C = 55
TEMP_ALERT_C = 65
TEMP_POLL_EVERY_N = 30


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)

    for mid in MOTOR_IDS:
        _, comm, err = sts.ping(mid)
        if comm != 0 or err != 0:
            port.closePort()
            sys.exit(f"Motor {mid} not responding (comm={comm}, err={err})")

    print("Reading motors 1 & 2 (double joint). Ctrl-C to exit.")
    print(f"{'m1 ticks':>9} {'m1 rad':>8} {'m2 ticks':>9} {'m2 rad':>8} {'diff':>6}")

    period = 1.0 / RATE_HZ
    frame = 0
    try:
        while True:
            t0 = time.perf_counter()
            positions = []
            for mid in MOTOR_IDS:
                pos, comm, _ = sts.ReadPos(mid)
                positions.append(pos if comm == 0 else None)

            if positions[0] is not None and positions[1] is not None:
                r1 = SIGNS[0] * (positions[0] - CENTER_TICK) * TICKS_TO_RAD
                r2 = SIGNS[1] * (positions[1] - CENTER_TICK) * TICKS_TO_RAD
                diff = positions[0] - positions[1]
                print(f"  {positions[0]:>6} {r1:>+7.3f}   {positions[1]:>6} {r2:>+7.3f}  {diff:>+5d}",
                      end="\r")

            if frame % TEMP_POLL_EVERY_N == 0:
                for mid in MOTOR_IDS:
                    temp, comm, _ = sts.read1ByteTxRx(mid, ADDR_TEMP)
                    if comm != 0:
                        continue
                    if temp >= TEMP_ALERT_C:
                        print(f"\n!!! motor {mid} = {temp}C !!!")
                    elif temp >= TEMP_WARN_C:
                        print(f"\n  warning: motor {mid} = {temp}C")

            frame += 1
            dt = time.perf_counter() - t0
            if dt < period:
                time.sleep(period - dt)
    except KeyboardInterrupt:
        print("\nDone.")
    finally:
        port.closePort()


if __name__ == "__main__":
    main()