import assert from 'node:assert/strict';
import { mkdir, readFile, writeFile } from 'node:fs/promises';
import { createHash } from 'node:crypto';
import createModule from '../build-wasm/linuxcnc-sim-core.js';
import { WasmProgramRuntime } from '../web/worker/wasm-program-runtime.mjs';
import { WasmXyzacKinematics } from '../web/worker/wasm-xyzac-kinematics.mjs';
import { SimSession, CONTEXT_ID } from '../web/worker/sim-session.mjs';
import { loadSimulationCase, toWorkerConfiguration } from '../web/workspace/sim-case-loader.mjs';
import { writeTrtGeometry } from '../web/workspace/trt-geometry.mjs';
import { MACHINE_DEFINITIONS } from '../web/workspace/machine-definitions.mjs';

const root = new URL('../', import.meta.url), out = new URL('docs/teaching/rtcp-beginners/', root);
await mkdir(new URL('assets/', out), { recursive: true });
await mkdir(new URL('verification/', out), { recursive: true });
const wasm = await readFile(new URL('build-wasm/linuxcnc-sim-core.wasm', root));
const core = await createModule({ wasmBinary: wasm });
const limits = Object.fromEntries(['x','y','z','a','b','c'].map((a) => [a,[-100000,100000]]));
const k = new WasmXyzacKinematics(core, { coordinates: 'XYZBC', limits });
const interpreter = new WasmProgramRuntime(core);
interpreter.load('M2\n', 'beginner-reference.ngc');
const blocks = [
  'N10 G21 G90 G94 G17',
  'N20 G10 L2 P1 X40 Y20 Z10',
  'N30 G54',
  'N40 M428',
  'N50 G1 X10 Y0 Z5 B0 C0 F600',
  'N60 C90',
  'N70 G91 X10',
  'N80 G90 X20 Y0 Z5 B30 C90 F300',
  'N90 X0 Y10',
  'N100 M429',
  'N110 M428',
];
const rows = [];
const axes = ['x','y','z','a','b','c','u','v','w'];
let joints = [0,0,0,0,0], mode = 0;
const poseOf = (state) => Object.fromEntries(axes.map((a) => [a, state[a] ?? 0]));
let canonical = poseOf({});
for (const block of blocks) {
  const result = interpreter.executeBlock(block);
  assert.equal(result.status, 'verified', JSON.stringify(result));
  const code = result.program.control_code;
  if ([428,429].includes(code)) {
    mode = code === 428 ? 1 : 0;
    const selected = k.selectPhysical(mode, joints);
    assert.equal(selected.status, 'verified');
    interpreter.synchronizePosition(selected.pose);
    canonical = { ...selected.pose };
  } else if (result.motion) {
    const inverse = k.inverseMode(mode, result.motion.end);
    assert.equal(inverse.status, 'verified'); joints = inverse.joints;
    canonical = { ...result.motion.end };
  }
  const state = interpreter.snapshot(), origin = state.coordinate_context.offsets[state.coordinate_context.system-1];
  rows.push({ block, has_motion: Boolean(result.motion), mode, canonical: { ...canonical }, work_xyz: ['x','y','z'].map((a,i)=>canonical[a]-origin[i]), joints: [...joints], origin: origin.slice(0,3), units: state.units, distance_mode: state.distance_mode, feed: state.feed, motion: result.motion });
}
const near = (actual, expected) => actual.forEach((v,i) => assert.ok(Math.abs(v-expected[i])<1e-8, `${v} != ${expected[i]}`));
near(rows[4].joints,[50,20,15,0,0]);
near(rows[5].joints,[-20,50,15,0,90]);
near(rows[6].joints,[-20,60,15,0,90]);
near(rows[7].joints,[-9.820508075688775,60,22.99038105676658,30,90]);
near(rows[9].joints,rows[8].joints); near(rows[10].joints,rows[8].joints);

