import type { OlpCalibrationRecord, OlpProjectModel, Pose6D } from "./model.js";

export interface CalibrationApplyResult {
  model: OlpProjectModel;
  applied: string[];
}

export function applyCalibration(model: OlpProjectModel, calibrationId: string): CalibrationApplyResult {
  const calibration = model.calibrations.find((candidate) => candidate.id === calibrationId);
  if (!calibration) {
    throw new Error(`Calibration ${calibrationId} not found`);
  }
  const next = clone(model);
  const applied: string[] = [];
  applyToTools(next, calibration, applied);
  applyToFrames(next, calibration, applied);
  applyToExternalAxes(next, calibration, applied);
  if (applied.length === 0) {
    throw new Error(`Calibration ${calibrationId} did not match a supported resource`);
  }
  return { model: next, applied };
}

function applyToTools(model: OlpProjectModel, calibration: OlpCalibrationRecord, applied: string[]): void {
  const tool = model.resources.tools.find((candidate) => candidate.id === calibration.targetResourceId);
  if (!tool || !calibration.poseDelta) {
    return;
  }
  tool.tcp = addPose(tool.tcp, calibration.poseDelta);
  applied.push(tool.id);
}

function applyToFrames(model: OlpProjectModel, calibration: OlpCalibrationRecord, applied: string[]): void {
  const frame = model.resources.frames.find((candidate) => candidate.id === calibration.targetResourceId);
  if (!frame || !calibration.poseDelta) {
    return;
  }
  frame.pose = addPose(frame.pose, calibration.poseDelta);
  applied.push(frame.id);
}

function applyToExternalAxes(model: OlpProjectModel, calibration: OlpCalibrationRecord, applied: string[]): void {
  const axis = model.externalAxes.find((candidate) => candidate.id === calibration.targetResourceId);
  if (!axis || calibration.axisOffset === undefined) {
    return;
  }
  axis.metadata = {
    ...(axis.metadata ?? {}),
    calibrationOffset: calibration.axisOffset
  };
  applied.push(axis.id);
}

function addPose(left: Pose6D, right: Pose6D): Pose6D {
  return [
    left[0] + right[0],
    left[1] + right[1],
    left[2] + right[2],
    left[3] + right[3],
    left[4] + right[4],
    left[5] + right[5]
  ];
}

function clone<T>(value: T): T {
  return JSON.parse(JSON.stringify(value)) as T;
}
