abstracted navigation rerouting (#2597)

Co-authored-by: hannibal2 <24389977+hannibal00212@users.noreply.github.com>

13 files changed, 332 insertions, 142 deletions

diff --git a/src/main/java/at/hannibal2/skyhanni/data/IslandGraphs.kt b/src/main/java/at/hannibal2/skyhanni/data/IslandGraphs.kt
index 1f8e45afb..bd417e5f7 100644
--- a/src/main/java/at/hannibal2/skyhanni/data/IslandGraphs.kt
+++ b/src/main/java/at/hannibal2/skyhanni/data/IslandGraphs.kt
@@ -4,8 +4,8 @@ import at.hannibal2.skyhanni.SkyHanniMod
 import at.hannibal2.skyhanni.api.event.HandleEvent
 import at.hannibal2.skyhanni.data.model.Graph
 import at.hannibal2.skyhanni.data.model.GraphNode
-import at.hannibal2.skyhanni.data.model.findShortestPathAsGraphWithDistance
 import at.hannibal2.skyhanni.data.repo.RepoUtils
+import at.hannibal2.skyhanni.events.EntityMoveEvent
 import at.hannibal2.skyhanni.events.IslandChangeEvent
 import at.hannibal2.skyhanni.events.LorenzRenderWorldEvent
 import at.hannibal2.skyhanni.events.LorenzTickEvent
@@ -14,8 +14,10 @@ import at.hannibal2.skyhanni.events.RepositoryReloadEvent
 import at.hannibal2.skyhanni.events.skyblock.ScoreboardAreaChangeEvent
 import at.hannibal2.skyhanni.features.misc.IslandAreas
 import at.hannibal2.skyhanni.skyhannimodule.SkyHanniModule
-import at.hannibal2.skyhanni.test.SkyHanniDebugsAndTests
+import at.hannibal2.skyhanni.utils.ChatUtils
+import at.hannibal2.skyhanni.utils.CollectionUtils.sorted
 import at.hannibal2.skyhanni.utils.DelayedRun
+import at.hannibal2.skyhanni.utils.GraphUtils
 import at.hannibal2.skyhanni.utils.LocationUtils
 import at.hannibal2.skyhanni.utils.LocationUtils.canBeSeen
 import at.hannibal2.skyhanni.utils.LocationUtils.distanceSqToPlayer
@@ -24,11 +26,16 @@ import at.hannibal2.skyhanni.utils.LorenzColor
 import at.hannibal2.skyhanni.utils.LorenzUtils
 import at.hannibal2.skyhanni.utils.LorenzUtils.isInIsland
 import at.hannibal2.skyhanni.utils.LorenzVec
+import at.hannibal2.skyhanni.utils.NumberUtil.roundTo
 import at.hannibal2.skyhanni.utils.RegexUtils.matches
 import at.hannibal2.skyhanni.utils.RenderUtils.draw3DLine
 import at.hannibal2.skyhanni.utils.RenderUtils.draw3DPathWithWaypoint
-import at.hannibal2.skyhanni.utils.RenderUtils.drawWaypointFilled
+import at.hannibal2.skyhanni.utils.chat.Text.asComponent
+import at.hannibal2.skyhanni.utils.chat.Text.hover
+import at.hannibal2.skyhanni.utils.chat.Text.onClick
+import at.hannibal2.skyhanni.utils.chat.Text.send
 import at.hannibal2.skyhanni.utils.repopatterns.RepoPattern
+import net.minecraft.client.Minecraft
 import net.minecraftforge.fml.common.eventhandler.SubscribeEvent
 import java.awt.Color
 import java.io.File
@@ -98,9 +105,16 @@ object IslandGraphs {
     val existsForThisIsland get() = currentIslandGraph != null
 
     var closedNote: GraphNode? = null
+    var secondClosedNote: GraphNode? = null
 
     private var currentTarget: LorenzVec? = null
+    private var currentTargetNode: GraphNode? = null
+    private var label = ""
+    private var distanceViaNodes = 0.0
+    private var distanceToNextNode = 0.0
+    private var totalDistance = 0.0
     private var color = Color.WHITE
+    private var shouldAllowRerouting = false
     private var showGoalExact = false
     private var onFound: () -> Unit = {}
     private var goal: GraphNode? = null
@@ -143,6 +157,9 @@ object IslandGraphs {
     @SubscribeEvent
     fun onWorldChange(event: LorenzWorldChangeEvent) {
         currentIslandGraph = null
+        if (currentTarget != null) {
+            "§e[SkyHanni] Navigation stopped because of world switch!".asComponent().send(PATHFIND_ID)
+        }
         reset()
     }
 
@@ -188,11 +205,11 @@ object IslandGraphs {
         }
 
         val graph = RepoUtils.getConstant(SkyHanniMod.repo.repoLocation, constant, Graph.gson, Graph::class.java)
+        IslandAreas.display = null
         setNewGraph(graph)
     }
 
     fun setNewGraph(graph: Graph) {
-        IslandAreas.display = null
         reset()
         currentIslandGraph = graph
 
@@ -205,26 +222,26 @@ object IslandGraphs {
     }
 
     private fun reset() {
+        stop()
         closedNote = null
-        currentTarget = null
-        goal = null
-        fastestPath = null
     }
 
     @SubscribeEvent
     fun onTick(event: LorenzTickEvent) {
         if (!LorenzUtils.inSkyBlock) return
         handleTick()
+        if (event.isMod(2)) {
+            checkMoved()
+        }
     }
 
     private fun handleTick() {
         val prevClosed = closedNote
 
-        val graph = currentIslandGraph ?: return
-
         currentTarget?.let {
             if (it.distanceToPlayer() < 3) {
                 onFound()
+                "§e[SkyHanni] Navigation reached §r$label§e!".asComponent().send(PATHFIND_ID)
                 reset()
             }
             if (!condition()) {
@@ -232,17 +249,44 @@ object IslandGraphs {
             }
         }
 
-        val newClosest = if (SkyHanniDebugsAndTests.c == 0.0) {
-            graph.minBy { it.position.distanceSqToPlayer() }
-        } else null
+        val graph = currentIslandGraph ?: return
+        val sortedNodes = graph.sortedBy { it.position.distanceSqToPlayer() }
+        val newClosest = sortedNodes.first()
         if (closedNote == newClosest) return
+        if (onCurrentPath()) return
+
         closedNote = newClosest
+        secondClosedNote = sortedNodes.getOrNull(1)
         onNewNote()
+        hasMoved = false
+        if (newClosest == prevClosed) return
+        findNewPath()
+    }
+
+    private fun onCurrentPath(): Boolean {
+        val path = fastestPath ?: return false
+        val closest = path.nodes.minBy { it.position.distanceSqToPlayer() }
+        val distance = closest.position.distanceToPlayer()
+        if (distance > 5) return false
+
+        if (distance < 3) {
+            val index = path.nodes.indexOf(closest)
+            val newNodes = path.drop(index)
+            val newGraph = Graph(newNodes)
+            fastestPath = newGraph
+            newNodes.getOrNull(1)?.let {
+                secondClosedNote = it
+            }
+            setFastestPath(newGraph to newGraph.totalLenght(), setPath = false)
+        }
+        return true
+    }
+
+    private fun findNewPath() {
+        val goal = IslandGraphs.goal ?: return
         val closest = closedNote ?: return
-        val goal = goal ?: return
-        if (closest == prevClosed) return
 
-        val (path, distance) = graph.findShortestPathAsGraphWithDistance(closest, goal)
+        val (path, distance) = GraphUtils.findShortestPathAsGraphWithDistance(closest, goal)
         val first = path.firstOrNull()
         val second = path.getOrNull(1)
 
@@ -260,64 +304,194 @@ object IslandGraphs {
         setFastestPath(path to (distance + nodeDistance))
     }
 
-    private fun setFastestPath(path: Pair<Graph, Double>) {
-        fastestPath = path.takeIf { it.first.isNotEmpty() }?.first
+    private fun Graph.totalLenght(): Double = nodes.zipWithNext().sumOf { (a, b) -> a.position.distance(b.position) }
+
+    private fun handlePositionChange() {
+        val secondClosestNode = secondClosedNote ?: return
+        distanceToNextNode = secondClosestNode.position.distanceToPlayer()
+        updateChat()
+    }
+
+    private var hasMoved = false
 
-        fastestPath?.let {
-            fastestPath = Graph(cutByMaxDistance(it.nodes, 3.0))
+    private fun checkMoved() {
+        if (hasMoved) {
+            hasMoved = false
+            if (goal != null) {
+                handlePositionChange()
+            }
         }
     }
 
+    @SubscribeEvent
+    fun onPlayerMove(event: EntityMoveEvent) {
+        if (LorenzUtils.inSkyBlock && event.entity == Minecraft.getMinecraft().thePlayer) {
+            hasMoved = true
+        }
+    }
+
+    private fun setFastestPath(path: Pair<Graph, Double>, setPath: Boolean = true) {
+        val (fastestPath, distance) = path.takeIf { it.first.isNotEmpty() } ?: return
+        val nodes = fastestPath.nodes.toMutableList()
+        if (Minecraft.getMinecraft().thePlayer.onGround) {
+            nodes.add(0, GraphNode(0, LocationUtils.playerLocation()))
+        }
+        if (setPath) {
+            this.fastestPath = Graph(cutByMaxDistance(nodes, 3.0))
+        }
+
+        val diff = fastestPath.getOrNull(1)?.let {
+            fastestPath.first().position.distance(it.position)
+        } ?: 0.0
+        this.distanceViaNodes = distance - diff
+        updateChat()
+    }
+
     private fun onNewNote() {
         // TODO create an event
         IslandAreas.noteMoved()
+        if (shouldAllowRerouting) {
+            tryRerouting()
+        }
+    }
+
+    private fun tryRerouting() {
+        val target = currentTargetNode ?: return
+        val closest = closedNote ?: return
+        val map = GraphUtils.findAllShortestDistances(closest).distances.filter { it.key.sameNameAndTags(target) }
+        val newTarget = map.sorted().keys.firstOrNull() ?: return
+        if (newTarget != target) {
+            ChatUtils.debug("Rerouting navigation..")
+            newTarget.pathFind(label, color, onFound, allowRerouting = true, condition)
+        }
     }
 
     fun stop() {
         currentTarget = null
         goal = null
         fastestPath = null
+        currentTargetNode = null
+        label = ""
+        distanceToNextNode = 0.0
+        distanceViaNodes = 0.0
+        totalDistance = 0.0
+    }
+
+    /**
+     * Activates pathfinding, with this graph node as goal.
+     *
+     * @param label The name of the naviation goal in chat.
+     * @param color The color of the lines in world.
+     * @param onFound The callback that gets fired when the goal is reached.
+     * @param allowRerouting When a different node with same name and tags as the origianl goal is closer to the player, starts routing to this instead.
+     * @param condition The pathfinding stops when the condition is no longer valid.
+     */
+    fun GraphNode.pathFind(
+        label: String,
+        color: Color = LorenzColor.WHITE.toColor(),
+        onFound: () -> Unit = {},
+        allowRerouting: Boolean = false,
+        condition: () -> Boolean = { true },
+    ) {
+        reset()
+        currentTargetNode = this
+        shouldAllowRerouting = allowRerouting
+        pathFind0(location = position, label, color, onFound, showGoalExact = false, condition)
     }
 
+    /**
+     * Activates pathfinding to a location in the island.
+     *
+     * @param location The goal of the pathfinder.
+     * @param label The name of the naviation goal in chat.
+     * @param color The color of the lines in world.
+     * @param onFound The callback that gets fired when the goal is reached.
+     * @param showGoalExact Wether the exact location should be shown as a waypoint, as well as shwoing a line from last node to the goal location.
+     * @param condition The pathfinding stops when the condition is no longer valid.
+     */
     fun pathFind(
         location: LorenzVec,
+        label: String,
         color: Color = LorenzColor.WHITE.toColor(),
         onFound: () -> Unit = {},
         showGoalExact: Boolean = false,
         condition: () -> Boolean = { true },
     ) {
         reset()
+        shouldAllowRerouting = false
+        pathFind0(location, label, color, onFound, showGoalExact, condition)
+    }
+
+    private fun pathFind0(
+        location: LorenzVec,
+        label: String,
+        color: Color = LorenzColor.WHITE.toColor(),
+        onFound: () -> Unit = {},
+        showGoalExact: Boolean = false,
+        condition: () -> Boolean = { true },
+    ) {
         currentTarget = location
+        this.label = label
         this.color = color
         this.onFound = onFound
         this.showGoalExact = showGoalExact
         this.condition = condition
         val graph = currentIslandGraph ?: return
         goal = graph.minBy { it.position.distance(currentTarget!!) }
+        updateChat()
+    }
+
+    private const val PATHFIND_ID = -6457563
+
+    private fun updateChat() {
+        if (label == "") return
+        val finalDistance = distanceViaNodes + distanceToNextNode
+        if (finalDistance == 0.0) return
+        val distance = finalDistance.roundTo(1)
+        if (totalDistance == 0.0 || distance > totalDistance) {
+            totalDistance = distance
+        }
+        sendChatDistance(distance)
+    }
+
+    private fun sendChatDistance(distance: Double) {
+        val percentage = (1 - (distance / totalDistance)) * 100
+        val componentText = "§e[SkyHanni] Navigating to §r$label §f[§e$distance§f] §f(§c${percentage.roundTo(1)}%§f)".asComponent()
+        componentText.onClick(
+            onClick = {
+                stop()
+                "§e[SkyHanni] Navigation manually stopped!".asComponent().send(PATHFIND_ID)
+            },
+        )
+        componentText.hover = "§eClick to stop navigating!".asComponent()
+        componentText.send(PATHFIND_ID)
     }
 
     @SubscribeEvent
     fun onRenderWorld(event: LorenzRenderWorldEvent) {
         if (!LorenzUtils.inSkyBlock) return
-        val path = fastestPath ?: return
+        var path = fastestPath ?: return
 
-        var graph = path
-        graph = skipNodes(graph) ?: graph
+        if (path.nodes.size > 1) {
+            val hideNearby = if (Minecraft.getMinecraft().thePlayer.onGround) 5 else 7
+            path = Graph(path.nodes.takeLastWhile { it.position.distanceToPlayer() > hideNearby })
+        }
+//         graph = skipNodes(graph) ?: graph
 
         event.draw3DPathWithWaypoint(
-            graph,
+            path,
             color,
             6,
             true,
             bezierPoint = 2.0,
             textSize = 1.0,
+            markLastBlock = showGoalExact,
         )
-        val lastNode = graph.nodes.last().position
-        val targetLocation = currentTarget ?: return
-        event.draw3DLine(lastNode.add(0.5, 0.5, 0.5), targetLocation.add(0.5, 0.5, 0.5), color, 4, true)
 
         if (showGoalExact) {
-            event.drawWaypointFilled(targetLocation, color)
+            val targetLocation = currentTarget ?: return
+            val lastNode = path.nodes.last().position
+            event.draw3DLine(lastNode.add(0.5, 0.5, 0.5), targetLocation.add(0.5, 0.5, 0.5), color, 4, true)
         }
     }
 
diff --git a/src/main/java/at/hannibal2/skyhanni/data/model/Graph.kt b/src/main/java/at/hannibal2/skyhanni/data/model/Graph.kt
index bc4be57a1..6e1f1d07f 100644
--- a/src/main/java/at/hannibal2/skyhanni/data/model/Graph.kt
+++ b/src/main/java/at/hannibal2/skyhanni/data/model/Graph.kt
@@ -1,5 +1,6 @@
 package at.hannibal2.skyhanni.data.model
 
+import at.hannibal2.skyhanni.features.misc.pathfind.NavigationHelper
 import at.hannibal2.skyhanni.utils.LorenzVec
 import at.hannibal2.skyhanni.utils.NumberUtil.roundTo
 import at.hannibal2.skyhanni.utils.json.SkyHanniTypeAdapters.registerTypeAdapter
@@ -8,7 +9,6 @@ import com.google.gson.GsonBuilder
 import com.google.gson.JsonElement
 import com.google.gson.annotations.Expose
 import com.google.gson.stream.JsonToken
-import java.util.PriorityQueue
 
 // TODO: This class should be disambiguated into a NodePath and a Graph class
 @JvmInline
@@ -138,6 +138,10 @@ value class Graph(
         fun fromJson(json: String): Graph = gson.fromJson<Graph>(json)
         fun fromJson(json: JsonElement): Graph = gson.fromJson<Graph>(json)
     }
+
+    fun toPositionsList() = this.map { it.position }
+
+    fun toJson(): String = gson.toJson(this)
 }
 
 // The node object that gets parsed from/to json
@@ -164,9 +168,11 @@ class GraphNode(val id: Int, val position: LorenzVec, val name: String? = null,
 
         return true
     }
-}
 
-fun Graph.findShortestPathAsGraph(start: GraphNode, end: GraphNode): Graph = this.findShortestPathAsGraphWithDistance(start, end).first
+    fun sameNameAndTags(other: GraphNode): Boolean = name == other.name && allowedTags == other.allowedTags
+
+    private val allowedTags get() = tags.filter { it in NavigationHelper.allowedTags }
+}
 
 data class DijkstraTree(
     val origin: GraphNode,
@@ -188,57 +194,6 @@ data class DijkstraTree(
     val lastVisitedNode: GraphNode,
 )
 
-/**
- * Find a tree of distances to the [start] node using dijkstra's algorithm.
- */
-fun Graph.findDijkstraDistances(
-    start: GraphNode,
-    /**
-     * Bail out early before collecting all the distances to all nodes in the graph. This will not collect valid distance data for *all*
-     * nodes for which bailout matches, but only the closest one.
-     */
-    bailout: (GraphNode) -> Boolean,
-): DijkstraTree {
-    val distances = mutableMapOf<GraphNode, Double>()
-    val previous = mutableMapOf<GraphNode, GraphNode>()
-    val visited = mutableSetOf<GraphNode>()
-    val queue = PriorityQueue<GraphNode>(compareBy { distances.getOrDefault(it, Double.MAX_VALUE) })
-    var lastVisitedNode: GraphNode = start
-
-    distances[start] = 0.0
-    queue.add(start)
-
-    while (queue.isNotEmpty()) {
-        val current = queue.poll()
-        lastVisitedNode = current
-        if (bailout(current)) break
-
-        visited.add(current)
-
-        current.neighbours.forEach { (neighbour, weight) ->
-            if (neighbour !in visited) {
-                val newDistance = distances.getValue(current) + weight
-                if (newDistance < distances.getOrDefault(neighbour, Double.MAX_VALUE)) {
-                    distances[neighbour] = newDistance
-                    previous[neighbour] = current
-                    queue.add(neighbour)
-                }
-            }
-        }
-    }
-
-    return DijkstraTree(
-        start,
-        distances,
-        previous,
-        lastVisitedNode,
-    )
-}
-
-fun Graph.findAllShortestDistances(start: GraphNode): DijkstraTree {
-    return findDijkstraDistances(start) { false }
-}
-
 fun DijkstraTree.findPathToDestination(end: GraphNode): Pair<Graph, Double> {
     val distances = this
     val reversePath = buildList {
@@ -251,16 +206,3 @@ fun DijkstraTree.findPathToDestination(end: GraphNode): Pair<Graph, Double> {
     }
     return Graph(reversePath.reversed()) to distances.distances[end]!!
 }
-
-fun Graph.findShortestPathAsGraphWithDistance(start: GraphNode, end: GraphNode): Pair<Graph, Double> {
-    val distances = findDijkstraDistances(start) { it == end }
-    return distances.findPathToDestination(end)
-}
-
-fun Graph.findShortestPath(start: GraphNode, end: GraphNode): List<LorenzVec> = this.findShortestPathAsGraph(start, end).toPositionsList()
-
-fun Graph.findShortestDistance(start: GraphNode, end: GraphNode): Double = this.findShortestPathAsGraphWithDistance(start, end).second
-
-fun Graph.toPositionsList() = this.map { it.position }
-
-fun Graph.toJson(): String = Graph.gson.toJson(this)
diff --git a/src/main/java/at/hannibal2/skyhanni/features/event/hoppity/HoppityEggLocator.kt b/src/main/java/at/hannibal2/skyhanni/features/event/hoppity/HoppityEggLocator.kt
index 468fecbcb..dbe06fbf9 100644
--- a/src/main/java/at/hannibal2/skyhanni/features/event/hoppity/HoppityEggLocator.kt
+++ b/src/main/java/at/hannibal2/skyhanni/features/event/hoppity/HoppityEggLocator.kt
@@ -280,7 +280,7 @@ object HoppityEggLocator {
 
         val color = config.waypointColor.toChromaColor()
 
-        IslandGraphs.pathFind(location, color, condition = { config.showPathFinder })
+        IslandGraphs.pathFind(location, "Hoppity Egg", color, condition = { config.showPathFinder })
     }
 
     fun isValidEggLocation(location: LorenzVec): Boolean = HoppityEggLocations.islandLocations.any { it.distance(location) < 5.0 }
@@ -333,7 +333,7 @@ object HoppityEggLocator {
         HoppityEggLocations.apiEggLocations[LorenzUtils.skyBlockIsland]?.let {
             for ((i, location) in it.values.withIndex()) {
                 if (i == target) {
-                    IslandGraphs.pathFind(location)
+                    IslandGraphs.pathFind(location, "Hoppity Test")
                     return
                 }
             }
diff --git a/src/main/java/at/hannibal2/skyhanni/features/event/hoppity/HoppityNpc.kt b/src/main/java/at/hannibal2/skyhanni/features/event/hoppity/HoppityNpc.kt
index 70e148aa9..c120c3d64 100644
--- a/src/main/java/at/hannibal2/skyhanni/features/event/hoppity/HoppityNpc.kt
+++ b/src/main/java/at/hannibal2/skyhanni/features/event/hoppity/HoppityNpc.kt
@@ -58,7 +58,10 @@ object HoppityNpc {
             "New rabbits are available at §aHoppity's Shop§e!",
             config::hoppityShopReminder,
             actionName = "warp to hub",
-            action = { HypixelCommands.warp("hub") },
+            action = {
+                HypixelCommands.warp("hub")
+                //afterNextIslandwarpTtp hub: IslandGraphs.pathFind(hoppity)
+            },
         )
 
         lastReminderSent = SimpleTimeMark.now()
diff --git a/src/main/java/at/hannibal2/skyhanni/features/mining/TunnelsMaps.kt b/src/main/java/at/hannibal2/skyhanni/features/mining/TunnelsMaps.kt
index 5214ff162..eb89da696 100644
--- a/src/main/java/at/hannibal2/skyhanni/features/mining/TunnelsMaps.kt
+++ b/src/main/java/at/hannibal2/skyhanni/features/mining/TunnelsMaps.kt
@@ -5,8 +5,6 @@ import at.hannibal2.skyhanni.data.ClickType
 import at.hannibal2.skyhanni.data.IslandType
 import at.hannibal2.skyhanni.data.model.Graph
 import at.hannibal2.skyhanni.data.model.GraphNode
-import at.hannibal2.skyhanni.data.model.findShortestDistance
-import at.hannibal2.skyhanni.data.model.findShortestPathAsGraphWithDistance
 import at.hannibal2.skyhanni.events.ConfigLoadEvent
 import at.hannibal2.skyhanni.events.GuiContainerEvent
 import at.hannibal2.skyhanni.events.GuiRenderEvent
@@ -28,6 +26,7 @@ import at.hannibal2.skyhanni.utils.ColorUtils.getFirstColorCode
 import at.hannibal2.skyhanni.utils.ColorUtils.toChromaColor
 import at.hannibal2.skyhanni.utils.ConditionalUtils.onToggle
 import at.hannibal2.skyhanni.utils.DelayedRun
+import at.hannibal2.skyhanni.utils.GraphUtils
 import at.hannibal2.skyhanni.utils.HypixelCommands
 import at.hannibal2.skyhanni.utils.ItemUtils.getInternalNameOrNull
 import at.hannibal2.skyhanni.utils.ItemUtils.getLore
@@ -81,6 +80,7 @@ object TunnelsMaps {
     private var active: String = ""
 
     private lateinit var fairySouls: Map<String, GraphNode>
+
     // TODO what is this? why is there a difference? can this be replaced with GraphNodeTag.GRIND_ORES?
     private lateinit var newGemstones: Map<String, List<GraphNode>>
     private lateinit var oldGemstones: Map<String, List<GraphNode>>
@@ -98,7 +98,7 @@ object TunnelsMaps {
         val list = possibleLocations[name] ?: return null
 
         val offCooldown = list.filter { cooldowns[it]?.isInPast() != false }
-        val best = offCooldown.minByOrNull { graph.findShortestDistance(closed, it) } ?: list.minBy {
+        val best = offCooldown.minByOrNull { GraphUtils.findShortestDistance(closed, it) } ?: list.minBy {
             cooldowns[it] ?: SimpleTimeMark.farPast()
         }
         if (cooldowns[best]?.isInPast() != false) {
@@ -249,7 +249,8 @@ object TunnelsMaps {
         this.oldGemstones = oldGemstone
         normalLocations = other
         translateTable.clear()
-        DelayedRun.runNextTick { // Needs to be delayed since the config may not be loaded
+        DelayedRun.runNextTick {
+            // Needs to be delayed since the config may not be loaded
             locationDisplay = generateLocationsDisplay()
         }
     }
@@ -398,7 +399,7 @@ object TunnelsMaps {
         val closest = closedNote ?: return
         val goal = goal ?: return
         if (closest == prevClosed && goal == prevGoal) return
-        val (path, distance) = graph.findShortestPathAsGraphWithDistance(closest, goal)
+        val (path, distance) = GraphUtils.findShortestPathAsGraphWithDistance(closest, goal)
         val first = path.firstOrNull()
         val second = path.getOrNull(1)
 
diff --git a/src/main/java/at/hannibal2/skyhanni/features/misc/IslandAreas.kt b/src/main/java/at/hannibal2/skyhanni/features/misc/IslandAreas.kt
index 5ece5897c..cddbd201d 100644
--- a/src/main/java/at/hannibal2/skyhanni/features/misc/IslandAreas.kt
+++ b/src/main/java/at/hannibal2/skyhanni/features/misc/IslandAreas.kt
@@ -2,6 +2,7 @@ package at.hannibal2.skyhanni.features.misc
 
 import at.hannibal2.skyhanni.SkyHanniMod
 import at.hannibal2.skyhanni.data.IslandGraphs
+import at.hannibal2.skyhanni.data.IslandGraphs.pathFind
 import at.hannibal2.skyhanni.data.model.Graph
 import at.hannibal2.skyhanni.data.model.GraphNode
 import at.hannibal2.skyhanni.data.model.GraphNodeTag
@@ -13,7 +14,6 @@ import at.hannibal2.skyhanni.events.LorenzRenderWorldEvent
 import at.hannibal2.skyhanni.events.LorenzTickEvent
 import at.hannibal2.skyhanni.events.LorenzWorldChangeEvent
 import at.hannibal2.skyhanni.skyhannimodule.SkyHanniModule
-import at.hannibal2.skyhanni.utils.ChatUtils
 import at.hannibal2.skyhanni.utils.CollectionUtils.addSearchString
 import at.hannibal2.skyhanni.utils.CollectionUtils.sorted
 import at.hannibal2.skyhanni.utils.ColorUtils.toChromaColor
@@ -81,7 +81,7 @@ object IslandAreas {
         nodes = finalNodes
     }
 
-    var hasMoved = false
+    private var hasMoved = false
 
     @SubscribeEvent
     fun onTick(event: LorenzTickEvent) {
@@ -144,11 +144,6 @@ object IslandAreas {
             val isTarget = node.name == targetNode?.name
             val color = if (isTarget) LorenzColor.GOLD else tag.color
 
-            // trying to find a faster path to the existing target
-            if (isTarget && node != targetNode) {
-                ChatUtils.debug("Found a faster node, rerouting...")
-                setTarget(node)
-            }
             val coloredName = "${color.getChatColor()}${name}"
 
             var suffix = ""
@@ -159,6 +154,7 @@ object IslandAreas {
                 passedAreas.remove("null")
                 passedAreas.remove(currentAreaName)
                 // so show areas needed to pass thorough
+                // TODO show this pass through in the /shnavigate command
                 if (passedAreas.isNotEmpty()) {
 //                     suffix = " §7${passedAreas.joinToString(", ")}"
                 }
@@ -268,9 +264,13 @@ object IslandAreas {
 
     private fun setTarget(node: GraphNode) {
         targetNode = node
+        val tag = node.getAreaTag() ?: return
+        val displayName = tag.color.getChatColor() + node.name
         val color = config.pathfinder.color.get().toChromaColor()
-        IslandGraphs.pathFind(
-            node.position, color,
+        node.pathFind(
+            displayName,
+            color,
+            allowRerouting = true,
             onFound = {
                 targetNode = null
                 updatePosition()
diff --git a/src/main/java/at/hannibal2/skyhanni/features/misc/pathfind/NavigationHelper.kt b/src/main/java/at/hannibal2/skyhanni/features/misc/pathfind/NavigationHelper.kt
index e753948bc..48719e63e 100644
--- a/src/main/java/at/hannibal2/skyhanni/features/misc/pathfind/NavigationHelper.kt
+++ b/src/main/java/at/hannibal2/skyhanni/features/misc/pathfind/NavigationHelper.kt
@@ -2,11 +2,12 @@ package at.hannibal2.skyhanni.features.misc.pathfind
 
 import at.hannibal2.skyhanni.SkyHanniMod
 import at.hannibal2.skyhanni.data.IslandGraphs
+import at.hannibal2.skyhanni.data.IslandGraphs.pathFind
 import at.hannibal2.skyhanni.data.model.GraphNode
 import at.hannibal2.skyhanni.data.model.GraphNodeTag
-import at.hannibal2.skyhanni.data.model.findShortestDistance
 import at.hannibal2.skyhanni.features.misc.IslandAreas
 import at.hannibal2.skyhanni.utils.CollectionUtils.sorted
+import at.hannibal2.skyhanni.utils.GraphUtils
 import at.hannibal2.skyhanni.utils.NumberUtil.roundTo
 import at.hannibal2.skyhanni.utils.chat.Text
 import at.hannibal2.skyhanni.utils.chat.Text.asComponent
@@ -57,7 +58,8 @@ object NavigationHelper {
             val distance = distances[node]!!.roundTo(1)
             val component = "$name §e$distance".asComponent()
             component.onClick {
-                IslandGraphs.pathFind(node.position)
+//                 node.pathFind(label = node.name!!, allowRerouting = true)
+                node.pathFind(label = name, allowRerouting = true)
                 sendNavigateMessage(name, goBack)
             }
             val tag = node.tags.first { it in allowedTags }
@@ -102,7 +104,7 @@ object NavigationHelper {
             val remainingTags = node.tags.filter { it in allowedTags }
             if (remainingTags.isEmpty()) continue
             if (name.lowercase().contains(searchTerm)) {
-                distances[node] = graph.findShortestDistance(closedNote, node)
+                distances[node] = GraphUtils.findShortestDistance(closedNote, node)
             }
             if (remainingTags.size != 1) {
                 println("found node with invalid amount of tags: ${node.name} (${remainingTags.map { it.cleanName }}")
diff --git a/src/main/java/at/hannibal2/skyhanni/features/rift/everywhere/EnigmaSoulWaypoints.kt b/src/main/java/at/hannibal2/skyhanni/features/rift/everywhere/EnigmaSoulWaypoints.kt
index 2a4613f7a..41ad12763 100644
--- a/src/main/java/at/hannibal2/skyhanni/features/rift/everywhere/EnigmaSoulWaypoints.kt
+++ b/src/main/java/at/hannibal2/skyhanni/features/rift/everywhere/EnigmaSoulWaypoints.kt
@@ -111,7 +111,8 @@ object EnigmaSoulWaypoints {
             ChatUtils.chat("§5Tracking the $name Enigma Soul!", prefixColor = "§5")
             if (config.showPathFinder) {
                 soulLocations[name]?.let {
-                    IslandGraphs.pathFind(it, config.color.toChromaColor(), condition = { config.showPathFinder })
+
+                    IslandGraphs.pathFind(it, "$name Enigma Soul", config.color.toChromaColor(), condition = { config.showPathFinder })
                 }
             }
             trackedSouls.add(name)
diff --git a/src/main/java/at/hannibal2/skyhanni/test/SkyHanniDebugsAndTests.kt b/src/main/java/at/hannibal2/skyhanni/test/SkyHanniDebugsAndTests.kt
index 540dc7fa2..55f88d2bf 100644
--- a/src/main/java/at/hannibal2/skyhanni/test/SkyHanniDebugsAndTests.kt
+++ b/src/main/java/at/hannibal2/skyhanni/test/SkyHanniDebugsAndTests.kt
@@ -126,7 +126,7 @@ object SkyHanniDebugsAndTests {
         val location = LorenzVec(x, y, z)
         testLocation = location
         if (args.getOrNull(3) == "pathfind") {
-            IslandGraphs.pathFind(location)
+            IslandGraphs.pathFind(location, "/shtestwaypoint", showGoalExact = true)
         }
         ChatUtils.chat("set test waypoint")
     }
diff --git a/src/main/java/at/hannibal2/skyhanni/test/graph/GraphEditor.kt b/src/main/java/at/hannibal2/skyhanni/test/graph/GraphEditor.kt
index 162386d43..9c9b5bb06 100644
--- a/src/main/java/at/hannibal2/skyhanni/test/graph/GraphEditor.kt
+++ b/src/main/java/at/hannibal2/skyhanni/test/graph/GraphEditor.kt
@@ -6,8 +6,6 @@ import at.hannibal2.skyhanni.data.model.Graph
 import at.hannibal2.skyhanni.data.model.GraphNode
 import at.hannibal2.skyhanni.data.model.GraphNodeTag
 import at.hannibal2.skyhanni.data.model.TextInput
-import at.hannibal2.skyhanni.data.model.findShortestPathAsGraph
-import at.hannibal2.skyhanni.data.model.toJson
 import at.hannibal2.skyhanni.events.GuiRenderEvent
 import at.hannibal2.skyhanni.events.LorenzRenderWorldEvent
 import at.hannibal2.skyhanni.events.LorenzTickEvent
@@ -15,6 +13,7 @@ import at.hannibal2.skyhanni.skyhannimodule.SkyHanniModule
 import at.hannibal2.skyhanni.test.command.ErrorManager
 import at.hannibal2.skyhanni.utils.ChatUtils
 import at.hannibal2.skyhanni.utils.ColorUtils
+import at.hannibal2.skyhanni.utils.GraphUtils
 import at.hannibal2.skyhanni.utils.KeyboardManager
 import at.hannibal2.skyhanni.utils.KeyboardManager.isKeyClicked
 import at.hannibal2.skyhanni.utils.KeyboardManager.isKeyHeld
@@ -581,7 +580,7 @@ object GraphEditor {
         val current = compiled.firstOrNull { it.position == savedCurrent.position } ?: return
         val goal = compiled.firstOrNull { it.position == savedActive.position } ?: return
 
-        val path = compiled.findShortestPathAsGraph(current, goal)
+        val path = GraphUtils.findShortestPathAsGraph(current, goal)
 
         val inGraph = path.map { nodes[it.id] }
         highlightedNodes.addAll(inGraph)
diff --git a/src/main/java/at/hannibal2/skyhanni/test/graph/GraphEditorBugFinder.kt b/src/main/java/at/hannibal2/skyhanni/test/graph/GraphEditorBugFinder.kt
index 2c5656580..75d8c3ea2 100644
--- a/src/main/java/at/hannibal2/skyhanni/test/graph/GraphEditorBugFinder.kt
+++ b/src/main/java/at/hannibal2/skyhanni/test/graph/GraphEditorBugFinder.kt
@@ -2,6 +2,7 @@ package at.hannibal2.skyhanni.test.graph
 
 import at.hannibal2.skyhanni.SkyHanniMod
 import at.hannibal2.skyhanni.data.IslandGraphs
+import at.hannibal2.skyhanni.data.IslandGraphs.pathFind
 import at.hannibal2.skyhanni.data.model.GraphNode
 import at.hannibal2.skyhanni.events.LorenzRenderWorldEvent
 import at.hannibal2.skyhanni.features.misc.IslandAreas.getAreaTag
@@ -9,7 +10,6 @@ import at.hannibal2.skyhanni.features.misc.pathfind.NavigationHelper
 import at.hannibal2.skyhanni.skyhannimodule.SkyHanniModule
 import at.hannibal2.skyhanni.test.graph.GraphEditor.distanceSqToPlayer
 import at.hannibal2.skyhanni.utils.GraphUtils
-import at.hannibal2.skyhanni.utils.LorenzVec
 import at.hannibal2.skyhanni.utils.RenderUtils.drawDynamicText
 import kotlinx.coroutines.launch
 import net.minecraftforge.fml.common.eventhandler.SubscribeEvent
@@ -18,7 +18,7 @@ import java.awt.Color
 // Trying to find errors in Area Graph for the current graph editor instance
 @SkyHanniModule
 object GraphEditorBugFinder {
-    private var errorsInWorld = emptyMap<LorenzVec, String>()
+    private var errorsInWorld = emptyMap<GraphNode, String>()
 
     fun runTests() {
         SkyHanniMod.coroutineScope.launch {
@@ -28,21 +28,22 @@ object GraphEditorBugFinder {
 
     private fun asyncTest() {
         val graph = IslandGraphs.currentIslandGraph ?: return
-        val errorsInWorld: MutableMap<LorenzVec, String> = mutableMapOf()
+        val errorsInWorld: MutableMap<GraphNode, String> = mutableMapOf()
         val nodes = graph.nodes
 
         for (node in nodes) {
             if (node.tags.any { it in NavigationHelper.allowedTags }) {
                 val remainingTags = node.tags.filter { it in NavigationHelper.allowedTags }
                 if (remainingTags.size != 1) {
-                    errorsInWorld[node.position] = "§cConflicting tags: $remainingTags"
+                    errorsInWorld[node] = "§cConflicting tags: $remainingTags"
                 }
             }
         }
 
+
         val nearestArea = mutableMapOf<GraphNode, GraphNode>()
         for (node in nodes) {
-            val pathToNearestArea = GraphUtils.findFastestPath(graph, node) { it.getAreaTag(ignoreConfig = true) != null }?.first
+            val pathToNearestArea = GraphUtils.findFastestPath(node) { it.getAreaTag(ignoreConfig = true) != null }?.first
             if (pathToNearestArea == null) {
                 continue
             }
@@ -57,7 +58,7 @@ object GraphEditorBugFinder {
                 if (neighbouringAreaNode == areaNode) continue
                 if ((null == node.getAreaTag(ignoreConfig = true))) {
                     bugs++
-                    errorsInWorld[node.position] = "§cConflicting areas $areaNode and $neighbouringAreaNode"
+                    errorsInWorld[node] = "§cConflicting areas $areaNode and $neighbouringAreaNode"
                 }
             }
         }
@@ -65,11 +66,11 @@ object GraphEditorBugFinder {
             val nameNull = node.name.isNullOrBlank()
             val tagsEmpty = node.tags.isEmpty()
             if (nameNull > tagsEmpty) {
-                errorsInWorld[node.position] = "§cMissing name despite having tags"
+                errorsInWorld[node] = "§cMissing name despite having tags"
                 bugs++
             }
             if (tagsEmpty > nameNull) {
-                errorsInWorld[node.position] = "§cMissing tags despite having name"
+                errorsInWorld[node] = "§cMissing tags despite having name"
                 bugs++
             }
         }
@@ -80,18 +81,18 @@ object GraphEditorBugFinder {
             val foreignClusters = clusters.filter { it !== closestCluster }
             val closestForeignNodes = foreignClusters.map { network -> network.minBy { it.position.distanceSqToPlayer() } }
             closestForeignNodes.forEach {
-                errorsInWorld[it.position] = "§cDisjoint node network"
+                errorsInWorld[it] = "§cDisjoint node network"
                 bugs++
             }
             val closestForeignNode = closestForeignNodes.minBy { it.position.distanceSqToPlayer() }
             val closestNodeToForeignNode = closestCluster.minBy { it.position.distanceSq(closestForeignNode.position) }
-            IslandGraphs.pathFind(closestNodeToForeignNode.position, Color.RED)
+            closestNodeToForeignNode.pathFind("Graph Editor Bug", Color.RED)
         }
 
         println("found $bugs bugs!")
         this.errorsInWorld = errorsInWorld
         if (clusters.size <= 1) {
-            IslandGraphs.pathFind(errorsInWorld.keys.minByOrNull { it.distanceSqToPlayer() } ?: return, Color.RED)
+            errorsInWorld.keys.minByOrNull { it.position.distanceSqToPlayer() }?.pathFind("Graph Editor Bug", Color.RED)
         }
     }
 
@@ -99,8 +100,8 @@ object GraphEditorBugFinder {
     fun onRenderWorld(event: LorenzRenderWorldEvent) {
         if (!isEnabled()) return
 
-        for ((location, text) in errorsInWorld) {
-            event.drawDynamicText(location, text, 1.5)
+        for ((node, text) in errorsInWorld) {
+            event.drawDynamicText(node.position, text, 1.5)
         }
     }
 
diff --git a/src/main/java/at/hannibal2/skyhanni/utils/GraphUtils.kt b/src/main/java/at/hannibal2/skyhanni/utils/GraphUtils.kt
index 4a245a894..0d15b99ed 100644
--- a/src/main/java/at/hannibal2/skyhanni/utils/GraphUtils.kt
+++ b/src/main/java/at/hannibal2/skyhanni/utils/GraphUtils.kt
@@ -1,10 +1,10 @@
 package at.hannibal2.skyhanni.utils
 
+import at.hannibal2.skyhanni.data.model.DijkstraTree
 import at.hannibal2.skyhanni.data.model.Graph
 import at.hannibal2.skyhanni.data.model.GraphNode
-import at.hannibal2.skyhanni.data.model.findAllShortestDistances
-import at.hannibal2.skyhanni.data.model.findDijkstraDistances
 import at.hannibal2.skyhanni.data.model.findPathToDestination
+import java.util.PriorityQueue
 import java.util.Stack
 
 object GraphUtils {
@@ -12,11 +12,10 @@ object GraphUtils {
      * Find the fastest path from [closestNode] to *any* node that matches [condition].
      */
     fun findFastestPath(
-        graph: Graph,
         closestNode: GraphNode,
         condition: (GraphNode) -> Boolean,
     ): Pair<Graph, Double>? {
-        val distances = graph.findDijkstraDistances(closestNode, condition)
+        val distances = findDijkstraDistances(closestNode, condition)
         val entry = distances.lastVisitedNode.takeIf(condition)
         return entry?.let {
             distances.findPathToDestination(it)
@@ -34,7 +33,7 @@ object GraphUtils {
         val paths = mutableMapOf<GraphNode, Graph>()
 
         val map = mutableMapOf<GraphNode, Double>()
-        val distances = graph.findAllShortestDistances(closestNode)
+        val distances = findAllShortestDistances(closestNode)
         for (graphNode in graph.nodes) {
             if (!condition(graphNode)) continue
             val (path, distance) = distances.findPathToDestination(graphNode)
@@ -65,4 +64,66 @@ object GraphUtils {
         }
         return allClusters
     }
+
+    fun findShortestPathAsGraph(start: GraphNode, end: GraphNode): Graph = findShortestPathAsGraphWithDistance(start, end).first
+
+    /**
+     * Find a tree of distances to the [start] node using dijkstra's algorithm.
+     */
+    fun findDijkstraDistances(
+        start: GraphNode,
+        /**
+         * Bail out early before collecting all the distances to all nodes in the graph. This will not collect valid distance data for *all*
+         * nodes for which bailout matches, but only the closest one.
+         */
+        bailout: (GraphNode) -> Boolean,
+    ): DijkstraTree {
+        val distances = mutableMapOf<GraphNode, Double>()
+        val previous = mutableMapOf<GraphNode, GraphNode>()
+        val visited = mutableSetOf<GraphNode>()
+        val queue = PriorityQueue<GraphNode>(compareBy { distances.getOrDefault(it, Double.MAX_VALUE) })
+        var lastVisitedNode: GraphNode = start
+
+        distances[start] = 0.0
+        queue.add(start)
+
+        while (queue.isNotEmpty()) {
+            val current = queue.poll()
+            lastVisitedNode = current
+            if (bailout(current)) break
+
+            visited.add(current)
+
+            current.neighbours.forEach { (neighbour, weight) ->
+                if (neighbour !in visited) {
+                    val newDistance = distances.getValue(current) + weight
+                    if (newDistance < distances.getOrDefault(neighbour, Double.MAX_VALUE)) {
+                        distances[neighbour] = newDistance
+                        previous[neighbour] = current
+                        queue.add(neighbour)
+                    }
+                }
+            }
+        }
+
+        return DijkstraTree(
+            start,
+            distances,
+            previous,
+            lastVisitedNode,
+        )
+    }
+
+    fun findAllShortestDistances(start: GraphNode): DijkstraTree {
+        return findDijkstraDistances(start) { false }
+    }
+
+    fun findShortestPathAsGraphWithDistance(start: GraphNode, end: GraphNode): Pair<Graph, Double> {
+        val distances = findDijkstraDistances(start) { it == end }
+        return distances.findPathToDestination(end)
+    }
+
+    fun findShortestPath(start: GraphNode, end: GraphNode): List<LorenzVec> = findShortestPathAsGraph(start, end).toPositionsList()
+
+    fun findShortestDistance(start: GraphNode, end: GraphNode): Double = findShortestPathAsGraphWithDistance(start, end).second
 }
diff --git a/src/main/java/at/hannibal2/skyhanni/utils/RenderUtils.kt b/src/main/java/at/hannibal2/skyhanni/utils/RenderUtils.kt
index 805db90f5..d22ecc85e 100644
--- a/src/main/java/at/hannibal2/skyhanni/utils/RenderUtils.kt
+++ b/src/main/java/at/hannibal2/skyhanni/utils/RenderUtils.kt
@@ -5,7 +5,6 @@ import at.hannibal2.skyhanni.data.GuiEditManager
 import at.hannibal2.skyhanni.data.GuiEditManager.getAbsX
 import at.hannibal2.skyhanni.data.GuiEditManager.getAbsY
 import at.hannibal2.skyhanni.data.model.Graph
-import at.hannibal2.skyhanni.data.model.toPositionsList
 import at.hannibal2.skyhanni.events.GuiContainerEvent
 import at.hannibal2.skyhanni.events.GuiRenderItemEvent
 import at.hannibal2.skyhanni.events.LorenzRenderWorldEvent
@@ -1280,7 +1279,8 @@ object RenderUtils {
         waypointColor: Color =
             (path.lastOrNull()?.name?.getFirstColorCode()?.toLorenzColor() ?: LorenzColor.WHITE).toColor(),
         bezierPoint: Double = 1.0,
-        showNoteNames: Boolean = false
+        showNoteNames: Boolean = false,
+        markLastBlock: Boolean = true,
     ) {
         if (path.isEmpty()) return
         val points = if (startAtEye) {
@@ -1307,8 +1307,10 @@ object RenderUtils {
                 this.drawDynamicText(it.position, it.name!!, textSize)
             }
         }
-        val last = path.last()
-        drawWaypointFilled(last.position, waypointColor, seeThroughBlocks = true)
+        if (markLastBlock) {
+            val last = path.last()
+            drawWaypointFilled(last.position, waypointColor, seeThroughBlocks = true)
+        }
     }
 
     class LineDrawer @PublishedApi internal constructor(val tessellator: Tessellator) {
@@ -1586,7 +1588,11 @@ object RenderUtils {
 
     // TODO nea please merge with 'draw3DLine'
     fun LorenzRenderWorldEvent.draw3DLine_nea(
-        p1: LorenzVec, p2: LorenzVec, color: Color, lineWidth: Int, depth: Boolean,
+        p1: LorenzVec,
+        p2: LorenzVec,
+        color: Color,
+        lineWidth: Int,
+        depth: Boolean,
     ) {
         GlStateManager.disableCull()