/* ============================================================================
 *  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 x = d[0], y = d[1], z = d[2], c = Math.cos(a), s = Math.sin(a), 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]
];

/* ==========================================================================*/
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: -180, hi: 180, continuous: true };
    LIM[id] = { lo: +t.lo, hi: +t.hi, cont: !!t.continuous };
  }
  const TCP0 = (cfg.tcp || [18.5, 128, 93]).slice();
  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]];
  }

  /* ---------- 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) 的闭式分支 ---------- */
  function branch(th4, th5, A, R, ph, tool) {
    const L = K.L5 + (+tool || 0);
    const s5 = Math.sin(th5 * D2R), c5 = Math.cos(th5 * D2R);
    const s4 = Math.sin(th4 * D2R), c4 = Math.cos(th4 * 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 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-7) return out;
      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))));
    const TH = [sig, -sig, Math.PI - sig, -(Math.PI - sig)];
    for (let k = 0; k < 4; k++) {
      const th3 = TH[k];
      const kap = Vx * Math.cos(th3), 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 * Math.sin(th3);
      const th1 = (Math.abs(b) + Math.abs(cc) > 1e-12) ? (ph - Math.atan2(cc, b)) * R2D : 0;
      out.push([wrap180(th1), wrap180(th2), wrap180(th3 * R2D), th4, th5, 0]);
    }
    return out;
  }

  const inLimit = q => {
    for (let i = 0; i < 5; i++) {
      const t = LIM[ORDER[i]];
      if (q[i] < t.lo - 1e-9 || q[i] > t.hi + 1e-9) return false;
    }
    return true;
  };
  const costOf = (q, W) => {
    let c = 0; for (let i = 0; i < 6; i++) c += Math.abs(q[i]) * (W ? W[i] : 1);
    return c;
  };
  const nowMs = () => (typeof performance !== 'undefined' ? performance.now() : Date.now());

  /* ---------- 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;
    const KEEP = 4000;

    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 p = fk(q, tool);
        const e = Math.hypot(p[0] - target[0], p[1] - target[1], p[2] - target[2]);
        if (e > tol) continue;
        const c = costOf(q, W);
        if (!best || c < best.cost) best = { q: q, cost: c, err: e };
      }
      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), g5 = axis(L5.lo, L5.hi, h);

    /* --- 4.1 全域粗扫：覆盖整个二维冗余 (J4,J5) --- */
    let all = [], samples = 0, nSol = 0, trimmed = false;
    for (let i = 0; i < g4.length; i++) {
      for (let j = 0; j < g5.length; j++) {
        samples++;
        const s = evalAt(g4[i], g5[j]);
        if (s) { all.push(s); nSol++; }
      }
      if (all.length > KEEP * 12) {                 // 有界保留：只留代价最优者
        all.sort((x, y) => x.cost - y.cost);
        all.length = KEEP;
        trimmed = true;
      }
    }
    all.sort((x, y) => x.cost - y.cost);

    /* --- 4.2 多起点局部加密 + 模式搜索 --- */
    const seeds = [];
    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);
    }
    if (opt.refine !== false) {
      const cl = (v, a, b) => v < a ? a : (v > b ? b : v);
      for (const s of seeds) {                      // 局部加密网格（h/8，±2h 窗口）
        const hh = 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 best = s;
        for (const a of w4) for (const b of w5) {
          samples++;
          const c = evalAt(a, b);
          if (c) { nSol++; if (c.cost < best.cost) best = c; }
        }
        all.push(best);
      }
      all.sort((x, y) => x.cost - y.cost);
      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], best = 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.cost < best.cost - 1e-10) { best = c; c4 = t4; c5 = t5; moved = true; }
          }
          if (!moved) step *= 0.5;
        }
        all.push(best);
      }
    }
    all.sort((x, y) => x.cost - y.cost);
    const best = all.length ? all[0] : null;
    return {
      ok: !!best, reachable: !!best, target: target.slice(), tool: tool,
      q: best ? best.q.slice() : null, cost: best ? best.cost : null,
      err: best ? best.err : null, nSol: nSol, truncated: trimmed,
      samples: samples, ms: nowMs() - t0, list: all.slice(0, KEEP)
    };
  }

  /* ---------- 5. 最近可达位姿（阻尼最小二乘，兜底用） ---------- */
  function closest(target, tool, tries) {
    const dp = 0.05, lam = 4.0;
    let bestQ = null, bestD = Infinity;
    const run = q => {
      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]];
          q[c] = Math.max(t.lo, Math.min(t.hi, q[c] + dq[c] * gain));
        }
      }
    };
    run([0, 0, 0, 0, 0, 0]);
    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 + Math.random() * (t.hi - t.lo); }
      run(q);
    }
    return { q: bestQ, dist: bestD };
  }

  /* ---------- 6. 一键求解（不可达时返回最近可达位姿） ---------- */
  function solveTo(target, opt) {
    opt = opt || {};
    const res = solve(target, opt);
    if (res.ok) return res;
    const cp = closest(target, +opt.tool || 0, opt.tries);
    if (!cp.q) return res;
    return {
      ok: false, reachable: false, target: target.slice(), tool: +opt.tool || 0,
      q: cp.q.slice(), cost: costOf(cp.q, opt.weights), err: cp.dist, resid: cp.dist,
      nSol: res.nSol, samples: res.samples, ms: res.ms, list: [],
      note: '目标超出可达工作空间：返回最近可达位姿（阻尼最小二乘）'
    };
  }

  /* ---------- 7. 自检：闭式 FK ↔ 矩阵链；逆解重构 ---------- */
  function selfTest(n) {
    n = n || 400;
    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++;
    }
    return { maxFkErr: maxFk, maxIkErr: maxIk, ikHit: hit, ikTried: m, structWarn: WARN, K: K, LIM: LIM };
  }

  return { fk: fk, fkChain: fkChain, solve: solve, solveTo: solveTo,
           closest: closest, branch: branch, selfTest: selfTest,
           K: K, LIM: LIM, P: P, D: D, ORDER: ORDER, wrap180: wrap180, costOf: costOf };
}

return { make: make, ORDER: ORDER };
}));