#!/usr/bin/env python3
# -*- coding: utf-8 -*-
"""复现实验：高频回发 CMD（与电机当前目标一致的 no-op）+ 全拍监控。

用户反馈：数据错乱要"频繁发指令"才出现。本脚本把 TX 压力拉起来，同时
段2(全拍)与段4(全拍)双路监控，命中即记录 dlc/完整48B/当时CONFIG/时刻，
用于判定残余拼接的方向与机制。

安全性：CMD 载荷 = 电机当前自报 CONFIG 的回声(mode/limitV/cur/vel/ang)
+ 页面默认 PID(100/100/100)，不改变电机行为。

用法: python scripts/repro_cmdload.py [秒数=90] [发送Hz=500]
"""
import json
import math
import socket
import struct
import sys
import threading
import time

HOST, PORT = "127.0.0.1", 8090
DUR_S = float(sys.argv[1]) if len(sys.argv) > 1 else 90.0
SEND_HZ = float(sys.argv[2]) if len(sys.argv) > 2 else 500.0


def post(path, body=b"{}"):
    req = (f"POST {path} HTTP/1.0\r\nHost: {HOST}:{PORT}\r\n"
           f"Content-Type: application/json\r\nContent-Length: {len(body)}\r\n"
           "Connection: close\r\n\r\n").encode("ascii") + body
    s = socket.create_connection((HOST, PORT))
    try:
        s.settimeout(5)
        s.sendall(req)
        buf = bytearray()
        while True:
            c = s.recv(65536)
            if not c:
                break
            buf.extend(c)
    finally:
        s.close()
    return json.loads(bytes(buf).partition(b"\r\n\r\n")[2].decode("utf-8", "replace"))


def build_cmd(cfg, ang, vel):
    """CMD 帧：mode3 速度+位置，limitV 回声当前 CONFIG，角度=正弦轨迹。
    vel 按节点取 200/150 —— 给 CONFIG 回显的 B7-10 速度字段打节点指纹。"""
    b = bytearray(32)
    b[0] = 3                           # mode: Velocity_Angle
    b[1:3] = cfg[1:3]                  # limitV u16 回声
    b[3:7] = struct.pack("<f", 0.0)    # target cur 0
    b[7:11] = struct.pack("<f", vel)   # vel limit = 节点指纹
    b[11:15] = struct.pack("<f", ang)  # target angle = sine
    for off in (15, 17, 19):
        struct.pack_into("<h", b, off, 100)
    return bytes(b).hex()


stop = threading.Event()
lock = threading.Lock()
stats = {"sends": 0, "send_err": 0, "t0": time.time()}
torn_events = []          # (t, node, dlc, data48hex, cfg_hex)
batch_events = []
torn_base = [0]


def sender():
    """每拍把两电机当前 CONFIG 回声成 CMD 发出去。"""
    cfgs = {}
    interval = 1.0 / SEND_HZ
    while not stop.is_set():
        t0 = time.time()
        try:
            rr = post("/api/motor/rxraw")
            raw = bytes.fromhex(rr.get("raw") or "")
            off = 0
            for _ in range(rr.get("count") or 0):
                if off + 8 > len(raw):
                    break
                dlc = raw[off + 1]
                fid = struct.unpack_from("<I", raw, off + 4)[0]
                w = min(dlc, 32)
                cfgs[(fid >> 16) & 0xFF] = raw[off + 8: off + 8 + w]
                off += 8 + w
        except Exception:
            time.sleep(0.05)
            continue
        if len(cfgs) >= 2:
            # 判别实验：两电机反相正弦 + 不同速度限，给帧内容打节点指纹。
            # node2: ang=1000+200sin, vel=200   node3: ang=1000-200sin, vel=150
            s = math.sin(2.0 * math.pi * (time.time() - stats["t0"]) / 10.0)
            frames = []
            for n, c in sorted(cfgs.items()):
                ang = 1000.0 + (200.0 * s if n == 2 else -200.0 * s)
                vel = 200.0 if n == 2 else 150.0
                frames.append({"ch": 2, "id": hex((n << 16) | 0x0200), "dlc": 32,
                               "data": build_cmd(c, ang, vel)})
            try:
                post("/api/motor/send", json.dumps({"frames": frames}).encode())
                with lock:
                    stats["sends"] += 1
            except Exception:
                with lock:
                    stats["send_err"] += 1
        dt = interval - (time.time() - t0)
        if dt > 0:
            time.sleep(dt)


