/* ============================================================================
 *  arm-ik-solver.js  —  六轴手臂逆运动学求解器（最小总关节角）
 *
 *  正运动学：World = A1(θ1)·A2(θ2)·A3(θ3)·A4(θ4)·A5(θ5)·A6(θ6)·TCP，
 *            Ai(θ) = T(Pi)·R(axis_i, θ)·T(−Pi)，与查看器场景图完全一致。
 *  逆运动学：以 (θ4,θ5) 为二维冗余参数全域扫描，其余三轴闭式求解。
 *
 *  MIT / 无依赖。可在浏览器（window.ArmIK）或 Node（module.exports）中加载。
 * ==========================================================================*/
(function (root, factory) {
  if (typeof module === 'object' && module.exports) module.exports = factory();
  else root.ArmIK = factory();
}(typeof self !== 'undefined' ? self : this, function () {
'use strict';

const D2R = Math.PI / 180, R2D = 180 / Math.PI;
const ORDER = ['J1', 'J2', 'J3', 'J4', 'J5', 'J6'];

/* ---------- 小工具 ---------- */
const wrap180 = a => { a = (a + 180) % 360; if (a < 0) a += 360; return a - 180; };
const nrm = v => { const n = Math.hypot(v[0], v[1], v[2]) || 1; return [v[0] / n, v[1] / n, v[2] / n]; };
function rotAxis(d, a) {                       // 3x3 旋转：绕单位轴 d 转 a 弧度（右手法）
  const c = Math.cos(a), s = Math.sin(a);
  /* 轴对齐特化：查看器 6 个关节轴均为 ±X/±Y/±Z，Rodrigues 通用式退化为每轴 4 个非零元素
     （数学恒等；绕 −X 转 a = 绕 X 转 −a）。姿态路径热点（rotAxis 曾占 ~11%）。 */
  const x = d[0], y = d[1], z = d[2];
  if (y === 0 && z === 0) {
    if (x > 0) return [1, 0, 0, 0, c, -s, 0, s, c];
    return [1, 0, 0, 0, c, s, 0, -s, c];
  }
  if (x === 0 && z === 0) {
    if (y > 0) return [c, 0, s, 0, 1, 0, -s, 0, c];
    return [c, 0, -s, 0, 1, 0, s, 0, c];
  }
  if (x === 0 && y === 0) {
    if (z > 0) return [c, -s, 0, s, c, 0, 0, 0, 1];
    return [c, s, 0, -s, c, 0, 0, 0, 1];
  }
  const C = 1 - c;
  return [
    x * x * C + c,     x * y * C - z * s, x * z * C + y * s,
    y * x * C + z * s, y * y * C + c,     y * z * C - x * s,
    z * x * C - y * s, z * y * C + x * s, z * z * C + c
  ];
}
const mul3 = (A, B) => {
  const C = new Array(9);
  for (let i = 0; i < 3; i++) for (let j = 0; j < 3; j++)
    C[i * 3 + j] = A[i * 3] * B[j] + A[i * 3 + 1] * B[3 + j] + A[i * 3 + 2] * B[6 + j];
  return C;
};
const mv3 = (A, v) => [
  A[0] * v[0] + A[1] * v[1] + A[2] * v[2],
  A[3] * v[0] + A[4] * v[1] + A[5] * v[2],
  A[6] * v[0] + A[7] * v[1] + A[8] * v[2]
];
const cross3 = (a, b) => [a[1] * b[2] - a[2] * b[1], a[2] * b[0] - a[0] * b[2], a[0] * b[1] - a[1] * b[0]];
const T3 = M => [M[0], M[3], M[6], M[1], M[4], M[7], M[2], M[5], M[8]];   // 3x3 转置
const clamp1 = v => v < -1 ? -1 : (v > 1 ? 1 : v);

/* 两线段最近距离（Ericson《Real-Time Collision Detection》标准式）。
   动态限位的度量基础：解析式必须敢被数值校验——node 侧对随机线段与
   暴力密集采样对比（scratch 自检脚本），别只看"数值像是对的"。
   内核收标量直传（碰撞热循环每次调用上万次，建数组的分配开销实测占一半），
   数组版仅作对外包装/测试用。 */
function segSegDistRaw(x1, y1, z1, x2, y2, z2, x3, y3, z3, x4, y4, z4) {
  const d1x = x2 - x1, d1y = y2 - y1, d1z = z2 - z1;
  const d2x = x4 - x3, d2y = y4 - y3, d2z = z4 - z3;
  const rx = x1 - x3, ry = y1 - y3, rz = z1 - z3;
  const a = d1x * d1x + d1y * d1y + d1z * d1z;
  const e = d2x * d2x + d2y * d2y + d2z * d2z;
  const f = d2x * rx + d2y * ry + d2z * rz;
  const cl01 = v => v < 0 ? 0 : (v > 1 ? 1 : v);
  let s = 0, t = 0;
  if (a <= 1e-12 && e <= 1e-12) { s = 0; t = 0; }
  else if (a <= 1e-12) { s = 0; t = cl01(f / e); }
  else {
    const c = d1x * rx + d1y * ry + d1z * rz;
    if (e <= 1e-12) { t = 0; s = cl01(-c / a); }
    else {
      const b = d1x * d2x + d1y * d2y + d1z * d2z;
      const denom = a * e - b * b;
      s = denom > 1e-12 ? cl01((b * f - c * e) / denom) : 0;
      t = (b * s + f) / e;
      if (t < 0) { t = 0; s = cl01(-c / a); }
      else if (t > 1) { t = 1; s = cl01((b - c) / a); }
    }
  }
  const cx = x1 + d1x * s - (x3 + d2x * t);
  const cy = y1 + d1y * s - (y3 + d2y * t);
  const cz = z1 + d1z * s - (z3 + d2z * t);
  return Math.sqrt(cx * cx + cy * cy + cz * cz);
}
function segSegDist(p1, q1, p2, q2) {
  return segSegDistRaw(p1[0], p1[1], p1[2], q1[0], q1[1], q1[2],
                       p2[0], p2[1], p2[2], q2[0], q2[1], q2[2]);
}

/* 两旋转矩阵间的夹角（度）：θ = acos((tr(AᵀB) − 1) / 2)，A、B 均为正交阵。
   tr(AᵀB) = Σᵢⱼ AᵢⱼBᵢⱼ —— 行主序即两数组逐元素点积。 */
function rotErrDeg(A, B) {
  let tr = 0;
  for (let i = 0; i < 9; i++) tr += A[i] * B[i];
  return Math.acos(clamp1((tr - 1) / 2)) * R2D;
}

/* 姿态参数（CAD 系）→ 工具坐标系旋转矩阵（行主序 9 数）。
   az：工具轴（法兰面 +Z）绕 CAD Z 轴的方位角；el：工具轴自 CAD +Z 抬起的仰角（0=朝上，180=朝下）；
   roll：绕工具轴自身的滚转角（右手系）。
   列向量构成工具坐标系：z = 工具轴；x0 = 子午面基（−sin az, cos az, 0），恒垂直于 z；
   x = 绕 z 滚转 roll 后的 x0；y = z × x 保证右手系。 */
function poseMat(azDeg, elDeg, rollDeg) {
  const az = azDeg * D2R, el = elDeg * D2R, ro = rollDeg * D2R;
  const ca = Math.cos(az), sa = Math.sin(az);
  const z = [Math.sin(el) * ca, Math.sin(el) * sa, Math.cos(el)];
  const x0 = [-sa, ca, 0];
  const x = mv3(rotAxis(z, ro), x0);
  const y = cross3(z, x);
  return [x[0], y[0], z[0], x[1], y[1], z[1], x[2], y[2], z[2]];
}

/* 姿态矩阵 → {az, el, roll}（poseMat 的逆分解，度）。el ∈ [0,180]，az ∈ (−180,180]，roll ∈ (−180,180]
   R = R0·Rx(roll)（R0 = poseMat(az,el,0)，列为 x0/y0/z）⇒ 工具 x 列 = cos(roll)·x0 + sin(roll)·y0 */
function poseAng(R) {
  const z = [R[2], R[5], R[8]], x = [R[0], R[3], R[6]];   // 行主序：列 2 = 工具轴，列 0 = 工具 x
  const az = Math.atan2(z[1], z[0]) * R2D;
  const el = Math.atan2(Math.hypot(z[0], z[1]), z[2]) * R2D;
  const x0 = [-Math.sin(az * D2R), Math.cos(az * D2R), 0];
  const y0 = cross3(z, x0);
  const d0 = x[0] * x0[0] + x[1] * x0[1] + x[2] * x0[2];
  const d1 = x[0] * y0[0] + x[1] * y0[1] + x[2] * y0[2];
  return { az: wrap180(az), el: el, roll: wrap180(Math.atan2(d1, d0) * R2D) };
}

/* ==========================================================================*/
function make(cfg) {
  /* ---------- 0. 关节表 → 运动学常量 ---------- */
  const JJ = cfg.joints, TR = cfg.travel || {};
  const P = {}, D = {};
  for (const id of ORDER) {
    if (!JJ[id]) throw new Error('缺少关节 ' + id);
    P[id] = JJ[id].rear.slice();
    D[id] = nrm(JJ[id].d);
  }
  const LIM = {};
  for (const id of ORDER) {
    const t = TR[id] || { lo: -360, hi: 360, continuous: true };
    /* 注意：LIM 是【求解器的规范代表角区间】，不是界面允许范围。
       连续轴（J1/J3/J6）物理上无机械限位——界面滑块/动态限位已放到 ±360°（见下面的
       joint sliders 与 dynLimit），但解一律取 |θ| 最小的等价角，故规范区间仍保持 ±180°。
       若此处也放到 ±360°：closest 的随机重启按 [LIM.lo, LIM.hi] 撒点，会散布到 ±360°，
       实测不可达兜底的连续轴最大 |θ| 从 176° 涨到 356°、平均 Σ|θ| +24%，动画会绕远路。
       非连续轴仍用真实行程。 */
    LIM[id] = { lo: +t.lo, hi: +t.hi, cont: !!t.continuous };
  }

  /* ---------- 0b. 动态碰撞模型（胶囊近似，cfg.collide 可选） ----------
     数据由 scratch/fit_capsules.py 从 CAD 网格拟合写 docs/collide.json：
     caps[i] = {g, cn, a, b, r}，g=0(root 固定)..6(J6)，端点为零位 CAD 世界坐标。
     组 g 的胶囊受 A1..Ag 嵌套旋转（与查看器场景图/此处 fkChain 同一约定）。
     只检 |Δg|≥2 的零件对：同组相对静止（零位单实体检查已保证不干涉），
     相邻组相对位姿只由中间关节决定、与其它关节无关（sweep_v2 单关节硬限位
     即此类，如 P04×P05/P06×P07/P07×P08），非相邻对（含撞固定立柱）才随姿态
     大变——这正是静态限位过松/过紧的来源。
     两级宽相位：零件级包围球（变换 19 个球心）先筛，球对分离即跳过该零件对
     的全部胶囊对；只有邻近零件对才变换胶囊端点做线段距离。 */
  const COL = cfg.collide || null;
  const CAPG = COL ? COL.caps : null;                 // [{g,cn,a,b,r}]
  const CAPM = COL ? +(COL.margin_mm != null ? COL.margin_mm : 1.0) : 0;
  const MOTOR = COL ? +(COL.motor_limit_deg || 0) : 0; // 电机机械限位（度）
  /* 静态限位每侧硬度（fit_capsules.py 按 travel_detail 首个卡死对判定）：
     硬边 = 相邻组零件自碰撞，与其它关节无关 → 任何姿态都取静态值，不放宽；
     可放宽 = 卡死对跨 ≥2 组（典型：腕/法兰撞固定立柱）→ 只在零位成立，
     由动态模型按当前姿态重算。 */
  const HARD = (COL && COL.hard) || {};
  const ADJ = (COL && COL.adj) || {};   // 相邻打印件对的 1D 自由角区间（度）
  const PART = [], PARTPAIR = [];                     // 零件级（宽相位）与零件对
  if (COL) {
    const pidx = {};
    for (const nm in COL.parts) {
      const caps = COL.parts[nm];
      if (!caps.length) continue;
      const g = CAPG[caps[0]].g;
      let cx = 0, cy = 0, cz = 0;
      for (const i of caps) {
        cx += (CAPG[i].a[0] + CAPG[i].b[0]) / 2; cy += (CAPG[i].a[1] + CAPG[i].b[1]) / 2;
        cz += (CAPG[i].a[2] + CAPG[i].b[2]) / 2;
      }
      cx /= caps.length; cy /= caps.length; cz /= caps.length;
      let rad = 0, rmax = 0;
      for (const i of caps) {
        const c = CAPG[i];
        rad = Math.max(rad, Math.hypot((c.a[0] + c.b[0]) / 2 - cx, (c.a[1] + c.b[1]) / 2 - cy,
                                       (c.a[2] + c.b[2]) / 2 - cz)
                       + 0.5 * Math.hypot(c.b[0] - c.a[0], c.b[1] - c.a[1], c.b[2] - c.a[2]) + c.r);
        rmax = Math.max(rmax, c.r);
      }
      pidx[nm] = PART.length;
      PART.push({ cn: CAPG[caps[0]].cn, g: g, caps: caps, c: [cx, cy, cz], rad: rad, rmax: rmax,
                  _i: PART.length });
    }
    /* 簇级宽相位：每件胶囊按主轴投影分桶（每桶 ≤CAPS_PER 枚）。桶球半径 ~15-25，
       零位臂-立柱间隙 ~44mm 也只能筛掉一部分——实测再细分（目标半径 20mm）会让
       簇球对测试量爆炸（62 万次，27ms/check），得不偿失；速度靠戳记缓存与用法层
       的边界缓存解决（见 dynLimit 与 glue 的滑块夹取）。 */
    const CAPS_PER = 4;
    for (const pt of PART) {
      const mids = pt.caps.map(i => [(CAPG[i].a[0] + CAPG[i].b[0]) / 2,
                                     (CAPG[i].a[1] + CAPG[i].b[1]) / 2,
                                     (CAPG[i].a[2] + CAPG[i].b[2]) / 2]);
      let u = [1, 0, 0];                            // 幂迭代求主轴（5 轮足够定向）
      for (let it = 0; it < 5; it++) {
        const sm = [0, 0, 0];
        for (const m of mids) {
          const d = m[0] * u[0] + m[1] * u[1] + m[2] * u[2];
          sm[0] += d * m[0]; sm[1] += d * m[1]; sm[2] += d * m[2];
        }
        const n = Math.hypot(sm[0], sm[1], sm[2]);
        if (n < 1e-9) { u = [1, 0, 0]; break; }
        u = [sm[0] / n, sm[1] / n, sm[2] / n];
      }
      const ord = pt.caps.map((c, k) => [mids[k][0] * u[0] + mids[k][1] * u[1] + mids[k][2] * u[2], k])
                        .sort((a, b) => a[0] - b[0]).map(x => x[1]);
      const G = Math.max(1, Math.ceil(pt.caps.length / CAPS_PER));
      pt.clus = [];
      for (let k = 0; k < G; k++) {
        const cs = ord.slice(Math.floor(k * pt.caps.length / G),
                             Math.floor((k + 1) * pt.caps.length / G));
        if (!cs.length) continue;
        let mx = 0, my = 0, mz = 0;
        for (const k2 of cs) { mx += mids[k2][0]; my += mids[k2][1]; mz += mids[k2][2]; }
        mx /= cs.length; my /= cs.length; mz /= cs.length;
        let rad = 0, rmax = 0;
        for (const k2 of cs) {
          const c = CAPG[pt.caps[k2]];
          rad = Math.max(rad, Math.hypot(mids[k2][0] - mx, mids[k2][1] - my, mids[k2][2] - mz)
                         + 0.5 * Math.hypot(c.b[0] - c.a[0], c.b[1] - c.a[1], c.b[2] - c.a[2]) + c.r);
          rmax = Math.max(rmax, c.r);
        }
        pt.clus.push({ c: [mx, my, mz], rad: rad, rmax: rmax, caps: cs.map(k2 => pt.caps[k2]) });
      }
    }
    const names = Object.keys(COL.parts).filter(n => COL.parts[n].length);
    for (let x = 0; x < names.length; x++) for (let y = x + 1; y < names.length; y++) {
      const A = PART[pidx[names[x]]], B = PART[pidx[names[y]]];
      const dg = Math.abs(A.g - B.g);
      if (dg < 2) continue;                            // 同组/相邻组 → 静态限位或 1D 间隔表
      /* 相邻组打印件对（P04×P05 / P06×P07 / P07×P08）**不能**塞进胶囊模型：两件
         同心跨在同一关节轴上，胶囊轴距≈0，给多少重叠容忍都会在零位误报（实测
         P04×P03 超报 15mm、P06×P07 超报 14mm）。它们是"只由中间关节决定的 1D
         约束"，由 fit_capsules 生成的 ADJ 区间表（collide.json 的 adj 字段）
         在 dynLimit 里按角度查表处理。 */
      /* 相邻组打印件对（P04×P05 / P06×P07 / P07×P08）纳入动态模型：其相对位姿只由
         中间关节决定、与姿态无关，但在另一侧可能被动态对（撞立柱）抢先卡死，静态
         单轴扫描只会记下先撞的那一对——不含它们就会在另一侧漏判（实测 J5+ 在
         J4=-100 时放到电机限 106，真值 96，就是 P07×P08 在 +96 侧的自碰撞）。 */

      const cl = [];
      for (let ia = 0; ia < A.clus.length; ia++) for (let ib = 0; ib < B.clus.length; ib++) {
        const ca = A.clus[ia], cb = B.clus[ib];
        const pr = [];
        for (const i of ca.caps) for (const j of cb.caps)
          pr.push([i, j, CAPG[i].r + CAPG[j].r + CAPM]);
        if (pr.length) cl.push([ia, ib, pr, ca.rad + cb.rad, ca.rmax + cb.rmax]);
      }
      if (cl.length) PARTPAIR.push([A, B, cl, A.rad + B.rad, A.rmax + B.rmax]);
    }
  }
  /* 组累积刚体变换 cum[k]：(R,t) 使 world = R·local + t，k=1..6（0 为恒等） */
  /* 碰撞热路径预分配（一次性）：替代每次 check 重建 clc/cache/pcc（约 2500 个小数组/次，
     GC 占单次判定 ~13%）。普通数组 + 代际戳，元素类型统一（数字 + 一个字符串 cn）——
     实测优于 Float64Array 方案（混入字符串会污染元素类型反而更慢）。 */
  const XFBUF = COL ? CAPG.map(c => { const e = new Array(8); e[6] = c.cn; e[7] = -1; return e; }) : null;
  const CLBUF = COL ? PART.map(pt => pt.clus.map(() => { const e = new Array(4); e[3] = -1; return e; })) : null;
  const PCBUF = COL ? PART.map(() => { const e = new Array(4); e[3] = -1; return e; }) : null;
  let CGEN = 0;
  function cumXf(q) {
    let R = [1, 0, 0, 0, 1, 0, 0, 0, 1], T = [0, 0, 0];
    const Rs = [R.slice()], Ts = [T.slice()];
    for (let k = 0; k < 6; k++) {
      const Pk = P[ORDER[k]], Rk = rotAxis(D[ORDER[k]], q[k] * D2R);
      const rp = mv3(Rk, Pk);                      // A_k = T(Pk)·Rk·T(−Pk)
      const tk = [Pk[0] - rp[0], Pk[1] - rp[1], Pk[2] - rp[2]];
      const nR = mul3(R, Rk);
      const nT = [R[0] * tk[0] + R[1] * tk[1] + R[2] * tk[2] + T[0],
                  R[3] * tk[0] + R[4] * tk[1] + R[5] * tk[2] + T[1],
                  R[6] * tk[0] + R[7] * tk[1] + R[8] * tk[2] + T[2]];
      R = nR; T = nT;
      Rs.push(R.slice()); Ts.push(T.slice());
    }
    return { R: Rs, T: Ts };
  }
  /* 逐对扫描（簇级宽相位 → 胶囊线段距离）。visit(cnA,cnB,d,need) 对每个距离
     < need 的胶囊对回调；无回调（collideFree）时首对即短路返回。 */
  function scanPairs(q, visit) {
    if (!COL) return false;
    const { R: Rs, T: Ts } = cumXf(q);
    /* clc/cache/pcc 全部预分配（声明见 cumXf 前）：CGEN 代际戳标记"本 check 已变换" */
    CGEN++;
    const clOf = (pi, ci) => {
      const e = CLBUF[pi][ci];
      if (e[3] !== CGEN) {
        const pt = PART[pi], R = Rs[pt.g], t = Ts[pt.g], c = pt.clus[ci].c;
        e[0] = R[0] * c[0] + R[1] * c[1] + R[2] * c[2] + t[0];
        e[1] = R[3] * c[0] + R[4] * c[1] + R[5] * c[2] + t[1];
        e[2] = R[6] * c[0] + R[7] * c[1] + R[8] * c[2] + t[2];
        e[3] = CGEN;
      }
      return e;
    };
    const pcOf = pi => {
      const e = PCBUF[pi];
      if (e[3] !== CGEN) {
        const pt = PART[pi], R = Rs[pt.g], t = Ts[pt.g], c = pt.c;
        e[0] = R[0] * c[0] + R[1] * c[1] + R[2] * c[2] + t[0];
        e[1] = R[3] * c[0] + R[4] * c[1] + R[5] * c[2] + t[1];
        e[2] = R[6] * c[0] + R[7] * c[1] + R[8] * c[2] + t[2];
        e[3] = CGEN;
      }
      return e;
    };
    const xf = i => {
      const e = XFBUF[i];
      if (e[7] === CGEN) return e;
      const c = CAPG[i], R = Rs[c.g], t = Ts[c.g];
      e[0] = R[0] * c.a[0] + R[1] * c.a[1] + R[2] * c.a[2] + t[0];
      e[1] = R[3] * c.a[0] + R[4] * c.a[1] + R[5] * c.a[2] + t[1];
      e[2] = R[6] * c.a[0] + R[7] * c.a[1] + R[8] * c.a[2] + t[2];
      e[3] = R[0] * c.b[0] + R[1] * c.b[1] + R[2] * c.b[2] + t[0];
      e[4] = R[3] * c.b[0] + R[4] * c.b[1] + R[5] * c.b[2] + t[1];
      e[5] = R[6] * c.b[0] + R[7] * c.b[1] + R[8] * c.b[2] + t[2];
      e[7] = CGEN;
      return e;
    };
    /* 宽相位阈值必须保守：球心距 > rad_A+rad_B+rmax_A+rmax_B+margin 才可筛除。
       推导：胶囊轴上任点都在球内，故轴距 ≥ 球心距−rad_A−rad_B；要保证它对所有
       胶囊对都 > r_i+r_j+margin（=need），阈值得带上双方最大胶囊半径与 margin。
       早期版本只加 rad（漏了 rmax 与 margin），实测把 P01_C×P06 在 J1=20/J2=49
       的 1618mm³ 深撞整对筛掉——球相交但轴距仍小于半径和的对被误跳。 */
    let hit = false;
    for (let k = 0; k < PARTPAIR.length; k++) {
      const [A, B, cl, rsum0, rmax0] = PARTPAIR[k];
      const pa = pcOf(A._i), pb = pcOf(B._i);
      const ddx = pa[0] - pb[0], ddy = pa[1] - pb[1], ddz = pa[2] - pb[2];
      const lim0 = rsum0 + rmax0 + CAPM;
      if (ddx * ddx + ddy * ddy + ddz * ddz > lim0 * lim0) continue;     // 零件级分离
      for (let m = 0; m < cl.length; m++) {
        const [ia, ib, pr, rsum, rmaxS] = cl[m];
        const p1 = clOf(A._i, ia), p2 = clOf(B._i, ib);
        const dx = p1[0] - p2[0], dy = p1[1] - p2[1], dz = p1[2] - p2[2];
        const lim = rsum + rmaxS + CAPM;
        if (dx * dx + dy * dy + dz * dz > lim * lim) continue;          // 簇球分离
        for (let n = 0; n < pr.length; n++) {
          const e1 = xf(pr[n][0]), e2 = xf(pr[n][1]);
          const need = pr[n][2];
          const d = segSegDistRaw(e1[0], e1[1], e1[2], e1[3], e1[4], e1[5],
                                  e2[0], e2[1], e2[2], e2[3], e2[4], e2[5]);
          if (d < need) {
            hit = true;
            if (visit) visit(e1[6], e2[6], d, need);
            else return true;
          }
        }
      }
    }
    return hit;
  }
  /* 全姿态动态碰撞判定：True = 无动态干涉（模型近似，含 margin 外扩） */
  function collideFree(q) {
    return !COL || !scanPairs(q, null);
  }
  /* 干涉明细（调参/界面提示用）：返回 [{cnA, cnB, d, need}]，按 d−need 升序
     （最贴合的排前面）。空数组 = 无动态干涉。 */
  function collideDetail(q) {
    const out = [];
    if (!COL) return out;
    scanPairs(q, (na, nb, d, need) => out.push({ cnA: na, cnB: nb, d: d, need: need }));
    out.sort((x, y) => (x.d - x.need) - (y.d - y.need));
    return out;
  }
  /* 某轴在给定姿态下的动态边界（度）：从 q[id] 向 dir 方向二分至首次干涉，
     上限为电机机械限位。返回可安全抵达的极值。 */
  function dynLimit(id, q, dir, tol) {
    tol = tol || 0.25;
    const k = ORDER.indexOf(id);
    if (!COL || k < 0 || !dir) return q[k];
    const sign = dir > 0 ? 1 : -1;
    const side = dir > 0 ? 'hi' : 'lo';
    const hard = HARD[id] && HARD[id][side];
    const hi0 = dir > 0 ? LIM[id].hi : LIM[id].lo;
    /* 连续轴的搜索端点取 ±180：转过的角度以 360 为周期，360° 与 0° 是同一个位姿，
       拿它当端点探一次必然"安全"，二分会把区间内真实存在的撞点整段跳过（实测 J3 在
       J4=-17.875 的折肘位形下首撞 148°，而旧口径直接返回 360）。 */
    let hi = sign * (LIM[id].cont ? 180 : Math.min(MOTOR || 180, 180));
    /* 相邻打印件对的 1D 自由区间（adj 表，精确 CAD 实测、与姿态无关）封顶：
       典型漏判场景——J4=-100 时 J5+ 被 P07×P08 卡在 96°，而该对落在静态扫描的
       视野外（零位时立柱先把 J5 卡在 34°），动态胶囊模型也看不见相邻对。 */
    const adj = ADJ[id];
    if (adj && dir > 0) hi = Math.min(hi, adj[1]);
    if (adj && dir < 0) hi = Math.max(hi, adj[0]);
    /* 硬边侧（静态限位由相邻打印件对决定、与姿态无关，胶囊模型看不见那一对）：
       静态值只当区间上限，区间内仍要按当前姿态二分——否则折肘位形下动态检查被整个
       跳过，滑块放到静态 -93°，而实测 J5 在 J3=-180° 时 -28° 末端法兰已撞立柱下段。 */
    if (hard && Math.abs(hi0) < Math.abs(hi)) hi = hi0;
    if (hi === q[k]) return q[k];
    const at = v => { const qq = q.slice(); qq[k] = v; return collideFree(qq); };
    if (!at(q[k])) return q[k];                   // 当前姿已干涉：不放大
    let lo = q[k];
    if (!LIM[id].cont) {                          // 限位轴：折过去就是折过去，区间内单峰，二分即可
      if (at(hi)) return hi;
      while (Math.abs(hi - lo) > tol) {
        const mid = 0.5 * (lo + hi);
        if (at(mid)) lo = mid; else hi = mid;
      }
      return lo;
    }
    /* 连续轴（J1/J3/J6）：整周内可行集不连续——末段绕过立柱后会重新变安全，
       "端点安全"根本不代表中间没撞（实测 J1=-81 位形向 + 方向 180° 端点安全，
       但 -26° 起撞立柱中段）。所以先按固定步长粗扫出首个干涉角，再在该段内二分。
       步长 4° 起（区间 360° 时约 40 次 collideFree ≈ 150 ms，只在滑块缓存未命中时发生）。 */
    const step = (hi > q[k] ? 1 : -1) * Math.max(4, Math.abs(hi - q[k]) / 40);
    let jam = null;
    for (let v = lo + step; (hi > q[k] ? v < hi : v > hi); v += step) {
      if (at(v)) lo = v; else { jam = v; break; }
    }
    if (jam === null) { if (at(hi)) return hi; jam = hi; }
    while (Math.abs(jam - lo) > tol) {
      const mid = 0.5 * (lo + jam);
      if (at(mid)) lo = mid; else jam = mid;
    }
    return lo;
  }
  /* 六轴动态限位表（UI 显示/滑块夹取用） */
  function dynLimits(q, tol) {
    const out = {};
    for (const id of ORDER) out[id] = { lo: dynLimit(id, q, -1, tol), hi: dynLimit(id, q, +1, tol) };
    return out;
  }
  const TCP0 = (cfg.tcp || [P.J6[0], P.J6[1], 93]).slice();   // 缺省 TCP 落在 J6 轴线上（随整机前移自动更新）
  const ax = D.J6;                                    // 工具沿 J6 轴向外伸
  const K = {
    X0: P.J3[0], Y0: P.J6[1],
    Z1: P.J1[2], Z2: P.J2[2], Z3: P.J3[2], Z4: P.J4[2], Z5: P.J5[2],
    TCP0: TCP0, ax: ax
  };
  K.C0 = K.Z2 - K.Z1;
  K.A43 = K.Z4 - K.Z3;
  K.A32 = K.Z3 - K.Z2;
  K.A54 = K.Z5 - K.Z4;
  K.L5 = K.Z5 - TCP0[2];                              // J5 轴 → TCP（沿 −Z）
  /* 结构前提断言：J2/J4/J5 同向平行；J1 轴为 X；J3/J6 轴为 Z；TCP 落在 J6 轴线上 */
  const AXP = v => Math.abs(Math.abs(v[2]) - 1) < 1e-9;
  const WARN = [];
  if (Math.abs(D.J1[0]) < 1e-9 || Math.abs(Math.abs(D.J1[0]) - 1) > 1e-9) WARN.push('J1 轴不是 X');
  for (const id of ['J2', 'J4', 'J5']) if (Math.abs(Math.abs(D[id][1]) - 1) > 1e-9) WARN.push(id + ' 轴不是 Y');
  if (!AXP(D.J3)) WARN.push('J3 轴不是 Z');
  if (!AXP(D.J6)) WARN.push('J6 轴不是 Z');
  if (Math.abs(P.J3[0] - P.J2[0]) > 1e-9 || Math.abs(P.J4[0] - P.J2[0]) > 1e-9 || Math.abs(P.J5[0] - P.J2[0]) > 1e-9)
    WARN.push('J2/J4/J5 不在同一 x 平面');
  if (Math.abs(TCP0[0] - P.J6[0]) > 1e-9 || Math.abs(TCP0[1] - P.J6[1]) > 1e-9)
    WARN.push('TCP 不在 J6 轴线上（θ6 将影响位置）');

  /* ---------- 1. 严格矩阵链正运动学（对照基准） ---------- */
  function mul4(A, B) {
    const C = new Array(16);
    for (let i = 0; i < 4; i++) for (let j = 0; j < 4; j++) {
      let s = 0;
      for (let k = 0; k < 4; k++) s += A[i * 4 + k] * B[k * 4 + j];
      C[i * 4 + j] = s;
    }
    return C;
  }
  function mv4(M, v) {
    return [0, 1, 2, 3].map(i => M[i * 4] * v[0] + M[i * 4 + 1] * v[1] + M[i * 4 + 2] * v[2] + M[i * 4 + 3] * v[3]);
  }

  /* 标准 4x4 同质链 World = A1·A2·…·A6·TCP，Ai = T(Pi)·R(轴,θ)·T(−Pi)
     —— 与查看器场景图（逐级左乘的 matrix 局部变换）逐位等价 */
  function fkChain(q, tool) {
    const tcp = [TCP0[0], TCP0[1], TCP0[2] + (+tool || 0) * ax[2]];
    let M = [1,0,0,0, 0,1,0,0, 0,0,1,0, 0,0,0,1];
    for (let i = 0; i < 6; i++) {
      const id = ORDER[i], p = P[id], R = rotAxis(D[id], q[i] * D2R);
      const rp = mv3(R, p);                       // Ai 的旋转=R，平移=p−R·p
      const A = [R[0], R[1], R[2], p[0] - rp[0],
                 R[3], R[4], R[5], p[1] - rp[1],
                 R[6], R[7], R[8], p[2] - rp[2], 0, 0, 0, 1];
      M = mul4(M, A);
    }
    const w = mv4(M, [tcp[0], tcp[1], tcp[2], 1]);
    return [w[0], w[1], w[2]];
  }

  /* ---------- 1b. 工具姿态链：前 thru 个关节旋转之积（CAD 系，右乘法向） ---------- */
  /* 工具坐标系姿态 = R1·R2·…·R6。因 R6 只绕工具自身 z 轴转动：
     · 工具轴（法兰面 +Z 指向）= (R1…R5)·ẑ —— 与 θ6 无关，这正是姿态任务可解的关键；
     · θ6 只提供绕工具轴的滚转。 */
  function orient(q, thru) {
    let M = [1, 0, 0, 0, 1, 0, 0, 0, 1];
    const n = thru == null ? 6 : thru;
    for (let i = 0; i < n; i++) M = mul3(M, rotAxis(D[ORDER[i]], q[i] * D2R));
    return M;
  }
  /* M5 第 3 列（工具轴）列向量传播：J4/J5 同轴 ⇒ R4·R5=R(θ4+θ5) 合并为一次旋转，
     只乘单位向量、不构造 3×3、免 T3/mul3/rotErrDeg/acos。与 orient(q,5) 列 2 一致（≤1e-15）。 */
  const SAMEAX = D.J4[0] === D.J5[0] && D.J4[1] === D.J5[1] && D.J4[2] === D.J5[2];
  function axisCol(q) {
    /* 零分配特化：J4/J5 同轴合并为 R(Y,θ4+θ5) 作用在 ẑ 上（= (sin,0,cos)），J3 为 −Z、
       J2 为 Y、J1 为 X，逐级展开向量传播（与通用矩阵链数学恒等；每级系数为 0 的项直接省去）。
       不满足结构前提时回落通用矩阵链。 */
    if (SAMEAX && D.J4[1] === 1 && D.J1[0] === 1 && D.J2[1] === 1 && D.J3[2] === -1) {
      const r45 = (q[3] + q[4]) * D2R;
      const x5 = Math.sin(r45), z5 = Math.cos(r45);
      const c3 = Math.cos(q[2] * D2R), s3 = Math.sin(q[2] * D2R);
      const x3 = c3 * x5, y3 = -s3 * x5;                          // J3：R(−Z,θ3)·(x5,0,z5)
      const c2 = Math.cos(q[1] * D2R), s2 = Math.sin(q[1] * D2R);
      const x2 = c2 * x3 + s2 * z5, z2 = -s2 * x3 + c2 * z5;      // J2：R(Y,θ2)
      const c1 = Math.cos(q[0] * D2R), s1 = Math.sin(q[0] * D2R);
      return [x2, c1 * y3 - s1 * z2, s1 * y3 + c1 * z2];          // J1：R(X,θ1)
    }
    let v;
    if (SAMEAX) v = mv3(rotAxis(D.J4, (q[3] + q[4]) * D2R), [0, 0, 1]);
    else v = mv3(mul3(rotAxis(D.J4, q[3] * D2R), rotAxis(D.J5, q[4] * D2R)), [0, 0, 1]);
    v = mv3(rotAxis(D.J3, q[2] * D2R), v);
    v = mv3(rotAxis(D.J2, q[1] * D2R), v);
    v = mv3(rotAxis(D.J1, q[0] * D2R), v);
    return v;
  }

  /* ---------- 2. 闭式正运动学（快速，用于求解内部） ---------- */
  function fk(q, tool) {
    const L = K.L5 + (+tool || 0);
    const s1 = Math.sin(q[0] * D2R), c1 = Math.cos(q[0] * D2R);
    const s2 = Math.sin(q[1] * D2R), c2 = Math.cos(q[1] * D2R);
    const s3 = Math.sin(q[2] * D2R), c3 = Math.cos(q[2] * D2R);
    const s4 = Math.sin(q[3] * D2R), c4 = Math.cos(q[3] * D2R);
    const s5 = Math.sin(q[4] * D2R), c5 = Math.cos(q[4] * D2R);
    const u = -L * s5, w = K.A54 - L * c5;
    const dx = u * c4 + w * s4;
    const dz = K.A43 - u * s4 + w * c4;
    const Vx = dx, Vz = dz + K.A32;
    const b = -Vx * s3;
    const c = K.C0 + (-Vx * c3 * s2 + Vz * c2);
    const a = Vx * c3 * c2 + Vz * s2;
    return [K.X0 + a, K.Y0 + b * c1 - c * s1, K.Z1 + b * s1 + c * c1];
  }

  /* ---------- 3. 逆解：给定 (θ4,θ5) 的闭式分支 ---------- */
  /* 单槽 sin/cos 缓存：网格扫描外层 θ4 固定，每行只算一次（浮点同值必命中，NaN 不等必重算） */
  let _bt4 = NaN, _bt5 = NaN, _bs4 = 0, _bc4 = 0, _bs5 = 0, _bc5 = 0;
  function branch(th4, th5, A, R, ph, tool) {
    const L = K.L5 + (+tool || 0);
    if (th4 !== _bt4) { _bs4 = Math.sin(th4 * D2R); _bc4 = Math.cos(th4 * D2R); _bt4 = th4; }
    if (th5 !== _bt5) { _bs5 = Math.sin(th5 * D2R); _bc5 = Math.cos(th5 * D2R); _bt5 = th5; }
    const s5 = _bs5, c5 = _bc5, s4 = _bs4, c4 = _bc4;
    const u = -L * s5, w = K.A54 - L * c5;
    const dx = u * c4 + w * s4;
    const dz = K.A43 - u * s4 + w * c4;
    const Vx = dx, Vz = dz + K.A32;
    const out = [];
    const n = (R * R + A * A - Vx * Vx - Vz * Vz - K.C0 * K.C0) / (2 * K.C0);
    const cc = K.C0 + n;
    if (Math.abs(Vx) < 1e-7) {                        // 退化：θ3 不影响 TCP
      if (Math.abs(Vz) < 1e-9) return out;
      const s2 = A / Vz, c2 = n / Vz;
      if (Math.abs(s2 * s2 + c2 * c2 - 1) > 1e-12) return out;   // 收紧：无 fk 复核后须自行保证精确（原松解本就会被 fk 拒）
      const th1 = (Math.abs(cc) > 1e-12) ? (ph - Math.atan2(cc, 0)) * R2D : 0;
      out.push([wrap180(th1), wrap180(Math.atan2(s2, c2) * R2D), 0, th4, th5, 0]);
      return out;
    }
    const m2 = (n * n + A * A - Vz * Vz) / (Vx * Vx);
    if (m2 < -1e-9 || m2 > 1 + 1e-9) return out;
    const sig = Math.acos(Math.sqrt(Math.max(0, Math.min(1, m2))));
    /* θ3 四支（±σ、±(π−σ)）的 cos/sin 只需一对三角函数值，按支下标取符号（原实现每支各算一次） */
    const c3s = Math.cos(sig), s3s = Math.sin(sig);
    for (let k = 0; k < 4; k++) {
      const c3 = (k < 2) ? c3s : -c3s, s3 = (k === 0 || k === 2) ? s3s : -s3s;
      const kap = Vx * c3, lam = Vz;
      const det = kap * kap + lam * lam;
      if (det < 1e-12) continue;
      const th2 = Math.atan2((lam * A - kap * n) / det, (kap * A + lam * n) / det) * R2D;
      const b = -Vx * s3;
      const th1 = (Math.abs(b) + Math.abs(cc) > 1e-12) ? (ph - Math.atan2(cc, b)) * R2D : 0;
      out.push([wrap180(th1), wrap180(th2), wrap180(Math.atan2(s3, c3) * R2D), th4, th5, 0]);
    }
    return out;
  }

  const LJ5 = [LIM.J1, LIM.J2, LIM.J3, LIM.J4, LIM.J5];
  const inLimit = q => {                 // 取否定式：NaN 候选必须落选（限位数组预取，免每点字符串键查表）
    for (let i = 0; i < 5; i++) {
      const t = LJ5[i];
      if (!(q[i] >= t.lo - 1e-9 && q[i] <= t.hi + 1e-9)) return false;
    }
    return true;
  };
  /* 选解代价。
     - 不给 ref：Σ|θ|（对外显示口径；离线批算也走这一支）
     - 给 ref：用户指定的判据「**所有电机需要转过的角度之和**」最小，
       即相对当前姿态的行程最小。连续轴（J1/J3/J6）无机械限位，θ 与 θ±360 是同一
       物理位形，故角差一律取 wrap180。末尾 1e-6·Σ|θ| 只作并列时的次序稳定项。
     （旧实现是 Σ|θ| + lam·Σwrap(θ−ref)² 的二次连续项，已被本判据取代；
       参数 lam 保留仅为兼容旧调用，不再参与计算。） */
  const costOf = (q, W, ref, lam) => {
    let c = 0;
    for (let i = 0; i < 6; i++) { const w = W ? W[i] : 1; c += Math.abs(q[i]) * w; }
    if (!ref) return c;
    let t = 0;
    for (let i = 0; i < 6; i++) { const w = W ? W[i] : 1; t += Math.abs(wrap180(q[i] - ref[i])) * w; }
    return t + 1e-6 * c;
  };
  const nowMs = () => (typeof performance !== 'undefined' ? performance.now() : Date.now());

  /* 姿态求解原语工厂：位置闭式支把 θ1..θ3 吸收掉之后，姿态任务只剩「工具轴指向 =
     目标轴」这 2 个方程、2 个未知数 (θ4,θ5)，解是孤立点，滚转再由 θ6 闭式补上。
     粗扫根枚举（solve 内）与实时续解（solveWarm）共用同一套。 */
  function poseTools(target, tool, Rtar) {
    const Aa = target[0] - K.X0, Bq = target[1] - K.Y0, Br = target[2] - K.Z1;
    const Rr = Math.hypot(Bq, Br), phh = Math.atan2(Br, Bq);
    const L4 = LIM.J4, L5 = LIM.J5;
    let poseQAt, poseRes, poseNewton;
    const Ad = [Rtar[2], Rtar[5], Rtar[8]];
    const pz = cross3(Ad, [0, 0, 1]);
    const bu = Math.hypot(pz[0], pz[1], pz[2]) > 0.5 ? nrm(pz) : nrm(cross3(Ad, [1, 0, 0]));
    const bv = cross3(Ad, bu);
    poseQAt = (t4, t5, qc) => {                   // 同支匹配：θ1..θ3 最近
      const ss = branch(t4, t5, Aa, Rr, phh, tool);
      let bb = null, bd = Infinity;
      for (const s0 of ss) {
        if (!inLimit(s0)) continue;
        let d = 0; for (let i = 0; i < 3; i++) d += Math.abs(wrap180(s0[i] - qc[i]));
        if (d < bd) { bd = d; bb = s0; }
      }
      return bb;
    };
    poseRes = q => {
      const w = axisCol(q);
      const dp = w[0] * Ad[0] + w[1] * Ad[1] + w[2] * Ad[2];
      const tx = w[0] - dp * Ad[0], ty = w[1] - dp * Ad[1], tz = w[2] - dp * Ad[2];
      return [tx * bu[0] + ty * bu[1] + tz * bu[2], tx * bv[0] + ty * bv[1] + tz * bv[2], dp];
    };
    poseNewton = q => {                            // 返回 {q, ne, exact}
      if (!q) return null;
      let lam = 1e-3, bq = q, e = poseRes(q), bn = Math.hypot(e[0], e[1]);
      /* 切向残差在「工具轴与目标轴平行」处同样为零，反平族也收敛到 0，
         所以同向判据只能在收口时验，不能当迭代中止条件（否则每支一进来就废）。 */
      for (let it = 0; it < 28; it++) {
        if (bn < 1e-12) break;
        const dpx = 0.02;
        const qa = poseQAt(bq[3] + dpx, bq[4], bq), qb = poseQAt(bq[3], bq[4] + dpx, bq);
        if (!qa || !qb) break;
        const ea = poseRes(qa), eb = poseRes(qb);
        const j00 = (ea[0] - e[0]) / dpx, j01 = (eb[0] - e[0]) / dpx;
        const j10 = (ea[1] - e[1]) / dpx, j11 = (eb[1] - e[1]) / dpx;
        const a00 = j00 * j00 + j10 * j10, a01 = j00 * j01 + j10 * j11, a11 = j01 * j01 + j11 * j11;
        const g0 = -(j00 * e[0] + j10 * e[1]), g1 = -(j01 * e[0] + j11 * e[1]);
        const d0 = a00 * (1 + lam), d1 = a11 * (1 + lam);
        const det = d0 * d1 - a01 * a01;
        if (!(Math.abs(det) > 1e-20)) break;
        let s0 = (d1 * g0 - a01 * g1) / det, s1 = (-a01 * g0 + d0 * g1) / det;
        const mx = Math.max(Math.abs(s0), Math.abs(s1));
        if (mx > 2) { s0 *= 2 / mx; s1 *= 2 / mx; }
        const n4 = Math.max(L4.lo, Math.min(L4.hi, bq[3] + s0));
        const n5 = Math.max(L5.lo, Math.min(L5.hi, bq[4] + s1));
        const nq = poseQAt(n4, n5, bq);
        if (!nq) { lam *= 8; if (lam > 1e8) break; continue; }
        const en = poseRes(nq);
        const nn = Math.hypot(en[0], en[1]);
        if (nn < bn) { bq = nq; bn = nn; e = en; lam = Math.max(1e-10, lam * 0.4); }
        else { lam *= 8; if (lam > 1e8) break; }
        if (Math.abs(s0) < 1e-13 && Math.abs(s1) < 1e-13) break;
      }
      return { q: bq, ne: bn, exact: bn < 1e-12 && e[2] > 0.999999 };
    };
    return { qAt: poseQAt, res: poseRes, newton: poseNewton };
  }

  /* 给定 θ1..θ5 的闭式位置解，闭式补滚转角 θ6 并给出全姿态偏差（度）。
     solve 的粗扫与 solveWarm 的续解共用同一份，避免两处判据漂移。 */
  function rollClose(q, Rtar) {
    const M5 = orient(q, 5);
    const N = mul3(T3(M5), Rtar);                       // 需满足 N = Rz(−θ6)
    const th6 = -Math.atan2(N[3], N[0]) * R2D;
    const q2 = q.slice(); q2[5] = th6;
    return { q: q2, oerr: rotErrDeg(mul3(M5, rotAxis(D.J6, th6 * D2R)), Rtar) };
  }

  /* ---------- 4. 主求解入口 ---------- */
  function solve(target, opt) {
    opt = opt || {};
    const t0 = nowMs();
    const tool = +opt.tool || 0;
    const W = opt.weights || [1, 1, 1, 1, 1, 1];
    const h = Math.max(0.02, +(opt.step || 0.25));
    const A = target[0] - K.X0;
    const BY = target[1] - K.Y0, BZ = target[2] - K.Z1;
    const R = Math.hypot(BY, BZ), ph = Math.atan2(BZ, BY);
    const tol = opt.tol || 1e-6, tol2 = tol * tol;
    const KEEP = 4000;
    /* 参考位姿（ref）：交互求解时以当前姿态为参考，选解判据取「所有电机需要转过的
   角度之和」最小 —— 即这一步行程最短的构型（取代原先的 Σ|θ| 最小）。
   动机：等价多分支（θ3 的 ±σ/±(π−σ)、肘上/肘下、以及 J1/J3 同号反号的镜像族）
   之间只按 Σ|θ| 选优会随目标微动反复横跳（实测目标挪 20 mm 就翻族、绕一大圈），
   而两族 Σ|θ| 常只差 0~20°，判据本身病态；改以行程为准后，宁可多花几度关节角，
   也不让手臂绕远路。ref 缺省时退化为 Σ|θ| 最小（离线批算口径不变）。 */
    const REF = (opt.ref && opt.ref.length === 6) ? opt.ref.slice() : null;
    const LAM = +opt.refW || 0;
    /* 姿态任务：除 TCP 位置外同时指定工具姿态（R 为行主序 9 数，CAD 系）。
       解法完全复用位置闭式支：(θ4,θ5) 扫描给出一族精确到点的解，每支的姿态
       M = R1…R5 随之确定；对每支解算滚转角 θ6 并只保留工具轴对齐（偏差≤POSETOL）者，
       代价加 POSEW·e_ori² 把精修拉向零偏差。θ6 ∈ ±360° 连续，总在限位内。 */
    const POSE = (opt.pose && opt.pose.R && opt.pose.R.length === 9) ? opt.pose.R.slice() : null;
    const PT = POSE ? poseTools(target, tool, POSE) : null;
    /* 优先级模式（双优先级范式）：
       'pos'  位置优先 —— 位置必须精确；姿态取「位置精确解集内」残差最小者。
              姿态在 POSETOL 内不可达时**不丢弃候选**，改从轴夹角最小的种子出发做
              最小残差搜索，返回位置精确、姿态最近的解并如实上报 oerr。
       'ori'  姿态优先 —— 姿态必须精确；位置取「姿态精确解集内」距目标最近者
              （由 solveTo 调 closestOri 完成，见 §5c）。
       省略（undefined）—— 与改动前逐字一致：POSETOL 硬门、超门即弃并回落 DLS。
              既有调用方（pose_ik_check / bench_ik_split / ikopt 系列）行为不变。 */
    const PRIO = (opt.priority === 'pos' || opt.priority === 'ori') ? opt.priority : null;
    const POSETOL = 2.0;          // 粗扫接受的最大姿态偏差（度）
    const POSEFINE = 0.05;        // 达标线：偏差 ≤ 此值视为姿态精确到达（机械上已无意义之差）
    let poseW = 200.0;            // 姿态偏差惩罚权重（对 deg²）：0.3° 偏差即折合 Σ|θ| 量级
                                  // （数十度）的代价； escalating 时 ×100 直至出现 POSEFINE 内解
    /* 轴夹角预筛：θ6 只提供绕工具轴的滚转、不改工具轴，故全姿态偏差 oerr ≥ 工具轴夹角
       （20 万随机样本验证零违反）。轴夹角就超 POSETOL 的候选必被 closeRoll 拒，
       先用一次点积挡掉——粗扫里这是姿态开销的大头（每候选省一次 orient 全矩阵 + acos）。 */
    const A0 = POSE ? [POSE[2], POSE[5], POSE[8]] : null;
    const AXREJ = POSE ? 2 * Math.sin(POSETOL * D2R / 2) ** 2 : 0;   // = 1 − cos(POSETOL)
    const closeRoll = q => rollClose(q, POSE);   // 给定位置解，解 θ6 并返回姿态偏差（度）
    /* 位置优先模式的「姿态最近解」种子：工具轴夹角恒 ≤ 全姿态偏差（θ6 只滚转不改轴），
       故轴夹角最小的点最有希望成为 oerr 最小解。AXREJ 那道门只放行轴夹角 ≤POSETOL 的
       候选，姿态在 POSETOL 外不可达时会把候选全挡下——这里另存一批廉价种子
       （只用已算过的 1−w·A0，不算滚转），供落选后做最小残差搜索。 */
    const SEEDN = 24;
    let axisSeeds = [];
    const keepSeed = (q, key) => {
      let wi = -1;
      if (axisSeeds.length >= SEEDN) {              // 先与最差种子比：不比它好就直接丢（避免每次全表扫）
        wi = 0;
        for (let i = 1; i < SEEDN; i++) if (axisSeeds[i].key > axisSeeds[wi].key) wi = i;
        if (key >= axisSeeds[wi].key) return;
      }
      for (const s of axisSeeds) if (Math.hypot(s.q[3] - q[3], s.q[4] - q[4]) <= 2 * h) return;
      if (wi < 0) axisSeeds.push({ q: q, key: key });
      else axisSeeds[wi] = { q: q, key: key };
    };

    const scoreBranch = (q, e) => {              // 对一条位置精确支打分（位置误差 e 已由闭式保证）
      let oerr = 0;
      if (POSE) {
        const w = axisCol(q);
        const dp = w[0] * A0[0] + w[1] * A0[1] + w[2] * A0[2];
        if (PRIO === 'pos') keepSeed(q, 1 - dp);
        if (1 - dp > AXREJ) return null;
        const cl = closeRoll(q);
        if (cl.oerr > POSETOL) return null;
        q = cl.q; oerr = cl.oerr;
      }
      const c = costOf(q, W);
      const rc0 = REF ? costOf(q, W, REF, LAM) : c;   // 含连续项、不含姿态惩罚（分层选解用）
      let rc = rc0;
      if (POSE) rc += poseW * oerr * oerr;
      return { q: q, cost: c, rc0: rc0, rcost: rc, err: e, oerr: oerr };
    };
    const evalAt = (t4, t5) => {
      let best = null;
      const sols = branch(t4, t5, A, R, ph, tool);
      for (let k = 0; k < sols.length; k++) {
        const q = sols[k];
        if (!inLimit(q)) continue;
        const c = scoreBranch(q, 0);      // 闭式支已精确命中目标；真残差在返回前对入选候选统一补算（O5）
        if (c && (!best || c.rcost < best.rcost - 1e-12)) best = c;
      }
      return best;
    };

    /* 采样轴：含上下限位端点，避免恰在限位上的最优解被漏掉 */
    const axis = (lo, hi, step) => {
      const v = [];
      const n = Math.floor((hi - lo) / step + 1e-9);
      for (let i = 0; i <= n; i++) v.push(lo + i * step);
      if (v[v.length - 1] < hi - 1e-9) v.push(hi);
      return v;
    };
    const L4 = LIM.J4, L5 = LIM.J5;
    const g4 = axis(L4.lo, L4.hi, h);

    /* 逐行裁剪可行段：位置闭式支只在平面支有解时才存在，限位矩形里只有 7~27%
       的网格点能过 branch 判据（实测 5 个目标 × 全部 θ4 行，每行可行集均为单一
       连续段、零多段）。先用探测步 hp 找每行的可行段，再向两端各放宽一个 hp 兜住
       段界的不确定性，段内仍按原步长 h 精扫 ⇒ 采样密度与全域扫一致、解的质量不动。 */
    const rowFeasible = (t4, t5) => {
      const ss = branch(t4, t5, A, R, ph, tool);
      for (let k = 0; k < ss.length; k++) if (inLimit(ss[k])) return true;
      return false;
    };
    /* 可行段探测步。原为 max(h,2)：2° 的下限与用户设置的扫描步无关，
       而实测本机最窄的可行段只有 0.98°，于是整条窄带被逐行判为「无可行段」，
       表现为 nSol==0 的误报「姿态不可达」（典型目标 6.86,-190.94,240.46）。
       降到 0.5° 后该目标由 2.55° 近似变为 0.00000° 精确，位置任务代价 +14%、
       姿态任务 +54%（与引入兜底全扫相比便宜一个数量级）。 */
    /* 探测步：姿态任务的可行 (θ4,θ5) 集是一条窄带（实测最窄 0.98°），
       2° 的探测步会整条漏掉 → 误报「姿态不可达」；位置任务的可行集宽，
       2° 探测既准又快。故只在姿态任务下降细到 0.5°，位置任务保持逐位不变。 */
    const hp = Math.max(h, POSE ? 0.5 : 2);
    /* 段边界外扩量。原实现用 hp 兼顾「探测步」与「边界外扩」两件事，
       探测步降到 0.5° 后外扩也随之收窄，实测位置任务 2/100 组的 Σ|θ| 会变大
       （最大 +0.182°，最优解正好落在段边界上）。两者职责不同，故解耦：
       探测步可以细（找全薄可行段），外扩必须足够（兜住段界的不确定性）。 */
    const PAD = 2;
    const rowSpans = t4 => {
      const spans = [];
      let lo = null;
      for (let t5 = L5.lo; t5 < L5.hi + 1e-9; t5 += hp) {
        const hi5 = Math.min(t5, L5.hi);
        if (rowFeasible(t4, hi5)) { if (lo === null) lo = hi5; }
        else if (lo !== null) { spans.push([lo - PAD, hi5 + PAD]); lo = null; }
      }
      if (lo !== null) spans.push([lo - PAD, L5.hi + PAD]);
      return spans;
    };


    /* 每行可行段探测一次，粗扫与 4.1b 的根枚举各自复用。两者的行集刻意分开：
       粗扫必须保持 h 的密度（根枚举目前还不保证完备，漏根时要靠粗扫的窄带候选兜底，
       实测把粗扫一起放松会换来上百度级的代价劣化）；根枚举只在粗行集上跑，它是
       加分项而不是替代品。 */
    const spansScan = [];
    for (let i = 0; i < g4.length; i++) spansScan.push(rowSpans(g4[i]));
    const g4r = axis(L4.lo, L4.hi, hp), spansRoot = [];
    for (let i = 0; i < g4r.length; i++) spansRoot.push(rowSpans(g4r[i]));

    /* --- 4.1 全域粗扫：覆盖整个二维冗余 (J4,J5)，按行裁剪到可行段内 --- */
    let all = [], samples = 0, nSol = 0, trimmed = false;
    /* 姿态任务另收"达标带"：oerr ≤ 1° 的候选。达标解在 (θ4,θ5) 平面上是一条窄带，
       按 rcost 取种子会整体错过它（带的 Σ|θ| 常很大）；按 oerr 排序取种子才能保证
       局部加密窗口罩住曲线，再由模式搜索在带上找 Σ|θ| 最小点。 */
    let band = [];
    for (let i = 0; i < g4.length; i++) {
      const spans = spansScan[i];
      for (let sp = 0; sp < spans.length; sp++) {
        const g5 = axis(Math.max(L5.lo, spans[sp][0]), Math.min(L5.hi, spans[sp][1]), h);
        for (let j = 0; j < g5.length; j++) {
          samples++;
          const sv = evalAt(g4[i], g5[j]);
          if (sv) { all.push(sv); nSol++; if (POSE && sv.oerr <= 1) band.push(sv); }
        }
      }
      if (!POSE && all.length > KEEP * 12) {      // 有界保留：只留代价最优者
        all.sort((x, y) => x.rcost - y.rcost);
        all.length = KEEP;
        trimmed = true;
      }
    }
    if (POSE) {                                   // 姿态模式同样需要有界保留（带优先，再补全局最优）
      band.sort((x, y) => x.oerr - y.oerr);
      const keepBand = band.slice(0, KEEP);
      const keepSet = new Set(keepBand);
      const rest = all.filter(s => !keepSet.has(s)).sort((x, y) => x.rcost - y.rcost).slice(0, KEEP);
      all = keepBand.concat(rest);
      trimmed = band.length + rest.length < nSol;
    } else all.sort((x, y) => x.rcost - y.rcost);

    /* --- 4.1b 姿态孤立根枚举 ---
       不再用细网格密度去撞那条窄带（原路径因此要背 POSETOL 硬筛、band 按 oerr
       取种子、escalating ×100 重扫一整套补偿，残差也只到 ~0.02°）：逐行对残差
       两个分量找变号括号，从括号中点起、只对发生变号的那一支跑阻尼高斯-牛顿
       （奇异时加大阻尼而非退出）。同向判据只能收口时验——工具轴反平同样令切向
       残差为 0，是这套判据最容易自证的地方。无根的刀尖构型收敛到 ‖e‖ 的真实最小。 */
    if (POSE && opt.refine !== false) {
      const roots = [], bests = [];
      const seedNewton = (t4, t5, qc) => {
        const q0 = PT.qAt(t4, t5, qc);
        if (!q0) return;
        const r = PT.newton(q0);
        if (!r) return;
        const tgtL = r.exact ? roots : bests;
        let dup = false;
        for (const x of tgtL) { let d = 0; for (let i = 0; i < 5; i++) d += Math.abs(x.q[i] - r.q[i]); if (d < 0.02) { dup = true; break; } }
        if (!dup) tgtL.push(r);
      };
      for (let ri = 0; ri < g4r.length; ri++) {
        const t4 = g4r[ri];
        const rowBest = [null, null, null, null];
        for (const sp of spansRoot[ri]) {
          const a5 = Math.max(L5.lo, sp[0]), b5 = Math.min(L5.hi, sp[1]);
          let prev = null;
          for (let t5 = a5; t5 <= b5 + 1e-9; t5 += hp) {
            const hi5 = Math.min(t5, b5);
            const ss = branch(t4, hi5, A, R, ph, tool);
            /* 按支下标对齐（不可行的支留 null 占位）：换个写法就会让 prev/cur 的
               下标错开，括号判据随之失配，实测会整支漏掉一个真根、多付上百度关节角。 */
            const cur = [];
            for (let k = 0; k < ss.length; k++) {
              if (!inLimit(ss[k])) { cur.push(null); continue; }
              const e = PT.res(ss[k]);
              cur.push({ q: ss[k], e: e });
              if (e[2] > 0 && (!rowBest[k] || Math.hypot(e[0], e[1]) < rowBest[k].n))
                rowBest[k] = { q: ss[k], t5: hi5, n: Math.hypot(e[0], e[1]) };
            }
            if (prev) {
              for (let k = 0; k < Math.min(prev.length, cur.length); k++) {
                const pv = prev[k], cu = cur[k];
                if (!pv || !cu || !pv.e[2] || !cu.e[2]) continue;
                if ((pv.e[0] < 0) !== (cu.e[0] < 0) || (pv.e[1] < 0) !== (cu.e[1] < 0))
                  seedNewton(t4, hi5 - hp / 2, pv.q);        // 变号括号：中点起、沿该支
              }
            }
            prev = cur;
          }
          /* 切向相切的根不产生变号，另按每支 ‖e‖ 最小的节点补种一次 */
          for (const rb of rowBest) if (rb) seedNewton(t4, rb.t5, rb.q);
        }
      }
      for (const r of roots) {                               // 精确根直接进候选池
        const p = fk(r.q, tool);
        const e2 = (p[0] - target[0]) ** 2 + (p[1] - target[1]) ** 2 + (p[2] - target[2]) ** 2;
        if (!(e2 <= tol * tol)) continue;
        const c = scoreBranch(r.q, Math.sqrt(e2));
        if (c) { all.push(c); nSol++; if (c.oerr <= 1) band.push(c); }
      }
      if (!roots.length) for (const r of bests) {            // 无根时的真实最小残差兜底
        const p = fk(r.q, tool);
        const e2 = (p[0] - target[0]) ** 2 + (p[1] - target[1]) ** 2 + (p[2] - target[2]) ** 2;
        const c = e2 <= tol * tol ? scoreBranch(r.q, Math.sqrt(e2)) : null;
        if (c) { all.push(c); if (c.oerr <= 1) band.push(c); }
      }
    }

    /* --- 4.2 多起点局部加密 + 模式搜索 ---
       姿态任务分层处理：先 escalating（poseW ×100 重扫）直到出现 oerr ≤ POSEFINE 的解，
       再在达标候选里按含连续项的代价取优——避免"省几度关节角、丢 0.2° 姿态"的次优交易。 */
    const refinePass = () => {
      if (!all.length) return;                     // 一个候选都没有（姿态完全不可达）：无从精修
      const seeds = [];
      if (POSE) {
        /* 姿态种子：达标带按 oerr 升序、空间去同（相距 ≤ 2h 只留最优），
           保证种子贴在窄带上，局部窗口才能罩住精确解 */
        for (const s of band) {
          if (seeds.length >= 32) break;
          let dup = false;
          for (const t of seeds) if (Math.hypot(t.q[3] - s.q[3], t.q[4] - s.q[4]) <= 2 * h) { dup = true; break; }
          if (!dup) seeds.push(s);
        }
        if (!seeds.length) seeds.push(all[0]);
      } else {        for (let i = 0; i < all.length && seeds.length < 32; i++) {
          const s = all[i];
          let dup = false;
          for (const t of seeds) if (Math.hypot(t.q[3] - s.q[3], t.q[4] - s.q[4]) <= h * 1.5) { dup = true; break; }
          if (!dup) seeds.push(s);
        }
      }
      const cl = (v, a, b) => v < a ? a : (v > b ? b : v);
      for (const s of seeds) {                      // 局部加密网格（h/8，±2h 窗口）
        const hh = POSE ? h / 8 : (h <= 0.5 ? h / 4 : h / 8);   // 精修是耗时地板：常用小步长（h≤0.5°）下加密网格降为 h/4（4× 提速，窗口保持 ±2h 不变）；实测 h>0.25° 的加密步长会跳过 <0.5° 宽的低代价窄谷（60,128,150 目标 +61°），故粗步长档完全保持原行为；姿态任务种子贴窄带、保持 h/8
        const w4 = axis(Math.max(L4.lo, s.q[3] - 2 * h), Math.min(L4.hi, s.q[3] + 2 * h), hh);
        const w5 = axis(Math.max(L5.lo, s.q[4] - 2 * h), Math.min(L5.hi, s.q[4] + 2 * h), hh);
        let lb = s;
        for (const a of w4) for (const b of w5) {
          samples++;
          const c = evalAt(a, b);
          if (c) { nSol++; if (c.rcost < lb.rcost) lb = c; }
        }
        all.push(lb);
      }
      all.sort((x, y) => x.rcost - y.rcost);
      const NB = [[1,0],[-1,0],[0,1],[0,-1],[1,1],[1,-1],[-1,1],[-1,-1]];
      for (const s of all.slice(0, 24)) {           // 模式搜索
        let c4 = s.q[3], c5 = s.q[4], lb = s, step = h / 4, guard = 0;
        while (step > 2e-4 && guard++ < 400) {
          let moved = false;
          for (const d of NB) {
            const t4 = cl(c4 + d[0] * step, L4.lo, L4.hi), t5 = cl(c5 + d[1] * step, L5.lo, L5.hi);
            if (t4 === c4 && t5 === c5) continue;
            samples++;
            const c = evalAt(t4, t5);
            if (c && c.rcost < lb.rcost - 1e-10) { lb = c; c4 = t4; c5 = t5; moved = true; }
          }
          if (!moved) step *= 0.5;
        }
        all.push(lb);
      }
    };
    if (opt.refine !== false) {
      refinePass();
      for (let k = 0; k < 3; k++) {                 // escalating：尚无 POSEFINE 内解则姿态权重 ×100 重扫
        if (!POSE || all.some(s => s.oerr != null && s.oerr <= POSEFINE)) break;
        poseW *= 100;
        refinePass();
      }
    }
    /* --- 4.3 姿态收口：(θ4,θ5) 二维牛顿迭代 ---
       达标带是 (θ4,θ5) 平面上的斜窄谷（局部灵敏度可达 ~10°/°），8 邻域模式搜索会整在
       谷壁上（实测最好只收到 0.106°）。这里直接对"工具轴在目标轴法平面上的切向偏差"
       做牛顿迭代：每步在新 (θ4,θ5) 上重解闭式位置支、按 θ1..θ3 最近匹配取同支，
       位置天然保持精确，轴偏差压到 ~1e-12 量级。 */
    if (POSE && opt.refine !== false && all.length) {
      const A0 = [POSE[2], POSE[5], POSE[8]];                       // 目标工具轴（列 2）
      const uz = cross3(A0, [0, 0, 1]);
      const u = Math.hypot(uz[0], uz[1], uz[2]) > 0.5 ? nrm(uz) : nrm(cross3(A0, [1, 0, 0]));
      const v = cross3(A0, u);
      const err2 = q => {
        const w = axisCol(q);                       // 列 2 = 工具轴（原取 [M5[6..8]] 实为第 2 行）
        const d = w[0] * A0[0] + w[1] * A0[1] + w[2] * A0[2];
        const tx = w[0] - d * A0[0], ty = w[1] - d * A0[1], tz = w[2] - d * A0[2];
        return [tx * u[0] + ty * u[1] + tz * u[2], tx * v[0] + ty * v[1] + tz * v[2]];
      };
      const match = (t4, t5, qcur) => {                             // 同支匹配：θ1..θ3 最近
        let best = null, bd = Infinity;
        for (const s of branch(t4, t5, A, R, ph, tool)) {
          if (!inLimit(s)) continue;
          let d = 0;
          for (let i = 0; i < 3; i++) d += Math.abs(wrap180(s[i] - qcur[i]));
          if (d < bd) { bd = d; best = s; }
        }
        return best;
      };
      const newton = q0 => {
        let q = q0.slice(), dp = 0.05;
        for (let it = 0; it < 24; it++) {
          const e0 = err2(q);
          if (Math.hypot(e0[0], e0[1]) < 1e-12) break;
          const c4 = q[3], c5 = q[4];
          const qa = q.slice(); qa[3] = c4 + dp;
          const qb = q.slice(); qb[4] = c5 + dp;
          const e4 = err2(qa), e5 = err2(qb);
          const j00 = (e4[0] - e0[0]) / dp, j01 = (e5[0] - e0[0]) / dp;
          const j10 = (e4[1] - e0[1]) / dp, j11 = (e5[1] - e0[1]) / dp;
          const det = j00 * j11 - j01 * j10;
          if (Math.abs(det) < 1e-12) {                 // 近奇异（刀尖构型）：缩差分步长重试
            if (dp > 0.002) { dp *= 0.25; continue; }
            break;
          }
          let d4 = -(j11 * e0[0] - j01 * e0[1]) / det;
          let d5 = -(-j10 * e0[0] + j00 * e0[1]) / det;
          const mx = Math.max(Math.abs(d4), Math.abs(d5));
          if (mx > 2) { d4 *= 2 / mx; d5 *= 2 / mx; }               // 阻尼：单步 ≤ 2°
          const t4 = Math.max(L4.lo, Math.min(L4.hi, c4 + d4));
          const t5 = Math.max(L5.lo, Math.min(L5.hi, c5 + d5));
          if (t4 === c4 && t5 === c5) break;
          const nb = match(t4, t5, q);
          if (!nb) { if (dp > 0.002) { dp *= 0.25; continue; } break; }
          q = nb;
        }
        return q;
      };
      const cands = all.slice().sort((x, y) => x.oerr - y.oerr).slice(0, 16);
      for (const s of cands) {
        if (s.oerr <= POSEFINE) continue;                           // 已达标，无需收口
        const tries = [s.q];
        for (const d of [[0.15, 0], [-0.15, 0], [0, 0.15], [0, -0.15], [0.25, 0.25], [-0.25, -0.25]]) {
          const qp = s.q.slice();
          qp[3] = Math.max(L4.lo, Math.min(L4.hi, qp[3] + d[0]));
          qp[4] = Math.max(L5.lo, Math.min(L5.hi, qp[4] + d[1]));
          const nb = match(qp[3], qp[4], s.q);
          if (nb) tries.push(nb);                                   // 微扰重启：探盆地边界（奇异刀尖构型）
        }
        for (const t of tries) {
          const q2 = newton(t);
          if (!q2) continue;
          const p2 = fk(q2, tool);
          const e2 = Math.hypot(p2[0] - target[0], p2[1] - target[1], p2[2] - target[2]);
          if (e2 > tol) continue;
          const c2 = scoreBranch(q2, e2);
          if (c2 && c2.oerr < s.oerr - 1e-9) { nSol++; all.push(c2); break; }
        }
      }
    }
    /* --- 4.4 终了精修：把代价最优的若干条入选候选都收敛成真根 ---
       根枚举的变号括号只收「残差穿过零」的根，与某个精修候选相邻的那一支可能漏检；
       漏检时若把达标门收死，就会为 0.01° 姿态多付几十度关节角。这里让入选集合自己
       走到 ‖e‖=0，精确集合与最优集合随之重合。 */
    if (POSE && opt.refine !== false && all.length) {
      const top = all.slice().sort((x, y) => x.rc0 - y.rc0).slice(0, 12);
      for (const s0 of top) {
        if (s0.oerr != null && s0.oerr < 1e-6) continue;
        const r = PT.newton(s0.q);
        if (!r || !r.exact) continue;
        const p = fk(r.q, tool);
        const e2 = (p[0] - target[0]) ** 2 + (p[1] - target[1]) ** 2 + (p[2] - target[2]) ** 2;
        if (!(e2 <= tol * tol)) continue;
        const c = scoreBranch(r.q, Math.sqrt(e2));
        if (c) { all.push(c); if (c.oerr <= 1) band.push(c); }
      }
    }
    /* --- 4.5 位置优先的「姿态最近解」搜索（仅在 POSETOL 硬门把候选全挡下时启用）---
       位置优先的语义是 min oerr s.t. 位置精确。姿态在 POSETOL 内不可达时上面全部
       阶段都会得到空池（这也是改动前回落到 DLS 的原因）。此处不丢弃已有信息：
       从轴夹角最小的若干种子出发，在 (θ4,θ5) 上做模式搜索直接最小化 oerr。
       每个搜索点仍走 branch() 闭式位置支 ⇒ 位置保持精确，残差如实上报、不虚报。
       常态（姿态精确可达）下本段完全不执行，扫描路径与耗时不变。 */
    /* 原条件 !all.length 只在「一个候选都没有」时启用，实测姿态精确解常在
       粗扫与根枚举之后仍未被生成（目标 17.9,194.8,436.5 与 383.9,241.8,824.2），
       此时 all 非空但其中最好的也差 0.07°~1.33°，于是被误标为"精确到达"。
       放宽为「没有任何 oerr ≤ POSEFINE 的候选」，让本段从轴夹角最小的种子
       出发把残差收下去；姿态精确可达时本段仍不执行，扫描路径不变。 */
    if (POSE && PRIO === 'pos' && !all.some(sv => sv.oerr != null && sv.oerr <= POSEFINE) && axisSeeds.length) {
      const pack = (r) => {
        const c = costOf(r.q, W);
        const rc0 = REF ? costOf(r.q, W, REF, LAM) : c;
        return { q: r.q, cost: c, rc0: rc0, rcost: rc0, err: r.err, oerr: r.oerr };
      };
      /* 在 (θ4,θ5) 上取最优 oerr（同一点的多条位置支都试） */
      const oerrAt = (t4, t5) => {
        let best = null;
        const ss = branch(t4, t5, A, R, ph, tool);
        for (let k = 0; k < ss.length; k++) {
          const s0 = ss[k];
          if (!inLimit(s0)) continue;
          const p = fk(s0, tool);
          const dx = p[0] - target[0], dy = p[1] - target[1], dz = p[2] - target[2];
          const e2 = dx * dx + dy * dy + dz * dz;
          if (!(e2 <= tol2)) continue;
          const cl = closeRoll(s0);
          if (!best || cl.oerr < best.oerr) best = { q: cl.q, oerr: cl.oerr, err: Math.sqrt(e2) };
        }
        return best;
      };
      const NB = [[1,0],[-1,0],[0,1],[0,-1],[1,1],[1,-1],[-1,1],[-1,-1]];
      const cl4 = (v, a, b) => v < a ? a : (v > b ? b : v);
      let bs = null;
      for (const sd of axisSeeds) {
        let lb = oerrAt(sd.q[3], sd.q[4]);
        if (!lb) continue;
        samples++;
        let c4 = sd.q[3], c5 = sd.q[4], step = Math.max(h, 0.5), guard = 0;
        while (step > 2e-4 && guard++ < 400) {          // 模式搜索：步长减半直到收敛
          let moved = false;
          for (const d of NB) {
            const t4 = cl4(c4 + d[0] * step, L4.lo, L4.hi), t5 = cl4(c5 + d[1] * step, L5.lo, L5.hi);
            if (t4 === c4 && t5 === c5) continue;
            samples++;
            const c = oerrAt(t4, t5);
            if (c && c.oerr < lb.oerr - 1e-12) { lb = c; c4 = t4; c5 = t5; moved = true; }
          }
          if (!moved) step *= 0.5;
        }
        if (lb && (!bs || lb.oerr < bs.oerr - 1e-12)) bs = lb;
      }
      if (bs) { nSol++; all.push(pack(bs)); }
    }
    all.sort((x, y) => x.rcost - y.rcost);
    /* 采用解：姿态任务优先从 oerr ≤ POSEFINE 的达标候选里按含连续项代价选；无达标解才尽力而为 */
    let pool = all, poolByOerr = false;
    if (POSE) {
      /* 有真根时优先取真根，但不为姿态精度付大额关节角：精确根只有在代价不高于
         达标最优 +2° 时才接管（实测两者通常差 <0.05°，个别漏检情形能差到 40°）。 */
      const acc = all.filter(s => s.oerr != null && s.oerr <= POSEFINE);
      const ex = all.filter(s => s.oerr != null && s.oerr <= 1e-4);
      if (acc.length) {
        const ca = Math.min(...acc.map(s => s.rc0));
        pool = (ex.length && Math.min(...ex.map(s => s.rc0)) <= ca + 2) ? ex : acc;
      } else if (PRIO === 'pos') {
        /* 位置优先：无姿态达标解 → 取姿态残差最小者（同量级时取 Σ|θ| 更小者） */
        poolByOerr = true;
        pool = all.slice();
      }
    }
    pool.sort((x, y) => poolByOerr ? ((x.oerr - y.oerr) || (x.rc0 - y.rc0))
                                   : (POSE ? x.rc0 - y.rc0 : x.rcost - y.rcost));
    const best = pool.length ? pool[0] : null;
    const list = pool.slice().sort((x, y) => POSE ? x.rc0 - y.rc0 : x.rcost - y.rcost).slice(0, KEEP);
    if (best && list.indexOf(best) < 0) list.unshift(best);
    /* 真残差补算：热循环里延后的 fk 校验在这里对入选候选统一补一次（err 为真值） */
    if (best) {
      const errOf = q => { const p = fk(q, tool); const dx = p[0] - target[0], dy = p[1] - target[1], dz = p[2] - target[2]; return Math.hypot(dx, dy, dz); };
      best.err = errOf(best.q);
      const nErr = Math.min(list.length, 64);
      for (let i = 0; i < nErr; i++) if (list[i].err === 0) list[i].err = errOf(list[i].q);
    }
    return {
      ok: !!best, reachable: !!best, target: target.slice(), tool: tool,
      pose: !!POSE, priority: PRIO,
      /* 姿态分级：reachable 只表示"有解"，不足以区分"精确"与"近似"（缺陷 3）。
         poseFine=姿态偏差 ≤POSEFINE(0.05°) 才算精确到达；poseTier 供上层显式区分：
           fine  = 精确   approx = 落在 POSETOL(2°) 内的近似   poor = 连 POSETOL 都没进
           none  = 没有任何候选
         reachablePose 是严格版（位置与姿态同时达标），需要严格语义时用它。 */
      poseFine: POSE ? !!(best && best.oerr != null && best.oerr <= POSEFINE) : null,
      poseTier: !POSE ? null : (!best ? 'none'
        : (best.oerr != null && best.oerr <= POSEFINE ? 'fine'
        : (best.oerr != null && best.oerr <= POSETOL ? 'approx' : 'poor'))),
      reachablePose: POSE
        ? !!(best && best.oerr != null && best.oerr <= POSEFINE
             && best.err != null && best.err <= Math.sqrt(tol2)) : null,
      q: best ? best.q.slice() : null, cost: best ? best.cost : null,
      /* 本步行程 = 判据值：Σ|wrap(θ−ref)|（连续轴按等价角计），便于界面核对 */
      moveSum: (REF && best) ? (() => {
        let t = 0; for (let i = 0; i < 6; i++) t += Math.abs(wrap180(best.q[i] - REF[i])); return t;
      })() : null,
      err: best ? best.err : null, oerr: best ? best.oerr : null,
      nSol: nSol, truncated: trimmed,
      samples: samples, ms: nowMs() - t0, list: list
    };
  }

  /* ---------- 5. 最近可达位姿（阻尼最小二乘，兜底用） ---------- */
  function closest(target, tool, tries, ref) {
    const dp = 0.05, lam = 4.0;
    let bestQ = null, bestD = Infinity;
    /* anchor：参考位姿锚定。准退化构型（手臂近似伸直）下 J3 等轴的雅可比列梯度≈0，
       DLS 伪逆仍会给它分配微小步长，160 次迭代累积成大幅无用漂移（实测 82°）。
       每次迭代施加 0.03·(q−anchor) 的回拉，把退化方向的漂移压掉，任务真正需要的
       方向（远大于此回拉）不受影响。 */
    const run = (q, anchor) => {
      const J = new Float64Array(18);
      for (let it = 0; it < 160; it++) {
        const p = fk(q, tool);
        const d = Math.hypot(p[0] - target[0], p[1] - target[1], p[2] - target[2]);
        if (d < bestD) { bestD = d; bestQ = q.slice(); }
        if (d < 1e-9) break;
        for (let c = 0; c < 6; c++) {
          const s = q.slice(); s[c] += dp;
          const pp = fk(s, tool);
          J[c * 3] = (pp[0] - p[0]) / dp; J[c * 3 + 1] = (pp[1] - p[1]) / dp; J[c * 3 + 2] = (pp[2] - p[2]) / dp;
        }
        let m00 = lam, m01 = 0, m02 = 0, m11 = lam, m12 = 0, m22 = lam;
        for (let c = 0; c < 6; c++) {
          const ax2 = J[c * 3], ay = J[c * 3 + 1], az2 = J[c * 3 + 2];
          m00 += ax2 * ax2; m01 += ax2 * ay; m02 += ax2 * az2;
          m11 += ay * ay; m12 += ay * az2; m22 += az2 * az2;
        }
        const det = m00 * (m11 * m22 - m12 * m12) - m01 * (m01 * m22 - m12 * m02) + m02 * (m01 * m12 - m11 * m02);
        if (Math.abs(det) < 1e-12) break;
        const i00 = (m11 * m22 - m12 * m12) / det, i01 = -(m01 * m22 - m12 * m02) / det,
              i02 = (m01 * m12 - m11 * m02) / det, i11 = (m00 * m22 - m02 * m02) / det,
              i12 = -(m00 * m12 - m01 * m02) / det, i22 = (m00 * m11 - m01 * m01) / det;
        const ex = p[0] - target[0], ey = p[1] - target[1], ez = p[2] - target[2];
        const y0 = -(i00 * ex + i01 * ey + i02 * ez);
        const y1 = -(i01 * ex + i11 * ey + i12 * ez);
        const y2 = -(i02 * ex + i12 * ey + i22 * ez);
        const dq = [0, 0, 0, 0, 0, 0]; let mx = 0;
        for (let c = 0; c < 6; c++) {
          dq[c] = J[c * 3] * y0 + J[c * 3 + 1] * y1 + J[c * 3 + 2] * y2;
          mx = Math.max(mx, Math.abs(dq[c]));
        }
        const gain = mx > 12 ? 12 / mx : 1;
        for (let c = 0; c < 6; c++) {
          const t = LIM[ORDER[c]];
          let stp = dq[c] * gain;
          if (anchor) stp -= 0.03 * (q[c] - anchor[c]);
          q[c] = Math.max(t.lo, Math.min(t.hi, q[c] + stp));
        }
      }
    };
    /* 零空间抛光：手臂伸直（目标在可达边界外）时 J3 轴与臂线共线，转 J3 不改变
       TCP，DLS 对该方向梯度为零、停在哪里全看随机初始角。这里在"到目标距离几乎
       不变"（≤slack）的前提下令姿态滑向最小总关节角，把随机角收敛回确定代表值。
       同时兼顾连续性：ref 给出时从 ref 起步，邻近目标 successive 解算平滑跟进。 */
    const distQ = q => Math.hypot.apply(null, fk(q, tool).map((v, i) => v - target[i]));
    const polish = (q, slack) => {
      if (!q) return;
      const d0 = distQ(q);
      let step = 8;
      for (let pass = 0; pass < 4; pass++) {
        let moved = true, guard = 0;
        while (moved && guard++ < 40) {
          moved = false;
          for (let i = 0; i < 5; i++) {                  // J6 不改 TCP 位置，抛光无意义
            const t = LIM[ORDER[i]];
            for (const s of [1, -1]) {
              const v = q[i] + s * step;
              if (v < t.lo || v > t.hi) continue;
              const old = q[i], c0 = costOf(q, null);
              q[i] = v;
              const d = distQ(q);
              if (d <= d0 + slack && costOf(q, null) < c0 - 1e-9) moved = true;
              else q[i] = old;
            }
          }
        }
        step *= 0.25;
      }
    };
    let anchorQ = null, anchorD = Infinity;
    if (ref && ref.length === 6) {
      anchorQ = ref.slice();
      run(anchorQ, ref);                               // 锚定轮：从当前姿态出发保连续
      anchorD = distQ(anchorQ);
    }
    run([0, 0, 0, 0, 0, 0]);
    /* 确定性拟随机重启：固定种子的 LCG，取代 Math.random —— 同一目标重复解算
       结果必须逐位一致，否则边界目标每次刷新姿态都不同，就是"随机倒摆"的根源 */
    let seed = 0x5bd1e995;
    const rnd = () => { seed = (seed * 1103515245 + 12345) & 0x7fffffff; return seed / 0x7fffffff; };
    const N = tries == null ? 160 : tries;
    for (let k = 0; k < N; k++) {
      const q = [0, 0, 0, 0, 0, 0];
      for (let c = 0; c < 5; c++) { const t = LIM[ORDER[c]]; q[c] = t.lo + rnd() * (t.hi - t.lo); }
      run(q);
    }
    /* 锚定解与全局最优距离差 ≤0.5mm 时优先锚定解：刚过边界的目标（差 mm 以内）
       保住原构型自然伸出，不为零点几毫米换个 Fold 姿态；差得远时仍取全局最近 */
    if (anchorQ && anchorD <= bestD + 0.5) { bestQ = anchorQ; bestD = anchorD; }
    if (bestQ) {                                        // 报告抛光后真实距离，不虚报
      polish(bestQ, 0.05);                              // 距离容忍 0.05mm（远小于边界误差量级）
      bestD = distQ(bestQ);
    }
    return { q: bestQ, dist: bestD };
  }

  /* ---------- 5b. 最近可达位姿（位置 + 姿态联合，阻尼最小二乘） ---------- */
  /* 误差 6 维：位置 3（mm）+ 姿态 3（R_curᵀ·R_des 的旋转矢量，rad）。
     姿态项按 20 mm/rad 折算进误差向量（≈0.35mm/度），阻尼项吸收量纲差。 */
  function closest6(target, Rdes, tool, tries, ref) {
    const OW = 20.0;                                  // mm per rad
    const dp = 0.05, lam = 6.0;
    let bestQ = null, bestE = Infinity, bestOri = 0;
    const errOf = q => {
      const p = fk(q, tool), N = mul3(T3(orient(q)), Rdes);
      const tr = N[0] + N[4] + N[8], ang = Math.acos(clamp1((tr - 1) / 2));
      const s = Math.sin(ang);
      const k = s > 1e-6 ? 1 / (2 * s) : 0.5;         // 小角时直接用斜对称向量/2，免奇异
      const rv = [(N[7] - N[5]) * k, (N[2] - N[6]) * k, (N[3] - N[1]) * k];
      return [p[0] - target[0], p[1] - target[1], p[2] - target[2],
              rv[0] * OW, rv[1] * OW, rv[2] * OW];
    };
    const norm6 = e => Math.hypot(e[0], e[1], e[2], e[3], e[4], e[5]);
    const oriOf = q => {                              // 纯姿态偏差（度）
      const N = mul3(T3(orient(q)), Rdes);
      return Math.acos(clamp1((N[0] + N[4] + N[8] - 1) / 2)) * R2D;
    };
    /* 6×6 线性方程组：高斯消去 + 列主元（A = J·Jᵀ + λ²I，y = A⁻¹e，dq = Jᵀy） */
    const solveLin = (A, b, n) => {
      const M = A.slice(), x = b.slice();
      for (let c = 0; c < n; c++) {
        let p = c;
        for (let r = c + 1; r < n; r++) if (Math.abs(M[r * n + c]) > Math.abs(M[p * n + c])) p = r;
        if (Math.abs(M[p * n + c]) < 1e-14) return null;
        if (p !== c) { for (let j = 0; j < n; j++) { const t = M[c * n + j]; M[c * n + j] = M[p * n + j]; M[p * n + j] = t; } const t = x[c]; x[c] = x[p]; x[p] = t; }
        for (let r = 0; r < n; r++) {
          if (r === c) continue;
          const f = M[r * n + c] / M[c * n + c];
          if (!f) continue;
          for (let j = c; j < n; j++) M[r * n + j] -= f * M[c * n + j];
          x[r] -= f * x[c];
        }
      }
      for (let c = 0; c < n; c++) x[c] /= M[c * n + c];
      return x;
    };
    const run = (q, anchor) => {
      const J = new Float64Array(36);                 // 6×6（误差 × 关节）
      for (let it = 0; it < 160; it++) {
        const e = errOf(q);
        const ne = norm6(e);
        if (ne < bestE) { bestE = ne; bestQ = q.slice(); bestOri = oriOf(q); }
        if (ne < 1e-9) break;
        for (let c = 0; c < 6; c++) {
          const s = q.slice(); s[c] += dp;
          const ep = errOf(s);
          for (let r = 0; r < 6; r++) J[r * 6 + c] = (ep[r] - e[r]) / dp;
        }
        const A = new Array(36).fill(0);
        for (let i = 0; i < 6; i++) {
          for (let j = 0; j < 6; j++) { let s = 0; for (let k = 0; k < 6; k++) s += J[i * 6 + k] * J[j * 6 + k]; A[i * 6 + j] = s + (i === j ? lam * lam : 0); }
        }
        const y = solveLin(A, e.map(v => -v), 6);
        if (!y) break;
        const dq = new Array(6);
        let mx = 0;
        for (let c = 0; c < 6; c++) { let s = 0; for (let r = 0; r < 6; r++) s += J[r * 6 + c] * y[r]; dq[c] = s; mx = Math.max(mx, Math.abs(s)); }
        const gain = mx > 12 ? 12 / mx : 1;
        for (let c = 0; c < 6; c++) {
          const t = LIM[ORDER[c]];
          let stp = dq[c] * gain;
          if (anchor) stp -= 0.03 * (q[c] - anchor[c]);
          q[c] = Math.max(t.lo, Math.min(t.hi, q[c] + stp));
        }
      }
    };
    /* 零空间抛光：在位置与姿态偏差都不增的前提下令 Σ|θ| 最小（步进、步长逐轮衰减） */
    const slackOf = q => { const e = errOf(q); return [Math.hypot(e[0], e[1], e[2]), Math.hypot(e[3], e[4], e[5]) / OW]; };
    const polish = (q, slack) => {
      if (!q) return;
      const [d0, o0] = slackOf(q);
      let step = 8;
      for (let pass = 0; pass < 4; pass++) {
        let moved = true, guard = 0;
        while (moved && guard++ < 40) {
          moved = false;
          for (let i = 0; i < 5; i++) {                  // J6 是纯滚转，抛它只会劣化姿态偏差
            const t = LIM[ORDER[i]];
            for (const s of [1, -1]) {
              const v = q[i] + s * step;
              if (v < t.lo || v > t.hi) continue;
              const old = q[i], c0 = costOf(q, null);
              q[i] = v;
              const [d, o] = slackOf(q);
              if (d <= d0 + slack && o <= o0 + 0.05 && costOf(q, null) < c0 - 1e-9) moved = true;
              else q[i] = old;
            }
          }
        }
        step *= 0.25;
      }
    };
    let anchorQ = null, anchorE = Infinity;
    if (ref && ref.length === 6) {
      anchorQ = ref.slice();
      run(anchorQ, ref);
      anchorE = norm6(errOf(anchorQ));
    }
    run([0, 0, 0, 0, 0, 0]);
    let seed = 0x5bd1e995;
    const rnd = () => { seed = (seed * 1103515245 + 12345) & 0x7fffffff; return seed / 0x7fffffff; };
    const N = tries == null ? 160 : tries;
    for (let k = 0; k < N; k++) {
      const q = [0, 0, 0, 0, 0, 0];
      for (let c = 0; c < 5; c++) { const t = LIM[ORDER[c]]; q[c] = t.lo + rnd() * (t.hi - t.lo); }
      run(q);
    }
    if (anchorQ && anchorE <= bestE + 0.5) { bestQ = anchorQ; bestE = anchorE; }
    if (bestQ) {
      polish(bestQ, 0.05);                                // 位置 0.05mm / 姿态 0.05° 内不劣化
      bestE = norm6(errOf(bestQ));
      bestOri = oriOf(bestQ);
    }
    return { q: bestQ, dist: Math.hypot.apply(null, bestQ ? fk(bestQ, tool).map((v, i) => v - target[i]) : [1e9, 1e9, 1e9]), ori: bestOri };
  }

  /* ---------- 5c. 姿态优先的最近解：姿态必须精确，位置取最近 ----------
     位置精确解集是 (θ4,θ5) 平面上的 2 维族；姿态精确（工具轴对齐 + θ6 闭式补滚转）解集
     是 3 维族——两者不是同一集合，姿态优先必须离开位置精确族去搜索。本机构有降阶结构
     （J4∥J5 ⇒ R4·R5 = R_y(θ4+θ5)，与 axisCol 同一约定）：

       工具轴  w = Rx(θ1)·Ry(θ2)·Rz(−θ3)·Ry(s)·ẑ ,   s = θ4 + θ5

     给定 (θ3, s) 后 (θ1,θ2) 可闭式解出（至多两支，见 axisSolve），剩余自由参数只有 θ4
     （θ5 = s − θ4）⇒ 在 (θ3, s, θ4) 三维上粗扫取种子、再坐标下降精修即可。轴一旦精确对齐，
     由 rollClose 定出的 θ6 令 oerr 恒为 0，不再有姿态残差；位置误差由 fk 如实给出。 */
  function closestOri(target, Rdes, tool, ref) {
    if (!SAMEAX) return null;                     // J4 与 J5 不同轴：降阶结构不成立，交回 DLS
    const t = [Rdes[2], Rdes[5], Rdes[8]];
    const L4 = LIM.J4, L5 = LIM.J5, L2 = LIM.J2;
    const sLo = L4.lo + L5.lo, sHi = L4.hi + L5.hi;
    /* 解 Rx(θ1)·Ry(θ2)·v = t（v = Rz(−θ3)·Ry(s)·ẑ），返回至多两支 [θ1,θ2]（度） */
    const axisSolve = (th3, s) => {
      const v = mv3(rotAxis(D.J3, th3 * D2R), mv3(rotAxis(D.J4, s * D2R), [0, 0, 1]));
      const Rv = Math.hypot(v[0], v[2]);
      if (Rv < 1e-12) return [];
      const cs = t[0] / Rv;
      if (cs < -1 || cs > 1) return [];
      const psi = Math.atan2(v[2], v[0]), d2 = Math.acos(cs), out = [];
      for (let k = 0; k < 2; k++) {
        const th2 = psi + (k ? -d2 : d2);
        const c2 = Math.cos(th2), s2 = Math.sin(th2);
        const P = s2 * v[0] - c2 * v[2], Q = v[1], det = P * P + Q * Q;
        if (det < 1e-14) continue;                // 工具轴落在 ±X：θ1 退化，交给别的种子
        if (th2 < L2.lo - 1e-9 || th2 > L2.hi + 1e-9) continue;
        const th1 = Math.atan2((P * t[1] + Q * t[2]) / det, (Q * t[1] - P * t[2]) / det);
        out.push([wrap180(th1 * R2D), wrap180(th2 * R2D)]);
      }
      return out;
    };
    const e2Of = q => {
      const p = fk(q, tool);
      const dx = p[0] - target[0], dy = p[1] - target[1], dz = p[2] - target[2];
      return dx * dx + dy * dy + dz * dz;
    };
    /* 给定 (θ3, s, θ4) 组装关节角：θ1..θ3 闭式、θ5 = s − θ4、θ4 夹进可行区间 */
    const qOf = (th3, s, th4, prev) => {
      const br = axisSolve(th3, s);
      if (!br.length) return null;
      let b = br[0], bd = Infinity;
      for (const x of br) {                       // 同支匹配：与上一步的 θ1/θ2 最近
        const d = Math.abs(wrap180(x[0] - prev[0])) + Math.abs(wrap180(x[1] - prev[1]));
        if (d < bd) { bd = d; b = x; }
      }
      const a4 = Math.max(L4.lo, s - L5.hi), b4 = Math.min(L4.hi, s - L5.lo);
      if (a4 > b4 + 1e-9) return null;
      const th4c = Math.max(a4, Math.min(b4, th4));
      return [b[0], b[1], wrap180(th3), th4c, s - th4c, 0];
    };
    /* 粗扫 (θ3, s, θ4)：只保留每格最优，再按位置误差取空间去同的若干种子 */
    const cand = [];
    for (let th3 = -180; th3 <= 180 + 1e-9; th3 += 5) {
      for (let s = sLo; s <= sHi + 1e-9; s += 5) {
        const sf = Math.min(s, sHi);
        const br = axisSolve(th3, sf);
        if (!br.length) continue;
        const a4 = Math.max(L4.lo, sf - L5.hi), b4 = Math.min(L4.hi, sf - L5.lo);
        if (a4 > b4 + 1e-9) continue;
        for (const b of br) {
          let bc = null;
          for (let th4 = a4; th4 <= b4 + 1e-9; th4 += 5) {
            const th4c = Math.min(th4, b4);
            const q = [b[0], b[1], wrap180(th3), th4c, sf - th4c, 0];
            const e2 = e2Of(q);
            if (!bc || e2 < bc.d2) bc = { q: q, d2: e2 };
          }
          if (bc) cand.push(bc);
        }
      }
    }
    if (!cand.length) return null;
    cand.sort((x, y) => x.d2 - y.d2);
    const seeds = [];
    for (const c of cand) {
      if (seeds.length >= 32) break;
      let dup = false;
      for (const sd of seeds)
        if (Math.hypot(wrap180(sd.q[2] - c.q[2]), sd.q[3] - c.q[3], sd.q[4] - c.q[4]) < 15) { dup = true; break; }
      if (!dup) seeds.push(c);
    }
    /* 坐标下降精修（θ3 / s / θ4 三方向，步长减半直到收敛） */
    const refine = q0 => {
      let q = q0.slice(), step = 5;
      for (let pass = 0; pass < 60; pass++) {
        let moved = false;
        for (let i = 0; i < 3; i++) {
          for (const sg of [1, -1]) {
            const th3 = i === 0 ? q[2] + sg * step : q[2];
            const s = (i === 1 ? q[3] + q[4] + sg * step : q[3] + q[4]);
            if (s < sLo - 1e-9 || s > sHi + 1e-9) continue;
            const th4 = i === 2 ? q[3] + sg * step : q[3];
            const nq = qOf(th3, s, th4, q);
            if (nq && e2Of(nq) < e2Of(q) - 1e-12) { q = nq; moved = true; }
          }
        }
        if (!moved) { step *= 0.5; if (step < 1e-4) break; }
      }
      return q;
    };
    let bestQ = null, bestE = Infinity;
    const pool = [];                              // 精修结果集：用于带 ref 的连续性仲裁
    for (const sd of seeds) {
      const q = refine(sd.q), e2 = e2Of(q);
      pool.push({ q: q, d2: e2 });
      if (e2 < bestE) { bestE = e2; bestQ = q; }
    }
    if (!bestQ) return null;
    if (ref && ref.length === 6) {
      /* 与「最近可达位姿」同一约定：位置差在 0.5 mm 内时优先保连续性，避免等价支间来回跳 */
      let bd = Infinity, bq = null;
      for (const p of pool) {
        if (p.d2 > bestE + 1.0) continue;         // d2 差 1.0 ≈ 距离差 0.5mm 量级
        let d = 0;
        for (let i = 0; i < 6; i++) d += Math.abs(wrap180(p.q[i] - ref[i]));
        if (d < bd) { bd = d; bq = p.q; }
      }
      if (bq) bestQ = bq;
    }
    const rc = rollClose(bestQ, Rdes);
    return { q: rc.q, dist: Math.sqrt(e2Of(bestQ)), ori: rc.oerr };
  }

/* ---------- 5d. 混合优先：位置与姿态按可调权重折中 ----------
     滑杆 w ∈ [0,1] 自由分配两端的比重：
       w = 1 与 w = 0 由 solveTo 直接交给两条既有专用算法（位置优先 / 姿态优先），
       结果逐位一致；本段只负责 0 < w < 1 的加权折中。

     折中语义 =「沿折中历程的进度」，代价与归一化尺度都取两端锚点自身：
         J(q) = w·(e/e_ref)² + (1−w)·(o/o_ref)²
         e_ref = 姿态端锚点的位置差（姿态优先解的 TCP 位置差），
         o_ref = 位置端锚点的姿态残差（位置优先解的姿态残差）。
     两端锚点各把「本端最差方向」定标，于是滑杆读数与目标的绝对误差量级无关
     （50% 即两端误差各自归一后对半）。两端锚点复用两条既有算法：
     位置端 = solve(priority:'pos')（位置精确族 + 闭式滚转 + 孤立根收口；位置不可达
     时退化为 closest() 的最近可达位置），姿态端 = closestOri()（工具轴闭式对齐 +
     θ6 补滚转 ⇒ 姿态精确、位置最近）。

     中间值以两锚点及关节空间插值为种子，对 6 维残差
     [√w·(p−t)/e_ref , √(1−w)·RV°/o_ref] 做阻尼高斯-牛顿 + 单轴抛光，取 J 最小者
     （J 同量级带内取相对当前姿态的行程 Σ|wrap(θ−ref)| 更小者）。两端任一维已精确（≤0.05）时
     折中无意义，直接返回位置端解。全程无随机数：同一输入逐位可复现。 */
  let mixScale = null;                   // 最近一次全量折中的归一化尺度（拖动快路沿用）
  const mixClamp = v => Math.max(0, Math.min(1, v == null || !isFinite(+v) ? 0.5 : +v));

  /* 折中共用内核：归一化代价 + 加权阻尼高斯-牛顿（§5d 全量与快路共用） */
  function mixKernel(target, tool, Rtar, w, eRef, oRef) {
    const kp = Math.sqrt(w) / eRef, ko = Math.sqrt(1 - w) / oRef;
    const RS = 100;                      // 残差整体放大：量级与 §5b 的 mm 制对齐，阻尼比例可沿用
    const lam = 6.0, dp = 0.05;
    let samples = 0;
    /* 闭式滚转（θ6）后的真实 (e mm, o 度) 与归一化代价 J */
    const canon = q => {
      const rc = rollClose(q, Rtar), p = fk(rc.q, tool);
      const e = Math.hypot(p[0] - target[0], p[1] - target[1], p[2] - target[2]);
      samples++;
      return { q: rc.q, e: e, o: rc.oerr, J: w * (e / eRef) ** 2 + (1 - w) * (rc.oerr / oRef) ** 2 };
    };
    /* 6 维残差（RS 倍放大）：位置 3 + 姿态旋转矢量 3（度制，范数 = 姿态夹角） */
    const resid6 = q => {
      const p = fk(q, tool), N = mul3(T3(orient(q)), Rtar);
      const ang = Math.acos(clamp1((N[0] + N[4] + N[8] - 1) / 2));
      const s = Math.sin(ang), k = s > 1e-8 ? ang / (2 * s) : 0.5;    // 旋转矢量 = vee·k（弧度）
      samples++;
      return [(p[0] - target[0]) * kp * RS, (p[1] - target[1]) * kp * RS, (p[2] - target[2]) * kp * RS,
              (N[7] - N[5]) * k * R2D * ko * RS, (N[2] - N[6]) * k * R2D * ko * RS, (N[3] - N[1]) * k * R2D * ko * RS];
    };
    /* 阻尼高斯-牛顿（与 §5b 同构：A = J·Jᵀ + λ²I，y = A⁻¹(−e)，dq = Jᵀy，步长 ≤12°） */
    /* 6×6 线性方程组：高斯消去 + 列主元（A = J·Jᵀ + λ²I，y = A⁻¹e，dq = Jᵀy） */
    const solveLin = (A, b, n) => {
      const M = A.slice(), x = b.slice();
      for (let c = 0; c < n; c++) {
        let p = c;
        for (let r = c + 1; r < n; r++) if (Math.abs(M[r * n + c]) > Math.abs(M[p * n + c])) p = r;
        if (Math.abs(M[p * n + c]) < 1e-14) return null;
        if (p !== c) { for (let j = 0; j < n; j++) { const t = M[c * n + j]; M[c * n + j] = M[p * n + j]; M[p * n + j] = t; } const t = x[c]; x[c] = x[p]; x[p] = t; }
        for (let r = 0; r < n; r++) {
          if (r === c) continue;
          const f = M[r * n + c] / M[c * n + c];
          if (!f) continue;
          for (let j = c; j < n; j++) M[r * n + j] -= f * M[c * n + j];
          x[r] -= f * x[c];
        }
      }
      for (let c = 0; c < n; c++) x[c] /= M[c * n + c];
      return x;
    };
    const dls = (seed, anchor) => {
      const q = seed.slice(), Jm = new Float64Array(36);
      let best = null;
      for (let it = 0; it < 160; it++) {
        const e = resid6(q);
        let n2 = 0; for (let i = 0; i < 6; i++) n2 += e[i] * e[i];
        if (!best || n2 < best.n2) best = { q: q.slice(), n2: n2 };
        if (n2 < 1e-10) break;
        for (let c = 0; c < 6; c++) {
          const s = q.slice(); s[c] += dp;
          const ep = resid6(s);
          for (let r = 0; r < 6; r++) Jm[r * 6 + c] = (ep[r] - e[r]) / dp;
        }
        const A = new Array(36).fill(0);
        for (let i = 0; i < 6; i++) {
          for (let j = 0; j < 6; j++) { let s = 0; for (let k = 0; k < 6; k++) s += Jm[i * 6 + k] * Jm[j * 6 + k]; A[i * 6 + j] = s + (i === j ? lam * lam : 0); }
        }
        const y = solveLin(A, e.map(v => -v), 6);
        if (!y) break;
        const dq = new Array(6); let mx = 0, moved = 0;
        for (let c = 0; c < 6; c++) { let s = 0; for (let r = 0; r < 6; r++) s += Jm[r * 6 + c] * y[r]; dq[c] = s; mx = Math.max(mx, Math.abs(s)); }
        const gain = mx > 12 ? 12 / mx : 1;
        for (let c = 0; c < 6; c++) {
          const t = LIM[ORDER[c]];
          let stp = dq[c] * gain;
          if (anchor) stp -= 0.03 * (q[c] - anchor[c]);       // 参考位姿锚定：退化方向不漂移
          const v = Math.max(t.lo, Math.min(t.hi, q[c] + stp));
          moved += Math.abs(v - q[c]);
          q[c] = v;
        }
        if (moved < 1e-9) break;
      }
      return best ? best.q : null;
    };
    /* 单轴抛光：J 不再下降为止，步长逐轮减半（θ6 由 rollClose 闭式定，不参与抛光） */
    const polish = q0 => {
      let cur = canon(q0), q = cur.q, step = 1.5;
      for (let pass = 0; pass < 24 && step > 1e-3; pass++) {
        let moved = false;
        for (let c = 0; c < 5; c++) {
          const t = LIM[ORDER[c]];
          for (const sg of [1, -1]) {
            const v = q[c] + sg * step;
            if (v < t.lo || v > t.hi) continue;
            const qq = q.slice(); qq[c] = v;
            const cc = canon(qq);
            if (cc.J < cur.J - 1e-12) { cur = cc; q = cc.q; moved = true; }
          }
        }
        if (!moved) step *= 0.5;
      }
      return cur;
    };
    return { canon: canon, dls: dls, polish: polish, nSamples: () => samples };
  }

  function mixSolve(target, opt) {
    opt = opt || {};
    if (!opt.pose || !opt.pose.R || opt.pose.R.length !== 9) return solve(target, opt);   // 无姿态任务：退回纯位置求解
    const t0 = nowMs();
    const tool = +opt.tool || 0;
    const W = opt.weights || [1, 1, 1, 1, 1, 1];
    const REF = (opt.ref && opt.ref.length === 6) ? opt.ref.slice() : null;
    const LAM = +opt.refW || 0;
    const Rtar = opt.pose.R;
    const w = mixClamp(opt.mixW);
    const EXACT = 0.05;                  // 两端任一维已 ≤0.05 ⇒ 视为已精确到达（同 POSEFINE）
    const scaleOf = (e, o) => { mixScale = { e: Math.max(e, 0.05), o: Math.max(o, 0.05) }; };
    /* 位置端锚点：既有位置优先解（位置精确族内姿态残差最小者，含 §4.5 最近姿态搜索） */
    const pa = solve(target, Object.assign({}, opt, { priority: 'pos' }));
    let pEnd = null;
    if (pa.ok && pa.q) pEnd = { q: pa.q.slice(), e: pa.err || 0, o: pa.oerr || 0 };
    /* 姿态端锚点：姿态精确族的最小位置差（工具轴闭式对齐 + θ6 补滚转） */
    const oa = closestOri(target, Rtar, tool, REF);
    let oEnd = null;
    if (oa && oa.q) {
      const rc = rollClose(oa.q, Rtar), p = fk(rc.q, tool);
      oEnd = { q: rc.q, e: Math.hypot(p[0] - target[0], p[1] - target[1], p[2] - target[2]), o: rc.oerr };
    }
    const stamp = (res, pn, on) => Object.assign({}, res, {
      priority: 'mix', mixW: w,
      mixP: pn ? { e: pn.e, o: pn.o } : null, mixO: on ? { e: on.e, o: on.o } : null });
    /* 两端中任一端在另一维也达标 ⇒ 已同时精确，折中无意义：返回该端（标准解）。
       先判位置端：位置优先解本身就最优；位置端不达标而姿态端在位置上也达标时，
       姿态端在「位置 + 姿态」两维同时占优（位置优先族搜不到的到达解，姿态端找到了），
       此时必须返回姿态端，不能返回被占优的位置端解。 */
    if (pEnd && pEnd.o <= EXACT) { scaleOf(oEnd ? oEnd.e : 1, pEnd.o); return stamp(pa, pEnd, oEnd); }
    if (oEnd && oEnd.e <= EXACT) {
      scaleOf(oEnd.e, pEnd ? pEnd.o : 1);
      const cost = costOf(oEnd.q, W);
      return stamp({ ok: true, reachable: true, target: target.slice(), tool: tool, pose: true,
                     q: oEnd.q.slice(), cost: cost, err: oEnd.e, oerr: oEnd.o,
                     nSol: 1, samples: 0, ms: nowMs() - t0,
                     list: [{ q: oEnd.q.slice(), cost: cost, rc0: cost, rcost: cost, err: oEnd.e, oerr: oEnd.o }] },
                   pEnd, oEnd);
    }
    /* 位置端：位置不可达时退化为最近可达位置（closest），姿态残差如实取 */
    if (!pEnd) {
      const cp = closest(target, tool, opt.tries, REF);
      if (!cp.q) return pa;              // 连最近可达位置都求不出：交回原结果（异常路径）
      const rc = rollClose(cp.q, Rtar), p = fk(rc.q, tool);
      pEnd = { q: rc.q, e: Math.hypot(p[0] - target[0], p[1] - target[1], p[2] - target[2]), o: rc.oerr };
    }
    if (!oEnd) {                         // 姿态端锚点缺失（结构前提不满足，或限位内无姿态精确解）：
      /* 按纯姿态优先的同一兜底取 6 维联合 DLS 最近位姿当姿态端，保证兜底语义一致 */
      const c6 = closest6(target, Rtar, tool, opt.tries, REF);
      if (c6 && c6.q) {
        const rc = rollClose(c6.q, Rtar), p = fk(rc.q, tool);
        oEnd = { q: rc.q, e: Math.hypot(p[0] - target[0], p[1] - target[1], p[2] - target[2]), o: rc.oerr };
      } else {
        scaleOf(pEnd.e, pEnd.o);
        return stamp(pa, pEnd, null);
      }
    }
    const eRef = Math.max(oEnd.e, 0.01), oRef = Math.max(pEnd.o, 0.01);
    scaleOf(eRef, oRef);
    const K = mixKernel(target, tool, Rtar, w, eRef, oRef);
    /* 种子：两端锚点 + 关节空间插值（落在两端之间的盆地里）+ 参考位姿 */
    const seeds = [pEnd.q, oEnd.q];
    for (const t of [0.25, 0.5, 0.75]) {
      const q = pEnd.q.slice();
      for (let i = 0; i < 6; i++) {
        const tl = LIM[ORDER[i]];
        q[i] = Math.max(tl.lo, Math.min(tl.hi, q[i] + wrap180(oEnd.q[i] - q[i]) * t));
      }
      seeds.push(q);
    }
    if (REF) seeds.push(REF);
    const cands = [K.canon(pEnd.q), K.canon(oEnd.q)];
    for (const sd of seeds) {
      const r = K.dls(sd, REF);
      if (r) cands.push(K.polish(r));
    }
    /* 选优：J 最小；J 同量级（2% 带内）取行程（相对当前姿态的 Σ|wrap(θ−ref)|）更小者，防等价支横跳 */
    cands.sort((x, y) => x.J - y.J);
    const j0 = cands[0].J, jBand = j0 + 0.02 * j0 + 1e-6;
    let best = cands[0], bc = Infinity;
    for (const c of cands) {
      if (c.J > jBand) break;
      const cst = REF ? costOf(c.q, W, REF, LAM) : costOf(c.q, W);
      if (cst < bc - 1e-12) { bc = cst; best = c; }
    }
    const cost = costOf(best.q, W);
    /* 候选表（供动态限位/碰撞改选）：与最优 J 同量级者，按行程（有 ref）升序 */
    const list = [];
    for (const c of cands) {
      if (c !== best && c.J > j0 * 1.5 + 0.05) continue;
      const cst = costOf(c.q, W);
      const rc = REF ? costOf(c.q, W, REF, LAM) : cst;
      list.push({ q: c.q.slice(), cost: cst, rc0: rc, rcost: rc, err: c.e, oerr: c.o });
    }
    list.sort((x, y) => x.rcost - y.rcost);
    return {
      ok: true, reachable: !!pa.ok, target: target.slice(), tool: tool, pose: true,
      priority: 'mix', mixW: w,
      mixP: { e: pEnd.e, o: pEnd.o }, mixO: { e: oEnd.e, o: oEnd.o },
      q: best.q.slice(), cost: cost, err: best.e, oerr: best.o,
      resid: best.e > EXACT ? best.e : null,
      nSol: list.length, truncated: false, samples: K.nSamples(), ms: nowMs() - t0, list: list,
      note: pa.ok ? null
        : '混合优先（位置 ' + Math.round(w * 100) + '%）：目标位姿超可达空间 → 两端锚点折中最优（位置差 ' +
          best.e.toFixed(2) + ' mm · 姿态差 ' + best.o.toFixed(2) + '°）'
    };
  }

  /* 混合优先的拖动快路：沿上一帧折中解做一次加权 DLS + 抛光，归一化尺度沿用最近一次
     全量折中的缓存（目标微动下两端误差量级不变）；位移超限（换盆地）返回 null 交回全量。
     与 §6a 同一哲学：快路只负责「能快的时候快」，松手后的全量求解才是最终结果。 */
  function mixWarm(target, opt) {
    const q0 = opt.warm;
    if (!q0 || q0.length !== 6 || !mixScale) return null;
    const t0 = nowMs();
    const tool = +opt.tool || 0;
    const W = opt.weights || [1, 1, 1, 1, 1, 1];
    const REF = (opt.ref && opt.ref.length === 6) ? opt.ref : null;
    const w = mixClamp(opt.mixW);
    const K = mixKernel(target, tool, opt.pose.R, w, mixScale.e, mixScale.o);
    const q1 = K.polish(K.dls(q0, REF) || q0);
    let dsh = 0;
    for (let i = 0; i < 5; i++) dsh += Math.abs(wrap180(q1.q[i] - q0[i]));
    if (dsh > 25) return null;           // 同支位移过大：可能已换盆地，交回全量重算
    const cost = costOf(q1.q, W);
    return {
      ok: true, reachable: true, warm: true, target: target.slice(), tool: tool, pose: true,
      priority: 'mix', mixW: w, q: q1.q.slice(), cost: cost, err: q1.e, oerr: q1.o,
      nSol: 1, truncated: false, samples: K.nSamples(), ms: nowMs() - t0,
      list: [{ q: q1.q.slice(), cost: cost, rc0: cost, rcost: cost, err: q1.e, oerr: q1.o }]
    };
  }

    /* ---------- 6. 一键求解（不可达时返回最近可达位姿/位姿） ---------- */
  function solveTo(target, opt) {
    opt = opt || {};
    /* 混合优先（§5d）：两端（0% / 100%）直接交给两条既有专用算法——结果与「姿态优先 /
       位置优先」逐位一致；中间值走加权折中搜索（拖动优先快路，校验不过回落全量）。 */
    if (opt.pose && opt.pose.R && opt.pose.R.length === 9 && opt.priority === 'mix') {
      const w = mixClamp(opt.mixW);
      if (w >= 1 - 1e-9 || w <= 1e-9)
        return solveTo(target, Object.assign({}, opt, { priority: w >= 0.5 ? 'pos' : 'ori' }));
      const m = (opt.warm ? mixWarm(target, opt) : null) || mixSolve(target, opt);
      if (m) return m;
    }
    if (opt.warm) {                                  // 快路优先，校验不过自动回落全量
      const w = solveWarm(target, opt);
      if (w) return w;
    }
    const res = solve(target, opt);
    if (res.ok) return res;
    const pose = opt.pose && opt.pose.R && opt.pose.R.length === 9;
    /* 姿态优先：目标点够不到指定姿态时，不用 6 维 DLS 折中位置与姿态，而是在
       「姿态精确解集」上最小化位置误差——姿态保持精确、位置差如实上报。 */
    if (pose && opt.priority === 'ori') {
      const ro = closestOri(target, opt.pose.R, +opt.tool || 0, opt.ref);
      if (ro && ro.q) return {
        ok: false, reachable: false, target: target.slice(), tool: +opt.tool || 0,
        pose: true, priority: 'ori',
        q: ro.q.slice(), cost: costOf(ro.q, opt.weights), err: ro.dist, resid: ro.dist,
        oerr: ro.ori, nSol: res.nSol, samples: res.samples, ms: res.ms, list: [],
        note: '姿态优先：姿态精确到达，TCP 取最近可达点'
      };
    }
    const cp = pose ? closest6(target, opt.pose.R, +opt.tool || 0, opt.tries, opt.ref)
                    : closest(target, +opt.tool || 0, opt.tries, opt.ref);
    if (!cp.q) return res;
    return {
      ok: false, reachable: false, target: target.slice(), tool: +opt.tool || 0,
      pose: pose, priority: res.priority,
      q: cp.q.slice(), cost: costOf(cp.q, opt.weights), err: cp.dist, resid: cp.dist,
      oerr: pose ? cp.ori : null,
      nSol: res.nSol, samples: res.samples, ms: res.ms, list: [],
      note: pose ? '目标位姿超出可达空间：返回最近位姿（阻尼最小二乘，位置 mm + 姿态联合）'
                 : '目标超出可达工作空间：返回最近可达位姿（阻尼最小二乘）'
    };
  }

  /* ---------- 6a. 实时续解：跟随上一帧的解走，而不是每次重扫全冗余维 ----------
     拖动时目标每帧只挪几毫米、姿态只转一两度，真解在 (θ4,θ5) 平面上连续移动；
     全域粗扫（0.25° 下 6~30 万点）为这点位移付全价，是 100 ms 级延迟的唯一来源。
     这里只在上一帧解附近取同一闭式支、走一小步同支搜索，姿态任务再用 poseTools
     的牛顿把工具轴收口到精确。任一校验不过（换支、越限、位置残差超差、姿态没收口、
     关节跳太大）一律返回 null 交回全量求解——快路只负责"能快的时候快"。 */
  function solveWarm(target, opt) {
    opt = opt || {};
    const q0 = opt.warm;
    if (!q0 || q0.length !== 6) return null;
    const tool = +opt.tool || 0, tol = opt.tol || 1e-6, t0 = nowMs();
    const W = opt.weights || [1, 1, 1, 1, 1, 1];
    const REF = (opt.ref && opt.ref.length === 6) ? opt.ref : null;
    const LAM = +opt.refW || 0;
    const POSE = (opt.pose && opt.pose.R && opt.pose.R.length === 9) ? opt.pose.R.slice() : null;
    const PRIO = (opt.priority === 'pos' || opt.priority === 'ori') ? opt.priority : null;
    const A = target[0] - K.X0, BY = target[1] - K.Y0, BZ = target[2] - K.Z1;
    const R = Math.hypot(BY, BZ), ph = Math.atan2(BZ, BY);
    const PT = POSE ? poseTools(target, tool, POSE) : null;
    const L4 = LIM.J4, L5 = LIM.J5;
    const cl = (v, a, b) => v < a ? a : (v > b ? b : v);
    let best = null, samples = 0;
    /* 递进窗口：目标挪得远时上一帧附近可能已经没有同支解，逐档放宽搜索半径，
       仍找不到才交回全量粗扫（姿态任务的命中率先受这一档影响最大）。 */
    const radii = [Math.max(+(opt.warmRad || 4), 0.5), 14, 40];
    for (const rad of radii) {
      if (best) break;
      for (const d4 of [0, 1, -1, 2, -2, 3, -3, 4, -4].map(v => v * rad / 4)) {
      for (const d5 of [0, 1, -1, 2, -2, 3, -3, 4, -4].map(v => v * rad / 4)) {
        const t4 = cl(q0[3] + d4, L4.lo, L4.hi), t5 = cl(q0[4] + d5, L5.lo, L5.hi);
        const dshCap = rad > 20 ? 60 : 25;              // 放宽窗口时允许同支稍大的移动
        const ss = branch(t4, t5, A, R, ph, tool); samples++;
        for (const s0 of ss) {
          let dsh = 0; for (let i = 0; i < 5; i++) dsh += Math.abs(wrap180(s0[i] - q0[i]));
          if (dsh > dshCap) continue;                    // 换了支/大幅倒摆：不是续解
          let p = fk(s0, tool);
          let e = Math.hypot(p[0] - target[0], p[1] - target[1], p[2] - target[2]);
          if (!(e <= tol)) continue;                     // 否定式：NaN 目标在此落选
          let q = s0, oerr = 0, exact = true;
          if (POSE) {
            /* 精确优先但不强制：拖动中目标姿态每帧只转一两度，同支跟随的姿态残差
               通常在 0.05° 内（松手的全量求解会纠偏），要求必须收到根会让快路频繁
               失效、反而退回 100 ms 级全扫。 */
            const r = PT.newton(s0);
            let qq = null;
            if (r && r.exact) qq = r.q;
            else {
              const e0 = PT.res(s0);
              if (e0 && e0[2] > 0.99999 && Math.hypot(e0[0], e0[1]) < 1e-3) qq = s0;
              /* 位置优先：姿态收不到根也照跟同支位置解（残差如实上报），
                 否则拖进「姿态不可达区」后每帧都要回落全量扫描。 */
              else if (PRIO === 'pos') qq = s0;
            }
            if (!qq) continue;
            p = fk(qq, tool);
            e = Math.hypot(p[0] - target[0], p[1] - target[1], p[2] - target[2]);
            if (!(e <= tol)) continue;
            const c2 = rollClose(qq, POSE);
            q = c2.q; oerr = c2.oerr;
            exact = !!(r && r.exact && qq === r.q);
            if (PRIO !== 'pos' && !(oerr <= 0.05)) continue;
          }
          const cost = costOf(q, W);
          const rc = REF ? costOf(q, W, REF, LAM) : cost;
          /* 位置优先：主键是姿态残差（轴精确的根 oerr≈0 自然胜出），次键才是 Σ|θ| */
          const better = PRIO === 'pos'
            ? (!best || oerr < best.oerr - 1e-9 ||
               (Math.abs(oerr - best.oerr) <= 1e-9 && rc < best.rcost - 1e-12))
            : (!best || (exact && !best.exact) || (exact === best.exact && rc < best.rcost - 1e-12));
          if (better)
            best = { q: q, cost: cost, rc0: REF ? costOf(q, W, REF, LAM) : cost, rcost: rc, err: e, oerr: oerr, exact: exact };
        }
      }
      }
    }
    if (!best) return null;
    return {
      ok: true, reachable: true, warm: true, target: target.slice(), tool: tool, pose: !!POSE,
      priority: PRIO,
      q: best.q.slice(), cost: best.cost, err: best.err, oerr: POSE ? best.oerr : null,
      nSol: samples, truncated: false, samples: samples, ms: nowMs() - t0, list: [best]
    };
  }

  /* ---------- 6a5. 路径安全路由：端点安全 ≠ 过渡安全 ----------
     查看器的动画是逐轴线性插值（ik-glue.js animTo，连续轴走短弧），所以"移动路径"
     就是关节空间里 from→to 的一条直线段。实测把目标球从臂的一侧横拖到另一侧：
     1300 帧里 870 帧（66.9%）处于穿模状态，主要是 立柱上段×J5 电机模组(189 帧)、
     立柱中段×J6 电机模组(186 帧)——端点解本身也撞，而 live 拖拽走的 solveTo
     一个碰撞判定都没做。这里把"这条直线能不能走"当一等判据：
       1) 候选表按代价升序，取第一个"端点安全 + 直线过渡安全"的解；
       2) 一个都没有 → 沿最优候选方向二分退回最近的可安全抵达位形（跟手但停在障碍前），标 blocked；
       3) 当前位形本身已干涉（滑块被拖越界）→ 退回旧的"只看端点"口径，至少把臂解出来。
     开销：首个候选通常一次就过（1 次端点 + PATH_N-1 次过渡 ≈ 8 次 collideFree）；
     最坏 CAND_MAX 个候选全查 ≈ 60 次，实测在 Worker 里跑，不冻结主线程。 */
  const PATH_N = 9, CAND_MAX = 24, SHORT_DEG = 6, FINE_DEG = 3;
  /* 关节空间步长（连续轴按短弧折算后的最大单轴变化量） */
  function stepDeg(from, to) {
    let m = 0;
    for (let i = 0; i < 6; i++) {
      const d = LIM[ORDER[i]] && LIM[ORDER[i]].cont ? wrap180(to[i] - from[i]) : to[i] - from[i];
      if (Math.abs(d) > m) m = Math.abs(d);
    }
    return m;
  }
  function transitPt(from, to, k) {
    return from.map((v, i) => {
      const d = LIM[ORDER[i]] && LIM[ORDER[i]].cont ? wrap180(to[i] - v) : to[i] - v;
      return v + d * k;
    });
  }
  /* 直线段过渡是否全程无干涉（不含端点：起点由调用侧保证，终点已单独查过）。
     步长 ≤SHORT_DEG° 的跟随帧只查中点：实测拖拽中相邻解的步长多在几度以内，
     9 点全扫会把 live 从 1~44ms 放大到 441ms/步（≈10×），而这么短的线段
     两端安全 + 中点安全已经覆盖了实际风险（穿模体是厘米级，不是度级）。 */
  function pathFree(from, to, n) {
    n = n || PATH_N;
    for (let i = 1; i < n; i++) if (!collideFree(transitPt(from, to, i / n))) return false;
    return true;
  }
  function findSafeTarget(cur, from) {
    if (cur.q && collideFree(cur.q)) return { q: cur.q, res: cur };
    let best = null, bestCost = Infinity;
    const n = Math.min((cur.list || []).length, 120);
    for (let i = 0; i < n; i++) {
      const s = cur.list[i];
      if (s && s.q && collideFree(s.q)) {
        const cst = costOf(s.q, null, from);
        if (cst < bestCost) {
          bestCost = cst;
          best = { q: s.q, res: Object.assign({}, cur, { q: s.q.slice(), cost: s.cost, err: s.err, oerr: s.oerr }) };
        }
      }
    }
    if (best) return best;
    const len = (cur.list || []).length;
    if (len > 120) {
      const stride = Math.max(1, Math.floor((len - 120) / 40));
      for (let i = 120; i < len; i += stride) {
        const s = cur.list[i];
        if (s && s.q && collideFree(s.q)) {
          return { q: s.q, res: Object.assign({}, cur, { q: s.q.slice(), cost: s.cost, err: s.err, oerr: s.oerr }) };
        }
      }
    }
    return null;
  }
  function routeFrom(from, cur) {
    if (!COL || !cur || !cur.q || !from) return cur;
    if (!collideFree(from)) return cur;                 // 起点已在干涉里：先把自己拔出来再说
    const seen = new Set(), cand = [];
    const push = q => {
      if (!q || q.length !== 6) return;
      const key = q.map(v => v.toFixed(2)).join(',');
      if (seen.has(key)) return;
      seen.add(key); cand.push(q.slice());
    };
    push(cur.q);
    for (const s of (cur.list || [])) push(s.q);
    // 优先验证与起点 from 关节距离最近的候选解
    cand.sort((a, b) => costOf(a, null, from) - costOf(b, null, from));
    const cands = cand.slice(0, CAND_MAX);
    let fineLeft = 4;      // 细验预算：全通过的候选才会走满 n 次探测
    for (const q of cands) {
      if (!collideFree(q)) continue;
      // 快速二分初筛：若中点干涉直接短路（1 次判定否决碰撞射线），节省大半无谓细验开销
      if (!collideFree(transitPt(from, q, 0.5))) continue;
      /* 粗筛（≤13 点）先廉价否掉明显穿模的候选，再用与审计同分辨率的 3° 细验定案 */
      if (!pathFree(from, q, Math.min(13, pathN(from, q, SHORT_DEG)))) continue;
      if (fineLeft <= 0) break;      // 预算用尽就不接受没细验过的候选（宁可贴边停，不赌）
      fineLeft--;
      if (pathFree(from, q, pathN(from, q, FINE_DEG))) {
        const same = q.every((v, i) => Math.abs(v - cur.q[i]) < 1e-9);
        return Object.assign({}, cur, { q: q, routed: true, blocked: false, kept: same });
      }
    }
    /* 没有一条直线能干净抵达：沿最优候选（优先端点安全的那个）方向二分退回安全边界，
       让臂贴着障碍停住，而不是穿过去。 */
    const safeT = findSafeTarget(cur, from);
    let bestQ = safeT ? safeT.q : null;
    if (!bestQ) {
      for (const q of cands) if (collideFree(q)) { bestQ = q; break; }
    }
    if (!bestQ) bestQ = cands[0] || cur.q;
    let lo = 0, hi = 1;          // lo=起点（已确认安全），hi=整跳（上面已证明不干净）
    /* 先按端点二分找到"射线上的第一个撞点"（便宜：每次 1 个探测），
       再对 from→q 整段做 3° 细验；细验不过就按 0.6 回缩重验。
       早先直接用 9 点粗扫当谓词，等于把 19° 的采样间隔当成安全边界，实测漏过一帧。 */
    while (hi - lo > 0.02) {
      const mid = 0.5 * (lo + hi);
      if (collideFree(transitPt(from, bestQ, mid))) lo = mid; else hi = mid;
    }
    let q = transitPt(from, bestQ, lo);
    for (let t = 0; t < 5 && lo > 0.02 && !pathFree(from, q, pathN(from, q, FINE_DEG)); t++) {
      lo *= 0.6; q = transitPt(from, bestQ, lo);
    }
    return Object.assign({}, cur, { q: q, safeT: safeT, routed: true, blocked: lo < 0.98, kept: false });
  }
  /* 拖拽/松手统一入口：solveTo + 路径路由（opt.from = 当前位形） */
  function solveRoute(target, opt) {
    opt = opt || {};
    if (opt.from && !opt.ref) opt.ref = opt.from;
    return routeOrVia(opt.from, target, solveTo(target, opt), opt);
  }
  /* 两段式绕行：直线被立柱挡住、但目标本身有安全解时，先解一个"抬高版"的中间位形
     （目标正上方 VIA_DZ），走直线过去，再从那里落回目标。实测 100 个拖拽步里
     14 步属于这一类（有安全端点、直线到不了）。只在松手全量求解时用：
     每多一个候选要多解一次 IK + 两段直线检查，实时拖拽扛不住。 */
  const VIA_DZ = [260, 520];
  function routeVia(from, target, opt) {
    for (const dz of VIA_DZ) {
      const Tw = [target[0], target[1], target[2] + dz];
      const rw = solveRoute(Tw, Object.assign({}, opt, { noVia: true, noAround: true }));
      if (!rw || !rw.q || rw.blocked || rw.unsafe || !collideFree(rw.q)) continue;
      const r2 = solveRoute(target, Object.assign({}, opt, { from: rw.q.slice(), noVia: true, noAround: true, ref: rw.q }));
      if (!r2 || !r2.q || r2.blocked || r2.unsafe || !collideFree(r2.q)) continue;
      /* 中间位形表必须把两段各自的子绕行都接上：from→(rw.via)→rw.q→(r2.via)→q。
         早先只放 [rw.q] 会把 rw/r2 自己的 via 覆盖掉，于是动画走的那条线并不是校验过的那条，
         实测松手口径因此在离起点 3° 处穿模 6 帧（立柱中段×末端工具法兰）。 */
      return Object.assign({}, r2, {
        via: (rw.via || []).concat([rw.q.slice()], r2.via || []), detourDz: dz, detour: 'via'
      });
    }
    return null;
  }
  /* 绕行使者：收臂 → 转 J1 → 再伸展。
     CRUISE 表由 scratch/find_cruise_poses.js 搜出并校验：那组 J2/J4/J5 组合在
     J1 整周每 10° 采样下 collideFree 全通过（280 组枚举里 243 组满足，取半径最贴身
     且收法不同的 4 组）。几何一改必须重跑该脚本，否则巡航位形可能已经会撞。
     三段分别是：当前位形→原地收臂（J1 不变）、收臂态把 J1 转到目标解的肩角、
     再伸展到目标解。每段都过 pathFree，段内 J1 按 ≤10° 采样（与搜索口径一致）。 */
  /* 巡航参考位形：按收缩幅度从浅到深排序。
     [0, -15, ...] 为轻微避让，[0, -75, ...] 为深折叠。优先选用微调姿态，避免激进蜷缩。 */
  const CRUISE = [
    [0, -15, 0, -95, -60, 0], [0, -45, 0, -95, -60, 0],
    [0, -75, 0, -95, -60, 0], [0, -90, 0, -65, -60, 0]
  ];
  /* 转 J1 那一段按 5° 网格查表（懒算 + 记忆化） */
  const CRUISE_OK = CRUISE.map(() => new Map());
  function cruiseFree(ci, j1) {
    const key = Math.round(j1 / 5) * 5;
    const m = CRUISE_OK[ci];
    let v = m.get(key);
    if (v === undefined) { const q = CRUISE[ci].slice(); q[0] = key; v = collideFree(q); m.set(key, v); }
    return v;
  }
  function turnFree(ci, j1a, j1b) {
    const n = Math.max(1, Math.ceil(Math.abs(wrap180(j1b - j1a)) / 5));
    for (let i = 0; i <= n; i++) if (!cruiseFree(ci, j1a + (wrap180(j1b - j1a)) * i / n)) return false;
    return true;
  }
  /* 采样点数按线段在关节空间的步长自适应：每 SHORT_DEG(6°) 一个点，最少 3 点、最多 40 点 */
  function pathN(from, to, deg) {
    return Math.max(3, Math.min(40, Math.round(stepDeg(from, to) / (deg || SHORT_DEG)) + 1));
  }
  /* 轻量偏移量集合：用于避开障碍物边缘（抬肩/微收肘），避免整臂蜷缩 */
  const GENTLE_OFFSETS = [
    [-10, 0], [-20, 0], [-15, -10], [0, -15], [-20, -15],
    [10, 0], [20, 0], [-30, 0], [-10, 10]
  ];
  /* 智能平滑协同避障规划（模拟人类避障直觉）：
     1) 微调避让：针对贴边擦碰，在直线弦中点微调 J2/J4，各轴保持平滑协同，绝不缩臂；
     2) 协同单中间点 + 二分松弛：J1 全程平滑旋转，中间位形只做必要的外展/微收，并向直线插值松弛；
     3) 模拟人类自然避障（回退一点 -> 绕行过去 -> 伸展到位）：
        当手臂靠近立柱或跨度大时，在起点方位先原位/微转收臂回退，跨过立柱后再展开到位；
     4) 保底安全回退：极端奇异位形下确保无碰撞。 */
  function routeAroundPlan(from, cur, quick, safeT) {
    let qT = safeT ? safeT.q : null, resT = safeT ? safeT.res : cur;
    if (!qT) {
      const found = findSafeTarget(cur, from);
      if (found) { qT = found.q; resT = found.res; }
    }
    if (!qT) return null;

    const chordMid = transitPt(from, qT, 0.5);
    const dj1 = wrap180(qT[0] - from[0]);
    const j1Mid = from[0] + dj1 * 0.5;

    // 阶段 1：微调避障（针对贴边穿模：直接在弦中点微调 J2/J4，完全不蜷缩手臂）
    for (const [dj2, dj4] of GENTLE_OFFSETS) {
      const testQ = chordMid.slice();
      testQ[1] += dj2; testQ[3] += dj4;
      if (!collideFree(testQ)) continue;
      const nA = Math.min(8, pathN(from, testQ, FINE_DEG));
      const nB = Math.min(8, pathN(testQ, qT, FINE_DEG));
      if (pathFree(from, testQ, nA) && pathFree(testQ, qT, nB)) {
        return { res: resT, via: [testQ], detour: 'gentle' };
      }
    }

    // 阶段 2：连续协同单中间点避障（J1 保持平滑转动，避障姿态向自然弦二分松弛）
    const use = quick ? [0, 1, 2] : [0, 1, 2, 3];
    for (const ci of use) {
      const c = CRUISE[ci];
      const qMidSafe = c.slice();
      qMidSafe[0] = j1Mid;
      if (!collideFree(qMidSafe)) continue;
      const nA = Math.min(8, pathN(from, qMidSafe, FINE_DEG));
      const nB = Math.min(8, pathN(qMidSafe, qT, FINE_DEG));
      if (pathFree(from, qMidSafe, nA) && pathFree(qMidSafe, qT, nB)) {
        // 二分松弛：寻找能安全通过的最小偏离量（靠近弦中点）
        let lo = 0, hi = 1;
        for (let it = 0; it < 4; it++) {
          const midA = (lo + hi) * 0.5;
          const testQ = transitPt(chordMid, qMidSafe, midA);
          if (collideFree(testQ) && pathFree(from, testQ, 6) && pathFree(testQ, qT, 6)) {
            hi = midA;
          } else {
            lo = midA;
          }
        }
        const optMid = transitPt(chordMid, qMidSafe, hi);
        return { res: resT, via: [optMid], detour: 'coordinated' };
      }
    }

    // 阶段 3：模拟人类自然避障（回退一点 -> 绕行过去 -> 伸展到位）
    // 用户需求："模拟人类，哪怕中间有东西挡了，应该回退一点然后再移动过去，而不是说挡着了就不计算了"
    // 第一中间点在起点方位（微转 frac）处收臂回退，第二中间点在避开立柱后跨过，再伸展入目标
    for (const ci of use) {
      const c = CRUISE[ci];
      for (const frac of [0.08, 0.0, 0.15, 0.25]) {
        const w1 = c.slice(); w1[0] = from[0] + dj1 * frac;
        const w2 = c.slice(); w2[0] = qT[0] - dj1 * frac;
        if (!collideFree(w1) || !collideFree(w2)) continue;
        const n1 = Math.min(8, pathN(from, w1, FINE_DEG));
        const n2 = Math.min(8, pathN(w1, w2, FINE_DEG));
        const n3 = Math.min(8, pathN(w2, qT, FINE_DEG));
        if (pathFree(from, w1, n1) && pathFree(w1, w2, n2) && pathFree(w2, qT, n3)) {
          return { res: resT, via: [w1, w2], detour: 'retract_pass' };
        }
      }
    }

    // 阶段 4：保底三段式（仅在所有平滑协同均被阻挡的极端奇异位形启用）
    for (const ci of use) {
      const c = CRUISE[ci];
      const fold = c.slice(); fold[0] = from[0];
      const turn = c.slice(); turn[0] = qT[0];
      const ok = pathFree(from, fold, Math.min(8, pathN(from, fold, FINE_DEG))) &&
        turnFree(ci, fold[0], turn[0]) &&
        pathFree(turn, qT, Math.min(8, pathN(turn, qT, FINE_DEG)));
      if (ok) return { res: resT, via: [fold, turn], detour: 'around' };
    }
    return null;
  }
  function routeOrVia(from, target, cur, opt) {
    const r = routeFrom(from, cur);
    if (!r.blocked || !from) return r;
    const around = opt.noAround ? null : routeAroundPlan(from, cur, opt.quick, r.safeT);
    if (around) return Object.assign({}, around.res, { via: around.via, routed: true, blocked: false, detour: around.detour || 'around' });
    if (opt.noVia) return r;
    return routeVia(from, target, opt) || r;
  }

  /* ---------- 6b. 动态限位求解：静态解 + 贴边轴放宽重解 ----------
     静态限位来自"其它轴全零"的单轴扫描，对非相邻碰撞对（典型：腕/法兰撞
     固定立柱）系统性过紧——实测 J2 正端随 J1 从 +9.75°（垂臂零位）放到
     +108.75°（肩侧抬 90°）。若静态解的某轴贴在边界上，就按当前姿态二分出
     该轴的动态边界，临时放宽该轴限位后重解；新解必须通过全姿态碰撞判定、
     且总关节角不升才采用，否则原样返回（最坏即静态行为，无回退风险）。 */
  function solveDyn(target, opt) {
    opt = opt || {};
    if (opt.from && !opt.ref) opt.ref = opt.from;
    let cur = solveTo(target, opt);
    if (!COL || opt.noDyn || !cur.q) return cur;
    /* 静态限位盒不保证组合姿态无干涉（单轴扫描各轴独立取限位，组合折叠会撞——
       sweep_v2 的组合扫描只记录不收紧滑块）。这里对最终解做全姿态碰撞校验：
       撞了就沿候选表找安全的解，优先采用最贴近起点的安全位形。 */
    if (!collideFree(cur.q)) {
      const safeT = findSafeTarget(cur, opt.from);
      if (safeT) cur = Object.assign({}, safeT.res, { swaped: true });
      else cur = Object.assign({}, cur, { unsafe: true });
    }
    for (let pass = 0; pass < 2; pass++) {
      const q = cur.q;
      let axis = null, dir = 0;
      for (let i = 0; i < 5; i++) {
        const t = LIM[ORDER[i]];
        if (t.cont) continue;
        if (q[i] >= t.hi - 0.5) { axis = ORDER[i]; dir = +1; break; }
        if (q[i] <= t.lo + 0.5) { axis = ORDER[i]; dir = -1; break; }
      }
      if (!axis) break;
      const bound = dynLimit(axis, q, dir, 0.5);
      if (Math.abs(bound - q[ORDER.indexOf(axis)]) < 1.0) break;   // 动态边界没放宽
      const save = { lo: LIM[axis].lo, hi: LIM[axis].hi };
      let r1 = null;
      try {
        if (dir > 0) LIM[axis].hi = bound; else LIM[axis].lo = bound;
        /* 增益 <5° 不值得一次全量重解（实测重解与首解同量级 ~190ms），直接跳过；
           大幅放宽（典型：J1=90° 时 J2 从 +9 放到 +106）才值得。 */
        if (bound - q[ORDER.indexOf(axis)] < (dir > 0 ? 5 : -5)) break;
        /* 重解步长粗一档：只需要"比静态更优"的解（接受判据就是 cost 不升），
           不需要全局最优——全量精扫贵在与首解同量级，粗档约省 3~4 倍。 */
        const step1 = Math.max((+opt.step || 0.25) * 2, 0.5);
        r1 = solveTo(target, Object.assign({}, opt, { noDyn: true, step: step1 }));
      } finally {
        LIM[axis].lo = save.lo; LIM[axis].hi = save.hi;
      }
      if (!r1 || !r1.q || r1.cost >= cur.cost - 1e-9) break;      // 没有更优 → 收工
      let picked = null;
      if (collideFree(r1.q)) picked = r1;
      else for (const s of r1.list) {              // 热循环不查碰撞：候选表里找第一个安全的
        if (s.cost < cur.cost - 1e-9 && collideFree(s.q)) {
          picked = Object.assign({}, r1, { q: s.q.slice(), cost: s.cost });
          break;
        }
      }
      if (!picked) break;
      cur = Object.assign({}, picked, { dynAxis: axis, dynBound: bound });
    }
    return opt.from ? routeOrVia(opt.from, target, cur, opt) : cur;
  }

  /* ---------- 7. 自检：闭式 FK ↔ 矩阵链；逆解重构；姿态重构 ---------- */
  function selfTest(n, nPose) {
    n = n || 400;
    nPose = nPose == null ? 8 : nPose;
    let maxFk = 0;
    for (let k = 0; k < n; k++) {
      const q = [];
      for (let i = 0; i < 6; i++) { const t = LIM[ORDER[i]]; q.push(t.cont ? (Math.random() * 720 - 360) : t.lo + Math.random() * (t.hi - t.lo)); }
      const a = fk(q, 0), b = fkChain(q, 0);
      maxFk = Math.max(maxFk, Math.hypot(a[0] - b[0], a[1] - b[1], a[2] - b[2]));
    }
    let maxIk = 0, hit = 0, m = 0;
    for (let k = 0; k < Math.min(n, 200); k++) {
      const q = [];
      for (let i = 0; i < 6; i++) { const t = LIM[ORDER[i]]; q.push(t.lo + Math.random() * (t.hi - t.lo)); }
      const T = fk(q, 0);
      const A = T[0] - K.X0, BY = T[1] - K.Y0, BZ = T[2] - K.Z1;
      const sols = branch(q[3], q[4], A, Math.hypot(BY, BZ), Math.atan2(BZ, BY), 0);
      if (!sols.length) continue;
      m++;
      let best = Infinity;
      for (const s of sols) best = Math.min(best, Math.hypot.apply(null, fk(s, 0).map((v, i) => v - T[i])));
      maxIk = Math.max(maxIk, best);
      if (best < 1e-6) hit++;
    }
    const ps = nPose > 0 ? poseSelfTest(nPose) : { poseHit: 0, poseTried: 0, poseErr: 0, poseMs: 0 };
    return { maxFkErr: maxFk, maxIkErr: maxIk, ikHit: hit, ikTried: m, structWarn: WARN, K: K, LIM: LIM,
             poseHit: ps.poseHit, poseTried: ps.poseTried, poseErr: ps.poseErr, poseMs: ps.poseMs };
  }

  /* 姿态重构自检：取随机可行位形的真实工具姿态为任务，闭式扫描应精确命中。
     单次约 70 ms（step=1 全量扫描），200 组同步跑 ≈ 14 s——放在页面启动路径里会把
     首屏从 ~1.4 s 拖到 ~15 s。故单独成函数，供界面分片调用。 */
  function poseSelfTest(nPose) {
    let pHit = 0, pTried = 0, pErr = 0, pMs = 0;
    for (let k = 0; k < nPose; k++) {
      const q = [];
      for (let i = 0; i < 6; i++) { const t = LIM[ORDER[i]]; q.push(t.lo + Math.random() * (t.hi - t.lo)); }
      const T = fk(q, 0), R = orient(q);
      pTried++;
      const r = solve(T, { pose: { R: R }, step: 1.0, tol: 1e-6 });
      pMs += r.ms;
      if (r.ok && r.oerr != null) {
        pErr = Math.max(pErr, r.oerr);
        if (r.oerr < 0.05 && r.err < 1e-3) pHit++;
      }
    }
    return { poseHit: pHit, poseTried: pTried, poseErr: pErr, poseMs: pTried ? pMs / pTried : 0 };
  }

  return { fk: fk, fkChain: fkChain, solve: solve, solveTo: solveTo, solveWarm: solveWarm, solveDyn: solveDyn,
           solveRoute: solveRoute, routeFrom: routeFrom, transitPt: transitPt,
           mixSolve: mixSolve, closest: closest, closest6: closest6, closestOri: closestOri, orient: orient,
           branch: branch, selfTest: selfTest, poseSelfTest: poseSelfTest,
           collideFree: collideFree, collideDetail: collideDetail,
           dynLimit: dynLimit, dynLimits: dynLimits,
           K: K, LIM: LIM, P: P, D: D, ORDER: ORDER, wrap180: wrap180, costOf: costOf };
}

return { make: make, ORDER: ORDER, poseMat: poseMat, poseAng: poseAng, rotErrDeg: rotErrDeg,
         segSegDist: segSegDist };
}));