const setup=require('./route_bench_setup.cjs');
const solverPath=process.argv[2];const N=+process.argv[3]||40;const live=process.argv[4]==='live';
const {S}=setup(solverPath);
const L=S.LIM,O=S.ORDER,w=S.wrap180;
let s=12345;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);
function 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}
// build cases: free start, free goal config, straight blocked
const cases=[];let tries=0;
while(cases.length<N&&tries<20000){tries++;const a=rq(),b=rq();if(!S.collideFree(a)||!S.collideFree(b))continue;
 if(segOK(a,b))continue; // only obstacle cases
 cases.push({from:a,T:S.fk(b,0)});}
const W=[1,1,1,1,1,1];
let res={n:cases.length,reached:0,valid:0,blocked:0,len:0,lenDirect:0,ms:0,rev:0,segs:0,timeExec:0,fail:[]};
const VMAX=[60,60,90,90,120,180]; // deg/s assumed
function execTime(pts){let t=0;for(let i=1;i<pts.length;i++){const dd=d(pts[i-1],pts[i]);t+=Math.max(...dd.map((x,k)=>Math.abs(x)/VMAX[k]))+ (pts.length>2&&i<pts.length-1?0.25:0)}return t}
for(const c of cases){
 const t0=performance.now();
 const opt={tool:0,step:0.5,ref:c.from,from:c.from.slice()};
 if(live){opt.noVia=true;opt.quick=true;opt.warm=c.from}
 const r=live?S.solveRoute(c.T,opt):S.solveDyn(c.T,opt);
 const ms=performance.now()-t0;res.ms+=ms;
 const pts=[c.from].concat(r.via||[],[r.q]);
 // unwrap
 for(let i=1;i<pts.length;i++)pts[i]=pts[i-1].map((v,k)=>L[O[k]].cont?v+w(pts[i][k]-v):pts[i][k]);
 let ok=true;for(let i=1;i<pts.length;i++)if(!segOK(pts[i-1],pts[i])){ok=false;break}
 const tcp=S.fk(r.q,0);const err=Math.hypot(tcp[0]-c.T[0],tcp[1]-c.T[1],tcp[2]-c.T[2]);
 const reached=err<1&&!r.blocked;
 if(reached)res.reached++; if(ok)res.valid++; if(r.blocked)res.blocked++;
 let len=0;for(let i=1;i<pts.length;i++)len+=d(pts[i-1],pts[i]).reduce((a,x)=>a+Math.abs(x),0);
 const direct=d(c.from,r.q).reduce((a,x)=>a+Math.abs(x),0);
 // reversals
 let rev=0;for(let k=0;k<6;k++){let sg=0;for(let i=1;i<pts.length;i++){const x=d(pts[i-1],pts[i])[k];if(Math.abs(x)<2)continue;const s2=Math.sign(x);if(sg&&s2!==sg)rev++;sg=s2}}
 res.per=res.per||[];res.per.push({ms:Math.round(ms),reached,ok,len:Math.round(len),rev,segs:pts.length-1,exec:+execTime(pts).toFixed(2),detour:r.detour||null});
 if(reached&&ok){res.len+=len;res.lenDirect+=direct;res.rev+=rev;res.segs+=pts.length-1;res.timeExec+=execTime(pts)}
 else res.fail.push({reached,ok,err:+err.toFixed(1),detour:r.detour||null,blocked:!!r.blocked});
}
const g=res.reached;const both=cases.length-res.fail.length;
require('fs').writeFileSync(process.argv[5]||require('path').join(require('os').tmpdir(),'route_bench_per.json'),JSON.stringify(res.per));console.log(JSON.stringify({n:res.n,reached:res.reached,validPath:res.valid,goodBoth:both,blocked:res.blocked,
 avgPlanMs:+(res.ms/res.n).toFixed(0),avgJointTravel:+(res.len/both).toFixed(0),avgDirect:+(res.lenDirect/both).toFixed(0),
 avgReversals:+(res.rev/both).toFixed(2),avgSegs:+(res.segs/both).toFixed(2),avgExecS:+(res.timeExec/both).toFixed(2),
 failTypes:res.fail.slice(0,12)}));