import { fnv1a64 } from "./replay.mjs";
import { parseIni } from "../workspace/ini-parser.mjs";

const STATES = ["IDLE", "START", "UNLOCK", "UNLOCK_WAIT", "INITIAL_BACKOFF_START", "INITIAL_BACKOFF_WAIT", "INITIAL_SEARCH_START", "INITIAL_SEARCH_WAIT", "SET_COARSE_POSITION", "FINAL_BACKOFF_START", "FINAL_BACKOFF_WAIT", "RISE_SEARCH_START", "RISE_SEARCH_WAIT", "FALL_SEARCH_START", "FALL_SEARCH_WAIT", "SET_SWITCH_POSITION", "INDEX_ONLY_START", "INDEX_SEARCH_START", "INDEX_SEARCH_WAIT", "SET_INDEX_POSITION", "FINAL_MOVE_START", "FINAL_MOVE_WAIT", "LOCK", "LOCK_WAIT", "FINISHED", "ABORT"];
export class WasmNativeHoming {
  constructor(core, configuration, kinematics) {
    this.core = core;
    this.kinematics = kinematics;
    this.jointCount = configuration.home_limits.joint_count;
    this.size = core._sim_native_home_size();
    this.pointer = core._malloc(this.size);
    this.buffer = core._malloc(12 * 8);
    if (!this.pointer || !this.buffer) throw new Error("native_home_allocation_failed");
    this.available = true;
    const parsed = parseIni({ entrypoint: configuration.entrypoint, workspace: { read: (path) => {
      const value = configuration.files[path.replace(/^\/workspace\//, "")];
      if (typeof value !== "string") throw new Error(`home_ini_missing:${path}`);
      return value;
    } } });
    this.parameters = Array.from({ length: this.jointCount }, (_, j) => {
      const section = parsed.sections[`JOINT_${j}`] ?? {};
      const n = (key, fallback) => section[key] === undefined ? fallback : Number(section[key]);
      const flags = [["HOME_IGNORE_LIMITS", 1], ["HOME_USE_INDEX", 2], ["HOME_IS_SHARED", 4], ["LOCKING_INDEXER", 8], ["HOME_INDEX_NO_ENCODER_RESET", 128]].reduce((mask, [key, bit]) => mask | (n(key, 0) ? bit : 0), 0);
      const encoder = n("HOME_ABSOLUTE_ENCODER", 0);
      if (![0, 1, 2].includes(encoder)) throw new Error("native_home_absolute_encoder_invalid");
      return [n("HOME_OFFSET", 0), n("HOME", 0), n("HOME_FINAL_VEL", -1), n("HOME_SEARCH_VEL", 0), n("HOME_LATCH_VEL", 0), flags | (encoder ? 16 | 32 | (encoder === 2 ? 64 : 0) : 0), n("HOME_SEQUENCE", 999), n("VOLATILE_HOME", 0), n("MIN_LIMIT", -1e9), n("MAX_LIMIT", 1e9), n("MAX_VELOCITY", 1), n("MAX_ACCELERATION", 1)];
    });
    this.period = configuration.kinematics.servo_cycle_time;
    this.reset();
  }
  invoke(name, ...args) {
    const rc = this.core[`_sim_native_home_${name}`](this.pointer, ...args);
    if (rc !== 0) throw new Error(`native_home_${name}_${rc}`);
  }
  get(j, field) { return this.core._sim_native_home_get(this.pointer, j, field); }
  get active() { return Boolean(this.get(0, 0)); }
  get mode() { return [null, "FREE", "COORD", "TELEOP"][this.get(0, 1)]; }
  setMode(mode) { this.invoke("mode", { FREE: 1, COORD: 2, TELEOP: 3 }[mode]); }
  sync(joints) { this.core.HEAPF64.set(joints, this.buffer / 8); this.invoke("sync", this.buffer); }
  home(seq, joint = null) { this.invoke("start", joint ?? -1, seq); return this.read(); }
  step() { this.invoke("step"); return this.read(); }
  cancel(volatile = false) { this.invoke("cancel", Number(volatile)); return this.read(); }
  reset() {
    this.invoke("init", this.jointCount, this.period);
    this.parameters.forEach((p, j) => { this.core.HEAPF64.set(p, this.buffer / 8); this.invoke("config", j, this.buffer); });
    return this.read();
  }
  check(joints) { if (!this.active) this.sync(joints); return this.read(); }
  read() {
    const joint = Array.from({ length: this.jointCount }, (_, j) => ({
      joint: j, homing: Boolean(this.get(j, 4)), homed: Boolean(this.get(j, 5)),
      home_state: this.get(j, 6), home_state_name: `HOME_${STATES[this.get(j, 6)]}`,
      position: this.get(j, 7), motor_position: this.get(j, 8), motor_offset: this.get(j, 9),
      home_sequence: this.get(j, 10), velocity: this.get(j, 11), index_enable: Boolean(this.get(j, 12)),
    }));
    const joints = joint.map((j) => j.position);
    const result = this.kinematics.forward(joints);
    return { status: result.status, ...(result.code ? { code: result.code } : {}), state: {
      schema: "sim_home_limit_state.v1", source: "linuxcnc-homing.c", joint_count: this.jointCount,
      homed: joint.every((j) => j.homed), homed_mask: joint.reduce((mask, j) => mask | (j.homed ? 1 << j.joint : 0), 0),
      homing: this.active, motion_mode: this.mode, joint, joints, pose: result.pose ?? null,
      home_sequence: this.get(0, 3), tick: this.get(0, 2), last_error: result.result ?? 0, limit_error: result.result === -702 ? -702 : 0, kinematics_error: result.result ?? 0,
    } };
  }
  exportState() {
    // emcmot_joint_t includes large unused compensation tables. Preserve the
    // complete native state while avoiding repeated zero bytes in checkpoints.
    const bytes = this.core.HEAPU8.subarray(this.pointer, this.pointer + this.size);
    const nonzero = [];
    for (let i = 0; i < bytes.length; i++) if (bytes[i] !== 0) nonzero.push([i, bytes[i]]);
    const value = { schema: "native-homing.v1", parameters: this.parameters, period: this.period,
      byte_length: this.size, nonzero_bytes: nonzero };
    return { ...value, state_hash: fnv1a64(value) };
  }
  restoreState(value) {
    const { state_hash, ...unsigned } = value;
    if (value.schema !== "native-homing.v1" || state_hash !== fnv1a64(unsigned) || value.byte_length !== this.size || value.period !== this.period || fnv1a64(value.parameters) !== fnv1a64(this.parameters) || !Array.isArray(value.nonzero_bytes)) throw new Error("native_home_restore_invalid");
    let previous = -1;
    const bytes = new Uint8Array(this.size);
    for (const [offset, byte] of value.nonzero_bytes) {
      if (!Number.isInteger(offset) || offset <= previous || offset >= this.size || !Number.isInteger(byte) || byte < 1 || byte > 255) throw new Error("native_home_restore_bytes_invalid");
      bytes[offset] = byte; previous = offset;
    }
    this.core.HEAPU8.set(bytes, this.pointer);
    return this.read();
  }
  destroy() { this.core._free(this.pointer); this.core._free(this.buffer); }
}