Further refine cable path search performance

src/main/kotlin/ru/dbotthepony/mc/otm/block/entity/cable/EnergyCableGraph.kt

@@ -21,6 +21,56 @@ import kotlin.collections.HashSet
 import kotlin.collections.LinkedHashSet
 import kotlin.math.ln
 
+private class LinkedPriorityQueue<T : Comparable<T>> {
+	private class Entry<T>(val value: T) {
+		var next: Entry<T>? = null
+	}
+
+	private var head: Entry<T>? = null
+
+	fun isNotEmpty(): Boolean {
+		return head != null
+	}
+
+	fun add(value: T) {
+		if (head == null) {
+			head = Entry(value)
+		} else if (head!!.value >= value) {
+			val entry = Entry(value)
+			entry.next = head
+			head = entry
+		} else {
+			var current = head
+			var previous = current
+
+			while (current != null) {
+				if (current.value >= value) {
+					val entry = Entry(value)
+					entry.next = current
+					previous!!.next = entry
+					return
+				}
+
+				previous = current
+				current = current.next
+			}
+
+			previous!!.next = Entry(value)
+		}
+	}
+
+	fun remove(): T? {
+		val head = head
+
+		if (head != null) {
+			this.head = head.next
+			return head.value
+		}
+
+		return null
+	}
+}
+
 class EnergyCableGraph : GraphNodeList<EnergyCableBlockEntity.Node, EnergyCableGraph>() {
 	private val livelyNodes = HashSet<EnergyCableBlockEntity.Node>()
 	private val livelyNodesList = ArrayList<EnergyCableBlockEntity.Node>()
@@ -421,7 +471,7 @@ class EnergyCableGraph : GraphNodeList<EnergyCableBlockEntity.Node, EnergyCableG
 			return null
 		}
 
-		val openNodes = PriorityQueue<SearchNode>()
+		val openNodes = LinkedPriorityQueue<SearchNode>()
 		val seenNodes = HashSet<EnergyCableBlockEntity.Node>()
 
 		openNodes.add(SearchNode(a, b, null, seenTop))
@@ -430,8 +480,7 @@ class EnergyCableGraph : GraphNodeList<EnergyCableBlockEntity.Node, EnergyCableG
 		existing.forEach { existingNodes.addAll(it.nodes) }
 
 		while (openNodes.isNotEmpty()) {
-			val first = openNodes.remove()
-			openNodes.remove(first)
+			val first = openNodes.remove()!!
 
 			if (first.node === b) {
 				// solution found