import { fnv1a64 } from "./replay.mjs";

const HOME_LIMIT_SCHEMA = "sim_home_limit_state.v1";
const KIN_TYPE_TRIVKINS = 1;
const KIN_TYPE_XYZAC_TRT = 2;
const KIN_CONFIG_SIZE = 1608;
const KIN_SIZE = 1760;
const HOME_LIMIT_SIZE = 192;
const HOME_STATE_SIZE = 184;
const HOME_STATE_OFFSET = 8;
const CONFIG_OFFSETS = Object.freeze({ type: 0, mode: 4, joint_count: 8, kinematics_joint_count: 12, extra_joint_count: 16, coordinates: 20, x_rot_point: 40, y_offset: 72, z_offset: 80, tool_offset: 88, limit_enabled: 1448, joint_min: 1464, joint_max: 1536 });
const HOME_STATE_OFFSETS = Object.freeze({ pose: 0, joints: 72, joint_count: 144, homed: 148, home_sequence: 152, tick: 156, last_error: 160, limit_error: 164, kinematics_error: 168, homed_mask: 172 });

function call(core, name, returnType, argTypes, args) {
  const direct = core?.[`_${name}`] ?? core?.[name];
  if (typeof direct === "function") return direct(...args);
  if (typeof core?.ccall === "function") return core.ccall(name, returnType, argTypes, args);
  throw new Error(`missing core export: ${name}`);
}

function bytesToHex(bytes) {
  let output = "";
  for (const byte of bytes) output += byte.toString(16).padStart(2, "0");
  return output;
}

function hexToBytes(value) {
  if (typeof value !== "string" || value.length !== HOME_STATE_SIZE * 2 || !/^[0-9a-f]+$/i.test(value)) throw new Error("wasm_home_state_bytes_invalid");
  const bytes = new Uint8Array(HOME_STATE_SIZE);
  for (let index = 0; index < bytes.length; index += 1) bytes[index] = Number.parseInt(value.slice(index * 2, index * 2 + 2), 16);
  return bytes;
}

function pose(heap, offset) {
  const doubles = heap.HEAPF64.subarray(offset / 8, offset / 8 + 9);
  return { x: doubles[0], y: doubles[1], z: doubles[2], a: doubles[3], b: doubles[4], c: doubles[5], u: doubles[6], v: doubles[7], w: doubles[8] };
}

function finitePose(value) {
  return value && [value.x, value.y, value.z, value.a, value.b, value.c, value.u, value.v, value.w].every((entry) => typeof entry === "number" && Number.isFinite(entry));
}

