import { readFileSync } from "node:fs";
import { fileURLToPath } from "node:url";
import { dirname, resolve } from "node:path";
import assert from "node:assert/strict";

import {
  createLinuxCncKinematicsSdk,
  linuxCncKinematicsWasmFile,
  supportedLinuxCncKinematicsModules,
} from "../../../runtime/sdk/src/index.js";

const EXPECTED_MODULES = [
  "trivkins",
  "5axiskins",
  "xyzac-trt",
  "xyzbc-trt",
  "corexy",
  "rotate",
  "rose",
  "max",
  "lineardelta",
  "rotarydelta",
  "scorbot",
  "tripod",
  "scara",
  "puma",
  "genser",
  "genhex",
  "pentakins",
];

const EXPECTED_META = {
  trivkins: { type: 1, switchable: 0 },
  "5axiskins": { type: 4, switchable: 1 },
  "xyzac-trt": { type: 4, switchable: 1 },
  "xyzbc-trt": { type: 4, switchable: 1 },
  corexy: { type: 4, switchable: 0 },
  rotate: { type: 4, switchable: 0 },
  rose: { type: 4, switchable: 0 },
  max: { type: 4, switchable: 0 },
  lineardelta: { type: 4, switchable: 0 },
  rotarydelta: { type: 4, switchable: 0 },
  scorbot: { type: 4, switchable: 0 },
  tripod: { type: 4, switchable: 0 },
  scara: { type: 4, switchable: 1 },
  puma: { type: 4, switchable: 1 },
  genser: { type: 4, switchable: 1 },
  genhex: { type: 4, switchable: 1 },
  pentakins: { type: 4, switchable: 0 },
};

function near(actual, expected, tolerance = 1e-7) {
  return Math.abs(actual - expected) < tolerance;
}

function assertNear(actual, expected, label, tolerance = 1e-7) {
  assert.equal(near(actual, expected, tolerance), true, `${label}: ${actual} != ${expected}`);
}

function assertArrayNear(actual, expected, label, tolerance = 1e-7) {
  assert.equal(actual.length >= expected.length, true, `${label}: missing values`);
  expected.forEach((value, index) => assertNear(actual[index], value, `${label}[${index}]`, tolerance));
}

function assertPoseNear(actual, expected, label, tolerance = 1e-7) {
  for (const key of ["x", "y", "z", "a", "b", "c", "u", "v", "w"]) {
    assertNear(actual[key] ?? 0, expected[key] ?? 0, `${label}.${key}`, tolerance);
  }
}

function assertProbeBasics(moduleId, probe) {
  assert.equal(probe.includes("kinematics_init=0"), true, `${moduleId}: init probe`);
  assert.equal(
    probe.includes(`kinematics_type=${EXPECTED_META[moduleId].type}`),
    true,
    `${moduleId}: type probe`,
  );
  assert.equal(
    probe.includes(`kinematics_switchable=${EXPECTED_META[moduleId].switchable}`),
    true,
    `${moduleId}: switchable probe`,
  );
}

function assertSwitchIdentity(kins, moduleId, joints, expectedPose) {
  assert.equal(kins.switchKinematics(1), 0, `${moduleId}: switch identity`);
  const identity = kins.forward(joints);
  assert.equal(identity.rc, 0, `${moduleId}: identity forward`);
  assertPoseNear(identity.pose, expectedPose, `${moduleId}: identity pose`);
}

function assertJointRoundtrip(kins, moduleId, joints, jointCount = joints.length, tolerance = 1e-7) {
  if (kins.switchable()) {
    assert.equal(kins.switchKinematics(0), 0, `${moduleId}: switch primary`);
  }
  const forward = kins.forward(joints);
  assert.equal(forward.rc, 0, `${moduleId}: forward`);
  const inverse = kins.inverse(forward.pose, jointCount, { seedJoints: joints });
  assert.equal(inverse.rc, 0, `${moduleId}: inverse`);
  assertArrayNear(inverse.joints, joints.slice(0, jointCount), `${moduleId}: joints`, tolerance);
}

function assertPoseRoundtrip(kins, moduleId, pose, jointCount, options = {}) {
  if (kins.switchable()) {
    assert.equal(kins.switchKinematics(0), 0, `${moduleId}: switch primary`);
  }
  const inverse = kins.inverse(pose, jointCount, options.inverseOptions ?? {});
  assert.equal(inverse.rc, 0, `${moduleId}: inverse`);
  if (options.warmupForward) {
    kins.forward(inverse.joints, { seedPose: pose });
  }
  const forward = kins.forward(inverse.joints, { seedPose: pose });
  assert.equal(forward.rc, 0, `${moduleId}: forward`);
  assertPoseNear(forward.pose, pose, `${moduleId}: pose`, options.tolerance ?? 1e-7);
  return { inverse, forward };
}

const __filename = fileURLToPath(import.meta.url);
const __dirname = dirname(__filename);
const rootDir = resolve(__dirname, "../../..");

assert.deepEqual(supportedLinuxCncKinematicsModules(), EXPECTED_MODULES);

