#!/usr/bin/env python3
"""车端 CAN 报文模拟器 / 上行抓包器（联调用，非工程代码）。

依据《洗消机器人遥控器通信协议20260810.xlsx》sheet RC（分类一/二/三，全部扩展帧、Intel 小端位序）。
线格式：帧头 0xAA + 32 位 canId + DLC(8) + 8 字节数据 + 校验和 + 帧尾 0xBB，共 16 字节，
多字节字段按小端排列，与 comm/car.cpp 的 CanFrame 一致。

用法示例：
  # 向 UI 的 9001 端口周期发 VCU 反馈，看底盘页电量/电压/车速刷新
  python can_sender.py --soc 85 --volt 360 --speed 2.0

  # 制造故障：MCU 码 61（电机缺相故障）+ 整系统通信异常（底盘BMS）
  python can_sender.py --mcu-fault 61 --comm-fault 2

  # 洗消 + 气体检测仪反馈
  python can_sender.py --wash disinfect --level-a 88 --press 0.3 --gas-conc 250 --gas-type 2 --gas-level 2

  # 反向：绑 9000 抓 UI 下发的控制指令并解出来
  python can_sender.py --listen
"""

import argparse
import socket
import struct
import time

HDR  = 0xAA   # 帧头
TAIL = 0xBB   # 帧尾

# 分类一 遥控器控制指令（上行，UI → 车端）
ID_CHASSIS_CMD  = 0x18208081
ID_ARM_CMD1     = 0x18218081
ID_ARM_CMD2     = 0x18228081
ID_DECONTAM_CMD = 0x18238081
ID_DECONTAM_DBG = 0x18248081
ID_DETECTOR_CMD = 0x18258081
ID_EMERGENCY    = 0x18268081

# 分类二/三 VCU 状态反馈（下行，车端 → UI）
ID_VEHICLE_FB1  = 0x18308180
ID_VEHICLE_FB2  = 0x18318180
ID_ARM_FB1      = 0x18328180
ID_ARM_FB2      = 0x18338180
ID_ARM_FB3      = 0x18348180
ID_ARM_FB4      = 0x18358180
ID_DECONTAM_FB1 = 0x18368180
ID_DECONTAM_FB2 = 0x18378180
ID_DETECTOR_FB1 = 0x18388180
ID_DETECTOR_FB2 = 0x18398180
ID_VEHICLE_FB3  = 0x183A8180
ID_VEHICLE_FB4  = 0x183B8180

NAMES = {
    ID_CHASSIS_CMD: "Chassis_ControlCmd", ID_ARM_CMD1: "Arm_ControlCmd1",
    ID_ARM_CMD2: "Arm_ControlCmd2", ID_DECONTAM_CMD: "Decontam_ControlCmd",
    ID_DECONTAM_DBG: "Decontam_DebugCmd", ID_DETECTOR_CMD: "Detector_ControlCmd",
    ID_EMERGENCY: "Emergency_Cmd", ID_VEHICLE_FB1: "Vehicle_StatusFeedBack1",
    ID_VEHICLE_FB2: "Vehicle_StatusFeedBack2", ID_ARM_FB1: "Arm_StatusFeedBack1",
    ID_ARM_FB2: "Arm_StatusFeedBack2", ID_ARM_FB3: "Arm_StatusFeedBack3",
    ID_ARM_FB4: "Arm_StatusFeedBack4", ID_DECONTAM_FB1: "Decontam_StatusFeedBack1",
    ID_DECONTAM_FB2: "Decontam_StatusFeedBack2", ID_DETECTOR_FB1: "Detector_StatusFeedBack1",
    ID_DETECTOR_FB2: "Detector_StatusFeedBack2", ID_VEHICLE_FB3: "Vehicle_StatusFeedBack3",
    ID_VEHICLE_FB4: "Vehicle_StatusFeedBack4",
}


def set_bits(d, start, ln, v):
    """Intel（小端）：起始位即最低位序号，位序由低到高，可跨字节。"""
    v = int(v)
    for i in range(ln):
        b = start + i
        m = 1 << (b & 7)
        if v & (1 << i):
            d[b >> 3] |= m
        else:
            d[b >> 3] &= ~m & 0xFF


