#!/usr/bin/env python3
"""安全单腿正弦测试 (保守参数, 小幅摆动).

跟 example_position.py 的区别:
  - 默认振幅小 (±0.3 rad), 频率慢 (0.5 Hz)
  - sin 中点保持当前实际关节角, 不强行移到 -2.0
  - 适合首次试电机控制

⚠️ 狗悬空, Ctrl+C 紧急停止
"""
import argparse
import math
import signal
import sys
import time

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

DT = 0.002
running = True


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


def main():
    p = argparse.ArgumentParser()
    p.add_argument('--state', default=None)
    p.add_argument('--amplitude', type=float, default=0.3, help='sin 振幅 (rad)')
    p.add_argument('--freq', type=float, default=0.5, help='sin 频率 (Hz)')
    p.add_argument('--duration', type=float, default=10)
    p.add_argument('--power-factor', type=int, default=1)
    args = p.parse_args()
    signal.signal(signal.SIGINT, sigint)

    print(f'🦿 单腿 sin 测试 — 振幅 {args.amplitude}, 频率 {args.freq}Hz, '
          f'{args.duration}s, power={args.power_factor}')

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

        # 用实际值作为 sin 中点
        mid = [state.motorState[i].q for i in range(3)]
        print(f'  sin 中点 (= 当前角): {[f"{x:+.3f}" for x in mid]}')

        # Phase 1: 软启动 (Kp/Kd 从 0 慢慢升)
        ramp_steps = 500
        print(f'  软启动 {ramp_steps*DT:.1f}s...')

        try:
            n_steps = int(args.duration / DT)
            for step in range(n_steps + ramp_steps):
                if not running:
                    break
                t0 = time.time()
                state = client.recv_latest() or state

                if step < ramp_steps:
                    # 软启动: Kp/Kd 从 0 到目标
                    ramp = step / ramp_steps
                    Kp = 3 * ramp
                    Kd = 0.5 * ramp
                    qDes = list(mid)
                else:
                    Kp = 3
                    Kd = 0.5
                    t = (step - ramp_steps) * DT
                    sin_v = args.amplitude * math.sin(2 * math.pi * args.freq * t)
                    qDes = [mid[0], mid[1] + sin_v, mid[2] - sin_v * 1.5]

                cmd = LowCmd()
                for i in range(3):
                    cmd.motorCmd[i] = MotorCmd(
                        mode=MotorMode.Servo,
                        q=qDes[i], dq=0, tau=0,
                        Kp=Kp, Kd=Kd,
                    )

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

                client.send(cmd)

                if step % 250 == 0 and step > 0:
                    actual = [state.motorState[i].q for i in range(3)]
                    print(f'  t={step*DT:5.2f}s des={[f"{x:+.3f}" for x in qDes]} '
                          f'act={[f"{x:+.3f}" for x in actual]}')

                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)