export class WasmHomeLimit {
  constructor(core, { jointCount = 3, limits = {}, kinematicsType = KIN_TYPE_TRIVKINS, coordinates = "XYZ", offsets = {} } = {}) {
    this.core = core;
    this.available = ["sim_kinematics_init", "sim_kinematics_home_limit_init", "sim_kinematics_home_limit_reset", "sim_kinematics_home_limit_home", "sim_kinematics_home_limit_home_joint", "sim_kinematics_home_limit_check", "sim_kinematics_home_limit_read"].every((name) => typeof core?.[`_${name}`] === "function" || typeof core?.[name] === "function");
    this.jointCount = jointCount;
    this.kinematicsType = kinematicsType;
    this.coordinates = String(coordinates).toUpperCase();
    this.offsets = { ...offsets };
    this.config = this.kinematics = this.homeLimit = this.state = this.joints = 0;
    this.stateBuffer = 0;
    this.limits = { x: [-500, 500], y: [-100, 100], z: [-240, 0.001], ...limits };
    if (!this.available || typeof core?._malloc !== "function") return;
    this.config = core._malloc(KIN_CONFIG_SIZE);
    this.kinematics = core._malloc(KIN_SIZE);
    this.homeLimit = core._malloc(HOME_LIMIT_SIZE);
    this.stateBuffer = core._malloc(HOME_STATE_SIZE);
    this.joints = core._malloc(9 * 8);
    if (![this.config, this.kinematics, this.homeLimit, this.stateBuffer, this.joints].every(Boolean)) throw new Error("wasm_home_limit_allocation_failed");
    this.core.HEAPU8.fill(0, this.config, this.config + KIN_CONFIG_SIZE);
    this.core.HEAPU8.fill(0, this.kinematics, this.kinematics + KIN_SIZE);
    this.core.HEAPU8.fill(0, this.homeLimit, this.homeLimit + HOME_LIMIT_SIZE);
    this.core.HEAPU8.fill(0, this.stateBuffer, this.stateBuffer + HOME_STATE_SIZE);
    this.core.HEAPU8.fill(0, this.joints, this.joints + 9 * 8);
    const configView = new DataView(this.core.HEAPU8.buffer, this.config, KIN_CONFIG_SIZE);
    configView.setInt32(CONFIG_OFFSETS.type, kinematicsType, true);
    configView.setInt32(CONFIG_OFFSETS.mode, 0, true);
    configView.setUint32(CONFIG_OFFSETS.joint_count, jointCount, true);
    configView.setUint32(CONFIG_OFFSETS.kinematics_joint_count, jointCount, true);
    configView.setUint32(CONFIG_OFFSETS.extra_joint_count, 0, true);
    for (const [index, axis] of [...this.coordinates].entries()) this.core.HEAPU8[this.config + CONFIG_OFFSETS.coordinates + index] = axis.charCodeAt(0);
    for (const [index, axis] of [...this.coordinates].entries()) {
      const lowerAxis = axis.toLowerCase();
      const limit = this.limits[lowerAxis] ?? [-1e9, 1e9];
      this.core.HEAPU8[this.config + CONFIG_OFFSETS.limit_enabled + index] = 1;
      configView.setFloat64(CONFIG_OFFSETS.joint_min + index * 8, limit[0], true);
      configView.setFloat64(CONFIG_OFFSETS.joint_max + index * 8, limit[1], true);
    }
    for (const [key, offset] of Object.entries({ x_rot_point: CONFIG_OFFSETS.x_rot_point, y_offset: CONFIG_OFFSETS.y_offset, z_offset: CONFIG_OFFSETS.z_offset, tool_offset: CONFIG_OFFSETS.tool_offset })) {
      configView.setFloat64(offset, Number(this.offsets[key] ?? 0), true);
    }
    const initialized = call(core, "sim_kinematics_init", "number", ["number", "number"], [this.kinematics, this.config]);
    if (initialized !== 0) throw new Error(`wasm_home_kinematics_init_${initialized}`);
    const homeInitialized = call(core, "sim_kinematics_home_limit_init", "number", ["number", "number", "number"], [this.homeLimit, this.kinematics, jointCount]);
    if (homeInitialized !== 0) throw new Error(`wasm_home_limit_init_${homeInitialized}`);
  }

  read() {
    if (!this.available) return { status: "blocked", state: { schema: HOME_LIMIT_SCHEMA, source: "wasm_home_limit_unavailable", homed: false, home_sequence: 0, tick: 0, joints: [0, 0, 0], pose: { x: 0, y: 0, z: 0 }, last_error: -840, limit_error: 0, kinematics_error: 0 } };
    const result = call(this.core, "sim_kinematics_home_limit_read", "number", ["number", "number"], [this.homeLimit, this.stateBuffer]);
    if (result !== 0) return { status: "error", code: `wasm_home_limit_read_${result}` };
    const ints = this.core.HEAPU32;
    const signed = this.core.HEAP32;
    const joints = [...this.core.HEAPF64.subarray(this.stateBuffer / 8 + 9, this.stateBuffer / 8 + 9 + this.jointCount)];
    return { status: "verified", state: { schema: HOME_LIMIT_SCHEMA, source: "wasm_home_limit", pose: pose(this.core, this.stateBuffer), joints, joint_count: ints[this.stateBuffer / 4 + 36], homed: Boolean(ints[this.stateBuffer / 4 + 37]), home_sequence: ints[this.stateBuffer / 4 + 38], tick: ints[this.stateBuffer / 4 + 39], last_error: signed[this.stateBuffer / 4 + 40], limit_error: signed[this.stateBuffer / 4 + 41], kinematics_error: signed[this.stateBuffer / 4 + 42], homed_mask: ints[this.stateBuffer / 4 + 43] } };
  }

