#!/usr/bin/env python3
"""用遥控器摇杆控制 FR 腿 (Demo: 摇杆 X → 髋外摆, 摇杆 Y → 大腿).

按键映射:
  左摇杆 X  → FR_0 hip (±0.4 rad)
  左摇杆 Y  → FR_1 thigh (mid ± 0.5 rad)
  L2 按下   → 立即 damping 退出 (软急停)
  其他腿    → Damping

⚠️ 狗必须悬空 (脚架/吊带)
"""
import argparse
import signal
import sys
import time

from go1_pro_sdk import (
    MCUClient, LowCmd, MotorCmd, MotorMode,
    apply_safety, PowerProtectViolation,
)

DT = 0.005    # 200 Hz 即可, 摇杆响应慢
running = True


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


def main():
    p = argparse.ArgumentParser()
    p.add_argument('--state', default=None)
    p.add_argument('--duration', type=float, default=60)
    p.add_argument('--hip-range', type=float, default=0.4, help='hip 摆幅 (rad)')
    p.add_argument('--thigh-range', type=float, default=0.5, help='thigh 摆幅 (rad)')
    args = p.parse_args()
    signal.signal(signal.SIGINT, sigint)

    print('🎮 摇杆遥控 FR 腿')
    print(f'  左 X → hip ±{args.hip_range}, 左 Y → thigh ±{args.thigh_range}')
    print(f'  L2 按下 → 退出')

    with MCUClient(state_path=args.state) as client:
        if client.wake_mcu(50) == 0:
            print('❌ 无回包')
            return 1
        state = client.last_state
        mid = [state.motorState[i].q for i in range(3)]
        print(f'  初始角: hip={mid[0]:+.3f} thigh={mid[1]:+.3f} knee={mid[2]:+.3f}')

        start = time.time()
        try:
            while running and time.time() - start < args.duration:
                t0 = time.time()
                state = client.recv_latest() or state
                r = state.remote

                # L2 按下 → 退出
                if r.is_pressed('L2'):
                    print('\nL2 → 退出')
                    break

                # 摇杆 → 目标 q
                q_hip = mid[0] + r.lx * args.hip_range
                q_thigh = mid[1] + r.ly * args.thigh_range
                q_knee = mid[2]   # 膝盖保持不变

                cmd = LowCmd()
                for i, q in enumerate([q_hip, q_thigh, q_knee]):
                    cmd.motorCmd[i] = MotorCmd(
                        mode=MotorMode.Servo, q=q, dq=0, tau=0, Kp=3, Kd=0.5)

                try:
                    apply_safety(cmd, state, power_factor=1,
                                 position_protect_limit=0.5)
                except PowerProtectViolation as e:
                    print(f'\n⚠️ {e}')
                    break

                client.send(cmd)

                # 每秒打印一次
                if int(time.time() * 2) != int((time.time() - DT) * 2):
                    print(f'  L=({r.lx:+.2f},{r.ly:+.2f})  '
                          f'des=({q_hip:+.3f},{q_thigh:+.3f}) '
                          f'act=({state.motorState[0].q:+.3f},'
                          f'{state.motorState[1].q:+.3f})')

                sleep_t = DT - (time.time() - t0)
                if sleep_t > 0:
                    time.sleep(sleep_t)
        finally:
            print('\n安全停机...')
            client.safe_stop()
            print('✅ 退出')


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