cnc_wams/web-rtcp-5axis-xyzbc-trt-sim-plan/app/src/runtime/axis-preview-path.js

export const AXIS_PREVIEW_SAMPLE_PERIOD_MS = 20;

const DEFAULT_XYZBC_TOOL = {
  id: 2,
  pocket: 2,
  length: 10,
  diameter: 8,
};

export function buildAxisPreviewPathFromProgram({
  filename = "",
  sourceRel = "",
  content = "",
  tool = DEFAULT_XYZBC_TOOL,
} = {}) {
  const programName = String(filename || sourceRel).split("/").at(-1);
  if (programName !== "xyzbc_switchkins.ngc") return null;
  const params = parseXyzbcSwitchkinsCall(content);
  if (!params) return null;
  const samples = resampleAxisPreviewSegments(
    buildXyzbcSwitchkinsSegments(params),
    AXIS_PREVIEW_SAMPLE_PERIOD_MS,
    tool,
  );
  return {
    source: "web-axis-preview-expanded-ngcgui-subroutines",
    samplePeriodMs: AXIS_PREVIEW_SAMPLE_PERIOD_MS,
    status: samples.length > 0 ? "ok" : "blocked",
    unavailableReason: samples.length > 0 ? null : "web AXIS preview expansion produced no samples",
    program: sourceRel || filename,
    subroutines: ["xyzbc_switchkins_sub.ngc", "helix_bc.ngc"],
    sampleCount: samples.length,
    samples,
    semanticBoundary: "web_axis_preview_path_matching_native_xyzbc_switchkins_ngcgui_expansion",
  };
}

export function parseXyzbcSwitchkinsCall(text = "") {
  const marker = "o<xyzbc_switchkins_sub> call";
  for (const line of String(text).split(/\r?\n/)) {
    if (!line.includes(marker)) continue;
    const values = Array.from(line.matchAll(/\[([^\]]+)\]/g))
      .map((match) => Number(match[1].trim()))
      .filter((value) => Number.isFinite(value));
    if (values.length >= 9) {
      return {
        zmax: values[0],
        zmin: values[1],
        radius: values[2],
        feed: values[3],
        turns: values[4],
        a: values[5],
        b: values[6],
        c: values[7],
        distance: values[8],
      };
    }
  }
  return null;
}

function buildXyzbcSwitchkinsSegments(params) {
  const feed = params.feed;
  const rapid = 2100;
  const zmax = params.zmax;
  const zmin = params.zmin;
  const radius = params.radius;
  const turns = params.turns;
  const bAxis = params.b;
  const cAxis = params.c;
  const distance = params.distance;
  const pose = { x: 0, y: 0, z: zmax, b: 0, c: 0 };
  const segments = [];

  const addLinear = (target, line, motionType = "rapid", activeKinematics = "identity", feedrate = rapid) => {
    const start = { ...pose };
    for (const [key, value] of Object.entries(target)) {
      pose[key] = Number(value);
    }
    segments.push({
      kind: "linear",
      line,
      motionType,
      activeKinematics,
      feed: feedrate,
      start,
      end: { ...pose },
    });
  };

  const addHelix = (line) => {
    const start = { ...pose };
    const center = { x: start.x + radius, y: start.y };
    const end = { ...pose, z: zmin };
    segments.push({
      kind: "helix",
      line,
      motionType: "arc",
      activeKinematics: "tcp-xyzbc",
      feed,
      start,
      end,
      center,
      radius,
      turns,
    });
    Object.assign(pose, end);
  };

  for (const [centerX, centerY, centerLine] of [
    [distance, distance, 18],
    [-distance, distance, 25],
    [-distance, -distance, 32],
    [distance, -distance, 39],
  ]) {
    addLinear({ x: 0, y: 0, z: zmax, b: 0, c: 0 }, centerLine - 2, "rapid", "identity");
    addLinear({ x: centerX, y: centerY, z: zmax }, centerLine, "rapid", "identity");
    addLinear({ x: centerX - radius }, 13, "rapid", "identity");
    addLinear({ b: bAxis, c: cAxis }, 16, "rapid", "tcp-xyzbc");
    addHelix(17);
    addLinear({ x: 0, y: 0, z: zmax, b: 0, c: 0 }, 19, "rapid", "identity");
    addLinear({ x: radius }, 20, "rapid", "identity");
  }
  addLinear({ x: 0, y: 0, z: zmax, b: 0, c: 0 }, 44, "rapid", "identity");
  return segments;
}

