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

const POSE_DOUBLES = 9;
const SEGMENT_DOUBLES = POSE_DOUBLES * 2 + 3 + 3 + 2;
const SEGMENT_INT_OFFSET = SEGMENT_DOUBLES * 2;
const QUEUE_HEADER_BYTES = 32;
const STATE_POSITION_BYTES = 72;
const MOTION_QUEUE_SCHEMA = "sim_motion_queue_state.v1";
const DEFAULT_CONTROLS = Object.freeze({ profile: 0, lookahead: 1, acceleration_limit: 1e9, jerk_limit: 1e12, feed_override: 1, paused: false });
const MOTION_ERROR_REASONS = Object.freeze({
  [-600]: "wasm_motion_invalid_argument", [-601]: "wasm_motion_nonfinite_value", [-602]: "wasm_motion_limit_range",
  [-603]: "wasm_motion_invalid_state", [-604]: "wasm_motion_queue_full", [-605]: "wasm_motion_paused",
});

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

function optionalCall(core, name, returnType, argTypes, args) {
  const fn = core[`_${name}`] ?? core[name];
  if (typeof fn === "function") return fn(...args);
  if (typeof core.ccall === "function") {
    try { return core.ccall(name, returnType, argTypes, args); } catch { return null; }
  }
  return null;
}

function poseToArray(pose = {}) {
  return ["x", "y", "z", "a", "b", "c", "u", "v", "w"].map((axis) => Number(pose[axis] ?? 0));
}

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 % 2 !== 0 || !/^[0-9a-f]*$/i.test(value)) throw new Error("wasm_motion_state_bytes_invalid");
  const bytes = new Uint8Array(value.length / 2);
  for (let index = 0; index < bytes.length; index += 1) bytes[index] = Number.parseInt(value.slice(index * 2, index * 2 + 2), 16);
  return bytes;
}

function controls(value = {}) {
  const result = {
    profile: value.profile ?? DEFAULT_CONTROLS.profile,
    lookahead: value.lookahead ?? DEFAULT_CONTROLS.lookahead,
    acceleration_limit: value.acceleration_limit ?? DEFAULT_CONTROLS.acceleration_limit,
    jerk_limit: value.jerk_limit ?? DEFAULT_CONTROLS.jerk_limit,
    feed_override: value.feed_override ?? DEFAULT_CONTROLS.feed_override,
    paused: value.paused ?? DEFAULT_CONTROLS.paused,
  };
  if (![0, 1].includes(result.profile) || !Number.isSafeInteger(result.lookahead) || result.lookahead < 1 || !Number.isFinite(result.acceleration_limit) || result.acceleration_limit <= 0 || !Number.isFinite(result.jerk_limit) || result.jerk_limit <= 0 || !Number.isFinite(result.feed_override) || result.feed_override < 0 || result.feed_override > 2 || typeof result.paused !== "boolean") throw new Error("wasm_motion_controls_invalid");
  return result;
}

export class WasmMotionQueue {
  constructor(core, { cycleTime = 0.01, velocityLimit = 1e9 } = {}) {
    this.core = core;
    this.available = ["sim_motion_size", "sim_motion_command_size", "sim_motion_state_size", "sim_motion_init", "sim_motion_enqueue", "sim_motion_step", "sim_motion_read_state"].every((name) => typeof core?.[`_${name}`] === "function" || typeof core?.[name] === "function");
    this.queue = 0;
    this.command = 0;
    this.state = 0;
    this.commandCapacity = 0;
    this.stateCapacity = 0;
    this.queueSize = 0;
    this.queueCapacity = 0;
    this.cycleTime = cycleTime;
    this.velocityLimit = velocityLimit;
    this.controls = controls();
    if (!this.available) return;
    const queueSize = call(core, "sim_motion_size", "number", [], []);
    this.queueSize = queueSize;
    this.commandCapacity = call(core, "sim_motion_command_size", "number", [], []);
    this.stateCapacity = call(core, "sim_motion_state_size", "number", [], []);
    if (![queueSize, this.commandCapacity, this.stateCapacity].every((value) => Number.isSafeInteger(value) && value > 0)) throw new Error("wasm_motion_queue_size_invalid");
    this.queueCapacity = (queueSize - QUEUE_HEADER_BYTES - this.stateCapacity) / this.commandCapacity;
    if (!Number.isSafeInteger(this.queueCapacity) || this.queueCapacity < 1) throw new Error("wasm_motion_queue_capacity_invalid");
    this.queue = core._malloc(queueSize);
    this.command = core._malloc(this.commandCapacity);
    this.state = core._malloc(this.stateCapacity);
    if (!this.queue || !this.command || !this.state) throw new Error("wasm_motion_queue_allocation_failed");
    this.core.HEAPU8.fill(0, this.queue, this.queue + queueSize);
    const result = call(core, "sim_motion_init", "number", ["number", "number", "number"], [this.queue, cycleTime, velocityLimit]);
    if (result !== 0) throw new Error(`wasm_motion_init_${result}`);
  }

