/* 实时拖拽仿真基准：目标沿直线从 A 的 TCP 拖到 B 的 TCP（中间被立柱挡住），每帧 16.7 ms。
 * 每帧：手臂沿当前路径按各轴速度上限前进；求解器空闲就用"此刻位形"作起点重规划（quick 模式）。
 * 统计：重规划耗时分布、穿模帧（1° 细验）、拖完 6 s 后的到达误差。
 *   node tests/rt_drag_bench.cjs <solver.js> [场景数=12] [帧数=120] */
const setup = require('./route_bench_setup.cjs');
const { S } = setup(process.argv[2]);
const NC = +process.argv[3] || 12, NF = +process.argv[4] || 120, DT = 1 / 60, TAIL = 360;
const L = S.LIM, O = S.ORDER, w = S.wrap180;
let s = 777; const rnd = () => { s = (s * 1664525 + 1013904223) >>> 0; return s / 4294967296; };
const rq = () => O.map(id => { const t = L[id]; return t.lo + rnd() * (t.hi - t.lo); });
const d = (a, b) => a.map((v, i) => L[O[i]].cont ? w(b[i] - v) : b[i] - v);
const segOK = (a, b) => { const dd = d(a, b); const m = Math.max(...dd.map(Math.abs)); const n = Math.max(1, Math.ceil(m / 1)); for (let i = 1; i <= n; i++) if (!S.collideFree(a.map((v, k) => v + dd[k] * i / n))) return false; return true; };
const VMAX = [60, 60, 90, 90, 120, 180];
const cases = []; let tries = 0;
while (cases.length < NC && tries < 50000) { tries++; const a = rq(), b = rq(); if (!S.collideFree(a) || !S.collideFree(b) || segOK(a, b)) continue; cases.push([a, b]); }
const lat = []; let badFrames = 0, frames = 0, reached = 0, stuck = 0;
for (const [a, b] of cases) {
  const TA = S.fk(a, 0), TB = S.fk(b, 0);
  let q = a.slice(), path = [], busyUntil = 0, tNow = 0;
  for (let f = 0; f < NF + TAIL; f++) {
    tNow = f * DT * 1000;
    const u = Math.min(1, f / NF), T = TA.map((v, i) => v + (TB[i] - v) * u);
    if (tNow >= busyUntil) {                          // 求解器空闲：派发一次重规划（结果在耗时后才生效）
      const t0 = performance.now();
      const r = S.solveRoute(T, { tool: 0, step: 0.5, ref: q, warm: q, from: q.slice(), noVia: true, quick: true });
      const ms = performance.now() - t0; lat.push(ms);
      busyUntil = tNow + ms;
      if (r && r.q) { path = (r.via || []).concat([r.q]); }
    }
    /* 按轴速上限沿折线前进 */
    let budget = 1;
    const q0 = q.slice();
    while (path.length && budget > 1e-9) {
      const tgt = q.map((v, i) => L[O[i]].cont ? v + w(path[0][i] - v) : path[0][i]);
      const dd = tgt.map((v, i) => v - q[i]);
      const need = Math.max(...dd.map((x, i) => Math.abs(x) / (VMAX[i] * DT)));
      if (need <= budget) { q = tgt; path.shift(); budget -= need; }
      else { q = q.map((v, i) => v + dd[i] * budget / need); budget = 0; }
    }
    frames++;
    if (!segOK(q0, q)) badFrames++;
  }
  const tcp = S.fk(q, 0), err = Math.hypot(tcp[0] - TB[0], tcp[1] - TB[1], tcp[2] - TB[2]);
  if (err < 1) reached++; else stuck++;
}
lat.sort((x, y) => x - y);
const pct = p => +lat[Math.min(lat.length - 1, Math.floor(lat.length * p))].toFixed(1);
console.log(JSON.stringify({ cases: cases.length, replans: lat.length, latP50: pct(0.5), latP90: pct(0.9), latP99: pct(0.99), latMax: pct(1),
  latMean: +(lat.reduce((x, y) => x + y, 0) / lat.length).toFixed(1), collidingFrames: badFrames, frames, reachedAfter6s: reached }));