function resampleAxisPreviewSegments(segments, samplePeriodMs, tool) {
  const samples = [];
  let timeMs = 0;
  let sampleIndex = 0;
  for (const segment of segments) {
    const durationMs = Math.max(samplePeriodMs, Math.ceil(axisPreviewSegmentDurationMs(segment)));
    const stepCount = Math.max(1, Math.ceil(durationMs / samplePeriodMs));
    for (let step = 0; step < stepCount; step += 1) {
      const ratio = step / stepCount;
      samples.push(pathSampleFromAxisPreviewPose({
        sampleIndex,
        timeMs,
        line: segment.line,
        motionType: segment.motionType,
        activeKinematics: segment.activeKinematics,
        pose: poseOnAxisPreviewSegment(segment, ratio),
        feed: segment.feed,
        tool,
      }));
      sampleIndex += 1;
      timeMs += samplePeriodMs;
    }
  }
  if (segments.length > 0) {
    const last = segments.at(-1);
    samples.push(pathSampleFromAxisPreviewPose({
      sampleIndex,
      timeMs,
      line: last.line,
      motionType: last.motionType,
      activeKinematics: last.activeKinematics,
      pose: poseOnAxisPreviewSegment(last, 1),
      feed: last.feed,
      tool,
    }));
  }
  return samples;
}

function axisPreviewSegmentDurationMs(segment) {
  const distance = segment.kind === "helix"
    ? Math.sqrt((2 * Math.PI * segment.radius * segment.turns) ** 2 + (segment.end.z - segment.start.z) ** 2)
    : Math.sqrt(["x", "y", "z", "b", "c"].reduce((sum, axis) => (
      sum + (segment.end[axis] - segment.start[axis]) ** 2
    ), 0));
  return distance / Math.max(1, numberOrZero(segment.feed)) * 60000;
}

function poseOnAxisPreviewSegment(segment, ratio) {
  const clamped = Math.max(0, Math.min(1, ratio));
  if (segment.kind === "helix") {
    const angle = 2 * Math.PI * segment.turns * clamped;
    return {
      x: segment.center.x - segment.radius * Math.cos(angle),
      y: segment.center.y - segment.radius * Math.sin(angle),
      z: segment.start.z + (segment.end.z - segment.start.z) * clamped,
      b: segment.start.b + (segment.end.b - segment.start.b) * clamped,
      c: segment.start.c + (segment.end.c - segment.start.c) * clamped,
    };
  }
  return Object.fromEntries(["x", "y", "z", "b", "c"].map((axis) => [
    axis,
    segment.start[axis] + (segment.end[axis] - segment.start[axis]) * clamped,
  ]));
}

function pathSampleFromAxisPreviewPose({
  sampleIndex,
  timeMs,
  line,
  motionType,
  activeKinematics,
  pose,
  feed,
  tool,
}) {
  const joint = {
    x: numberOrZero(pose.x),
    y: numberOrZero(pose.y),
    z: numberOrZero(pose.z),
    b: numberOrZero(pose.b),
    c: numberOrZero(pose.c),
  };
  return {
    sampleIndex,
    timeMs,
    line: Number(line) || 0,
    motionType,
    activeKinematics,
    tool,
    joint,
    tcp: {
      x: joint.x,
      y: joint.y,
      z: joint.z,
    },
    toolAxis: toolAxisFromBc(joint.b, joint.c),
    feed: numberOrZero(feed),
    spindle: 0,
  };
}

function toolAxisFromBc(bDeg, cDeg) {
  const b = bDeg * Math.PI / 180;
  const c = cDeg * Math.PI / 180;
  return {
    i: Math.sin(b) * Math.cos(c),
    j: Math.sin(b) * Math.sin(c),
    k: Math.cos(b),
  };
}

function numberOrZero(value) {
  const number = Number(value);
  return Number.isFinite(number) ? number : 0;
}