  enqueue(segment) {
    if (!this.available) return { status: "skipped", code: "wasm_motion_unavailable" };
    if (!segment) return { status: "skipped" };
    const heap = this.core.HEAPU8;
    heap.fill(0, this.command, this.command + this.commandCapacity);
    const doubles = this.core.HEAPF64;
    const ints = this.core.HEAP32;
    const base = this.command / 8;
    [...poseToArray(segment.start), ...poseToArray(segment.end), Number(segment.center?.x ?? 0), Number(segment.center?.y ?? 0), Number(segment.center?.z ?? 0), Number(segment.normal?.x ?? 0), Number(segment.normal?.y ?? 0), Number(segment.normal?.z ?? 0), Number(segment.velocity ?? 0), Number(segment.acceleration ?? 0)].forEach((value, index) => { doubles[base + index] = value; });
    ints[this.command / 4 + SEGMENT_INT_OFFSET] = Number(segment.motion_type ?? 1);
    ints[this.command / 4 + SEGMENT_INT_OFFSET + 1] = Number(segment.turn ?? 0);
    // sim_motion_command layout is segment (216 bytes), id (4), tag (4), flags (4).
    // Keep the WASM switchkins mode in the tag field; the C queue assigns id.
    ints[this.command / 4 + SEGMENT_INT_OFFSET + 3] = Number(segment.kinematics_mode ?? segment.tag ?? 0);
    ints[this.command / 4 + SEGMENT_INT_OFFSET + 4] = segment.kind === "rapid" ? 1 : 0;
    const result = call(this.core, "sim_motion_enqueue", "number", ["number", "number", "number"], [this.queue, this.command, 0]);
    return result === 0 ? { status: "verified", source_line: segment.source_line ?? null } : { status: "error", code: `wasm_motion_enqueue_${result}`, reason: MOTION_ERROR_REASONS[result] ?? `wasm_motion_enqueue_${result}`, message_key: MOTION_ERROR_REASONS[result] ?? `wasm_motion_enqueue_${result}`, source: "linuxcnc-wasm-motion" };
  }

  reset() {
    if (!this.available) return this.read();
    const previousFeedOverride = this.controls.feed_override;
    this.core.HEAPU8.fill(0, this.queue, this.queue + this.queueSize);
    const result = call(this.core, "sim_motion_init", "number", ["number", "number", "number"], [this.queue, this.cycleTime, this.velocityLimit]);
    if (result !== 0) throw new Error(`wasm_motion_reset_${result}`);
    this.controls = controls();
    this.setFeedOverride(previousFeedOverride);
    return this.read();
  }

  setProfile(profile) {
    const next = controls({ ...this.controls, profile });
    if (!this.available) { this.controls = next; return this.read(); }
    const result = optionalCall(this.core, "sim_motion_set_profile", "number", ["number", "number"], [this.queue, next.profile]);
    if (result !== null && result !== 0) throw new Error(`wasm_motion_profile_${result}`);
    this.controls = next;
    return this.read();
  }

