# -*- coding: utf-8 -*-
u"""把刀片竖插到底板槽位 + 干涉扫描，量出金手指插进插槽塑料体的深度 d。

用法：
    python place_and_probe.py validate [槽号0..4]   # 摆位到槽口上方，验证旋转约定
    python place_and_probe.py sweep    [槽号0..4]   # 扫 dz 找首次接触，再扫 y 找腔中心
    python place_and_probe.py restore               # 还原刀片到脚本首次运行前的位置

⛔ 绝不调用 CloseAllDocuments —— 底板零部件引用 %TEMP%\\swx*\\IC~~\\，一关就断链。

━━ 实测约定（都踩过，见 NOTES-probed.md）━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━

1. `CreateTransform` 只能走 dispid + `VARIANT(VT_ARRAY|VT_R8)`。
   传 list / 普通 VARIANT → "服务器出现意外情况"；
   传 tuple → **不报错但静默返回单位阵**。⇒ 必须回读校验。

2. ⛔⛔ `ToolsCheckInterference2` **是死的**：在"刀片板体直接穿过底板板体"
   这种必然干涉的姿态下仍返回 `None` / PComp 0 条。它不抛异常、返回值看着像
   正常答案 —— 拿它做判据会得出"全程无干涉"的假结论。**改用
   `IInterferenceDetectionMgr`**（还附带干涉体积）。

3. `GetInterferenceCount` / `GetInterferences` 在 tlb 里标 invkind=1（方法），
   但动态派发下必须**当属性取**（不加括号），同 NOTES §9。

4. 旋转矩阵 `ArrayData[0:9]` 是**列主序**（NOTES §14 原来写的"行主序"是错的）——
   本脚本用 `validate` 在装配体上现场判定，不写死。

5. 底板自身就有几十处预存干涉（器件 3D 模型压进板体等），所以**只统计
   刀片子树 ↔ 底板子树的跨对**，否则信号被淹没。
"""
import io
import json
import os
import sys
import time

sys.stdout = io.TextIOWrapper(sys.stdout.buffer, encoding='utf-8')

import pythoncom
import win32com.client as wc

HERE = os.path.dirname(os.path.abspath(__file__))
BACKUP = os.path.join(HERE, '_blade_xform_backup.json')

# ── 几何常量（出处：.claude/r2/pcb_base.py / pcb_blade.py，板局部坐标 mm）──────
SLOT_CX = 49.0                   # 底板上槽心 x
SLOT_YS = [8.36, 28.68, 49.0, 69.32, 89.64]
J1_X = 45.0                      # 刀片金手指阵列中心 x（= BW/2，刀片 90 宽）
BLADE_T = 1.67                   # STEP 实测板厚
DX = SLOT_CX - J1_X              # = 4.00
STEP_H = 12.0                    # 刀片台阶高（舌头两侧肩部底边在刀片局部 y=12.0）

# 底板 3D 实测（底板局部坐标）：板体 z 0.00~1.67；
# 插槽 PCIE-64P11L 塑料体 z 0.77~12.33，体高 11.56（**不是设计文档写的 ~9**）。
SLOT_TOP = 12.33                 # 塑料体顶面 z ⇒ 插入深度 d = SLOT_TOP − dz
SLOT_MODEL_DY = -0.45            # 插槽 3D 模型 y 中心相对焊盘中心的偏移（五槽一致）

ROT_A = [1.0, 0.0, 0.0,  0.0, 0.0, -1.0,  0.0, 1.0, 0.0]
ROT_B = [1.0, 0.0, 0.0,  0.0, 0.0, 1.0,  0.0, -1.0, 0.0]


def connect(tries=12):
    u"""⚠ SolidWorks 忙（长时间干涉计算 / 重建）时会**暂时从运行对象表摘掉自己**，
    `GetActiveObject` 报 `-2147221021 操作无法使用`。这**不是崩溃** ——
    进程还在、文档还开着、临时件引用链也没断，等几秒重试即可。
    别据此判定 SolidWorks 挂了而去重启或重导 STEP（那才会真把引用链搞断）。"""
    for k in range(tries):
        try:
            return wc.GetActiveObject('SldWorks.Application')
        except Exception:
            if k == tries - 1:
                raise
            time.sleep(4)


def mk_xform(app, arr16):
    u"""arr16 → IMathTransform。**必须回读校验**，见文件头第 1 条。"""
    did = app._oleobj_.GetIDsOfNames('GetMathUtility')
    mu = wc.Dispatch(app._oleobj_.Invoke(did, 0, pythoncom.DISPATCH_METHOD, 1))
    did2 = mu._oleobj_.GetIDsOfNames('CreateTransform')
    v = wc.VARIANT(pythoncom.VT_ARRAY | pythoncom.VT_R8, list(arr16))
    xf = wc.Dispatch(mu._oleobj_.Invoke(did2, 0, pythoncom.DISPATCH_METHOD, 1, v))
    got = list(xf.ArrayData)
    if max(abs(a - b) for a, b in zip(got, arr16)) > 1e-9:
        raise RuntimeError(u'CreateTransform 回读不符（多半被静默降成单位阵）：%r' % (got,))
    return xf