for (const moduleId of supportedLinuxCncKinematicsModules()) {
  const wasmFile = linuxCncKinematicsWasmFile(moduleId);
  const wasmPath = resolve(rootDir, "build/wasm/kinematics", wasmFile);
  const kins = await createLinuxCncKinematicsSdk({
    moduleId,
    moduleOptions: {
      wasmBinary: readFileSync(wasmPath),
      print() {},
      printErr(message) {
        console.error(`[${moduleId}] ${message}`);
      },
    },
  });

  assert.equal(kins.apiName, "linuxcnc-kinematics-wasm-sdk");
  assert.equal(kins.wasmFile, wasmFile);
  for (const functionName of ["lckins_forward", "lckins_inverse", "lckins_run_probe"]) {
    assert.equal(kins.hasWasmFunction(functionName), true, `${moduleId}: ${functionName}`);
  }
  assert.equal(kins.type(), EXPECTED_META[moduleId].type, `${moduleId}: type`);
  assert.equal(kins.switchable(), EXPECTED_META[moduleId].switchable, `${moduleId}: switchable`);
  assertProbeBasics(moduleId, kins.runProbe());

  switch (moduleId) {
    case "trivkins":
      assertJointRoundtrip(kins, moduleId, [1, 2, 3, 4, 5, 6, 7, 8, 9], 9);
      break;
    case "5axiskins":
      assertJointRoundtrip(kins, moduleId, [10, 20, 30, 45, 30, 5], 6);
      assertSwitchIdentity(kins, moduleId, [10, 20, 30, 45, 30, 5], { x: 10, y: 20, z: 30, b: 45, c: 30, w: 5 });
      break;
    case "xyzac-trt":
      assertJointRoundtrip(kins, moduleId, [10, 20, 30, 25, 40], 5);
      assertSwitchIdentity(kins, moduleId, [10, 20, 30, 25, 40], { x: 10, y: 20, z: 30, a: 25, c: 40 });
      break;
    case "xyzbc-trt":
      assertJointRoundtrip(kins, moduleId, [10, 20, 30, 35, 40], 5);
      assertSwitchIdentity(kins, moduleId, [10, 20, 30, 35, 40], { x: 10, y: 20, z: 30, b: 35, c: 40 });
      break;
    case "corexy":
      assertJointRoundtrip(kins, moduleId, [12, 4, 3, 4, 5, 6, 7, 8, 9], 9);
      break;
    case "rotate":
      assertJointRoundtrip(kins, moduleId, [10, 20, 30, 4, 5, 30, 7, 8, 9], 9);
      break;
    case "rose":
      assertJointRoundtrip(kins, moduleId, [12, 5, 30], 3);
      break;
    case "max":
      assertJointRoundtrip(kins, moduleId, [10, 20, 30, 4, 0, 25, 0, 0, 3.5], 9);
      break;
    case "lineardelta":
      assertPoseRoundtrip(kins, moduleId, { x: 10, y: 20, z: -30, a: 1, b: 2, c: 3, u: 4, v: 5, w: 6 }, 9);
      break;
    case "rotarydelta":
      assertPoseRoundtrip(kins, moduleId, { x: 0, y: 0, z: -12, a: 1, b: 2, c: 3, u: 4, v: 5, w: 6 }, 9);
      break;
    case "scorbot": {
      const forward = kins.forward([25, 20, 10, 4, 5]);
      assert.equal(forward.rc, 0, `${moduleId}: forward`);
      const inverse = kins.inverse(forward.pose, 5);
      assert.equal(inverse.rc, 0, `${moduleId}: inverse`);
      const roundtrip = kins.forward(inverse.joints);
      assert.equal(roundtrip.rc, 0, `${moduleId}: roundtrip forward`);
      assertPoseNear(roundtrip.pose, forward.pose, `${moduleId}: pose`);
      break;
    }
    case "tripod":
      assertPoseRoundtrip(kins, moduleId, { x: 0.25, y: 0.25, z: 0.5 }, 3);
      break;
    case "scara":
      assertJointRoundtrip(kins, moduleId, [20, 110, 30, 15, 4, 5], 6);
      assertSwitchIdentity(kins, moduleId, [20, 110, 30, 15, 4, 5], { x: 20, y: 110, z: 30, a: 15, b: 4, c: 5 });
      break;
    case "puma": {
      if (kins.switchable()) {
        assert.equal(kins.switchKinematics(0), 0, `${moduleId}: switch primary`);
      }
      const forward = kins.forward([20, -30, 40, 15, 35, -25]);
      assert.equal(forward.rc, 0, `${moduleId}: forward`);
      const inverse = kins.inverse(forward.pose, 6);
      assert.equal(inverse.rc, 0, `${moduleId}: inverse`);
      const roundtrip = kins.forward(inverse.joints);
      assert.equal(roundtrip.rc, 0, `${moduleId}: roundtrip forward`);
      assertPoseNear(roundtrip.pose, forward.pose, `${moduleId}: pose`);
      assertSwitchIdentity(kins, moduleId, [20, -30, 40, 15, 35, -25], { x: 20, y: -30, z: 40, a: 15, b: 35, c: -25 });
      break;
    }
    case "genser":
      assertJointRoundtrip(kins, moduleId, [10, -15, 20, 5, -10, 12, 7, 8, 9], 9);
      assertSwitchIdentity(kins, moduleId, [10, -15, 20, 5, -10, 12, 7, 8, 9], { x: 10, y: -15, z: 20, a: 5, b: -10, c: 12, u: 7, v: 8, w: 9 });
      break;
    case "genhex":
      assertPoseRoundtrip(kins, moduleId, { x: 1.25, y: -2.5, z: 3.75, a: 1, b: -2, c: 3 }, 6, { warmupForward: true });
      assert.equal(kins.switchKinematics(1), 0, `${moduleId}: switch alternate`);
      break;
    case "pentakins": {
      const { inverse } = assertPoseRoundtrip(kins, moduleId, { x: 12.5, y: -8.25, z: 25, a: 2, b: -1.5 }, 5, { tolerance: 1e-6 });
      assert.equal(inverse.joints.every((value) => value > 0), true, `${moduleId}: positive struts`);
      break;
    }
    default:
      throw new Error(`missing kinematics verification for ${moduleId}`);
  }
}

console.log("kinematics_wasm_node_smoke=ok");