def get_bits(d, start, ln):
    v = 0
    for i in range(ln):
        b = start + i
        if d[b >> 3] & (1 << (b & 7)):
            v |= 1 << i
    return v


def get_signed(d, start, ln):
    raw = get_bits(d, start, ln)
    sign = 1 << (ln - 1)
    return (raw ^ sign) - sign


def checksum(can_id, dlc, d):
    # 帧头 + canId 四字节 + DLC + 数据八字节 + 帧尾 求和取低 8 位
    return (HDR + sum((can_id >> (8 * i)) & 0xFF for i in range(4))
            + dlc + sum(d) + TAIL) & 0xFF


def pack(can_id, d):
    return struct.pack("<BI B 8B B B", HDR, can_id, 8, *d,
                       checksum(can_id, 8, d), TAIL)


def clamp(v, lo, hi):
    return max(lo, min(hi, int(v)))


# ===================== 下行：构造分类二/三 反馈帧 =====================
def fb_vehicle1(a):
    d = bytearray(8)
    set_bits(d, 0, 16, clamp(round(a.volt * 10), 0, 0xFFFF))     # Total_voltage 精度 0.1
    set_bits(d, 16, 16, clamp(round(a.soc * 10), 0, 0xFFFF))    # SOC 精度 0.1
    set_bits(d, 32, 8, a.mcu_fault)                            # MCU_FaultCode
    set_bits(d, 40, 2, a.mcu_level)                            # MCU_FaultLevel
    set_bits(d, 45, 2, a.celllow_level)                        # UnderOver_FaultLevel
    set_bits(d, 47, 2, a.cellhigh_level)                       # CellOver_FaultLevel（跨字节）
    return d


def fb_vehicle2(a):
    d = bytearray(8)
    set_bits(d, 22, 2, 1 if a.handbrake else 0)                 # Handbrake
    # Vehi_spd 协议表为 8 位 × 0.01（上限 2.55 km/h），与 comm/car.cpp 的读法一致；
    # 早先按 16 位写、并把后续字段整体后移 8 位，会让 rdyStatus/通信故障码错位。
    set_bits(d, 24, 8, clamp(round(a.speed * 100), 0, 255))     # Vehi_spd
    set_bits(d, 32, 2, a.rdy_status)                            # Vehi_RdyStatus
    set_bits(d, 40, 8, a.comm_fault)                            # All_Comm_FaultCode
    return d


def fb_vehicle3(a):
    # 底盘状态3：46 个 EPS/制动故障位 + EPS_FaultCode，全 0 即无故障
    d = bytearray(8)
    set_bits(d, 48, 8, a.eps_fault)                             # EPS_FaultCode
    return d


def fb_vehicle4(a):
    d = bytearray(8)
    set_bits(d, 0, 2, a.gear)                                   # 0=N挡 1=D挡 2=R挡
    set_bits(d, 2, 2, a.parking)                                # 0未驻车 1左电机 2右电机 3完全驻车
    set_bits(d, 4, 2, a.awd)                                    # 0二驱 1四驱 2四驱锁定
    set_bits(d, 8, 16, a.steer)                                 # Steering_Angle
    # ALIGN 是状态字节：0x11=对中失败、0xEE/0x55=成功。给 0 会被判成“方向盘未对中”
    set_bits(d, 24, 8, a.align)                                 # Angle_alignment_status
    set_bits(d, 32, 8, a.brake_r)                               # Brake_Right_Mot_fault
    set_bits(d, 40, 8, a.brake_l)                               # Brake_Left_Mot_fault
    return d


def fb_arm1(a):
    d = bytearray(8)
    set_bits(d, 0, 16, a.posx & 0xFFFF)                         # End_positionX (Signed mm)
    set_bits(d, 16, 16, a.posy & 0xFFFF)
    set_bits(d, 32, 16, a.posz & 0xFFFF)
    return d


def fb_arm2(a):
    d = bytearray(8)
    set_bits(d, 0, 16, a.rx & 0xFFFF)
    set_bits(d, 16, 16, a.ry & 0xFFFF)
    set_bits(d, 32, 16, a.rz & 0xFFFF)
    set_bits(d, 48, 8, clamp(a.endspd, -128, 127) & 0xFF)       # End_spd (Signed)
    return d


