"""SO-101 の制御基板 (Waveshare Bus Servo Adapter・CH343) のサーボを「読むだけ」で確かめる (依頼 248).

サーボへの書き込みは一切しない:
- connect(handshake=False) でポートを開くだけ
- 見つけるのは broadcast ping (読み出し), 各値は read_register (読み出し)
- 閉じるときは disconnect(disable_torque=False). 既定の True はトルクを切る書き込みをするので使わない

使い方 (conda の lerobot 環境で):
    python so101/read_servos_248.py            # USB ID が 1a86:55d3 のポートを全部読む
    python so101/read_servos_248.py --port /dev/tty.usbmodemXXXX
"""

import argparse
import json

import serial.tools.list_ports

from lerobot.motors import Motor, MotorNormMode, SerialMotorsBus

# CH343 (Waveshare Bus Servo Adapter が使う USB シリアル)
ADAPTER_VID_PID = (0x1A86, 0x55D3)
REGISTERS = [
    "Model_Number",
    "Firmware_Major_Version",
    "Firmware_Minor_Version",
    "Present_Position",
    "Present_Voltage",
    "Present_Temperature",
    "Torque_Enable",
    "Lock",
]


def adapter_ports():
    found = []
    for p in serial.tools.list_ports.comports():
        if (p.vid, p.pid) == ADAPTER_VID_PID and p.device.startswith("/dev/cu."):
            found.append(p.device.replace("/dev/cu.", "/dev/tty."))
    return sorted(found)


def open_bus(port, ids):
    bus = SerialMotorsBus(port, {f"m{i}": Motor(i, "sts3215", MotorNormMode.RANGE_M100_100) for i in ids})
    bus.connect(handshake=False)
    return bus


def read_port(port):
    # 1回目: ID 1 の sts3215 を1つだけ置いた bus で, 定義を使って broadcast ping で全 ID を探す
    bus = open_bus(port, [1])
    try:
        ids = bus._scan(bus._definitions[1])
    finally:
        bus.disconnect(disable_torque=False)
    if not ids:
        return []
    # 2回目: 見つかった ID を全部 bus に置き直して, 各値を読む
    bus = open_bus(port, sorted(ids))
    try:
        rows = []
        for motor_id, model_nb in sorted(ids.items()):
            row = {"id": motor_id, "model_number_scan": model_nb}
            for reg in REGISTERS:
                try:
                    row[reg] = bus._connected_bus.read_register_with_error(motor_id, reg.lower(), retries=2)[0]
                except Exception as e:  # 読めない値は理由を残して先へ進む
                    row[reg] = f"ERR {e}"
            rows.append(row)
        return rows
    finally:
        bus.disconnect(disable_torque=False)


def main():
    ap = argparse.ArgumentParser()
    ap.add_argument("--port", action="append", help="読むポート (省略時は CH343 のポートを全部)")
    ap.add_argument("--json", help="結果を JSON で書く先")
    args = ap.parse_args()

    ports = args.port or adapter_ports()
    if not ports:
        print("制御基板 (1a86:55d3) のポートが見えない")
        return
    result = {}
    for port in ports:
        print(f"\n== {port}")
        try:
            rows = read_port(port)
        except Exception as e:
            print(f"  読めない: {e}")
            result[port] = {"error": str(e)}
            continue
        result[port] = rows
        if not rows:
            print("  サーボが1つも答えない (電源が入っていない?)")
            continue
        volts = [r["Present_Voltage"] / 10 for r in rows if isinstance(r["Present_Voltage"], int)]
        print(f"  サーボ {len(rows)} 個, 電圧の平均 {sum(volts) / len(volts):.1f} V" if volts else "  電圧が読めない")
        print("  ID  型番  FW    位置  電圧V  温度C  トルク  Lock")
        for r in rows:
            v = r["Present_Voltage"]
            v = f"{v / 10:.1f}" if isinstance(v, int) else v
            print(
                f"  {r['id']:>2}  {r['Model_Number']}  {r['Firmware_Major_Version']}.{r['Firmware_Minor_Version']}"
                f"  {r['Present_Position']:>5}  {v:>5}  {r['Present_Temperature']:>5}"
                f"  {r['Torque_Enable']:>6}  {r['Lock']:>4}"
            )
    if args.json:
        with open(args.json, "w") as f:
            json.dump(result, f, ensure_ascii=False, indent=2)


if __name__ == "__main__":
    main()
