#!/usr/bin/env python3
"""高层走动示例 — 站起来 → 走一圈 → 趴下.

跟 lowlevel 控制不同, 高层走的是 sportMode 系统, 不需要 stop_sportmode.
"""
import sys
import time

from go1_pro_sdk import Go1, Velocity, Pose


def main():
    print('🐕 高层控制示例: stand_up → walk → 转向 → 趴下')
    print('   通过 MQTT 控制 Pi 上的 sportMode')

    with Go1() as dog:
        print('\n[1/5] stand_up (起立)...')
        dog.stand_up()
        time.sleep(3)

        print('[2/5] 进入 walk 模式...')
        dog.set_walk_mode()
        time.sleep(1)

        print('[3/5] 前进 2 秒 (vx=0.2)...')
        for _ in range(20):
            dog.walk(Velocity(vx=0.2))
            time.sleep(0.1)

        print('[4/5] 原地转向 2 秒 (vz=0.3)...')
        for _ in range(20):
            dog.walk(Velocity(vz=0.3))
            time.sleep(0.1)

        print('       停止 1 秒...')
        dog.stop()
        time.sleep(1)

        print('[5/5] stand_down (趴下)...')
        dog.stand_down()
        time.sleep(3)

    print('✅ 结束')


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