#!/usr/bin/env python3
"""关节活动范围校准 - 手动转动法.

流程:
  1. 狗悬空 + L2+B damping
  2. 跑这个脚本
  3. 慢慢把每条腿的每个关节转到极限两端 (软力, 听到阻力就停)
  4. 输出每个关节实测的 min/max/range
"""
import argparse
import os
import signal
import sys
import time

sys.path.insert(0, os.path.join(os.path.dirname(__file__), '..'))
from go1_pro_sdk import MCUClient, LowCmd, JOINT_NAMES
from go1_pro_sdk.utils.constants import JOINT_LIMITS_URDF


running = True


def sigint(s, f):
    global running
    running = False


def main():
    parser = argparse.ArgumentParser()
    parser.add_argument('--state', default=None)
    parser.add_argument('--duration', type=float, default=60)
    args = parser.parse_args()
    signal.signal(signal.SIGINT, sigint)

    print('🦾 关节活动范围校准')
    print(f'  采样时长: {args.duration}s')
    print()
    print('操作:')
    print('  1. 狗悬空, L2+B damping (电机失能)')
    print('  2. 按顺序逐条腿/逐个关节转到极限')
    print('  3. Ctrl+C 提前结束')
    input('回车开始采样...')

    with MCUClient(state_path=args.state) as client:
        recv = client.wake_mcu(50)
        if recv == 0:
            print('❌ 无回包')
            return 1

        damping = LowCmd().all_damping()
        q_min = [float('inf')] * 12
        q_max = [float('-inf')] * 12

        start = time.time()
        last_print = 0
        n_decoded = 0

        while running and time.time() - start < args.duration:
            client.send(damping)
            time.sleep(0.01)
            state = client.recv_latest()
            if state is None:
                continue
            n_decoded += 1
            for i in range(12):
                q = state.motorState[i].q
                if q < q_min[i]: q_min[i] = q
                if q > q_max[i]: q_max[i] = q

            now = time.time()
            if now - last_print >= 2.0:
                print(f'\r⏱  {now-start:5.1f}/{args.duration:.0f}s  解码 {n_decoded} 帧',
                      end='', flush=True)
                last_print = now

    print('\n\n实测范围 vs URDF 标称:')
    print('-' * 80)
    print(f'{"关节":8} {"实测min":>9} {"实测max":>9} {"范围":>8}   '
          f'{"URDF min":>9} {"URDF max":>9}')
    for i, name in enumerate(JOINT_NAMES):
        jt = ('hip' if i % 3 == 0 else 'thigh' if i % 3 == 1 else 'knee')
        urdf_lo, urdf_hi = JOINT_LIMITS_URDF[jt]
        lo, hi = q_min[i], q_max[i]
        if lo == float('inf'):
            print(f'{name:8} {"未采到":>9}')
            continue
        print(f'{name:8} {lo:+9.4f} {hi:+9.4f} {hi-lo:8.4f}   '
              f'{urdf_lo:+9.4f} {urdf_hi:+9.4f}')

    print('\nJOINT_LIMITS_MEASURED = {')
    for i, name in enumerate(JOINT_NAMES):
        if q_min[i] == float('inf'): continue
        print(f"    '{name}': ({q_min[i]:+.4f}, {q_max[i]:+.4f}),")
    print('}')


if __name__ == '__main__':
    sys.exit(main() or 0)