def blade_arr(rot9, slot_i, dz, ycen=None, ddx=0.0):
    u"""rot9 3x3 + 平移 → 16 元素数组（平移转米）。

    ycen = 刀片板厚中面要对到的底板 y（默认焊盘槽心）；
    ddx  = 在标称 DX 之上再沿 x 平移多少（用来试"舌头是不是比腔长"）。"""
    if ycen is None:
        ycen = SLOT_YS[slot_i]
    return list(rot9) + [(DX + ddx) / 1000.0, (ycen + BLADE_T / 2.0) / 1000.0,
                         dz / 1000.0, 1.0, 0.0, 0.0, 0.0]


def get(app):
    doc = app.ActiveDoc
    comps = doc.GetComponents(True)
    blade = [c for c in comps if 'blade' in c.Name2.lower()][0]
    base = [c for c in comps if c is not blade][0]
    return doc, blade, base


def box(c):
    return [round(v * 1000.0, 3) for v in c.GetBox(False, False)]


def place(app, doc, blade, xf):
    try:
        blade.SetTransformAndSolve2(xf)
    except Exception:
        blade.Transform2 = xf
    doc.EditRebuild3
    return box(blade)


def cross_pairs(doc, dump=False, fast=False):
    u"""只返回「刀片子树 ↔ 底板子树」的干涉，[(体积mm^3, 名A, 名B), ...]，按体积降序。

    fast=True：`TreatSubAssembliesAsComponents=True`，只按**两个顶层零部件**算 ——
    底板自身那 245 处内部干涉直接不参与计算，单次由 ~41 s 降到秒级，
    且结果天然只剩跨对。代价是拿不到叶子零件名（只知道"刀片撞底板"，不知撞的是谁）。
    ⇒ 布尔型的扫描（找窗口边界）用 fast；要指名道姓时用全量。
      两者必须在**同一个已知干涉的姿态**上对过口径才能混用（main 里会验一次）。
    """
    idm = doc.InterferenceDetectionManager
    idm.TreatCoincidenceAsInterference = False
    idm.IncludeMultibodyPartInterferences = not fast
    idm.TreatSubAssembliesAsComponents = fast
    idm.IgnoreHiddenBodies = True
    idm.UseTransform = False
    out, total = [], 0

    def members():
        u"""⚠ 连调几十轮后 `idm.GetInterferences` 会突然抛 AttributeError
        （动态派发的包装失效）。按 dispid 直调兜底，同 §17 的招数。"""
        try:
            return idm.GetInterferences
        except AttributeError:
            did = idm._oleobj_.GetIDsOfNames('GetInterferences')
            return idm._oleobj_.Invoke(did, 0, pythoncom.DISPATCH_PROPERTYGET, 1)

    try:
        try:
            n = idm.GetInterferenceCount
        except AttributeError:
            did = idm._oleobj_.GetIDsOfNames('GetInterferenceCount')
            n = idm._oleobj_.Invoke(did, 0, pythoncom.DISPATCH_PROPERTYGET, 1)
        total = n or 0
        if n:
            for it in members():
                o = it if hasattr(it, 'Volume') else wc.Dispatch(it)
                names = [wc.Dispatch(c).Name2 for c in o.Components]
                if dump and len(out) == 0:
                    print(u'   [诊断] 全名样本: %r' % (names,))
                if len(names) != 2:
                    continue
                a, b = names
                if ('blade' in a.lower()) != ('blade' in b.lower()):
                    out.append((o.Volume * 1e9, a.split('/')[-1], b.split('/')[-1]))
    finally:
        try: idm.Done()
        except Exception: pass
    out.sort(reverse=True)
    return out, total