  setLookahead(lookahead) {
    const next = controls({ ...this.controls, lookahead });
    if (!this.available) { this.controls = next; return this.read(); }
    const result = optionalCall(this.core, "sim_motion_set_lookahead", "number", ["number", "number"], [this.queue, next.lookahead]);
    if (result !== null && result !== 0) throw new Error(`wasm_motion_lookahead_${result}`);
    this.controls = next;
    return this.read();
  }

  setDynamics(accelerationLimit, jerkLimit) {
    const next = controls({ ...this.controls, acceleration_limit: accelerationLimit, jerk_limit: jerkLimit });
    if (!this.available) { this.controls = next; return this.read(); }
    const result = optionalCall(this.core, "sim_motion_set_dynamics", "number", ["number", "number", "number"], [this.queue, next.acceleration_limit, next.jerk_limit]);
    if (result !== null && result !== 0) throw new Error(`wasm_motion_dynamics_${result}`);
    this.controls = next;
    return this.read();
  }

  setFeedOverride(feedOverride) {
    const next = controls({ ...this.controls, feed_override: feedOverride });
    if (!this.available) { this.controls = next; return this.read(); }
    const result = optionalCall(this.core, "sim_motion_set_feed_override", "number", ["number", "number"], [this.queue, next.feed_override]);
    if (result !== null && result !== 0) throw new Error(`wasm_motion_feed_override_${result}`);
    this.controls = next;
    return this.read();
  }

  setPaused(paused) {
    const next = controls({ ...this.controls, paused: Boolean(paused) });
    if (!this.available) { this.controls = next; return this.read(); }
    const result = optionalCall(this.core, "sim_motion_set_paused", "number", ["number", "number"], [this.queue, next.paused ? 1 : 0]);
    if (result !== null && result !== 0) throw new Error(`wasm_motion_pause_${result}`);
    this.controls = next;
    return this.read();
  }

  abort() {
    if (!this.available) return this.read();
    const current = this.read();
    if (current.status !== "verified") throw new Error("wasm_motion_abort_state");
    const previous = this.core.HEAPU8.slice(this.queue, this.queue + this.queueSize);
    const previousFeedOverride = this.controls.feed_override;
    const headerOffset = this.queueCapacity * this.commandCapacity;
    const stateOffset = headerOffset + QUEUE_HEADER_BYTES;
    const previousView = new DataView(previous.buffer, previous.byteOffset, previous.byteLength);
    const nextId = previousView.getUint32(headerOffset + 8, true);
    this.reset();
    this.setFeedOverride(previousFeedOverride);
    const bytes = new Uint8Array(this.queueSize);
    const view = new DataView(bytes.buffer);
    // sim_motion_queue stores commands first, followed by its 32-byte header.
    // Preserve cycle/velocity configuration from that actual header offset.
    bytes.set(previous.subarray(headerOffset + 16, headerOffset + QUEUE_HEADER_BYTES), headerOffset + 16);
    view.setUint32(headerOffset + 8, nextId, true);
    view.setUint32(headerOffset + 12, current.state.tick, true);
    bytes.set(previous.subarray(stateOffset, stateOffset + STATE_POSITION_BYTES), stateOffset);
    view.setFloat64(stateOffset + 72, 0, true);
    view.setFloat64(stateOffset + 80, 0, true);
    view.setUint32(stateOffset + 88, current.state.tick, true);
    view.setUint32(stateOffset + 92, 0, true);
    view.setUint32(stateOffset + 96, 0, true);
    view.setUint32(stateOffset + 100, 0, true);
    view.setInt32(stateOffset + 104, 0, true);
    view.setInt32(stateOffset + 108, 0, true);
    this.core.HEAPU8.set(bytes, this.queue);
    return this.read();
  }

  step() {
    if (!this.available) return { status: "error", code: "wasm_motion_step_-605" };
    if (this.controls.paused) return { status: "error", code: "wasm_motion_step_-605" };
    const result = call(this.core, "sim_motion_step", "number", ["number"], [this.queue]);
    if (result !== 0) return { status: "error", code: `wasm_motion_step_${result}`, reason: MOTION_ERROR_REASONS[result] ?? `wasm_motion_step_${result}`, message_key: MOTION_ERROR_REASONS[result] ?? `wasm_motion_step_${result}`, source: "linuxcnc-wasm-motion" };
    return this.read();
  }

