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

export const AUTHORITATIVE_RUNTIME_SCHEMA = "sim_authoritative_runtime.v1";
const AXES = Object.freeze(["x", "y", "z"]);
const STEP_FRACTION = 0.25;
const KINEMATICS = new Set(["trivkins", "corexy", "tripod", "lineardelta", "rotarydelta"]);
const KINEMATICS_ALIASES = new Map([["corexykins", "corexy"], ["tripodkins", "tripod"], ["lineardeltakins", "lineardelta"], ["rotarydeltakins", "rotarydelta"]]);
const TRIPOD_BASE = Object.freeze([[0, 0], [10, 0], [5, 7.071]]);
const LINEAR_DELTA_RADIUS = 130.25;
const LINEAR_DELTA_ROD = 269;
const ROTARY_DELTA_PLATFORM = 10;
const ROTARY_DELTA_THIGH = 10;
const ROTARY_DELTA_SHIN = 14;
const ROTARY_DELTA_FOOT = 6;
const DEG_TO_RAD = Math.PI / 180;
const RAD_TO_DEG = 180 / Math.PI;

function clone(value) { return structuredClone(value); }
function finite(value) { return typeof value === "number" && Number.isFinite(value); }
function poseFromProgram(program) {
  const state = program?.state;
  if (!state || !AXES.every((axis) => finite(state[axis]))) return null;
  return { x: state.x, y: state.y, z: state.z };
}
function samePose(left, right) { return left && right && AXES.every((axis) => left[axis] === right[axis]); }
function tripodJoints(pose) {
  return TRIPOD_BASE.map(([x, y]) => Math.sqrt((pose.x - x) ** 2 + (pose.y - y) ** 2 + pose.z ** 2));
}
function linearDeltaJoints(pose) {
  const radius = LINEAR_DELTA_RADIUS;
  const rod = LINEAR_DELTA_ROD;
  const bases = [[0, radius], [-Math.sqrt(3) * 0.5 * radius, -0.5 * radius], [Math.sqrt(3) * 0.5 * radius, -0.5 * radius]];
  const joints = bases.map(([x, y]) => {
    const radicand = rod * rod - (x - pose.x) ** 2 - (y - pose.y) ** 2;
    return radicand < -1e-12 ? null : pose.z + Math.sqrt(Math.max(0, radicand));
  });
  return joints.some((value) => value === null || !finite(value)) ? null : joints;
}
function rotaryDeltaJoints(pose) {
  if (Math.abs(pose.z) < 1e-12) return null;
  const solve = (x, y) => {
    const pfr = ROTARY_DELTA_PLATFORM; const tl = ROTARY_DELTA_THIGH; const sl = ROTARY_DELTA_SHIN; const fr = ROTARY_DELTA_FOOT;
    const a = 0.5 * (x * x + (y - fr) ** 2 + pose.z ** 2 + tl ** 2 - sl ** 2 - pfr ** 2) / pose.z;
    const b = (fr - pfr - y) / pose.z;
    const d = tl ** 2 * (b ** 2 + 1) - (a - b * pfr) ** 2;
    if (d < -1e-12) return null;
    const kneeY = (pfr + a * b + Math.sqrt(Math.max(0, d))) / (b ** 2 + 1);
    const kneeZ = b * kneeY - a;
    return Math.atan2(kneeZ, kneeY - pfr) * RAD_TO_DEG;
  };
  const c = Math.cos(-2 * Math.PI / 3); const s = Math.sin(-2 * Math.PI / 3);
  const c2 = Math.cos(2 * Math.PI / 3); const s2 = Math.sin(2 * Math.PI / 3);
  const leg0 = solve(pose.x, pose.y);
  const leg1 = solve(pose.x * c - pose.y * s, pose.x * s + pose.y * c);
  const leg2 = solve(pose.x * c2 - pose.y * s2, pose.x * s2 + pose.y * c2);
  return [leg0, leg1, leg2].some((value) => value === null || !finite(value)) ? null : [leg0, leg1, leg2];
}
function jointsFor(type, pose) {
  if (type === "corexy") return [pose.x + pose.y, pose.x - pose.y, pose.z];
  if (type === "tripod") return tripodJoints(pose);
  if (type === "lineardelta") return linearDeltaJoints(pose);
  if (type === "rotarydelta") return rotaryDeltaJoints(pose);
  return [pose.x, pose.y, pose.z];
}
function mappingError(type, pose) { return type === "rotarydelta" && Math.abs(pose.z) < 1e-12 ? "authoritative_kinematics_singular" : "authoritative_kinematics_range"; }
function canonicalKinematicsType(type) { return KINEMATICS_ALIASES.get(type) ?? type; }

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

  reset() {
    const kinematicsType = this.state?.kinematics?.type ?? "trivkins";
    this.state = {
      schema: AUTHORITATIVE_RUNTIME_SCHEMA,
      tick: 0,
      status: "idle",
      program_status: "idle",
      kinematics: { type: kinematicsType, joint_count: 3, pose: { x: 0, y: 0, z: 0 }, joints: [0, 0, 0], status: 0, blocked: null },
      planner: { next_id: 1, queue: [], active: null, active_id: null, progress: 0, queue_depth: 0, last_error: 0 },
    };
    return this.export();
  }

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

  hash() { return this.export().state_hash; }

  setKinematics(type) {
    const selected = canonicalKinematicsType(type);
    if (!KINEMATICS.has(selected)) {
      this.state.kinematics.blocked = "authoritative_kinematics_unsupported";
      return { status: "blocked", code: "authoritative_kinematics_unsupported", type };
    }
    this.state.kinematics.type = selected;
    this.state.kinematics.blocked = null;
    const mapped = jointsFor(selected, this.state.kinematics.pose);
    if (!mapped && selected === "rotarydelta" && Math.abs(this.state.kinematics.pose.z) < 1e-12) {
      this.state.kinematics.type = selected;
      this.state.kinematics.blocked = null;
      this.state.kinematics.joints = [0, 0, 0];
      return { status: "verified", type: selected };
    }
    if (!mapped) {
      const code = mappingError(selected, this.state.kinematics.pose);
      this.state.kinematics.blocked = code;
      return { status: "blocked", code, type: selected };
    }
    this.state.kinematics.joints = mapped;
    return { status: "verified", type: selected };
  }

  syncProgram(program) {
    const target = poseFromProgram(program);
    if (!target) return { status: "blocked", code: "authoritative_program_pose_missing" };
    this.state.program_status = program.program_status ?? "idle";
    if (this.state.program_status === "aborted") {
      this.state.status = "aborted";
      this.state.planner.queue = [];
      this.state.planner.active_id = null;
      this.state.planner.progress = 0;
      this.state.planner.queue_depth = 0;
      return { status: "verified", command: "AUTHORITATIVE_ABORT" };
    }
    if (this.state.program_status === "completed") {
      this.state.status = "completed";
      return { status: "verified", command: "AUTHORITATIVE_COMPLETE", queue_depth: this.state.planner.queue_depth };
    }
    const planner = this.state.planner;
    const last = planner.queue.at(-1)?.end ?? (planner.active_id === null ? this.state.kinematics.pose : null);
    if (!samePose(target, last)) {
      const id = planner.next_id++;
      planner.queue.push({ id, source_line: program.line ?? null, start: clone(this.state.kinematics.pose), end: target });
    }
    planner.queue_depth = planner.queue.length + (planner.active_id === null ? 0 : 1);
    this.state.status = this.state.program_status === "completed" ? "completed" : "running";
    return { status: "verified", command: "AUTHORITATIVE_SYNC", queue_depth: planner.queue_depth };
  }

  abort() {
    this.state.status = "aborted";
    this.state.program_status = "aborted";
    this.state.planner.queue = [];
    this.state.planner.active = null;
    this.state.planner.active_id = null;
    this.state.planner.progress = 0;
    this.state.planner.queue_depth = 0;
    return this.export();
  }

  step(tick) {
    if (!Number.isSafeInteger(tick) || tick < this.state.tick) return { status: "error", code: "authoritative_tick_regression" };
    const planner = this.state.planner;
    for (let current = this.state.tick + 1; current <= tick; current += 1) {
      if (planner.active_id === null && planner.queue.length > 0) {
        const next = planner.queue.shift();
        planner.active_id = next.id;
        planner.active = next;
        planner.progress = 0;
      }
      if (planner.active_id !== null) {
        planner.progress = Math.min(1, planner.progress + STEP_FRACTION);
        const active = planner.active;
        const pose = {};
        for (const axis of AXES) pose[axis] = active.start[axis] + (active.end[axis] - active.start[axis]) * planner.progress;
        this.state.kinematics.pose = pose;
        const mapped = jointsFor(this.state.kinematics.type, pose);
        if (!mapped) {
          const code = mappingError(this.state.kinematics.type, pose);
          this.state.kinematics.blocked = code;
          return { status: "error", code };
        }
        this.state.kinematics.joints = mapped;
        if (planner.progress >= 1) {
          planner.active_id = null;
          planner.active = null;
          planner.progress = 0;
        }
      }
      planner.queue_depth = planner.queue.length + (planner.active_id === null ? 0 : 1);
      this.state.tick = current;
    }
    return { status: "verified", state: this.export() };
  }

  applyMotion(motion) {
    if (!motion?.state) return;
    const pose = this.state.kinematics.pose;
    motion.state.axes = { ...pose };
    motion.state.joints = [...this.state.kinematics.joints];
    motion.state.tick = this.state.tick;
    motion.state.velocity = this.state.planner.active_id === null ? 0 : STEP_FRACTION;
  }

  restore(value) {
    if (!value || value.schema !== AUTHORITATIVE_RUNTIME_SCHEMA) throw new Error("authoritative_restore_schema");
    const unsigned = clone(value);
    delete unsigned.state_hash;
    if (value.state_hash !== fnv1a64(unsigned)) throw new Error("authoritative_restore_hash");
    if (!KINEMATICS.has(unsigned.kinematics?.type) || unsigned.kinematics?.joint_count !== 3) throw new Error("authoritative_restore_kinematics");
    this.state = unsigned;
    return this.export();
  }
}