"use strict";

Object.defineProperty(exports, "__esModule", {
  value: true
});
exports.VisionPickNodeService = void 0;
const webscope_service_1 = require("../../webscope-service");
const waypoint_rpc_util_1 = require("../../utils/waypoint-rpc.util");
const visionpick_node_config_1 = require("./visionpick-node-config");
const visionpick_node_1 = require("./visionpick-node");
let visionPickUnloadFlagNextIndex = 1;
let visionPickUnloadFlagSeedInitialized = false;
let visionPickNodeWaypointBatchNextIndex = 1;
let visionPickNodeWaypointBatchSeedInitialized = false;
class VisionPickNodeService {
  constructor() {
    this.programNodeConfig = new visionpick_node_config_1.VisionPickNodeConfig();
    this.funcMap = new Map();
    this.waypointToNodeIds = new Map();
    this.nodeIdToWaypointNames = new Map();
    this.waypointIndexDirty = true;
    this.funcMap.set('visionpick/visual/add-variable', this.addNeedUnloadFlagVariable.bind(this));
    (0, waypoint_rpc_util_1.registerWaypointRpc)(this.funcMap, 'visionpick', {
      onWaypointUpdated: this.handleWaypointUpdated.bind(this)
    });
  }
  loadFromXml(dataModel) {
    const node = new visionpick_node_1.VisionPickNode(this.programModel);
    node.setDataModel(dataModel);
    this.waypointIndexDirty = true;
    return node;
  }
  setProgramModel(programModel) {
    this.programModel = programModel;
  }
  getProgramNodeConfig() {
    return this.programNodeConfig;
  }
  getRpcMap() {
    return this.funcMap;
  }
  createNode() {
    const node = new visionpick_node_1.VisionPickNode(this.programModel);
    this.initializeNodeDefaults(node);
    this.waypointIndexDirty = true;
    return {
      nodeId: this.programNodeConfig.id,
      node,
      children: []
    };
  }
  cloneNode(node) {
    this.waypointIndexDirty = true;
    return new visionpick_node_1.VisionPickNode(this.programModel).cloneNode(node);
  }
  normalizeWaypointName(name) {
    return typeof name === 'string' ? name.trim().toLowerCase() : '';
  }
  collectNodeWaypointNames(programNode) {
    const rawNames = [this.extractWaypointName(programNode.getCutWaypoint()), this.extractWaypointName(programNode.getCornerPose1()), this.extractWaypointName(programNode.getCornerPose2()), this.extractWaypointName(programNode.getCornerPose3()), this.extractWaypointName(programNode.getCornerPose4()), this.extractWaypointName(programNode.getCenterBottomPose())];
    return new Set(rawNames.map(name => this.normalizeWaypointName(name)).filter(Boolean));
  }
  clearWaypointIndexByNodeId(nodeId) {
    const oldNames = this.nodeIdToWaypointNames.get(nodeId);
    if (!oldNames) {
      return;
    }
    this.nodeIdToWaypointNames.delete(nodeId);
    for (const oldName of oldNames) {
      const nodeIds = this.waypointToNodeIds.get(oldName);
      if (!nodeIds) {
        continue;
      }
      nodeIds.delete(nodeId);
      if (nodeIds.size === 0) {
        this.waypointToNodeIds.delete(oldName);
      }
    }
  }
  indexTreeNode(treeNode) {
    var _a, _b, _c;
    const nodeId = (_a = treeNode === null || treeNode === void 0 ? void 0 : treeNode.model) === null || _a === void 0 ? void 0 : _a.id;
    const programNode = (_b = treeNode === null || treeNode === void 0 ? void 0 : treeNode.model) === null || _b === void 0 ? void 0 : _b.programNode;
    if (typeof nodeId !== 'string' || nodeId.length === 0) {
      return;
    }
    this.clearWaypointIndexByNodeId(nodeId);
    if (!(programNode instanceof visionpick_node_1.VisionPickNode)) {
      return;
    }
    const waypointNames = this.collectNodeWaypointNames(programNode);
    if (waypointNames.size === 0) {
      return;
    }
    this.nodeIdToWaypointNames.set(nodeId, waypointNames);
    for (const waypointName of waypointNames) {
      if (!this.waypointToNodeIds.has(waypointName)) {
        this.waypointToNodeIds.set(waypointName, new Set());
      }
      (_c = this.waypointToNodeIds.get(waypointName)) === null || _c === void 0 ? void 0 : _c.add(nodeId);
    }
  }
  rebuildWaypointIndexIfNeeded() {
    var _a, _b;
    if (!this.waypointIndexDirty) {
      return;
    }
    this.waypointToNodeIds.clear();
    this.nodeIdToWaypointNames.clear();
    const programTree = (_b = (_a = this.programModel) === null || _a === void 0 ? void 0 : _a.getProgramTree) === null || _b === void 0 ? void 0 : _b.call(_a);
    if (!programTree) {
      return;
    }
    const updateHasDefined = programTree.updateHasDefined;
    const rootNode = programTree.getTreeNode();
    if (!rootNode || typeof rootNode.walk !== 'function') {
      return;
    }
    rootNode.walk(treeNode => {
      this.indexTreeNode(treeNode);
      return true;
    });
    this.waypointIndexDirty = false;
  }
  initNeedUnloadFlagSeed(variableModel) {
    if (visionPickUnloadFlagSeedInitialized) {
      return;
    }
    const prefix = visionpick_node_1.VISION_PICK_NEED_UNLOAD_FLAG_SUFFIX;
    const escapedPrefix = prefix.replace(/[.*+?^${}()|[\]\\]/g, '\\$&');
    const escapedPrefixNoUnderscore = prefix.replace(/_+$/, '').replace(/[.*+?^${}()|[\]\\]/g, '\\$&');
    const namePattern = new RegExp(`^(?:${escapedPrefix}|${escapedPrefixNoUnderscore})(\\d+)$`);
    let maxIndex = 0;
    try {
      const all = typeof (variableModel === null || variableModel === void 0 ? void 0 : variableModel.getAll) === 'function' ? variableModel.getAll() : [];
      for (const variable of all || []) {
        const rawName = variable === null || variable === void 0 ? void 0 : variable.name;
        if (typeof rawName !== 'string') {
          continue;
        }
        const matched = rawName.match(namePattern);
        if (!matched) {
          continue;
        }
        const index = Number(matched[1]);
        if (Number.isFinite(index) && index > maxIndex) {
          maxIndex = index;
        }
      }
    } catch (error) {}
    visionPickUnloadFlagNextIndex = maxIndex + 1;
    visionPickUnloadFlagSeedInitialized = true;
  }
  getNextNeedUnloadFlagName(variableModel) {
    this.initNeedUnloadFlagSeed(variableModel);
    const next = `${visionpick_node_1.VISION_PICK_NEED_UNLOAD_FLAG_SUFFIX}${visionPickUnloadFlagNextIndex}`;
    visionPickUnloadFlagNextIndex += 1;
    return next;
  }
  initNodeWaypointBatchSeed(waypointModel) {
    if (visionPickNodeWaypointBatchSeedInitialized) {
      return;
    }
    const cutPattern = /^cut_waypoint(\d+)$/;
    const centerPattern = /^center_bottom_pose(\d+)$/;
    const cornerPattern = /^corner_pose_(\d+)$/;
    let maxNodeBatchIndex = 0;
    let maxCornerIndex = 0;
    try {
      const all = typeof (waypointModel === null || waypointModel === void 0 ? void 0 : waypointModel.getWaypoints) === 'function' ? waypointModel.getWaypoints() : [];
      for (const waypoint of all || []) {
        const rawName = (waypoint === null || waypoint === void 0 ? void 0 : waypoint.name) || (typeof (waypoint === null || waypoint === void 0 ? void 0 : waypoint.getName) === 'function' ? waypoint.getName() : '') || (typeof (waypoint === null || waypoint === void 0 ? void 0 : waypoint.getDisplayName) === 'function' ? waypoint.getDisplayName() : '');
        if (typeof rawName !== 'string') {
          continue;
        }
        const cutMatched = rawName.match(cutPattern);
        if (cutMatched) {
          const index = Number(cutMatched[1]);
          if (Number.isFinite(index) && index > maxNodeBatchIndex) {
            maxNodeBatchIndex = index;
          }
          continue;
        }
        const centerMatched = rawName.match(centerPattern);
        if (centerMatched) {
          const index = Number(centerMatched[1]);
          if (Number.isFinite(index) && index > maxNodeBatchIndex) {
            maxNodeBatchIndex = index;
          }
          continue;
        }
        const cornerMatched = rawName.match(cornerPattern);
        if (cornerMatched) {
          const index = Number(cornerMatched[1]);
          if (Number.isFinite(index) && index > maxCornerIndex) {
            maxCornerIndex = index;
          }
        }
      }
    } catch (error) {}
    const nodeIndexByCorner = maxCornerIndex > 0 ? Math.ceil(maxCornerIndex / 4) : 0;
    const maxExistingNodeIndex = Math.max(maxNodeBatchIndex, nodeIndexByCorner);
    visionPickNodeWaypointBatchNextIndex = maxExistingNodeIndex + 1;
    visionPickNodeWaypointBatchSeedInitialized = true;
  }
  getNextNodeWaypointNames(waypointModel) {
    this.initNodeWaypointBatchSeed(waypointModel);
    while (true) {
      const nodeBatchIndex = visionPickNodeWaypointBatchNextIndex;
      visionPickNodeWaypointBatchNextIndex += 1;
      const cornerStartIndex = (nodeBatchIndex - 1) * 4 + 1;
      const names = {
        corner1: `corner_pose_${cornerStartIndex}`,
        corner2: `corner_pose_${cornerStartIndex + 1}`,
        corner3: `corner_pose_${cornerStartIndex + 2}`,
        corner4: `corner_pose_${cornerStartIndex + 3}`,
        cut: `cut_waypoint${nodeBatchIndex}`,
        centerBottom: `center_bottom_pose${nodeBatchIndex}`
      };
      if (!this.waypointExists(waypointModel, names.corner1) && !this.waypointExists(waypointModel, names.corner2) && !this.waypointExists(waypointModel, names.corner3) && !this.waypointExists(waypointModel, names.corner4) && !this.waypointExists(waypointModel, names.cut) && !this.waypointExists(waypointModel, names.centerBottom)) {
        return names;
      }
    }
  }
  waypointExists(waypointModel, name) {
    try {
      const waypoint = typeof (waypointModel === null || waypointModel === void 0 ? void 0 : waypointModel.getByName) === 'function' ? waypointModel.getByName(name) : null;
      return !!waypoint;
    } catch (error) {
      return false;
    }
  }
  createNeedUnloadFlagVariable() {
    const webscopeService = (0, webscope_service_1.getWebscopeService)();
    if (!webscopeService) {
      throw new Error('Webscope service is not ready');
    }
    const variableModel = webscopeService.getVariableModel();
    const expression = webscopeService.getValueFactory().createExpression();
    expression.append('false');
    const expectedName = this.getNextNeedUnloadFlagName(variableModel);
    const variable = webscopeService.getVariableModel().getVariableFactory().createGlobalVariable(expectedName, expression);
    if (variable && variable.name !== expectedName) {
      variable.name = expectedName;
    }
    variableModel.addVariable(variable);
    return variable;
  }
  extractWaypointName(value) {
    if (!value || typeof value !== 'object') {
      return '';
    }
    const waypoint = value;
    if (typeof waypoint.name === 'string' && waypoint.name.trim().length > 0) {
      return waypoint.name;
    }
    if (typeof waypoint.getName === 'function') {
      const name = waypoint.getName();
      if (typeof name === 'string' && name.trim().length > 0) {
        return name;
      }
    }
    if (typeof waypoint.getDisplayName === 'function') {
      const displayName = waypoint.getDisplayName();
      if (typeof displayName === 'string' && displayName.trim().length > 0) {
        return displayName;
      }
    }
    return '';
  }
  addWaypointWithExpectedName(waypointModel, expectedName, pose, joint, offset) {
    const created = waypointModel.addWaypoint(expectedName, pose, joint, offset);
    const createdName = this.extractWaypointName(created);
    if (createdName && createdName !== expectedName) {
      try {
        waypointModel.renameWaypoint(createdName, expectedName);
        const renamed = typeof (waypointModel === null || waypointModel === void 0 ? void 0 : waypointModel.getByName) === 'function' ? waypointModel.getByName(expectedName) : null;
        return renamed || created;
      } catch (error) {}
    }
    return created;
  }
  addNeedUnloadFlagVariable() {
    const variable = this.createNeedUnloadFlagVariable();
    return variable.name;
  }
  initializeNodeDefaults(node) {
    const webscopeService = (0, webscope_service_1.getWebscopeService)();
    if (!webscopeService) {
      return;
    }
    try {
      const variable = this.createNeedUnloadFlagVariable();
      if (variable) {
        node.setNeedUnloadFlagVariable(variable);
      }
    } catch (error) {}
    try {
      const waypointModel = webscopeService.getWaypointModel();
      const zeroPose = [0, 0, 0, 0, 0, 0];
      const waypointNames = this.getNextNodeWaypointNames(waypointModel);
      const corner1 = this.addWaypointWithExpectedName(waypointModel, waypointNames.corner1, zeroPose, zeroPose, zeroPose);
      if (corner1) {
        node.setCornerPose1(corner1);
      }
      const corner2 = this.addWaypointWithExpectedName(waypointModel, waypointNames.corner2, zeroPose, zeroPose, zeroPose);
      if (corner2) {
        node.setCornerPose2(corner2);
      }
      const corner3 = this.addWaypointWithExpectedName(waypointModel, waypointNames.corner3, zeroPose, zeroPose, zeroPose);
      if (corner3) {
        node.setCornerPose3(corner3);
      }
      const corner4 = this.addWaypointWithExpectedName(waypointModel, waypointNames.corner4, zeroPose, zeroPose, zeroPose);
      if (corner4) {
        node.setCornerPose4(corner4);
      }
      const cutWaypoint = this.addWaypointWithExpectedName(waypointModel, waypointNames.cut, zeroPose, zeroPose, zeroPose);
      if (cutWaypoint) {
        node.setCutWaypoint(cutWaypoint);
      }
      const centerBottom = this.addWaypointWithExpectedName(waypointModel, waypointNames.centerBottom, zeroPose, zeroPose, zeroPose);
      if (centerBottom) {
        node.setCenterBottomPose(centerBottom);
      }
    } catch (error) {}
  }
  handleWaypointUpdated(waypointNames) {
    var _a, _b;
    if (!Array.isArray(waypointNames) || waypointNames.length === 0) {
      return;
    }
    const normalizedNames = new Set(waypointNames.map(name => this.normalizeWaypointName(name)).filter(Boolean));
    if (normalizedNames.size === 0) {
      return;
    }
    this.rebuildWaypointIndexIfNeeded();
    const programTree = (_b = (_a = this.programModel) === null || _a === void 0 ? void 0 : _a.getProgramTree) === null || _b === void 0 ? void 0 : _b.call(_a);
    if (!programTree) {
      return;
    }
    for (const waypointName of normalizedNames) {
      const nodeIds = Array.from(this.waypointToNodeIds.get(waypointName) || []);
      for (const nodeId of nodeIds) {
        const treeNode = programTree.getTreeNodeById(nodeId);
        if (!treeNode) {
          this.clearWaypointIndexByNodeId(nodeId);
          continue;
        }
        this.indexTreeNode(treeNode);
        const latestNames = this.nodeIdToWaypointNames.get(nodeId);
        if ((latestNames === null || latestNames === void 0 ? void 0 : latestNames.has(waypointName)) && typeof programTree.updateHasDefined === 'function') {
          programTree.updateHasDefined(true, treeNode);
        }
      }
    }
  }
}
exports.VisionPickNodeService = VisionPickNodeService;
//# sourceMappingURL=visionpick-node-service.js.map