247 lines
14 KiB
JavaScript
247 lines
14 KiB
JavaScript
import { fnv1a64 } from "./replay.mjs";
|
|
|
|
const SCHEMA = "wasm_program_runtime.v1";
|
|
const SEGMENT_BYTES = 216;
|
|
const POSE_BYTES = 72;
|
|
const AXES = ["x", "y", "z", "a", "b", "c", "u", "v", "w"];
|
|
const DEFAULT_RAPID_VELOCITY = 1000;
|
|
|
|
function call(core, name, returnType, argTypes, args) {
|
|
const fn = core?.[`_${name}`] ?? core?.[name];
|
|
if (typeof fn === "function") return fn(...args);
|
|
if (typeof core?.ccall === "function") return core.ccall(name, returnType, argTypes, args);
|
|
throw new Error(`missing core export: ${name}`);
|
|
}
|
|
|
|
function pose(core, pointer) {
|
|
const values = [...core.HEAPF64.subarray(pointer / 8, pointer / 8 + 9)];
|
|
return Object.fromEntries(AXES.map((axis, index) => [axis, values[index]]));
|
|
}
|
|
|
|
function segment(core, pointer) {
|
|
const start = pose(core, pointer);
|
|
const end = pose(core, pointer + 72);
|
|
const cart = (offset) => ({ x: core.HEAPF64[pointer / 8 + offset / 8], y: core.HEAPF64[pointer / 8 + offset / 8 + 1], z: core.HEAPF64[pointer / 8 + offset / 8 + 2] });
|
|
const ints = new DataView(core.HEAPU8.buffer, pointer, SEGMENT_BYTES);
|
|
return { start, end, center: cart(144), normal: cart(168), velocity: core.HEAPF64[pointer / 8 + 24], acceleration: core.HEAPF64[pointer / 8 + 25], motion_type: ints.getInt32(208, true), turn: ints.getInt32(212, true) };
|
|
}
|
|
|
|
function sampleSegment(core, segmentPointer, posePointer, value) {
|
|
const turn = Math.max(1, Math.abs(Number(value.turn ?? 1)));
|
|
const count = value.motion_type === 2 ? Math.max(32, Math.ceil(turn * 32)) : 1;
|
|
const samples = [];
|
|
for (let index = 0; index <= count; index += 1) {
|
|
const result = call(core, "sim_posemath_segment_point", "number", ["number", "number", "number"], [segmentPointer, index / count, posePointer]);
|
|
if (result !== 0) throw new Error(`wasm_program_segment_sample_${result}`);
|
|
samples.push(pose(core, posePointer));
|
|
}
|
|
return samples;
|
|
}
|
|
|
|
function readString(core, pointer, capacity) {
|
|
const bytes = core.HEAPU8.subarray(pointer, pointer + capacity);
|
|
const end = bytes.indexOf(0);
|
|
return new TextDecoder().decode(bytes.subarray(0, end < 0 ? bytes.length : end));
|
|
}
|
|
|
|
export class WasmProgramRuntime {
|
|
constructor(core) {
|
|
this.core = core;
|
|
this.available = ["sim_program_size", "sim_program_init", "sim_program_step", "sim_program_canonical", "sim_program_add_subroutine", "sim_program_set_rapid_velocity"].every((name) => typeof core?.[`_${name}`] === "function" || typeof core?.[name] === "function");
|
|
this.pointer = 0;
|
|
this.motionPointer = 0;
|
|
this.samplePointer = 0;
|
|
this.samplePosePointer = 0;
|
|
this.source = "";
|
|
this.path = null;
|
|
this.subroutines = [];
|
|
this.rapidVelocity = 35;
|
|
if (!this.available || typeof core?._malloc !== "function") return;
|
|
this.pointer = core._malloc(call(core, "sim_program_size", "number", [], []));
|
|
this.motionPointer = core._malloc(SEGMENT_BYTES);
|
|
this.samplePointer = core._malloc(SEGMENT_BYTES);
|
|
this.samplePosePointer = core._malloc(POSE_BYTES);
|
|
if (!this.pointer || !this.motionPointer || !this.samplePointer || !this.samplePosePointer) throw new Error("wasm_program_allocation_failed");
|
|
const result = call(core, "sim_program_init", "number", ["number"], [this.pointer]);
|
|
if (result !== 0) throw new Error(`wasm_program_init_${result}`);
|
|
}
|
|
|
|
invoke(name, args = [], errorPrefix = name) {
|
|
if (!this.available) return { status: "blocked", code: "wasm_program_unavailable" };
|
|
const result = call(this.core, name, "number", ["number", ...args.map(() => "number")], [this.pointer, ...args]);
|
|
return result === 0 ? { status: "verified", command: name } : { status: "error", code: `wasm_${errorPrefix}_${result}`, result };
|
|
}
|
|
|
|
load(source, path = null) {
|
|
if (typeof source !== "string" || source.length === 0) return { status: "error", code: "program_source_invalid" };
|
|
const bytes = new TextEncoder().encode(source);
|
|
const pointer = this.core._malloc(bytes.byteLength);
|
|
this.core.HEAPU8.set(bytes, pointer);
|
|
try {
|
|
const result = call(this.core, "sim_program_load", "number", ["number", "number", "number"], [this.pointer, pointer, bytes.byteLength]);
|
|
if (result !== 0) return { status: "error", code: `wasm_program_load_${result}` };
|
|
const velocityResult = call(this.core, "sim_program_set_rapid_velocity", "number", ["number", "number"], [this.pointer, this.rapidVelocity]);
|
|
if (velocityResult !== 0) return { status: "error", code: `wasm_program_rapid_velocity_${velocityResult}` };
|
|
this.source = source;
|
|
this.path = path;
|
|
this.subroutines = [];
|
|
return { status: "verified", program: this.snapshot() };
|
|
} finally { this.core._free(pointer); }
|
|
}
|
|
|
|
requiresSubroutines(source = this.source) {
|
|
if (typeof source !== "string" || source.length === 0) return { status: "error", code: "program_source_invalid" };
|
|
const bytes = new TextEncoder().encode(source);
|
|
const pointer = this.core._malloc(bytes.byteLength);
|
|
this.core.HEAPU8.set(bytes, pointer);
|
|
try {
|
|
const result = call(this.core, "sim_program_requires_subroutines", "number", ["number", "number"], [pointer, bytes.byteLength]);
|
|
if (result < 0) return { status: "error", code: `wasm_program_requires_subroutines_${result}` };
|
|
return { status: "verified", required: result === 1 };
|
|
} finally { this.core._free(pointer); }
|
|
}
|
|
|
|
preview() {
|
|
if (!this.source) return { status: "error", code: "program_preview_source_missing" };
|
|
const temporary = new WasmProgramRuntime(this.core);
|
|
const segments = [];
|
|
try {
|
|
const loaded = temporary.load(this.source, this.path);
|
|
if (loaded.status !== "verified") return loaded;
|
|
temporary.setRapidVelocity(this.rapidVelocity);
|
|
for (const subroutine of this.subroutines) {
|
|
const added = temporary.addSubroutine(subroutine.name, subroutine.source);
|
|
if (added.status !== "verified") return added;
|
|
}
|
|
const started = temporary.start();
|
|
if (started.status !== "verified") return started;
|
|
const limit = Math.min(100000, Math.max(1, temporary.snapshot().line_count * 8 + 1));
|
|
let kinematicsMode = 0;
|
|
for (let index = 0; index < limit; index += 1) {
|
|
const step = temporary.step();
|
|
if (step.status !== "verified") return { status: "error", code: step.code, line: temporary.snapshot().current_line, segments };
|
|
if (step.has_control && [428, 429, 430].includes(step.control_code)) kinematicsMode = { 428: 1, 429: 0, 430: 2 }[step.control_code];
|
|
if (step.motion) segments.push({ id: segments.length + 1, kinematics_mode: kinematicsMode, ...step.motion });
|
|
if ([4, 5, 6].includes(temporary.snapshot().status)) break;
|
|
}
|
|
return { status: "verified", schema: "wasm-canonical-preview.v1", source: "linuxcnc-wasm-sim_program", path: this.path, source_hash: fnv1a64(this.source), line_count: this.snapshot().line_count, segments, preview_hash: fnv1a64(segments) };
|
|
} finally { temporary.destroy(); }
|
|
}
|
|
|
|
start() { return this.invoke("sim_program_start", [], "program_start"); }
|
|
setRapidVelocity(velocity) {
|
|
if (!Number.isFinite(Number(velocity)) || Number(velocity) <= 0) return { status: "error", code: "program_rapid_velocity_invalid" };
|
|
this.rapidVelocity = Number(velocity);
|
|
const result = call(this.core, "sim_program_set_rapid_velocity", "number", ["number", "number"], [this.pointer, this.rapidVelocity]);
|
|
if (result !== 0) throw new Error(`wasm_program_set_rapid_velocity_${result}`);
|
|
return { status: "verified", command: "sim_program_set_rapid_velocity" };
|
|
}
|
|
addSubroutine(name, source) {
|
|
if (typeof source !== "string" || source.length === 0) return { status: "error", code: "program_subroutine_invalid" };
|
|
const bytes = new TextEncoder().encode(source);
|
|
const pointer = this.core._malloc(bytes.byteLength);
|
|
this.core.HEAPU8.set(bytes, pointer);
|
|
try {
|
|
const code = call(this.core, "sim_program_add_subroutine", "number", ["number", "number", "number"], [this.pointer, pointer, bytes.byteLength]);
|
|
if (code !== 0) return { status: "error", code: `wasm_program_add_subroutine_${code}`, result: code };
|
|
this.subroutines.push({ name: String(name ?? "subroutine"), source });
|
|
return { status: "verified", command: "sim_program_add_subroutine", name: String(name ?? "subroutine") };
|
|
} finally { this.core._free(pointer); }
|
|
}
|
|
pause() { return this.invoke("sim_program_pause", [], "program_pause"); }
|
|
resume() { return this.invoke("sim_program_resume", [], "program_resume"); }
|
|
abort() { return this.invoke("sim_program_abort", [], "program_abort"); }
|
|
seek(line) { return this.invoke("sim_program_seek", [line], "program_seek"); }
|
|
|
|
restoreState(value) {
|
|
if (!value || ![SCHEMA, "sim_program.v1"].includes(value.schema) || !Number.isSafeInteger(value.current_line) || value.current_line < 0 || !this.source) throw new Error("wasm_program_restore_schema");
|
|
if (value.source_hash && value.source_hash !== fnv1a64(this.source)) throw new Error("wasm_program_restore_source_hash");
|
|
const subroutines = [...this.subroutines];
|
|
const loaded = this.load(this.source, this.path);
|
|
if (loaded.status !== "verified") throw new Error(loaded.code);
|
|
for (const subroutine of subroutines) {
|
|
const added = this.addSubroutine(subroutine.name, subroutine.source);
|
|
if (added.status !== "verified") throw new Error(added.code);
|
|
}
|
|
if (value.current_line > 0) {
|
|
const seek = this.seek(value.current_line);
|
|
if (seek.status !== "verified") throw new Error(seek.code);
|
|
}
|
|
const status = value.program_status ?? ({ 0: "idle", 1: "loaded", 2: "running", 3: "paused", 4: "completed", 5: "aborted", 6: "fault" }[value.status] ?? "loaded");
|
|
if (status === "running") {
|
|
const resumed = this.resume();
|
|
if (resumed.status !== "verified") {
|
|
const started = this.start();
|
|
if (started.status !== "verified") throw new Error(started.code);
|
|
}
|
|
} else if (status === "completed") {
|
|
this.start();
|
|
for (let index = 0; index < this.snapshot().line_count + 1 && this.snapshot().status === 2; index += 1) this.step();
|
|
} else if (status === "aborted") {
|
|
const aborted = this.abort();
|
|
if (aborted.status !== "verified") throw new Error(aborted.code);
|
|
}
|
|
if (value.has_motion === 0) this.clearMotion();
|
|
return this.snapshot();
|
|
}
|
|
|
|
executeBlock(block) {
|
|
const bytes = new TextEncoder().encode(String(block ?? ""));
|
|
const pointer = this.core._malloc(bytes.byteLength);
|
|
this.core.HEAPU8.set(bytes, pointer);
|
|
try {
|
|
const result = call(this.core, "sim_program_execute_block", "number", ["number", "number", "number"], [this.pointer, pointer, bytes.byteLength]);
|
|
if (result !== 0) return { status: "error", code: `wasm_program_execute_${result}` };
|
|
return { status: "verified", program: this.snapshot(), motion: this.readMotion() };
|
|
} finally { this.core._free(pointer); }
|
|
}
|
|
|
|
readMotion() {
|
|
const state = this.snapshot();
|
|
if (!state.has_motion) return null;
|
|
const result = call(this.core, "sim_program_read_motion", "number", ["number", "number"], [this.pointer, this.motionPointer]);
|
|
if (result !== 0) return null;
|
|
const value = segment(this.core, this.motionPointer);
|
|
// LinuxCNC rapid blocks do not carry an F word. The trajectory queue
|
|
// requires a positive velocity, so use the simulation's configured rapid
|
|
// rate for that WASM-produced linear segment.
|
|
if (state.motion_mode === 0 && value.motion_type === 1 && value.velocity <= 0) value.velocity = DEFAULT_RAPID_VELOCITY;
|
|
value.samples = sampleSegment(this.core, this.motionPointer, this.samplePosePointer, value);
|
|
value.samples_source = "linuxcnc-wasm-sim_posemath_segment_point";
|
|
value.canonical_motion_mode = state.motion_mode;
|
|
value.kind = value.motion_type === 2 && Math.abs(Number(value.end?.z ?? 0) - Number(value.start?.z ?? 0)) > 1e-12 ? "helix" : value.motion_type === 2 ? "arc" : state.motion_mode === 0 ? "rapid" : value.motion_type === 1 ? "feed" : "traverse";
|
|
if (value.kind === "helix") {
|
|
value.helix = true;
|
|
value.radius = Math.hypot(Number(value.start?.x ?? 0) - Number(value.center?.x ?? 0), Number(value.start?.y ?? 0) - Number(value.center?.y ?? 0));
|
|
value.turns = Math.abs(Number(value.turn ?? 1));
|
|
value.axial_delta = Number(value.end?.z ?? 0) - Number(value.start?.z ?? 0);
|
|
}
|
|
call(this.core, "sim_program_clear_motion", "number", ["number"], [this.pointer]);
|
|
return { ...value, source_line: state.current_line };
|
|
}
|
|
|
|
clearMotion() { return this.invoke("sim_program_clear_motion", [], "program_clear_motion"); }
|
|
|
|
step() {
|
|
const result = this.invoke("sim_program_step", [], "program_step");
|
|
if (result.status !== "verified") return result;
|
|
const state = this.snapshot();
|
|
return { status: "verified", program_status: state.status, line: state.current_line, control_code: state.control_code ?? 0, has_control: state.has_control === 1, program: state, motion: this.readMotion() };
|
|
}
|
|
|
|
snapshot() {
|
|
if (!this.available) return { schema: SCHEMA, status: "blocked", code: "wasm_program_unavailable" };
|
|
const required = call(this.core, "sim_program_canonical", "number", ["number", "number", "number"], [this.pointer, 0, 0]);
|
|
const buffer = this.core._malloc(required);
|
|
try {
|
|
call(this.core, "sim_program_canonical", "number", ["number", "number", "number"], [this.pointer, buffer, required]);
|
|
const state = JSON.parse(readString(this.core, buffer, required));
|
|
const hash = call(this.core, "sim_program_state_hash", "number", ["number"], [this.pointer]);
|
|
const programStatus = { 0: "idle", 1: "loaded", 2: "running", 3: "paused", 4: "completed", 5: "aborted", 6: "fault" }[state.status] ?? "idle";
|
|
return { schema: SCHEMA, source: "linuxcnc-wasm-sim_program", ...state, loaded: state.status !== 0, line: state.current_line, currentLine: state.current_line, program_status: programStatus, path: this.path, subroutines: this.subroutines.map((subroutine) => subroutine.name), state_hash: `fnv1a64:${BigInt.asUintN(64, BigInt(hash >>> 0)).toString(16).padStart(16, "0")}`, source_hash: this.source ? fnv1a64(this.source) : null };
|
|
} finally { this.core._free(buffer); }
|
|
}
|
|
|
|
destroy() { for (const pointer of [this.pointer, this.motionPointer, this.samplePointer, this.samplePosePointer]) if (pointer && typeof this.core?._free === "function") this.core._free(pointer); this.pointer = this.motionPointer = this.samplePointer = this.samplePosePointer = 0; }
|
|
}
|