def fb_arm3(a):
    d = bytearray(8)
    for i in range(4):
        set_bits(d, i * 16, 16, getattr(a, "j%d" % (i + 1)) & 0xFFFF)
    return d


def fb_arm4(a):
    d = bytearray(8)
    set_bits(d, 0, 16, a.j5 & 0xFFFF)
    set_bits(d, 16, 16, a.j6 & 0xFFFF)
    return d


def fb_decontam1(a):
    d = bytearray(8)
    set_bits(d, 0, 2, a.wash_mode)                              # 2=消除 3=消毒
    set_bits(d, 2, 1, 1 if a.wash_start else 0)
    set_bits(d, 3, 1, 1 if a.plunger else 0)
    set_bits(d, 4, 1, 1 if a.air else 0)
    set_bits(d, 5, 1, 1 if a.leaf else 0)
    set_bits(d, 6, 1, 1 if a.level_a_low else 0)                # A_LiquidLevel_Low
    set_bits(d, 7, 1, 1 if a.level_b_low else 0)                # B_LiquidLevel_Low
    set_bits(d, 8, 1, 1 if a.d1open else 0)                     # MBV_D1_Sta
    set_bits(d, 9, 1, 1 if a.d2open else 0)
    set_bits(d, 10, 1, 1 if a.sys_fault else 0)
    set_bits(d, 16, 8, clamp(a.level_a, 0, 255))
    set_bits(d, 24, 8, clamp(a.level_foam, 0, 255))
    set_bits(d, 32, 8, clamp(a.flow_a, 0, 255))
    set_bits(d, 40, 8, clamp(a.flow_b, 0, 255))
    set_bits(d, 48, 8, clamp(a.air_flow_fb, 0, 255))
    set_bits(d, 56, 8, clamp(a.expansion, 0, 255))
    return d


def fb_decontam2(a):
    d = bytearray(8)
    set_bits(d, 0, 8, clamp(round(a.press * 10), 0, 255))        # Pipe_pressure 精度 0.1
    set_bits(d, 8, 8, clamp(round(a.air_press * 10), 0, 255))   # AirComp_pressure
    set_bits(d, 48, 8, a.air_fault)                             # AirComp_FaultCode
    set_bits(d, 56, 8, a.plunger_fault)                         # PlungerPump_FaultCode
    return d


def fb_detector1(a):
    d = bytearray(8)
    set_bits(d, 0, 8, a.detector_sta)
    set_bits(d, 8, 8, a.gas_type)                               # Toxic_types 位组合
    set_bits(d, 16, 8, a.gas_level)
    set_bits(d, 24, 8, a.warn)
    return d


def fb_detector2(a):
    d = bytearray(8)
    set_bits(d, 0, 40, clamp(a.gas_conc, 0, (1 << 40) - 1))     # Toxic_concentration 40 位
    return d


FEEDBACK = [
    (ID_VEHICLE_FB1, fb_vehicle1), (ID_VEHICLE_FB2, fb_vehicle2),
    (ID_VEHICLE_FB3, fb_vehicle3), (ID_VEHICLE_FB4, fb_vehicle4),
    (ID_ARM_FB1, fb_arm1), (ID_ARM_FB2, fb_arm2),
    (ID_ARM_FB3, fb_arm3), (ID_ARM_FB4, fb_arm4),
    (ID_DECONTAM_FB1, fb_decontam1), (ID_DECONTAM_FB2, fb_decontam2),
    (ID_DETECTOR_FB1, fb_detector1), (ID_DETECTOR_FB2, fb_detector2),
]


