feat: Implement PriorityQueue class for D* Lite pathfinding with min-heap functionality

server/pathfinding/PriorityQueue.js

@@ -0,0 +1,175 @@
+/**
+ * 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;
+      }
+    }
+  }
+}