import assert from "node:assert/strict";
import { readFile } from "node:fs/promises";
import createModule from "../../build-wasm/linuxcnc-sim-core.js";
import { loadSimulationCase, toWorkerConfiguration } from "../workspace/sim-case-loader.mjs";
import { WasmXyzacKinematics, XYZAC_MODES } from "./wasm-xyzac-kinematics.mjs";
import { XyzacSession, CONTEXT_ID } from "./xyzac-session.mjs";

const CASE = "linuxcnc/configs/sim/axis/vismach/5axis/table-rotary-tilting/xyzac-trt.ini";
const fetchImpl = async (url) => {
  try {
    const body = await readFile(new URL(`../../${String(url).replace(/^\//, "")}`, import.meta.url));
    return { ok: true, text: async () => body.toString("utf8") };
  } catch {
    return { ok: false, text: async () => "" };
  }
};
const close = (actual, expected, tolerance = 1e-9) => assert.ok(Math.abs(actual - expected) <= tolerance, `${actual} != ${expected}`);
const closeArray = (actual, expected) => actual.forEach((value, index) => close(value, expected[index]));

const bundle = await loadSimulationCase({ path: CASE, fetchImpl });
assert.equal(bundle.status, "ready");
const core = await createModule({ wasmBinary: await readFile(new URL("../../build-wasm/linuxcnc-sim-core.wasm", import.meta.url)) });
const limits = toWorkerConfiguration(bundle).home_limits;

const kinematics = new WasmXyzacKinematics(core, { x_rot_point: 0, y_offset: 20, z_offset: 10, tool_offset: 0, limits });
const initial = kinematics.snapshot();
assert.equal(initial.active_type, XYZAC_MODES.identity);
assert.equal(initial.coordinates, "XYZAC");
assert.equal(initial.linear_units, "mm");
assert.equal(initial.angular_units, "deg");
assert.deepEqual(initial.limits, { x: [-200, 200], y: [-100, 100], z: [-120, 120], a: [-100, 50], c: [-36000, 36000] });

const identityJoints = [12, -5, 20, 30, 45];
const identityForward = kinematics.forward(identityJoints);
assert.equal(identityForward.status, "verified");
assert.deepEqual(identityForward.pose, { x: 12, y: -5, z: 20, a: 30, b: 0, c: 45, u: 0, v: 0, w: 0 });
const identityInverse = kinematics.inverse(identityForward.pose);
assert.equal(identityInverse.status, "verified");
closeArray(identityInverse.joints, identityJoints);

const rangeError = kinematics.forward([0, 0, 121, 0, 0]);
assert.equal(rangeError.code, "wasm_xyzac_forward_-702");
const nonfiniteJoints = kinematics.forward([0, 0, Number.NaN, 0, 0]);
assert.equal(nonfiniteJoints.code, "wasm_xyzac_forward_-701");
const nonfinitePose = kinematics.inverse({ ...identityForward.pose, x: Number.NaN });
assert.equal(nonfinitePose.code, "wasm_xyzac_inverse_-701");

const selectedTrt = kinematics.select(XYZAC_MODES.xyzac, identityJoints);
assert.equal(selectedTrt.status, "verified");
assert.equal(selectedTrt.active_type, XYZAC_MODES.xyzac);
close(selectedTrt.pose.x, identityForward.pose.x);
close(selectedTrt.pose.y, identityForward.pose.y);
close(selectedTrt.pose.z, identityForward.pose.z);
const trtJoints = [50, 10, 30, 30, 45];
const trtForward = kinematics.forward(trtJoints);
assert.equal(trtForward.status, "verified");
const trtInverse = kinematics.inverse(trtForward.pose);
assert.equal(trtInverse.status, "verified");
closeArray(trtInverse.joints, trtJoints);

const pivotBoundary = kinematics.forward([20, 5, 30, -90, 45]);
assert.equal(pivotBoundary.status, "verified");
const pivotRoundtrip = kinematics.inverse(pivotBoundary.pose);
assert.equal(pivotRoundtrip.status, "verified");
closeArray(pivotRoundtrip.joints, [20, 5, 30, -90, 45]);

const tool0 = new WasmXyzacKinematics(core, { y_offset: 20, z_offset: 10, tool_offset: 0, limits });
const tool25 = new WasmXyzacKinematics(core, { x_rot_point: 5, y_offset: 20, z_offset: 10, tool_offset: 25, limits });
tool0.select(XYZAC_MODES.xyzac);
tool25.select(XYZAC_MODES.xyzac);
const offset0 = tool0.forward(trtJoints);
const offset25 = tool25.forward(trtJoints);
assert.equal(offset0.status, "verified");
assert.equal(offset25.status, "verified");
assert.notDeepEqual(offset25.pose, offset0.pose);
assert.deepEqual(offset25.offsets, { x_rot_point: 5, y_offset: 20, z_offset: 10, tool_offset: 25 });

const userPose = kinematics.forward(trtJoints).pose;
const userk = kinematics.select(XYZAC_MODES.userk, trtJoints);
assert.equal(userk.status, "verified");
assert.equal(userk.native_type, "userk-identity-template");
close(userk.pose.x, userPose.x);
close(userk.pose.y, userPose.y);
close(userk.pose.z, userPose.z);
const path = new TextEncoder().encode("/tmp/dynamic-userk.so\0");
const pathPointer = core._malloc(path.length);
core.HEAPU8.set(path, pathPointer);
assert.equal(core._sim_kinematics_user_reject_dynamic(pathPointer), -712);
core._free(pathPointer);