# ===================== 上行：解析分类一 控制帧 =====================
def describe(can_id, d):
    if can_id == ID_CHASSIS_CMD:
        return ("Chassis  模式Cmd=%d 使能=%d gear=%d(0后/2048中/4096前) "
                "目标速度=%.2fkm/h turning=%d(0左/2048中/4096右)"
                % (get_bits(d, 0, 1), get_bits(d, 3, 1), get_bits(d, 8, 16),
                   get_bits(d, 24, 16) * 0.01, get_bits(d, 40, 16)))
    if can_id == ID_ARM_CMD1:
        return ("Arm1     模式=%d 使能=%d 上下=%d 左右=%d 前后=%d"
                % (get_bits(d, 0, 1), get_bits(d, 1, 1), get_bits(d, 8, 16),
                   get_bits(d, 24, 16), get_bits(d, 40, 16)))
    if can_id == ID_ARM_CMD2:
        return ("Arm2     横滚=%d 俯仰=%d 偏航=%d 回位/暂停=0x%02X(0xAA回位/0x55暂停)"
                % (get_bits(d, 0, 16), get_bits(d, 16, 16), get_bits(d, 32, 16),
                   get_bits(d, 48, 8)))
    if can_id == ID_DECONTAM_CMD:
        return ("Decontam 模式=%d(2消除/3消毒) 启停=%d 发泡=%d 泡沫比例=%d "
                "空压流量=%d 柱塞流量=%d"
                % (get_bits(d, 0, 2), get_bits(d, 2, 1), get_bits(d, 8, 8),
                   get_bits(d, 16, 8), get_bits(d, 24, 8), get_bits(d, 32, 8)))
    if can_id == ID_DECONTAM_DBG:
        return ("DecontamDbg 调试=%d 柱塞上电=%d 空压上电=%d 叶片上电=%d D1=%d D2=%d "
                "空压rpm=%d 柱塞rpm=%d 叶片rpm=%d 启动=%d/%d/%d"
                % (get_bits(d, 0, 1), get_bits(d, 1, 1), get_bits(d, 2, 1), get_bits(d, 3, 1),
                   get_bits(d, 4, 1), get_bits(d, 5, 1), get_bits(d, 8, 16),
                   get_bits(d, 24, 16), get_bits(d, 40, 16),
                   get_bits(d, 56, 1), get_bits(d, 57, 1), get_bits(d, 58, 1)))
    if can_id == ID_DETECTOR_CMD:
        return "Detector 指令=0x%02X(0xAA启动/0x55停止)" % get_bits(d, 0, 8)
    if can_id == ID_EMERGENCY:
        return "Emergency 急停=0x%02X(0x55急停/0正常)" % get_bits(d, 0, 8)
    return None


def listen(host, port, once):
    sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
    sock.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
    sock.bind((host, port))
    print("监听 %s:%d，解 UI 下发的控制帧（Ctrl-C 退出）" % (host, port))
    last = {}
    try:
        while True:
            data, addr = sock.recvfrom(4096)
            off = 0
            while off + 16 <= len(data):
                hdr, cid, dlc = struct.unpack_from("<BIB", data, off)
                if hdr == HDR and dlc == 8 and data[off + 15] == TAIL:
                    body = data[off + 6:off + 14]
                    cks = data[off + 14]
                    if cks == checksum(cid, dlc, body):
                        txt = describe(cid, body)
                        if txt:
                            # 周期 50ms，只在内容变化时打印，避免刷屏
                            if once is None or last.get(cid) != body:
                                last[cid] = bytes(body)
                                print("%-16s %s" % (NAMES.get(cid, hex(cid)), txt))
                        off += 16
                        continue
                off += 1
    except KeyboardInterrupt:
        pass