function probe(blocks) {
  const p = new WasmProgramRuntime(core); p.load('M2\n','probe.ngc');
  let canonical = poseOf({});
  const rows = blocks.map((block) => {
    const result=p.executeBlock(block); assert.equal(result.status,'verified',block);
    const state=p.snapshot();
    if (result.motion) canonical = { ...result.motion.end };
    return { block, canonical: { ...canonical }, units:state.units,distance_mode:state.distance_mode,feed:state.feed, coordinates:state.coordinate_context,motion:result.motion };
  });
  p.destroy(); return rows;
}
const units = probe(['G21 G90 G1 X0 Y0 Z0 F600','G20 G91 X1 F2','G21 G90 X30']);
near([units[1].canonical.x,units[1].feed],[25.4,50.8]);
const touch = probe(['G21 G90 G1 X70 Y30 Z20 F600','G10 L20 P1 X0 Y0 Z0','G54 X10']);
near(touch[1].coordinates.offsets[0].slice(0,3),[70,30,20]); near([touch[2].canonical.x],[80]);
const switching = probe(['G21 G90 G10 L2 P1 X40 Y20 Z10','G10 L2 P2 X80 Y-10 Z10','G54 G1 X10 Y0 Z5 F600','G55','X10']);
near([switching[2].canonical.x,switching[3].canonical.x,switching[4].canonical.x],[50,50,90]);
interpreter.destroy(); k.destroy();

const fetchImpl = async (url) => { try { const text=await readFile(new URL(String(url).replace(/^\//,''),root),'utf8'); return {ok:true,text:async()=>text}; } catch {return {ok:false};} };
const definition=MACHINE_DEFINITIONS.find((m)=>m.coordinates==='XYZBC');
const sessionCore=await createModule({wasmBinary:wasm});
const session=new SimSession(sessionCore,{sessionId:'rtcp-beginner-teaching',planner:'linuxcnc-tp'});let seq=0;
const send=(kind,payload={})=>{
 const response=session.accept({schema_version:1,context_id:CONTEXT_ID,session_id:session.sessionId,kind,seq:++seq,operation_id:`lesson-${seq}`,payload});
 assert.notEqual(response.kind,'ERROR',JSON.stringify(response.payload));return response;
};
send('HELLO');send('SESSION_ATTACH');
const bundle=writeTrtGeometry(await loadSimulationCase({path:definition.entrypoint,fetchImpl}),{x_rot_point:0,y_rot_point:0,z_rot_point:0,x_offset:0,y_offset:0,z_offset:0});
send('LOAD_WORKSPACE',{root_path:'/workspace',configuration:toWorkerConfiguration(bundle)});
const program=blocks.slice(0,9).join('\n')+'\nM2\n';
send('PROGRAM_LOAD',{source:program,path:'beginner-g54-rtcp.ngc'});send('POWER',{active:true});send('HOME',{all:true});
for(let i=0;i<500&&!session.wasmHomeLimit.read().state.homed;i++)send('SIM_STEP',{cycles:1});
assert.ok(session.wasmHomeLimit.read().state.homed);send('PROGRAM_RUN');
let final;const trajectory=[];
for(let i=0;i<6000;i++){
 final=send('SIM_STEP',{cycles:5,response:'motion_delta'});
 const state=session.wasmMotion.read().state;
 if(i%10===0)trajectory.push({tick:session.tick,pose:state.position,joints:state.joint_commanded});
 if(final.payload.program?.program_status==='completed'&&state.queue_depth===0)break;
}
assert.equal(final.payload.program.program_status,'completed');
const end=session.wasmMotion.read().state;
near(end.joint_commanded,rows[8].joints);
const report={report_id:'rtcp-beginners-gcode-v1',status:'verified',wasm_sha256:createHash('sha256').update(wasm).digest('hex'),assumptions:{coordinates:'XYZBC',rot_point:[0,0,0],offsets:[0,0,0],tool_offset:0,work_origin:[40,20,10],rotation:'Ry(B)*Rz(C)',work_rotation:0},rows,units,touch,switching,executed_program:{source:program,status:final.payload.program.program_status,final_pose:end.position,final_joints:end.joint_commanded,samples:trajectory}};
await writeFile(new URL('gcode-evidence.json',out),JSON.stringify(report,null,2)+'\n');
console.log(JSON.stringify({report_id:report.report_id,status:report.status,blocks:rows.length,extra_blocks:units.length+touch.length+switching.length,trajectory_samples:trajectory.length,final:end.joint_commanded}));