const session = new XyzacSession(core, { sessionId: "xyzac-rtcp" });
const base = { schema_version: 1, context_id: CONTEXT_ID, session_id: session.sessionId };
let seq = 0;
const send = (kind, operationId, payload = {}) => session.accept({ ...base, kind, seq: ++seq, operation_id: operationId, payload });
send("HELLO", "hello");
send("SESSION_ATTACH", "attach");
assert.equal(send("LOAD_WORKSPACE", "workspace", { root_path: "/workspace", configuration: toWorkerConfiguration(bundle) }).payload.status, "verified");
assert.equal(send("POWER", "power-on", { active: true }).payload.status, "verified");
assert.equal(send("HOME", "home-all", { all: true }).payload.wasm_home_limit.state.homed, true);

const switchRows = [];
for (const [code, expected] of [["M429", 0], ["M428", 1], ["M430", 2], ["M428", 1]]) {
  const switched = send("MDI_EXECUTE", `switch-${code}-${seq}`, { block: code });
  assert.equal(switched.kind, "COMMAND_RESULT");
  assert.equal(switched.payload.kinematics.active_type, expected);
  assert.equal(switched.payload.remap.hal_pin, "motion.switchkins-type");
  assert.equal(switched.payload.remap.sync, "M66 E0 L0");
  switchRows.push({ code, active_type: expected, state_hash: switched.payload.kinematics.state_hash });
}

const helixSource = "G21 G90 G94\nG0 X10 Y0 Z10 A0 C0\nG93\nG2 X10 Y0 Z5 A30 C45 I-10 J0 P3 F2\nM2\n";
const helixLoad = send("PROGRAM_LOAD", "helix-load", { source: helixSource, path: "remap_subs/helix_ac-rtcp.ngc" });
assert.equal(helixLoad.kind, "COMMAND_RESULT");
const helix = helixLoad.payload.preview.segments.find((segment) => segment.motion_type === 2);
assert.ok(helix);
assert.equal(helix.turn, -3);
assert.equal(helix.end.a, 30);
assert.equal(helix.end.c, 45);
assert.equal(helix.end.z, 5);
assert.ok(Number.isFinite(helix.velocity) && helix.velocity > 0);

const recoveryPose = session.xyzacKinematics.forward([20, 5, 30, -90, 45]);
assert.equal(recoveryPose.status, "verified");
const run = send("PROGRAM_RUN", "helix-run");
assert.equal(run.payload.panel_snapshot.kinematics.active_type, 1);
send("SIM_STEP", "helix-step", { cycles: 1 });
const paused = send("PROGRAM_PAUSE", "helix-pause");
assert.equal(paused.payload.panel_snapshot.kinematics.active_type, 1);
const resumed = send("PROGRAM_RESUME", "helix-resume");
assert.equal(resumed.payload.panel_snapshot.kinematics.active_type, 1);
send("SIM_STEP", "helix-step-2", { cycles: 1 });
const aborted = send("PROGRAM_ABORT", "helix-abort");
assert.equal(aborted.payload.panel_snapshot.kinematics.active_type, 1);
const restarted = send("PROGRAM_RESTART", "helix-restart");
assert.equal(restarted.payload.panel_snapshot.kinematics.active_type, 1);

const beforeCheckpoint = session.xyzacKinematics.snapshot();
const checkpoint = send("CHECKPOINT", "rtcp-checkpoint");
assert.equal(checkpoint.kind, "CHECKPOINT_READY");
assert.equal(checkpoint.payload.checkpoint.state.kinematics.state_hash, beforeCheckpoint.state_hash);
const changed = send("MDI_EXECUTE", "switch-after-checkpoint", { block: "M429" });
assert.equal(changed.payload.kinematics.active_type, 0);
close(changed.payload.kinematics.pose.x, beforeCheckpoint.pose.x);
close(changed.payload.kinematics.pose.y, beforeCheckpoint.pose.y);
close(changed.payload.kinematics.pose.z, beforeCheckpoint.pose.z);
const recovered = send("RECOVER", "rtcp-recover", { checkpoint: checkpoint.payload.checkpoint });
assert.equal(recovered.payload.status, "recovered");
assert.equal(recovered.payload.state.kinematics.active_type, 1);
assert.deepEqual(recovered.payload.state.kinematics.pose, beforeCheckpoint.pose);
assert.deepEqual(recovered.payload.state.kinematics.joints, beforeCheckpoint.joints);
assert.equal(recovered.payload.state.kinematics.state_hash, beforeCheckpoint.state_hash);

console.log(JSON.stringify({
  schema: "xyzac-rtcp-08.v1",
  status: "verified",
  case: CASE,
  mapping: { coordinates: initial.coordinates, linear_units: initial.linear_units, angular_units: initial.angular_units, limits: initial.limits },
  identity: { pose: identityForward.pose, state_hash: identityInverse.state_hash },
  trt: { pose: trtForward.pose, joints: trtInverse.joints, pivot_boundary: pivotBoundary.pose, state_hash: trtInverse.state_hash },
  offsets: { base: offset0.pose, changed: offset25.pose, configured: offset25.offsets },
  userk: { native_type: userk.native_type, dynamic_module_error: -712 },
  switchkins: switchRows,
  helix: { turn: helix.turn, end: helix.end, velocity: helix.velocity, motion_type: helix.motion_type },
  boundaries: { range: rangeError.code, nonfinite_joints: nonfiniteJoints.code, nonfinite_pose: nonfinitePose.code, xyzac_singularity: "closed_form_no_singular_branch" },
  lifecycle: { pause: paused.payload.task.state, resume: resumed.payload.task.state, abort: aborted.payload.task.state, restart: restarted.payload.task.program_status, recovery: recovered.payload.status },
  recovery: { active_type: recovered.payload.state.kinematics.active_type, state_hash: recovered.payload.state.kinematics.state_hash },
  first_divergent_tick: null,
}));

for (const value of [kinematics, tool0, tool25]) value.destroy();