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

export const MOTION_SCHEMA = "sim_motion.v1";
const AXES = Object.freeze(["x", "y", "z"]);
const MAX_POSITION = 1e9;
const MAX_SPEED = 1e6;
const CYCLE_DISTANCE = 0.001;

function clone(value) { return structuredClone(value); }
function finite(value) { return typeof value === "number" && Number.isFinite(value); }
function axis(value) { return typeof value === "string" && AXES.includes(value.toLowerCase()) ? value.toLowerCase() : null; }

export class MotionState {
  constructor() { this.reset(); }

  reset() {
    this.state = {
      schema: MOTION_SCHEMA,
      tick: 0,
      axes: { x: 0, y: 0, z: 0 },
      joints: [0, 0, 0],
      velocity: 0,
      feed_override: 1,
      spindle_override: 1,
      homed: false,
      limits: { x: false, y: false, z: false },
      following_error: 0,
      jog: null,
      estop: false,
      fault: null,
    };
    return this.export();
  }

  export() { const value = clone(this.state); return { ...value, state_hash: fnv1a64(value) }; }
  hash() { return this.export().state_hash; }

  fail(code, detail = null) { return { status: "error", code, ...(detail ? { detail } : {}), state: this.export() }; }
  ok(command) { return { status: "verified", command, state: this.export() }; }

  home(target = null) {
    if (this.state.estop) return this.fail("motion_estop_active");
    if (target !== null && !axis(target)) return this.fail("motion_axis_invalid");
    const targets = target === null ? AXES : [axis(target)];
    for (const name of targets) { this.state.axes[name] = 0; this.state.joints[AXES.indexOf(name)] = 0; this.state.limits[name] = false; }
    this.state.homed = true;
    this.state.fault = null;
    this.state.following_error = 0;
    this.state.tick += 1;
    return this.ok("HOME");
  }

  setOverride(feed, spindle = this.state.spindle_override) {
    if (!finite(feed) || feed < 0 || feed > 2 || !finite(spindle) || spindle < 0 || spindle > 2) return this.fail("motion_override_range");
    this.state.feed_override = feed;
    this.state.spindle_override = spindle;
    return this.ok("SET_OVERRIDE");
  }

  startJog({ axis: requestedAxis, direction = 1, speed = 1 } = {}) {
    const name = axis(requestedAxis);
    if (!name) return this.fail("motion_axis_invalid");
    if (!this.state.homed) return this.fail("motion_unhomed");
    if (this.state.estop || this.state.fault) return this.fail("motion_faulted");
    if (!Number.isSafeInteger(direction) || ![-1, 1].includes(direction) || !finite(speed) || speed <= 0 || speed > MAX_SPEED) return this.fail("motion_jog_range");
    this.state.jog = { axis: name, direction, speed };
    this.state.velocity = speed * this.state.feed_override;
    return this.ok("JOG_START");
  }

  stopJog() {
    this.state.jog = null;
    this.state.velocity = 0;
    return this.ok("JOG_STOP");
  }

  setLimit(requestedAxis, active) {
    const name = axis(requestedAxis);
    if (!name || typeof active !== "boolean") return this.fail("motion_limit_argument");
    this.state.limits[name] = active;
    if (active && this.state.jog?.axis === name) this.stopJog();
    if (active) this.state.fault = "limit";
    else if (this.state.fault === "limit") this.state.fault = null;
    return this.ok("LIMIT_SET");
  }

  setFollowingError(value) {
    if (!finite(value) || Math.abs(value) > MAX_POSITION) return this.fail("motion_following_error_nonfinite");
    this.state.following_error = value;
    if (Math.abs(value) > 0.01) { this.state.fault = "following_error"; this.state.jog = null; this.state.velocity = 0; }
    return this.ok("FOLLOWING_ERROR");
  }

  estop(active) {
    if (typeof active !== "boolean") return this.fail("motion_estop_argument");
    this.state.estop = active;
    if (active) { this.state.jog = null; this.state.velocity = 0; this.state.fault = "estop"; }
    else if (this.state.fault === "estop") this.state.fault = null;
    return this.ok("ESTOP");
  }

  step(tick) {
    if (!Number.isSafeInteger(tick) || tick < this.state.tick) return this.fail("motion_tick_regression");
    this.state.tick = tick;
    if (!this.state.jog || this.state.estop || this.state.fault) { this.state.velocity = 0; return this.export(); }
    const { axis: name, direction, speed } = this.state.jog;
    if (this.state.limits[name]) { this.state.fault = "limit"; this.state.jog = null; this.state.velocity = 0; return this.export(); }
    const delta = direction * speed * this.state.feed_override * CYCLE_DISTANCE;
    const next = this.state.axes[name] + delta;
    if (!finite(next) || Math.abs(next) > MAX_POSITION) { this.state.fault = "position_range"; this.state.jog = null; this.state.velocity = 0; return this.export(); }
    this.state.axes[name] = next;
    this.state.joints[AXES.indexOf(name)] = next;
    this.state.velocity = Math.abs(speed * this.state.feed_override);
    return this.export();
  }

  restore(value) {
    if (!value || value.schema !== MOTION_SCHEMA) throw new Error("motion_restore_schema");
    const unsigned = { ...value }; delete unsigned.state_hash;
    if (value.state_hash !== fnv1a64(unsigned)) throw new Error("motion_restore_hash");
    if (!finite(value.tick) || !value.axes || !Array.isArray(value.joints) || value.joints.length !== 3) throw new Error("motion_restore_shape");
    this.state = clone(unsigned);
    return this.export();
  }
}

export { AXES };