import assert from "node:assert/strict";

import { getFiveAxisProfile } from "../../app/src/profiles/index.js";
import { createLinuxCncKinematicsRuntime } from "../../app/src/runtime/linuxcnc-kinematics-runtime.js";
import { buildRtcpFrame } from "../../app/src/runtime/rtcp-frame.js";

const DEG_TO_RAD = Math.PI / 180;

const cases = [
  {
    moduleId: "xyzac-trt",
    wasmFile: "linuxcnc_xyzac_trt_kinematics.wasm",
    joints: [10, 20, 30, 25, 40],
    fourthAxis: "A",
    fourthKey: "a",
    axisPose: { x: 10, y: 20, z: 30, a: 25, b: 0, c: 40 },
    kinsType: "tcp-xyzac",
    expectedToolAxis: xyzacToolAxis(25, 40),
  },
  {
    moduleId: "xyzbc-trt",
    wasmFile: "linuxcnc_xyzbc_trt_kinematics.wasm",
    joints: [10, 20, 30, 35, 40],
    fourthAxis: "B",
    fourthKey: "b",
    axisPose: { x: 10, y: 20, z: 30, a: 0, b: 35, c: 40 },
    kinsType: "tcp-xyzbc",
    expectedToolAxis: xyzbcToolAxis(35, 40),
  },
];

for (const testCase of cases) {
  const runtime = await createLinuxCncKinematicsRuntime({
    moduleId: testCase.moduleId,
    switchkinsType: 1,
  });
  const readiness = runtime.readiness();

  assert.equal(runtime.apiName, "web-rtcp-5axis-linuxcnc-kinematics-runtime");
  assert.equal(runtime.moduleId, testCase.moduleId);
  assert.equal(runtime.loaded, true);
  assert.equal(runtime.sourceMode, "source-derived-kinematics-wasm");
  assert.equal(runtime.semanticBoundary, "linuxcnc_kinematics_wasm_c_abi");
  assert.equal(runtime.executionContext, "direct");
  assert.equal(runtime.wasmFile, testCase.wasmFile);
  assert.equal(runtime.switchkinsType, 1);
  assert.equal(readiness.loaded, true);
  assert.equal(readiness.executionContext, "direct");
  assert.equal(readiness.supportedModules.includes("xyzac-trt"), true);
  assert.equal(readiness.supportedModules.includes("xyzbc-trt"), true);
  assert.equal(runtime.switchRc, 0);

  const linuxCncKinematicsResult = runtime.frameForJoints(testCase.joints);
  assert.equal(linuxCncKinematicsResult.moduleId, testCase.moduleId);
  assert.equal(linuxCncKinematicsResult.switchkinsType, 1);
  assert.equal(linuxCncKinematicsResult.forward.rc, 0);
  assert.equal(linuxCncKinematicsResult.inverse.rc, 0);
  assert.deepEqual(roundJoints(linuxCncKinematicsResult.inverse.joints), testCase.joints);

  const frame = buildRtcpFrame({
    axisPose: testCase.axisPose,
    activeLine: 777,
    kinsType: testCase.kinsType,
    rtcpEnabled: true,
    profile: getFiveAxisProfile(testCase.moduleId),
    linuxCncKinematicsResult,
  });

  assert.equal(frame.sourceMode, "source-derived-kinematics-wasm");
  assert.equal(frame.semanticBoundary, "linuxcnc_kinematics_wasm_c_abi");
  assert.equal(frame.readiness.linuxCncKinematicsReady, true);
  assert.equal(frame.readiness.promotionAllowed, true);
  assert.equal(frame.readiness.fullLinuxCncProgramExecutionReady, false);
  assert.equal(frame.profileId, testCase.moduleId);
  assert.equal(frame.kinematicsModuleId, testCase.moduleId);
  assert.equal(frame.kinematicsSwitchkinsType, 1);
  assert.equal(frame.kinematicsForwardRc, 0);
  assert.equal(frame.kinematicsInverseRc, 0);
  assert.equal(frame.jointPose[3].axis, testCase.fourthAxis);
  assert.equal(frame.jointPose[3].value, testCase.joints[3]);
  assert.equal(frame.jointPose[4].axis, "C");
  assert.equal(frame.jointPose[4].value, testCase.joints[4]);
  assert.equal(frame.tcpPose.x, linuxCncKinematicsResult.forward.pose.x);
  assert.equal(frame.tcpPose.y, linuxCncKinematicsResult.forward.pose.y);
  assert.equal(frame.tcpPose.z, linuxCncKinematicsResult.forward.pose.z);
  assert.equal(frame.tcpPose.a, linuxCncKinematicsResult.forward.pose.a);
  assert.equal(frame.tcpPose.b, linuxCncKinematicsResult.forward.pose.b);
  assert.equal(frame.tcpPose.c, linuxCncKinematicsResult.forward.pose.c);
  assert.equal(frame.tcpPose[testCase.fourthKey], testCase.axisPose[testCase.fourthKey]);
  assertVectorNear(frame.toolAxisVector, testCase.expectedToolAxis);

  console.log(`${testCase.moduleId.replace(/-/g, "_")}_kinematics_runtime=ok`);
}

console.log("linuxcnc_kinematics_runtime_smoke=ok");
console.log("xyzbc_b_axis_tcp_pose_preserved=1");

function roundJoints(joints) {
  return joints.map((value) => Math.round(value * 1e6) / 1e6);
}

function xyzacToolAxis(aDegrees, cDegrees) {
  const tilt = aDegrees * DEG_TO_RAD;
  const c = cDegrees * DEG_TO_RAD;
  return {
    x: Math.sin(tilt) * Math.sin(c),
    y: -Math.sin(tilt) * Math.cos(c),
    z: Math.cos(tilt),
  };
}

function xyzbcToolAxis(bDegrees, cDegrees) {
  const tilt = bDegrees * DEG_TO_RAD;
  const c = cDegrees * DEG_TO_RAD;
  return {
    x: Math.sin(tilt) * Math.cos(c),
    y: Math.sin(tilt) * Math.sin(c),
    z: Math.cos(tilt),
  };
}

function assertVectorNear(actual, expected, tolerance = 1e-12) {
  assert.ok(Math.abs(actual.x - expected.x) < tolerance);
  assert.ok(Math.abs(actual.y - expected.y) < tolerance);
  assert.ok(Math.abs(actual.z - expected.z) < tolerance);
}