def main():
    p = argparse.ArgumentParser()
    p.add_argument("--host", default="127.0.0.1")
    p.add_argument("--port", type=int, default=9000,
                   help="发送目标端口 = UI 的 kCanRxPort（默认 9000）；--listen 抓上行时改 3001")
    p.add_argument("--listen", action="store_true", help="改为绑端口抓 UI 上行控制帧")
    p.add_argument("--hz", type=float, default=10.0, help="反馈帧发送频率，默认 10Hz")
    p.add_argument("--count", type=int, default=0, help="0=一直发")

    # 电池 / 底盘
    p.add_argument("--soc", type=float, default=85.0, help="电量 %")
    p.add_argument("--volt", type=float, default=360.0, help="总电压 V（精度 0.1）")
    p.add_argument("--speed", type=float, default=2.0, help="km/h，精度 0.01，协议只给 8 位（上限 2.55）")
    p.add_argument("--no-handbrake", action="store_true")
    p.add_argument("--rdy-status", type=int, default=2, help="底盘 ready：0初始/1上电完成/2就绪")
    p.add_argument("--mcu-fault", type=int, default=0)
    p.add_argument("--mcu-level", type=int, default=0)
    p.add_argument("--celllow-level", type=int, default=0)
    p.add_argument("--cellhigh-level", type=int, default=0)
    p.add_argument("--comm-fault", type=int, default=0, help="整系统通信故障码 0~8")
    # 底盘状态3/4
    p.add_argument("--eps-fault", type=int, default=0, help="EPS 故障码，0=无")
    p.add_argument("--gear", type=int, default=0, help="0=N挡 1=D挡 2=R挡")
    p.add_argument("--parking", type=int, default=0, help="0未驻车 1左电机 2右电机 3完全驻车")
    p.add_argument("--awd", type=int, default=0, help="0二驱 1四驱 2四驱锁定")
    p.add_argument("--steer", type=int, default=0, help="方向盘角度")
    p.add_argument("--align", type=int, default=0x55, help="0x11对中失败 / 0xEE、0x55成功；别给 0")
    p.add_argument("--brake-r", type=int, default=0, help="右制动电机故障码")
    p.add_argument("--brake-l", type=int, default=0, help="左制动电机故障码")
    # 机械臂
    p.add_argument("--posx", type=int, default=1200)
    p.add_argument("--posy", type=int, default=300)
    p.add_argument("--posz", type=int, default=800)
    p.add_argument("--rx", type=int, default=0)
    p.add_argument("--ry", type=int, default=0)
    p.add_argument("--rz", type=int, default=0)
    p.add_argument("--endspd", type=int, default=0)
    for i in range(1, 7):
        p.add_argument("--j%d" % i, type=int, default=[30, 20, 0, 90, 45, 0][i - 1])
    # 洗消
    p.add_argument("--wash", choices=["clean", "disinfect"], default="clean")
    p.add_argument("--wash-start", action="store_true")
    p.add_argument("--plunger", action="store_true")
    p.add_argument("--air", action="store_true")
    p.add_argument("--leaf", action="store_true")
    p.add_argument("--d1open", action="store_true")
    p.add_argument("--d2open", action="store_true")
    p.add_argument("--sys-fault", action="store_true")
    p.add_argument("--level-a-low", action="store_true", help="A组分/清水液位低 → 首页液位行红闪")
    p.add_argument("--level-b-low", action="store_true", help="B组分/泡沫液位低 → 首页液位行红闪")
    p.add_argument("--level-a", type=float, default=100)
    p.add_argument("--level-foam", type=float, default=30)
    p.add_argument("--flow-a", type=float, default=20)
    p.add_argument("--flow-b", type=float, default=0.5)
    p.add_argument("--air-flow-fb", type=float, default=100)
    p.add_argument("--expansion", type=float, default=6)
    p.add_argument("--press", type=float, default=0.3, help="MPa，精度 0.1")
    p.add_argument("--air-press", type=float, default=0.35)
    p.add_argument("--air-fault", type=int, default=0)
    p.add_argument("--plunger-fault", type=int, default=0)
    # 气体检测仪
    p.add_argument("--detector-sta", type=int, default=4, help="4=检测中")
    p.add_argument("--gas-type", type=int, default=2, help="位组合 1=P 2=S 4=HNO 8=CH 16=AS")
    p.add_argument("--gas-level", type=int, default=2)
    p.add_argument("--warn", type=int, default=0, help="1=进气嘴未接 2=氢气不足 4=仪器故障 8=浓度过高")
    p.add_argument("--gas-conc", type=float, default=250.0, help="ppb")
    a = p.parse_args()
    a.wash_mode = 3 if a.wash == "disinfect" else 2
    a.handbrake = not a.no_handbrake

    if a.listen:
        listen(a.host, a.port, None)
        return

    sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
    period = 1.0 / a.hz
    print("→ %s:%d  反馈帧 %d 条 @ %gHz  电量=%.1f%% 电压=%.1fV 车速=%.2fkm/h 洗消=%s"
          % (a.host, a.port, len(FEEDBACK), a.hz, a.soc, a.volt, a.speed, a.wash))
    sent = 0
    try:
        while a.count == 0 or sent < a.count:
            for can_id, build in FEEDBACK:
                sock.sendto(pack(can_id, build(a)), (a.host, a.port))
            sent += 1
            time.sleep(period)
    except KeyboardInterrupt:
        pass
    print("已发送 %d 轮（每轮 %d 帧）" % (sent, len(FEEDBACK)))


if __name__ == "__main__":
    main()