"use strict";

Object.defineProperty(exports, "__esModule", {
  value: true
});
exports.registerWaypointRpc = exports.hasWaypointBeenUpdated = exports.markWaypointUpdated = void 0;
const webscope_service_1 = require("../webscope-service");
const updatedWaypointNameSet = new Set();
const markWaypointUpdated = name => {
  if (typeof name !== 'string') {
    return;
  }
  const normalized = name.trim().toLowerCase();
  if (!normalized) {
    return;
  }
  updatedWaypointNameSet.add(normalized);
};
exports.markWaypointUpdated = markWaypointUpdated;
const hasWaypointBeenUpdated = name => {
  if (typeof name !== 'string') {
    return false;
  }
  const normalized = name.trim().toLowerCase();
  if (!normalized) {
    return false;
  }
  return updatedWaypointNameSet.has(normalized);
};
exports.hasWaypointBeenUpdated = hasWaypointBeenUpdated;
const toNumberArray = value => {
  if (!Array.isArray(value)) {
    return [];
  }
  return value.map(item => Number(item !== null && item !== void 0 ? item : 0));
};
const getPayloadField = (payload, key, fallback) => {
  if (!payload || typeof payload !== 'object') {
    return fallback;
  }
  const raw = payload[key];
  if (raw === undefined || raw === null) {
    return fallback;
  }
  return raw;
};
const isWaypointLike = value => {
  return typeof value === 'object' && value !== null && typeof value.getPose === 'function' && typeof value.getJointPositions === 'function' && typeof value.getTcpOffset === 'function';
};
const getWaypointName = waypoint => {
  if (!waypoint || typeof waypoint !== 'object') {
    return '';
  }
  const waypointLike = waypoint;
  if (typeof waypointLike.name === 'string' && waypointLike.name.trim().length > 0) {
    return waypointLike.name;
  }
  if (typeof waypointLike.getDisplayName === 'function') {
    const displayName = waypointLike.getDisplayName();
    if (typeof displayName === 'string' && displayName.trim().length > 0) {
      return displayName;
    }
  }
  if (typeof waypointLike.getName === 'function') {
    const name = waypointLike.getName();
    if (typeof name === 'string' && name.trim().length > 0) {
      return name;
    }
  }
  return '';
};
const serializeWaypoint = waypoint => {
  if (!isWaypointLike(waypoint)) {
    throw new Error('Invalid waypoint data');
  }
  const name = getWaypointName(waypoint);
  if (!name) {
    throw new Error('Waypoint name is empty');
  }
  return {
    name,
    pose: toNumberArray(waypoint.getPose()),
    joint: toNumberArray(waypoint.getJointPositions()),
    offset: toNumberArray(waypoint.getTcpOffset())
  };
};
const hasNonZeroValues = values => {
  return values.some(item => Math.abs(Number(item !== null && item !== void 0 ? item : 0)) > 1e-9);
};
const isWaypointUnset = waypoint => {
  const serialized = serializeWaypoint(waypoint);
  return !hasNonZeroValues(serialized.pose) && !hasNonZeroValues(serialized.joint) && !hasNonZeroValues(serialized.offset);
};
const serializeWaypointForVisionInspectionGet = waypoint => {
  return serializeWaypoint(waypoint);
};
const serializeWaypointWithEmptyWhenUnset = waypoint => {
  const serialized = serializeWaypoint(waypoint);
  const hasDefined = hasNonZeroValues(serialized.pose) || hasNonZeroValues(serialized.joint) || hasNonZeroValues(serialized.offset);
  if (hasDefined) {
    return serialized;
  }
  return {
    ...serialized,
    pose: [],
    joint: [],
    offset: []
  };
};
const getWaypointModelOrThrow = () => {
  const webscopeService = (0, webscope_service_1.getWebscopeService)();
  if (!webscopeService) {
    throw new Error('Webscope service is not ready');
  }
  return webscopeService.getWaypointModel();
};
const assertName = name => {
  if (typeof name !== 'string' || name.trim().length === 0) {
    throw new Error('Waypoint name is required');
  }
  return name;
};
const getNameFromPayload = payload => {
  if (typeof payload === 'string') {
    return payload;
  }
  const directName = getPayloadField(payload, 'name', getPayloadField(payload, 'waypointName', ''));
  if (typeof directName === 'string' && directName.trim().length > 0) {
    return directName;
  }
  return getPayloadField(payload, 'oldName', getPayloadField(payload, 'value', ''));
};
const getNewNameFromPayload = payload => {
  if (!payload || typeof payload !== 'object') {
    return '';
  }
  return getPayloadField(payload, 'newName', getPayloadField(payload, 'targetName', getPayloadField(payload, 'waypointName', '')));
};
const getPoseFromPayload = payload => {
  return toNumberArray(getPayloadField(payload, 'pose', getPayloadField(payload, 'position', [])));
};
const getJointFromPayload = payload => {
  return toNumberArray(getPayloadField(payload, 'joint', getPayloadField(payload, 'joints', [])));
};
const getOffsetFromPayload = payload => {
  return toNumberArray(getPayloadField(payload, 'offset', getPayloadField(payload, 'tcpOffset', [])));
};
const arraysAlmostEqual = (a, b, epsilon = 1e-9) => {
  var _a, _b;
  if (a.length !== b.length) {
    return false;
  }
  for (let i = 0; i < a.length; i += 1) {
    if (Math.abs(Number((_a = a[i]) !== null && _a !== void 0 ? _a : 0) - Number((_b = b[i]) !== null && _b !== void 0 ? _b : 0)) > epsilon) {
      return false;
    }
  }
  return true;
};
const tryWriteWaypointPose = (waypoint, pose, joint, offset) => {
  waypoint.setPose(pose, joint, offset);
  const after = serializeWaypoint(waypoint);
  return arraysAlmostEqual(after.pose, pose) && arraysAlmostEqual(after.joint, joint) && arraysAlmostEqual(after.offset, offset);
};
const forceRewriteWaypointByName = (waypointModel, name, pose, joint, offset) => {
  try {
    waypointModel.removeWaypoint(name);
  } catch (error) {}
  const created = waypointModel.addWaypoint(name, pose, joint, offset);
  const createdName = getWaypointName(created);
  if (createdName && createdName !== name) {
    try {
      waypointModel.renameWaypoint(createdName, name);
    } catch (error) {}
  }
  return resolveWaypoint(name);
};
const writeWaypointPoseWithFallback = (waypointModel, waypoint, pose, joint, offset) => {
  const waypointName = getWaypointName(waypoint);
  if (!waypointName) {
    return waypoint;
  }
  const written = tryWriteWaypointPose(waypoint, pose, joint, offset);
  if (written) {
    return waypoint;
  }
  return forceRewriteWaypointByName(waypointModel, waypointName, pose, joint, offset);
};
const resolveWaypoint = name => {
  try {
    const waypoint = getWaypointModelOrThrow().getByName(name);
    if (!isWaypointLike(waypoint)) {
      throw new Error('Waypoint not found');
    }
    return waypoint;
  } catch (error) {
    throw new Error(`Waypoint not found: ${name}`);
  }
};
const findWaypoint = name => {
  try {
    return resolveWaypoint(name);
  } catch (error) {
    return null;
  }
};
const parseAddArgs = args => {
  if (args.length >= 3) {
    const preferredName = typeof args[3] === 'string' ? args[3].trim() : '';
    return {
      pose: toNumberArray(args[0]),
      joint: toNumberArray(args[1]),
      offset: toNumberArray(args[2]),
      preferredName
    };
  }
  const payload = args[0];
  return {
    pose: getPoseFromPayload(payload),
    joint: getJointFromPayload(payload),
    offset: getOffsetFromPayload(payload),
    preferredName: getPayloadField(payload, 'name', getPayloadField(payload, 'waypointName', ''))
  };
};
const parseUpdateArgs = args => {
  if (args.length >= 4) {
    return {
      name: assertName(args[0]),
      pose: toNumberArray(args[1]),
      joint: toNumberArray(args[2]),
      offset: toNumberArray(args[3])
    };
  }
  const payload = args[0];
  return {
    name: assertName(getNameFromPayload(payload)),
    pose: getPoseFromPayload(payload),
    joint: getJointFromPayload(payload),
    offset: getOffsetFromPayload(payload)
  };
};
const getNextIndexedWaypointName = (waypointModel, baseName) => {
  const escapedBase = baseName.replace(/[.*+?^${}()|[\]\\]/g, '\\$&');
  const namePattern = new RegExp(`^${escapedBase}(\\d+)$`);
  let maxIndex = 0;
  try {
    const all = typeof (waypointModel === null || waypointModel === void 0 ? void 0 : waypointModel.getWaypoints) === 'function' ? waypointModel.getWaypoints() : [];
    for (const waypoint of all || []) {
      const rawName = getWaypointName(waypoint);
      if (!rawName) {
        continue;
      }
      const matched = rawName.match(namePattern);
      if (!matched) {
        continue;
      }
      const index = Number(matched[1]);
      if (Number.isFinite(index) && index > maxIndex) {
        maxIndex = index;
      }
    }
  } catch (error) {}
  return `${baseName}${maxIndex + 1}`;
};
const getFirstUnsetIndexedWaypointName = (waypointModel, baseName) => {
  const escapedBase = baseName.replace(/[.*+?^${}()|[\]\\]/g, '\\$&');
  const namePattern = new RegExp(`^${escapedBase}(\\d+)$`);
  let bestIndex = Number.POSITIVE_INFINITY;
  let bestName = '';
  try {
    const all = typeof (waypointModel === null || waypointModel === void 0 ? void 0 : waypointModel.getWaypoints) === 'function' ? waypointModel.getWaypoints() : [];
    for (const waypoint of all || []) {
      const rawName = getWaypointName(waypoint);
      if (!rawName) {
        continue;
      }
      const matched = rawName.match(namePattern);
      if (!matched) {
        continue;
      }
      const index = Number(matched[1]);
      if (!Number.isFinite(index) || index <= 0 || index >= bestIndex) {
        continue;
      }
      if (!isWaypointLike(waypoint)) {
        continue;
      }
      if (!isWaypointUnset(waypoint)) {
        continue;
      }
      bestIndex = index;
      bestName = rawName;
    }
  } catch (error) {}
  return bestName;
};
const isVisionPickWaypointName = name => {
  return /^(?:cut_waypoint\d+|center_bottom_pose\d+|corner_pose_\d+)$/.test(name);
};
const getFirstUnsetVisionPickWaypointName = waypointModel => {
  let bestRank = Number.POSITIVE_INFINITY;
  let bestName = '';
  const getRank = name => {
    const cutMatched = name.match(/^cut_waypoint(\d+)$/);
    if (cutMatched) {
      return Number(cutMatched[1]) * 10;
    }
    const centerMatched = name.match(/^center_bottom_pose(\d+)$/);
    if (centerMatched) {
      return Number(centerMatched[1]) * 10 + 1;
    }
    const cornerMatched = name.match(/^corner_pose_(\d+)$/);
    if (cornerMatched) {
      return Number(cornerMatched[1]) * 10 + 2;
    }
    return Number.POSITIVE_INFINITY;
  };
  try {
    const all = typeof (waypointModel === null || waypointModel === void 0 ? void 0 : waypointModel.getWaypoints) === 'function' ? waypointModel.getWaypoints() : [];
    for (const waypoint of all || []) {
      if (!isWaypointLike(waypoint)) {
        continue;
      }
      const name = getWaypointName(waypoint);
      if (!name || !isVisionPickWaypointName(name)) {
        continue;
      }
      if (!isWaypointUnset(waypoint)) {
        continue;
      }
      const rank = getRank(name);
      if (rank < bestRank) {
        bestRank = rank;
        bestName = name;
      }
    }
  } catch (error) {}
  return bestName;
};
const registerWaypointRpc = (funcMap, prefix, options) => {
  const routePrefix = `${prefix}/visual/waypoint`;
  funcMap.set(`${routePrefix}/all`, () => {
    const waypointModel = getWaypointModelOrThrow();
    return waypointModel.getWaypoints().map(serializeWaypoint);
  });
  funcMap.set(`${routePrefix}/get`, payload => {
    const name = assertName(getNameFromPayload(payload));
    const waypoint = findWaypoint(name);
    if (!waypoint) {
      return false;
    }
    let normalized;
    if (prefix === 'visioninspection') {
      normalized = serializeWaypointForVisionInspectionGet(waypoint);
    } else if (prefix === 'visionpick') {
      normalized = serializeWaypointWithEmptyWhenUnset(waypoint);
    } else {
      normalized = serializeWaypoint(waypoint);
    }
    return {
      name: normalized.name,
      pose: normalized.pose,
      joint: normalized.joint,
      offset: normalized.offset
    };
  });
  funcMap.set(`${routePrefix}/add`, (...args) => {
    const {
      pose,
      joint,
      offset,
      preferredName
    } = parseAddArgs(args);
    const waypointModel = getWaypointModelOrThrow();
    if (prefix === 'visioninspection') {
      const preferred = typeof preferredName === 'string' ? preferredName.trim() : '';
      if (preferred.length > 0) {
        const existing = findWaypoint(preferred);
        if (existing) {
          const written = writeWaypointPoseWithFallback(waypointModel, existing, pose, joint, offset);
          return serializeWaypoint(written);
        }
      }
      const unsetName = getFirstUnsetIndexedWaypointName(waypointModel, 'inspection_waypoint');
      if (unsetName) {
        const existing = findWaypoint(unsetName);
        if (existing) {
          const written = writeWaypointPoseWithFallback(waypointModel, existing, pose, joint, offset);
          return serializeWaypoint(written);
        }
      }
    }
    if (prefix === 'visionpick') {
      const preferred = typeof preferredName === 'string' ? preferredName.trim() : '';
      if (preferred.length > 0) {
        const existing = findWaypoint(preferred);
        if (existing) {
          const written = writeWaypointPoseWithFallback(waypointModel, existing, pose, joint, offset);
          return serializeWaypoint(written);
        }
      }
      const unsetName = getFirstUnsetVisionPickWaypointName(waypointModel);
      if (unsetName) {
        const existing = findWaypoint(unsetName);
        if (existing) {
          const written = writeWaypointPoseWithFallback(waypointModel, existing, pose, joint, offset);
          return serializeWaypoint(written);
        }
      }
    }
    const suggestedName = prefix === 'visioninspection' ? typeof preferredName === 'string' && preferredName.trim().length > 0 ? preferredName.trim() : getNextIndexedWaypointName(waypointModel, 'inspection_waypoint') : prefix === 'visionpick' ? getNextIndexedWaypointName(waypointModel, 'cut_waypoint') : `${prefix}_waypoint`;
    const waypoint = waypointModel.addWaypoint(suggestedName, pose, joint, offset);
    if (prefix === 'visioninspection') {
      const createdName = getWaypointName(waypoint);
      if (createdName && createdName !== suggestedName) {
        try {
          waypointModel.renameWaypoint(createdName, suggestedName);
          const renamed = typeof (waypointModel === null || waypointModel === void 0 ? void 0 : waypointModel.getByName) === 'function' ? waypointModel.getByName(suggestedName) : null;
          if (renamed) {
            return serializeWaypoint(renamed);
          }
        } catch (error) {}
      }
    }
    if (prefix === 'visionpick') {
      const createdName = getWaypointName(waypoint);
      if (createdName && createdName !== suggestedName) {
        try {
          waypointModel.renameWaypoint(createdName, suggestedName);
          const renamed = typeof (waypointModel === null || waypointModel === void 0 ? void 0 : waypointModel.getByName) === 'function' ? waypointModel.getByName(suggestedName) : null;
          if (renamed) {
            return serializeWaypoint(renamed);
          }
        } catch (error) {}
      }
    }
    return serializeWaypoint(waypoint);
  });
  funcMap.set(`${routePrefix}/update`, (...args) => {
    const waypointModel = getWaypointModelOrThrow();
    const parsed = parseUpdateArgs(args);
    const name = parsed.name;
    const waypoint = resolveWaypoint(name);
    const pose = Array.isArray(parsed.pose) && parsed.pose.length > 0 ? parsed.pose : toNumberArray(waypoint.getPose());
    const joint = Array.isArray(parsed.joint) && parsed.joint.length > 0 ? parsed.joint : toNumberArray(waypoint.getJointPositions());
    const offset = Array.isArray(parsed.offset) && parsed.offset.length > 0 ? parsed.offset : toNumberArray(waypoint.getTcpOffset());
    const written = writeWaypointPoseWithFallback(waypointModel, waypoint, pose, joint, offset);
    const updatedWaypointNames = [name, getWaypointName(written)].filter(item => typeof item === 'string' && item.trim().length > 0).map(item => item.trim());
    for (const updatedName of updatedWaypointNames) {
      (0, exports.markWaypointUpdated)(updatedName);
    }
    if (typeof (options === null || options === void 0 ? void 0 : options.onWaypointUpdated) === 'function' && updatedWaypointNames.length > 0) {
      const deduplicated = Array.from(new Set(updatedWaypointNames));
      options.onWaypointUpdated(deduplicated);
    }
    const normalized = serializeWaypoint(written);
    return {
      name: normalized.name,
      pose: normalized.pose,
      joint: normalized.joint,
      offset: normalized.offset
    };
  });
  funcMap.set(`${routePrefix}/remove`, payload => {
    const name = assertName(getNameFromPayload(payload));
    getWaypointModelOrThrow().removeWaypoint(name);
    return {
      success: true
    };
  });
  funcMap.set(`${routePrefix}/rename`, payload => {
    const name = assertName(getNameFromPayload(payload));
    const newName = assertName(getNewNameFromPayload(payload));
    const waypointModel = getWaypointModelOrThrow();
    waypointModel.renameWaypoint(name, newName);
    return serializeWaypoint(resolveWaypoint(newName));
  });
};
exports.registerWaypointRpc = registerWaypointRpc;
//# sourceMappingURL=waypoint-rpc.util.js.map