def is_cfg_head(d, cfg):
    """段2槽头 8B 是否等于该节点当前 CONFIG 头（污染判据，比量程更准）。"""
    return cfg and len(d) >= 8 and d[:8] == cfg[:8] and d[0] == 0x03


def monitor():
    seen_s, seen_r = set(), set()
    cfgs = {}
    while not stop.is_set():
        try:
            st = post("/api/motor/state")
        except Exception:
            time.sleep(0.01)
            continue
        cyc = st.get("cycles")
        if cyc in seen_s:
            continue
        seen_s.add(cyc)
        if len(seen_s) > 400000:
            seen_s.clear()
        for m in st.get("motors") or []:
            if not m.get("online") or len(m.get("data") or "") < 96:
                continue
            d = bytes.fromhex(m["data"])
            mcu = struct.unpack_from("<h", d, 0)[0] * 0.01
            cfg = cfgs.get(m["node"])
            if not (-40.0 <= mcu <= 125.0) or is_cfg_head(d, cfg):
                with lock:
                    if len(torn_events) < 40:
                        torn_events.append((round(time.time() - stats["t0"], 2),
                                            m["node"], m.get("dlc"), d.hex(),
                                            (cfg or b"").hex()))
        try:
            rr = post("/api/motor/rxraw")
        except Exception:
            continue
        rc = rr.get("cycle")
        if rc in seen_r:
            continue
        seen_r.add(rc)
        if len(seen_r) > 400000:
            seen_r.clear()
        raw = bytes.fromhex(rr.get("raw") or "")
        off = 0
        for _ in range(rr.get("count") or 0):
            if off + 8 > len(raw):
                break
            dlc = raw[off + 1]
            fid = struct.unpack_from("<I", raw, off + 4)[0]
            w = min(dlc, 32)
            d = raw[off + 8: off + 8 + w]
            node = (fid >> 16) & 0xFF
            if (fid & 0xFFFF) == 0x0101:
                cfgs[node] = d
                if any(d[15:24]):
                    with lock:
                        if len(batch_events) < 40:
                            batch_events.append((round(time.time() - stats["t0"], 2),
                                                 node, d.hex()))
            off += 8 + w


def main():
    try:
        r = post("/api/app/load")
        torn_base[0] = r.get("rx_torn") or 0
    except Exception:
        pass
    ts = threading.Thread(target=sender, daemon=True)
    tm = threading.Thread(target=monitor, daemon=True)
    ts.start(); tm.start()
    time.sleep(DUR_S)
    stop.set(); ts.join(3); tm.join(3)
    try:
        r = post("/api/app/load")
        torn_now = r.get("rx_torn") or 0
    except Exception:
        torn_now = -1
    with lock:
        print(f"时长 {DUR_S:.0f}s  发送 {stats['sends']} 次(错{stats['send_err']})  "
              f"≈{stats['sends']/DUR_S:.0f} CMD拍/s")
        print(f"rx_torn: {torn_base[0]} -> {torn_now}  (Δ={torn_now-torn_base[0]})")
        print(f"段2 污染事件: {len(torn_events)}（前 8）")
        for e in torn_events[:8]:
            print(f"  t={e[0]}s node={e[1]} dlc={e[2]}")
            print(f"    slot: {e[3]}")
            print(f"    cfg : {e[4]}")
        print(f"段4 污染事件: {len(batch_events)}（前 8）")
        for e in batch_events[:8]:
            print(f"  t={e[0]}s node={e[1]} data={e[2]}")


if __name__ == "__main__":
    main()