"""_probe_wedge2.py — 按真实邮箱协议探针: 检查写邮箱空 → 写满 256B → 查事件/响应。"""
import struct
import sys
import time

import ecat_server as es
import pysoem

r = es.api_open({"substr": "I226"})
if not r.get("ok"):
    sys.exit(1)
m = es.g_master
s = m.slaves[0]


def fprd(addr, size):
    try:
        return bytes(s._fprd(addr, size, 20000))
    except Exception:
        return None


def sm_reg(addr):
    d = fprd(addr, 8)
    if d is None:
        return None
    start, length = struct.unpack("<HH", d[:4])
    return {"start": start, "len": length, "ctl": d[4], "st": d[5], "act": d[6],
            "raw": d.hex().upper()}


def show(tag):
    sm0, sm1 = sm_reg(0x0800), sm_reg(0x0808)
    ev = fprd(0x0220, 2)
    al = fprd(0x0130, 8)
    ev_v = struct.unpack("<H", ev)[0] if ev else -1
    al_v = struct.unpack("<H", al[:2])[0] if al else -1
    print(f"[{tag}] AL=0x{al_v:02X} 0x220=0x{ev_v:04X} SM0[st=0x{sm0['st']:02X} act=0x{sm0['act']:02X}] "
          f"SM1[st=0x{sm1['st']:02X} act=0x{sm1['act']:02X}] (SM0.bit3={sm0['st'] >> 3 & 1}写满 SM1.bit3={sm1['st'] >> 3 & 1}读满)",
          flush=True)
    return sm0, sm1, ev_v


m.state = pysoem.INIT_STATE | pysoem.STATE_ACK
m.write_state()
m.state_check(pysoem.INIT_STATE, 1_000_000)
m.config_init()
m.state = pysoem.PREOP_STATE | pysoem.STATE_ACK
m.write_state()
print("PREOP:", "OK" if m.state_check(pysoem.PREOP_STATE, 1_000_000) == pysoem.PREOP_STATE else "FAIL", flush=True)

sm0, sm1, ev = show("稳态")

# 合法 CoE SDO 读 0x1000:00(expedited), padded 到整个邮箱长度 256B
req_hdr = bytes([0x0A, 0x00, 0x00, 0x00, 0x00, 0x03,  # len=10, addr=0, pri=0, type=CoE(cnt=0)
                0x10, 0x00,  # CANOpen = 0x0010 (SDO req<<12 | 0x1000? 不对, 服务号 0x10?) —— 用标准 SOEM 格式
                0x00, 0x10,  # Command = 0x40(up req, exp)
                0x00, 0x10,  # Index = 0x1000
                0x00, 0x00]) # sub=0, +pad
# SOEM ecx_SDOread 的帧: CANOpen = (ECT_COES_SDOREQ<<12)=0x1000 | number; Command=0x40; Index; SubIndex
req = bytes([0x0A, 0x00, 0x00, 0x00, 0x00, 0x03,
             0x00, 0x10, 0x40, 0x00, 0x10, 0x00, 0x00, 0x00, 0x00, 0x00])
mbx = (req + bytes(sm0["len"]))[:sm0["len"]]
print(f"\n写满 SM0 RAM {sm0['len']}B(合法 CoE 读 0x1000:00)...", flush=True)
try:
    s._fpwr(sm0["start"], mbx)
    print("  写入 OK (WKC=1)", flush=True)
except Exception as e:
    print(f"  写入失败: {e}", flush=True)

sm0b, sm1b, ev2 = show("写入后立即")
time.sleep(0.2)
sm0c, sm1c, ev3 = show("写入后 200ms")

if sm1c and (sm1c["st"] & 0x08):
    d = fprd(sm1c["start"], 32)
    print(f"*** SSC 响应了! SM1 RAM = {d.hex().upper() if d else 'ERR'}", flush=True)
    print(">>> ESC+SSC 完全正常,楔死另有其因", flush=True)
else:
    print("\nSSC 无响应。检查 0x220 bit0 事件是否置位过...", flush=True)
    if sm0c and (sm0c["st"] & 0x08):
        print(f"SM0.bit3(写满)={sm0c['st'] >> 3 & 1} —— 写满标志挂着(主站侧协议已触发)", flush=True)
        # 模拟 SSC 解锁: 读 SM0 首 word + 末 word
        fprd(sm0c["start"], 2)
        fprd(sm0c["start"] + sm0c["len"] - 2, 2)
        sm0d, sm1d, ev4 = show("模拟 SSC 解锁后")
        if not (sm0d["st"] & 0x08):
            print("解锁有效(SM0.bit3 清零) —— SM 状态机活着", flush=True)
    else:
        print(f"SM0.bit3(写满)={sm0c['st'] >> 3 & 1 if sm0c else '?'} —— 写满标志都没置!写到末字节也没触发 mailbox full", flush=True)

es.api_close({})
print("DONE", flush=True)