  read() {
    if (!this.available) return { status: "blocked", state: { position: { x: 0, y: 0, z: 0 }, velocity: 0, progress: 0, tick: 0, queue_depth: 0, active_id: 0, active_tag: 0, motion_type: 0, status: -605, source: "wasm_motion_queue_unavailable" } };
    const result = call(this.core, "sim_motion_read_state", "number", ["number", "number"], [this.queue, this.state]);
    if (result !== 0) return { status: "error", code: `wasm_motion_read_${result}` };
    const doubles = this.core.HEAPF64.subarray(this.state / 8, this.state / 8 + 11);
    const ints = this.core.HEAPU32.subarray(this.state / 4 + 22, this.state / 4 + 26);
    return { status: "verified", state: { position: { 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] }, velocity: this.controls.paused ? 0 : doubles[9], progress: doubles[10], tick: ints[0], queue_depth: ints[1], active_id: ints[2], active_tag: ints[3], motion_type: this.core.HEAP32[this.state / 4 + 26], status: this.core.HEAP32[this.state / 4 + 27], source: "wasm_motion_queue" } };
  }

  exportState() {
    if (!this.available) {
      const unsigned = { schema: MOTION_QUEUE_SCHEMA, available: false, controls: { ...this.controls }, state: this.read().state };
      return { ...unsigned, state_hash: fnv1a64(unsigned) };
    }
    const current = this.read();
    if (current.status !== "verified") throw new Error("wasm_motion_state_unavailable");
    const unsigned = {
      schema: MOTION_QUEUE_SCHEMA,
      queue_size: this.queueSize,
      command_size: this.commandCapacity,
      state_size: this.stateCapacity,
      queue_capacity: this.queueCapacity,
      cycle_time: this.cycleTime,
      velocity_limit: this.velocityLimit,
      controls: { ...this.controls },
      state: current.state,
      bytes: bytesToHex(this.core.HEAPU8.slice(this.queue, this.queue + this.queueSize)),
    };
    return { ...unsigned, state_hash: fnv1a64(unsigned) };
  }

  restoreState(value) {
    if (!this.available) throw new Error("wasm_motion_restore_unavailable");
    if (!value || value.schema !== MOTION_QUEUE_SCHEMA) throw new Error("wasm_motion_restore_schema");
    const unsigned = structuredClone(value);
    delete unsigned.state_hash;
    if (value.state_hash !== fnv1a64(unsigned)) throw new Error("wasm_motion_restore_hash");
    if (value.queue_size !== this.queueSize || value.command_size !== this.commandCapacity || value.state_size !== this.stateCapacity || value.queue_capacity !== this.queueCapacity || value.cycle_time !== this.cycleTime || value.velocity_limit !== this.velocityLimit) throw new Error("wasm_motion_restore_layout");
    const bytes = hexToBytes(value.bytes);
    if (bytes.length !== this.queueSize) throw new Error("wasm_motion_restore_bytes");
    const nextControls = controls(value.controls);
    this.core.HEAPU8.fill(0, this.queue, this.queue + this.queueSize);
    const initialized = call(this.core, "sim_motion_init", "number", ["number", "number", "number"], [this.queue, this.cycleTime, this.velocityLimit]);
    if (initialized !== 0) throw new Error(`wasm_motion_restore_init_${initialized}`);
    this.controls = controls();
    this.setProfile(nextControls.profile);
    this.setLookahead(nextControls.lookahead);
    this.setDynamics(nextControls.acceleration_limit, nextControls.jerk_limit);
    this.setFeedOverride(nextControls.feed_override);
    this.setPaused(nextControls.paused);
    this.core.HEAPU8.set(bytes, this.queue);
    const actual = this.read();
    if (actual.status !== "verified" || fnv1a64(actual.state) !== fnv1a64(value.state)) throw new Error("wasm_motion_restore_state_mismatch");
    return this.exportState();
  }

  destroy() { for (const pointer of [this.queue, this.command, this.state]) if (pointer && typeof this.core._free === "function") this.core._free(pointer); this.queue = this.command = this.state = 0; }
}