def main():
    mode = sys.argv[1] if len(sys.argv) > 1 else 'validate'
    slot_i = int(sys.argv[2]) if len(sys.argv) > 2 else 2      # 默认中间槽 J36

    app = connect()
    doc, blade, base = get(app)
    print(u'活动文档: %s' % doc.GetTitle)
    print(u'刀片: %s   底板: %s' % (blade.Name2, base.Name2))

    if not os.path.exists(BACKUP):
        json.dump({'name': blade.Name2, 'arr': list(blade.Transform2.ArrayData)},
                  open(BACKUP, 'w'), indent=1)
        print(u'已备份刀片原始变换 → %s' % BACKUP)

    if mode == 'restore':
        arr = json.load(open(BACKUP))['arr']
        print(u'还原 → %s' % place(app, doc, blade, mk_xform(app, arr)))
        return

    # ── 验证旋转约定：用一个明显够不着的高度 ────────────────────────────
    good = None
    for tag, rot in ((u'ROT_A', ROT_A), (u'ROT_B 转置', ROT_B)):
        b = place(app, doc, blade, mk_xform(app, blade_arr(rot, slot_i, 20.0)))
        ok = (abs(b[0] - DX) < 0.3 and abs(b[3] - (DX + 90)) < 0.3
              and b[2] > 19.0 and b[5] > 80.0)
        print(u'  %-12s → box %s  %s' % (tag, b, u'✅' if ok else u'❌'))
        if ok:
            good = rot
            break
    if good is None:
        print(u'⛔ 两种约定都不对，停手'); return

    slot_y = SLOT_YS[slot_i]

    # ── 基线：刀片停在够不着的高度，跨对必须为 0 ─────────────────────────
    t0 = time.time()
    base_pairs, base_total = cross_pairs(doc, dump=True)
    print(u'\n基线 (dz=20，刀片悬空)：全装配干涉 %d 处，其中刀片↔底板跨对 %d 处  [%.1fs]'
          % (base_total, len(base_pairs), time.time() - t0))
    if base_pairs:
        print(u'  ⚠ 悬空时就有跨对，说明过滤口径不对，先看这些：')
        for v, a, b in base_pairs[:5]:
            print(u'    %10.3f mm^3  %s <-> %s' % (v, a[:34], b[:34]))
        return
    if base_total == 0:
        print(u'  ⛔ 全装配 0 处干涉 —— 底板自身预存干涉应该非 0，检测可能又是假阴性，停手')
        return

    if mode == 'validate':
        print(u'\n旋转约定：%s；检测基线正常。刀片停在 dz=20（槽口上方）。'
              % (u'ROT_A' if good is ROT_A else u'ROT_B 转置'))
        return

    def probe(dz, ycen=None, ddx=0.0, fast=False):
        place(app, doc, blade, mk_xform(app, blade_arr(good, slot_i, dz, ycen, ddx)))
        return cross_pairs(doc, fast=fast)[0]

    # ── 扫 dz ────────────────────────────────────────────────────────────
    print(u'\n槽 %d（焊盘槽心 y=%.2f）dz 扫描' % (slot_i + 1, slot_y))
    print(u'  dz = 刀片底边（金手指尖）的 z；塑料体顶面 z=%.2f；' % SLOT_TOP)
    print(u'  肩部（台阶 %.1f）碰塑料体顶面应发生在 dz=%.2f；底板板体顶面 z=1.67。'
          % (STEP_H, SLOT_TOP - STEP_H))
    first = None
    for k in range(24):
        dz = round(13.0 - 0.5 * k, 2)
        if dz < 1.5:
            break
        t0 = time.time()
        ps = probe(dz)
        tot = sum(p[0] for p in ps)
        print(u'  dz=%6.2f  跨对 %2d  体积 %9.3f mm^3  [%.1fs]  %s'
              % (dz, len(ps), tot, time.time() - t0,
                 (u'%s <-> %s' % (ps[0][1][:26], ps[0][2][:26])) if ps else u''))
        if ps:
            first = dz
            break
    if first is None:
        print(u'\n⛔ 一路扫到 dz=1.5（刀片快碰到底板板面）都没有跨对干涉')
        print(u'  ⇒ 插槽塑料体的 3D 模型很可能是**没有建腔的空壳/或腔通到底**，量不出 d。')
        return

    lo, hi = first, first + 0.5
    for _ in range(5):
        mid = round((lo + hi) / 2.0, 3)
        if probe(mid): lo = mid
        else: hi = mid
    d = SLOT_TOP - hi
    print(u'\n首次接触 dz ≈ %.3f（恰好不干涉 %.3f）' % (lo, hi))
    print(u'⇒ **插入深度 d = %.2f − %.3f = %.2f mm**' % (SLOT_TOP, hi, d))
    print(u'⇒ 刀片底边离底板板面 = %.2f mm（设计文档写的「约 11 mm」对不对看这个数）' % (hi - 1.67))
    ps = probe(lo)
    print(u'  刚接触时挡住的是：')
    for v, a, b in ps[:4]:
        print(u'    %10.4f mm^3  %s <-> %s' % (v, a[:36], b[:36]))

    # ── 对口径：fast 与全量必须在同一个已知干涉的姿态上一致 ────────────────
    place(app, doc, blade, mk_xform(app, blade_arr(good, slot_i, lo)))
    t0 = time.time(); n_full = len(cross_pairs(doc)[0]); t_full = time.time() - t0
    t0 = time.time(); n_fast = len(cross_pairs(doc, fast=True)[0]); t_fast = time.time() - t0
    print(u'\n口径对账 @dz=%.3f：全量 %d 处 [%.1fs] / fast %d 处 [%.1fs]'
          % (lo, n_full, t_full, n_fast, t_fast))
    if (n_full > 0) != (n_fast > 0):
        print(u'  ⛔ 两种口径不一致，后面的扫描不能用 fast，停手'); return
    print(u'  ✅ 一致，布尔扫描改用 fast（快 %.0f 倍）' % (t_full / max(t_fast, 1e-3)))

    # ── 在腔内扫 y：腔的真实中心在哪（分辨假设 B：模型 y 偏心 0.45）────────
    probe_z = round(hi - 1.0, 3)      # ⚠ 插得更深 = dz 更**小**，别写成 +
    print(u'\n[假设B] 在 dz=%.3f（比刚接触再低 1.0）处扫 y：' % probe_z)
    ys = [round(slot_y - 1.6 + 0.1 * k, 2) for k in range(33)]
    free = [y for y in ys if not probe(probe_z, y, fast=True)]
    if free:
        cen = (free[0] + free[-1]) / 2.0
        print(u'  无干涉窗口 y = %.2f ~ %.2f（宽 %.2f），中心 %.2f，相对焊盘槽心 %+.2f'
              % (free[0], free[-1], free[-1] - free[0], cen, cen - slot_y))
        print(u'  （包围盒推出的模型偏移是 %+.2f；若两者接近 ⇒ 假设B 成立，是对中问题）'
              % SLOT_MODEL_DY)
    else:
        cen = None
        print(u'  ⛔ ±1.6 内处处干涉 ⇒ 不是 y 对中的事，假设B 不成立')

    # ── 扫 x：舌头是不是比腔长（分辨假设 A：36.0 偏宽）────────────────────
    print(u'\n[假设A] 在同一深度左右平移刀片，看舌头两端是不是顶在腔的端壁上：')
    print(u'  （若是"太宽"，那么无论怎么平移都插不进；若只是偏心，平移能找到窗口）')
    hits = []
    for k in range(17):
        ddx = round(-2.0 + 0.25 * k, 2)
        n = len(probe(probe_z, cen, ddx=ddx, fast=True))
        hits.append((ddx, n))
    okx = [d for d, n in hits if n == 0]
    if okx:
        print(u'  x 平移 %+.2f ~ %+.2f 时无干涉 ⇒ **腔够长**，是对中/偏移问题' % (okx[0], okx[-1]))
    else:
        # ⛔⛔ 别在这里下"舌头太宽"的结论 —— 这个测试**没有分辨力**。
        #    y/x 扫描都是在首次接触**以下**做的。若插槽模型压根是个没建腔的实心块，
        #    那么任何 y、任何 x 都必然干涉，现象与"腔比舌头短"**完全相同**。
        #    ⇒ 必须先用「实体体积 / 包围盒体积 + 面数」判定模型有没有腔（见 NOTES §24），
        #      确认有腔之后，这里的"全程干涉"才能读作"舌头比腔长"。
        print(u'  全程 %+.2f ~ %+.2f 都干涉。' % (hits[0][0], hits[-1][0]))
        print(u'  ⚠ 但这**推不出**"舌头 36.0 比腔长" —— 若插槽模型是无腔实心块，'
              u'现象一模一样。')
        print(u'  ⇒ 先判模型有没有腔（体积比 + 面数），再回来读这一段。')

    place(app, doc, blade, mk_xform(app, blade_arr(good, slot_i, hi, cen)))
    print(u'\n刀片停在 dz=%.3f, ycen=%s，box=%s' % (hi, cen, box(blade)))


if __name__ == '__main__':
    try:
        main()
    except Exception:
        # ⛔⛔ 本会话的惨痛教训（NOTES §22）：脚本崩在干涉检查中途，会把 SolidWorks
        #    留在「干涉检查」这个**活动命令**里。之后它会把自己从运行对象表撤销，
        #    `GetActiveObject` 一律报 -2147221021，**客户端侧无解、只能重启**，
        #    而重启会让 %TEMP%\swx<PID>\ 里的临时件全丢、底板零部件断引用。
        #    ⇒ 这里无论如何都要把命令关掉再把异常抛出去。
        try:
            _a = wc.GetActiveObject('SldWorks.Application')
            _d = _a.ActiveDoc
            if _d is not None:
                _d.InterferenceDetectionManager.Done()
                print(u'[兜底] 已关闭干涉检查命令')
        except Exception as _e:
            print(u'[兜底] 关闭失败（可能已失联）: %s' % str(_e)[:90])
        raise