#!/usr/bin/env python3
"""实时监听 LowState (只读, 不控制电机).

用法:
    python examples/monitor_state.py --duration 30 --verbose
"""
import argparse
import math
import signal
import sys
import time

from go1_pro_sdk import MCUClient, JOINT_NAMES, decode_sn

running = True


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


def main():
    p = argparse.ArgumentParser()
    p.add_argument('--state', default=None, help='Blowfish state 文件 (默认用包内置)')
    p.add_argument('--duration', type=float, default=30)
    p.add_argument('--rate', type=float, default=5, help='打印频率 Hz')
    p.add_argument('--verbose', '-v', action='store_true')
    args = p.parse_args()
    signal.signal(signal.SIGINT, sigint)

    print('📡 监听 LowState')
    print(f'  时长 {args.duration}s, 打印 {args.rate}Hz')

    with MCUClient(state_path=args.state) as client:
        recv_count = client.wake_mcu(50)
        print(f'唤醒: 收到 {recv_count}/50 帧')
        if recv_count == 0:
            print('❌ 无回包. 检查 Pi 抢占源或 state 是否正确')
            return 1

        start = time.time()
        last_print = 0
        n_decoded = 0
        damping = __import__('go1_pro_sdk').LowCmd().all_damping()

        print(f"\n{'时间':>6} {'电量':>5} {'FR_0':>8} {'FR_1':>8} {'FR_2':>8} {'rpy[°]':>20}")
        print('-' * 70)

        while running and time.time() - start < args.duration:
            client.send(damping)
            time.sleep(0.005)
            state = client.recv_latest()
            if state is None:
                continue
            n_decoded += 1

            now = time.time()
            if now - last_print >= 1.0 / args.rate:
                t = now - start
                rpy_str = (f"{math.degrees(state.imu.rpy[0]):+5.1f},"
                           f"{math.degrees(state.imu.rpy[1]):+5.1f},"
                           f"{math.degrees(state.imu.rpy[2]):+5.1f}")
                print(f'{t:6.1f} {state.bms.SOC:4d}% '
                      f'{state.motorState[0].q:+8.3f} '
                      f'{state.motorState[1].q:+8.3f} '
                      f'{state.motorState[2].q:+8.3f} '
                      f'{rpy_str:>20}')
                if args.verbose:
                    print(f'    SN={decode_sn(state.SN)} '
                          f'acc_z={state.imu.accelerometer[2]:.2f} m/s²  '
                          f'BMS current={state.bms.current_a:.2f}A')
                last_print = now

        print(f'\n解码 {n_decoded} 帧, 退出.')


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