remoteturtle/server/pathfinding/PriorityQueue.js

/**
 * PriorityQueue - Min-heap priority queue for D* Lite
 * Uses two-element key pairs [k1, k2] for ordering
 */
export class PriorityQueue {
  constructor() {
    this.heap = [];
    this.keyMap = new Map(); // key string -> index in heap
  }

  get size() {
    return this.heap.length;
  }

  isEmpty() {
    return this.heap.length === 0;
  }

  /**
   * Compare two key pairs
   * Returns negative if a < b, positive if a > b, 0 if equal
   */
  static compareKeys(a, b) {
    if (a[0] !== b[0]) return a[0] - b[0];
    return a[1] - b[1];
  }

  /**
   * Get the minimum key without removing
   */
  topKey() {
    if (this.heap.length === 0) return [Infinity, Infinity];
    return this.heap[0].priority;
  }

  /**
   * Insert or update a node with a priority
   */
  insertOrUpdate(node, priority) {
    const key = node.key;

    if (this.keyMap.has(key)) {
      // Update existing entry
      const index = this.keyMap.get(key);
      const oldPriority = this.heap[index].priority;
      this.heap[index].priority = priority;
      this.heap[index].node = node;

      // Determine whether to bubble up or sift down
      if (PriorityQueue.compareKeys(priority, oldPriority) < 0) {
        this._bubbleUp(index);
      } else {
        this._siftDown(index);
      }
    } else {
      // Insert new entry
      const entry = { node, priority };
      this.heap.push(entry);
      const index = this.heap.length - 1;
      this.keyMap.set(key, index);
      this._bubbleUp(index);
    }
  }

  /**
   * Remove and return the minimum node
   */
  pop() {
    if (this.heap.length === 0) return null;

    const min = this.heap[0];
    this.keyMap.delete(min.node.key);

    const last = this.heap.pop();
    if (this.heap.length > 0) {
      this.heap[0] = last;
      this.keyMap.set(last.node.key, 0);
      this._siftDown(0);
    }

    return min.node;
  }

  /**
   * Remove a specific node by key
   */
  remove(node) {
    const key = node.key;
    if (!this.keyMap.has(key)) return;

    const index = this.keyMap.get(key);
    this.keyMap.delete(key);

    const last = this.heap.pop();
    if (index < this.heap.length) {
      this.heap[index] = last;
      this.keyMap.set(last.node.key, index);
      this._bubbleUp(index);
      this._siftDown(index);
    }
  }

  /**
   * Check if a node is in the queue
   */
  contains(node) {
    return this.keyMap.has(node.key);
  }

  /**
   * Clear the queue
   */
  clear() {
    this.heap = [];
    this.keyMap.clear();
  }

  // --- Internal heap operations ---

  _parent(i) {
    return Math.floor((i - 1) / 2);
  }

  _leftChild(i) {
    return 2 * i + 1;
  }

  _rightChild(i) {
    return 2 * i + 2;
  }

  _swap(i, j) {
    const temp = this.heap[i];
    this.heap[i] = this.heap[j];
    this.heap[j] = temp;

    this.keyMap.set(this.heap[i].node.key, i);
    this.keyMap.set(this.heap[j].node.key, j);
  }

  _bubbleUp(i) {
    while (i > 0) {
      const parent = this._parent(i);
      if (PriorityQueue.compareKeys(this.heap[i].priority, this.heap[parent].priority) < 0) {
        this._swap(i, parent);
        i = parent;
      } else {
        break;
      }
    }
  }

  _siftDown(i) {
    const n = this.heap.length;
    while (true) {
      let smallest = i;
      const left = this._leftChild(i);
      const right = this._rightChild(i);

      if (left < n && PriorityQueue.compareKeys(this.heap[left].priority, this.heap[smallest].priority) < 0) {
        smallest = left;
      }
      if (right < n && PriorityQueue.compareKeys(this.heap[right].priority, this.heap[smallest].priority) < 0) {
        smallest = right;
      }

      if (smallest !== i) {
        this._swap(i, smallest);
        i = smallest;
      } else {
        break;
      }
    }
  }
}