  home(sequence = 0, axis = null) {
    if (!this.available) return this.read();
    const index = axis === null || axis === undefined ? -1 : Number.isInteger(axis) ? axis : [...this.coordinates].findIndex((value) => value.toLowerCase() === String(axis).toLowerCase());
    if (index >= this.jointCount) return { status: "error", code: "wasm_home_joint_invalid", state: this.read().state };
    if (axis !== null && index < 0) return { status: "error", code: "wasm_home_axis_invalid", state: this.read().state };
    const result = index < 0
      ? call(this.core, "sim_kinematics_home_limit_home", "number", ["number", "number"], [this.homeLimit, sequence])
      : call(this.core, "sim_kinematics_home_limit_home_joint", "number", ["number", "number", "number"], [this.homeLimit, sequence, index]);
    if (result !== 0) return { status: "error", code: `wasm_home_${result}`, state: this.read().state };
    return this.read();
  }

  check(joints) {
    if (!this.available) return this.read();
    if (!Array.isArray(joints) || joints.length < this.jointCount || joints.some((value) => typeof value !== "number" || !Number.isFinite(value))) return { status: "error", code: "wasm_home_joints_invalid" };
    this.core.HEAPF64.set(joints.slice(0, this.jointCount), this.joints / 8);
    const result = call(this.core, "sim_kinematics_home_limit_check", "number", ["number", "number", "number"], [this.homeLimit, this.joints, this.jointCount]);
    return result === 0 ? this.read() : { status: "error", code: `wasm_home_check_${result}`, state: this.read().state };
  }

  reset() {
    if (!this.available) return this.read();
    const result = call(this.core, "sim_kinematics_home_limit_reset", "number", ["number"], [this.homeLimit]);
    if (result !== 0) throw new Error(`wasm_home_limit_reset_${result}`);
    return this.read();
  }

  exportState() {
    const current = this.read();
    const stateBytes = this.available ? bytesToHex(this.core.HEAPU8.slice(this.homeLimit + HOME_STATE_OFFSET, this.homeLimit + HOME_STATE_OFFSET + HOME_STATE_SIZE)) : null;
    const unsigned = { schema: HOME_LIMIT_SCHEMA, available: this.available, joint_count: this.jointCount, state: current.state, state_bytes: stateBytes };
    return { ...unsigned, state_hash: fnv1a64(unsigned) };
  }

  restoreState(value) {
    if (!value || value.schema !== HOME_LIMIT_SCHEMA) throw new Error("wasm_home_restore_schema");
    const unsigned = structuredClone(value);
    delete unsigned.state_hash;
    if (value.state_hash !== fnv1a64(unsigned)) throw new Error("wasm_home_restore_hash");
    if (!this.available || value.available !== true || value.joint_count !== this.jointCount) throw new Error("wasm_home_restore_unavailable");
    const bytes = hexToBytes(value.state_bytes);
    this.core.HEAPU8.set(bytes, this.homeLimit + HOME_STATE_OFFSET);
    const actual = this.read();
    if (actual.status !== "verified" || fnv1a64(actual.state) !== fnv1a64(value.state)) throw new Error("wasm_home_restore_state_mismatch");
    return this.exportState();
  }

  destroy() { for (const pointer of [this.config, this.kinematics, this.homeLimit, this.stateBuffer, this.joints]) if (pointer && typeof this.core?._free === "function") this.core._free(pointer); this.config = this.kinematics = this.homeLimit = this.stateBuffer = this.joints = 0; }
}

export { HOME_LIMIT_SCHEMA };