#!/usr/bin/env python3
"""遥控器帧模拟发送器（联调用，非工程代码）。

按 24 字节协议向 UI 的 net/rc_port（默认 9002）周期发送遥控器报文。
用法示例：
  python rc_sender.py --mode drive --gear 4096        # 行车模式，档位推到"前"
  python rc_sender.py --mode arm --posx 4096          # 机械臂模式（自动带上机械臂使能）
  python rc_sender.py --estop                         # bit4=1，触发急停
  python rc_sender.py --battery 42                    # 电量显示 42%
  python rc_sender.py --roll 4096 --pitch 0           # 右摇杆两轴反向
  python rc_sender.py --bad-checksum                  # 校验和故意错，应被丢弃
"""
import argparse
import socket
import struct
import time

MID = 2048


def build_frame(a):
    flags = 0
    if a.mode == "drive":
        flags |= 0x01                      # bit0 行车模式
        if not a.no_chassis_enable:
            flags |= 0x08                  # bit3 底盘使能
    else:
        flags |= 0x02 | 0x04               # bit1 机械臂模式须与 bit2 使能同时置 1
    if a.estop:
        flags |= 0x10                      # bit4 急停

    f = bytearray(24)
    f[0] = 0x11                            # 帧头
    f[1] = flags
    axes = (a.gear, a.steer, a.posz, a.posx, a.posy, a.roll, a.pitch, a.yaw)
    for i, v in enumerate(axes):
        struct.pack_into("<H", f, 3 + i * 2, v)   # BYTE3 起，小端 int16
    f[19] = max(0, min(100, a.battery))
    f[22] = 0x22                           # 帧尾
    f[23] = sum(f[0:23]) & 0xFF            # 和校验
    if a.bad_checksum:
        f[23] ^= 0xFF
    return bytes(f)


def main():
    p = argparse.ArgumentParser()
    p.add_argument("--host", default="127.0.0.1")
    p.add_argument("--port", type=int, default=9002)
    p.add_argument("--mode", choices=["drive", "arm"], default="drive")
    for name in ("gear", "steer", "posz", "posx", "posy", "roll", "pitch", "yaw"):
        p.add_argument("--" + name, type=int, default=MID)
    p.add_argument("--battery", type=int, default=88)
    p.add_argument("--estop", action="store_true")
    p.add_argument("--no-chassis-enable", action="store_true")
    p.add_argument("--bad-checksum", action="store_true")
    p.add_argument("--hz", type=float, default=10.0, help="默认 10Hz，即协议要求的 100ms")
    p.add_argument("--count", type=int, default=0, help="0=一直发")
    a = p.parse_args()

    frame = build_frame(a)
    sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
    print("→ %s:%d  mode=%s flags=0x%02X checksum=0x%02X bytes=%d rate=%gHz"
          % (a.host, a.port, a.mode, frame[1], frame[23], len(frame), a.hz))

    period = 1.0 / a.hz
    sent = 0
    try:
        while a.count == 0 or sent < a.count:
            sock.sendto(frame, (a.host, a.port))
            sent += 1
            time.sleep(period)
    except KeyboardInterrupt:
        pass
    print("已发送 %d 帧" % sent)


if __name__ == "__main__":
    main()