From abbb0d9ca0b1164d2fa720ba3e255d8f3ed2d6be Mon Sep 17 00:00:00 2001
From: someaddons <38401808+someaddons@users.noreply.github.com>
Date: Sun, 12 Jan 2025 18:09:10 +0100
Subject: [PATCH] Reworked stuck handling, nonsafe destinations are now
 eligible for partials stuck handling. (#10588)

Reworked stuck handling, nonsafe destinations are now eligible for partial stuck handling.
Calling .stop on the navigator signals no further entity movement is desired, also reset stuck handling.
Fixed a bug causing the stuck handler to not progress further than random moving(outside of global timeout tp)
Fixed a bug where citizens started from a wrong position, when they were standing on the edge of a block
Citizen info command no longer prints duplicate information, nor sends it to all OP. Now also displays stuck level.
Citizen info command now allows teleporting to the displayed coordinates on click
---
 .../IDynamicHeuristicNavigator.java           |   2 +-
 .../pathfinding/IMinecoloniesNavigator.java   |  71 +++++++
 .../api/entity/pathfinding/IStuckHandler.java |  15 +-
 .../citizencommands/CommandCitizenInfo.java   |  94 +++++-----
 .../core/entity/ai/minimal/EntityAISleep.java |   2 +-
 .../ai/workers/AbstractEntityAIBasic.java     |   6 +-
 .../entity/mobs/aitasks/RaiderWalkAI.java     |   2 +-
 .../entity/pathfinding/PathfindingUtils.java  |  31 +++-
 .../AbstractAdvancedPathNavigate.java         |  22 +--
 .../MinecoloniesAdvancedPathNavigate.java     | 175 ++++++++++--------
 .../navigation/PathingStuckHandler.java       | 155 +++++++++-------
 .../pathfinding/pathjobs/AbstractPathJob.java |   2 +-
 12 files changed, 355 insertions(+), 222 deletions(-)
 rename src/main/java/com/minecolonies/{core/entity/pathfinding/navigation => api/entity/pathfinding}/IDynamicHeuristicNavigator.java (78%)
 create mode 100644 src/main/java/com/minecolonies/api/entity/pathfinding/IMinecoloniesNavigator.java

diff --git a/src/main/java/com/minecolonies/core/entity/pathfinding/navigation/IDynamicHeuristicNavigator.java b/src/main/java/com/minecolonies/api/entity/pathfinding/IDynamicHeuristicNavigator.java
similarity index 78%
rename from src/main/java/com/minecolonies/core/entity/pathfinding/navigation/IDynamicHeuristicNavigator.java
rename to src/main/java/com/minecolonies/api/entity/pathfinding/IDynamicHeuristicNavigator.java
index 19494a22014..af7960c7de2 100644
--- a/src/main/java/com/minecolonies/core/entity/pathfinding/navigation/IDynamicHeuristicNavigator.java
+++ b/src/main/java/com/minecolonies/api/entity/pathfinding/IDynamicHeuristicNavigator.java
@@ -1,4 +1,4 @@
-package com.minecolonies.core.entity.pathfinding.navigation;
+package com.minecolonies.api.entity.pathfinding;
 
 /**
  * Interface for navigators which keep an internal heuristic mod
diff --git a/src/main/java/com/minecolonies/api/entity/pathfinding/IMinecoloniesNavigator.java b/src/main/java/com/minecolonies/api/entity/pathfinding/IMinecoloniesNavigator.java
new file mode 100644
index 00000000000..6f6543eb3c8
--- /dev/null
+++ b/src/main/java/com/minecolonies/api/entity/pathfinding/IMinecoloniesNavigator.java
@@ -0,0 +1,71 @@
+package com.minecolonies.api.entity.pathfinding;
+
+import com.minecolonies.core.entity.pathfinding.navigation.MinecoloniesAdvancedPathNavigate;
+import com.minecolonies.core.entity.pathfinding.pathjobs.AbstractPathJob;
+import com.minecolonies.core.entity.pathfinding.pathresults.PathResult;
+import net.minecraft.core.BlockPos;
+import net.minecraft.world.entity.Mob;
+import org.jetbrains.annotations.NotNull;
+import org.jetbrains.annotations.Nullable;
+
+/**
+ * Describes the Navigator used by minecolonies entities
+ */
+public interface IMinecoloniesNavigator
+{
+    /**
+     * Sets a new pathjob to execute
+     *
+     * @param job             to run
+     * @param dest
+     * @param speedFactor
+     * @param safeDestination
+     * @param <T>
+     * @return null or new pathresult
+     */
+    @Nullable
+    <T extends AbstractPathJob> PathResult<T> setPathJob(
+        @NotNull AbstractPathJob job,
+        BlockPos dest,
+        double speedFactor, boolean safeDestination);
+
+    /**
+     * Indirectly triggers a recalulation, by marking the navigator as done
+     */
+    void recalc();
+
+    /**
+     * Returns the pathresult holding the current pathing task and result
+     *
+     * @return
+     */
+    PathResult getPathResult();
+
+    /**
+     * Gets the safe destination the entity wants to travel to
+     *
+     * @return
+     */
+    BlockPos getSafeDestination();
+
+    /**
+     * Gets the entity of the navigator
+     *
+     * @return
+     */
+    Mob getOurEntity();
+
+    /**
+     * Pauses the navigator for X ticks from starting any new pathing tasks
+     *
+     * @param pauseTicks
+     */
+    void setPauseTicks(int pauseTicks);
+
+    /**
+     * Returns the stuck handler used by the navigator
+     *
+     * @return
+     */
+    IStuckHandler<MinecoloniesAdvancedPathNavigate> getStuckHandler();
+}
diff --git a/src/main/java/com/minecolonies/api/entity/pathfinding/IStuckHandler.java b/src/main/java/com/minecolonies/api/entity/pathfinding/IStuckHandler.java
index ff9d5b2b48a..a416778b4d2 100644
--- a/src/main/java/com/minecolonies/api/entity/pathfinding/IStuckHandler.java
+++ b/src/main/java/com/minecolonies/api/entity/pathfinding/IStuckHandler.java
@@ -1,16 +1,25 @@
 package com.minecolonies.api.entity.pathfinding;
 
-import com.minecolonies.core.entity.pathfinding.navigation.AbstractAdvancedPathNavigate;
+import net.minecraft.world.entity.ai.navigation.PathNavigation;
 
 /**
  * Stuck handler for pathing, gets called to check/deal with stuck status
  */
-public interface IStuckHandler
+public interface IStuckHandler<NAV extends PathNavigation & IMinecoloniesNavigator>
 {
     /**
      * Checks if the navigator is stuck
      *
      * @param navigator navigator to check
      */
-    void checkStuck(final AbstractAdvancedPathNavigate navigator);
+    void checkStuck(final NAV navigator);
+
+    void resetGlobalStuckTimers();
+
+    /**
+     * Returns the stuck level (0-9) indicating how long the entity is stuck and which stuck actions got used
+     *
+     * @return
+     */
+    public int getStuckLevel();
 }
diff --git a/src/main/java/com/minecolonies/core/commands/citizencommands/CommandCitizenInfo.java b/src/main/java/com/minecolonies/core/commands/citizencommands/CommandCitizenInfo.java
index 1d1223d307d..05fcfe9ee3c 100755
--- a/src/main/java/com/minecolonies/core/commands/citizencommands/CommandCitizenInfo.java
+++ b/src/main/java/com/minecolonies/core/commands/citizencommands/CommandCitizenInfo.java
@@ -4,7 +4,7 @@
 import com.minecolonies.api.colony.IColony;
 import com.minecolonies.api.colony.IColonyManager;
 import com.minecolonies.api.entity.citizen.AbstractEntityCitizen;
-import com.minecolonies.api.entity.citizen.Skill;
+import com.minecolonies.api.entity.pathfinding.IMinecoloniesNavigator;
 import com.minecolonies.api.util.constant.translation.CommandTranslationConstants;
 import com.minecolonies.core.colony.buildings.modules.WorkerBuildingModule;
 import com.minecolonies.core.commands.commandTypes.IMCColonyOfficerCommand;
@@ -15,7 +15,9 @@
 import com.mojang.brigadier.context.CommandContext;
 import net.minecraft.commands.CommandSourceStack;
 import net.minecraft.core.BlockPos;
+import net.minecraft.network.chat.ClickEvent;
 import net.minecraft.network.chat.Component;
+import net.minecraft.network.chat.Style;
 import net.minecraft.world.entity.Entity;
 import net.minecraft.world.level.Level;
 
@@ -44,7 +46,7 @@ public int onExecute(final CommandContext<CommandSourceStack> context)
         final IColony colony = IColonyManager.getInstance().getColonyByDimension(colonyID, sender == null ? Level.OVERWORLD : context.getSource().getLevel().dimension());
         if (colony == null)
         {
-            context.getSource().sendSuccess(() -> Component.translatable(CommandTranslationConstants.COMMAND_COLONY_ID_NOT_FOUND, colonyID), true);
+            context.getSource().sendSuccess(() -> Component.translatable(CommandTranslationConstants.COMMAND_COLONY_ID_NOT_FOUND, colonyID), false);
             return 0;
         }
 
@@ -52,10 +54,11 @@ public int onExecute(final CommandContext<CommandSourceStack> context)
 
         if (citizenData == null)
         {
-            context.getSource().sendSuccess(() -> Component.translatable(CommandTranslationConstants.COMMAND_CITIZEN_NOT_FOUND), true);
+            context.getSource().sendSuccess(() -> Component.translatable(CommandTranslationConstants.COMMAND_CITIZEN_NOT_FOUND), false);
             return 0;
         }
 
+        context.getSource().sendSuccess(() -> Component.translatable(CommandTranslationConstants.COMMAND_CITIZEN_INFO, citizenData.getId(), citizenData.getName()), false);
         final Optional<AbstractEntityCitizen> optionalEntityCitizen = citizenData.getEntity();
 
         if (optionalEntityCitizen.isPresent())
@@ -67,45 +70,31 @@ public int onExecute(final CommandContext<CommandSourceStack> context)
               .sendSuccess(() -> Component.translatable(CommandTranslationConstants.COMMAND_CITIZEN_INFO_POSITION,
                 citizenPosition.getX(),
                 citizenPosition.getY(),
-                citizenPosition.getZ()), true);
-            final BlockPos homePosition = citizenData.getHomePosition();
-            context.getSource()
-              .sendSuccess(() -> Component.translatable(CommandTranslationConstants.COMMAND_CITIZEN_INFO_HOME_POSITION,
-                homePosition.getX(),
-                homePosition.getY(),
-                homePosition.getZ()), true);
+                  citizenPosition.getZ()).withStyle(styleWithTeleport(citizenPosition)), false);
 
             context.getSource()
-              .sendSuccess(() -> Component.translatable(CommandTranslationConstants.COMMAND_CITIZEN_INFO_HEALTH, entityCitizen.getHealth(), entityCitizen.getMaxHealth()), true);
+                .sendSuccess(() -> Component.translatable(CommandTranslationConstants.COMMAND_CITIZEN_INFO_HEALTH, entityCitizen.getHealth(), entityCitizen.getMaxHealth()), false);
         }
         else
         {
             context.getSource().sendSuccess(() -> Component.translatable(CommandTranslationConstants.COMMAND_CITIZEN_INFO_POSITION,
               citizenData.getLastPosition().getX(),
               citizenData.getLastPosition().getY(),
-              citizenData.getLastPosition().getZ()), true);
+                citizenData.getLastPosition().getZ()).withStyle(styleWithTeleport(citizenData.getLastPosition())), false);
 
-            context.getSource().sendSuccess(() -> Component.translatable(CommandTranslationConstants.COMMAND_CITIZEN_NOT_LOADED), true);
+            context.getSource().sendSuccess(() -> Component.translatable(CommandTranslationConstants.COMMAND_CITIZEN_NOT_LOADED), false);
         }
 
-        context.getSource().sendSuccess(() -> Component.translatable(CommandTranslationConstants.COMMAND_CITIZEN_INFO, citizenData.getId(), citizenData.getName()), true);
-
-        final AbstractEntityCitizen entityCitizen = optionalEntityCitizen.get();
-
-        final BlockPos citizenPosition = entityCitizen.blockPosition();
-        context.getSource()
-          .sendSuccess(() -> Component.translatable(CommandTranslationConstants.COMMAND_CITIZEN_INFO_POSITION,
-            citizenPosition.getX(),
-            citizenPosition.getY(),
-            citizenPosition.getZ()), true);
         final BlockPos homePosition = citizenData.getHomePosition();
         context.getSource()
-          .sendSuccess(() -> Component.translatable(CommandTranslationConstants.COMMAND_CITIZEN_INFO_HOME_POSITION, homePosition.getX(), homePosition.getY(), homePosition.getZ()),
-            true);
+            .sendSuccess(() -> Component.translatable(CommandTranslationConstants.COMMAND_CITIZEN_INFO_HOME_POSITION,
+                homePosition.getX(),
+                homePosition.getY(),
+                homePosition.getZ()).withStyle(styleWithTeleport(homePosition)), false);
 
         if (citizenData.getWorkBuilding() == null)
         {
-            context.getSource().sendSuccess(() -> Component.translatable(CommandTranslationConstants.COMMAND_CITIZEN_INFO_NO_WORKING_POSITION), true);
+            context.getSource().sendSuccess(() -> Component.translatable(CommandTranslationConstants.COMMAND_CITIZEN_INFO_NO_WORKING_POSITION), false);
         }
         else
         {
@@ -114,45 +103,52 @@ public int onExecute(final CommandContext<CommandSourceStack> context)
               .sendSuccess(() -> Component.translatable(CommandTranslationConstants.COMMAND_CITIZEN_INFO_WORKING_POSITION,
                 workingPosition.getX(),
                 workingPosition.getY(),
-                workingPosition.getZ()), true);
+                  workingPosition.getZ()).withStyle(styleWithTeleport(workingPosition)), false);
         }
 
-
-        Object[] skills =
-          new Object[] {citizenData.getCitizenSkillHandler().getSkills().get(Skill.Athletics).getLevel(),
-            citizenData.getCitizenSkillHandler().getSkills().get(Skill.Dexterity).getLevel(),
-            citizenData.getCitizenSkillHandler().getSkills().get(Skill.Strength).getLevel(),
-            citizenData.getCitizenSkillHandler().getSkills().get(Skill.Agility).getLevel(),
-            citizenData.getCitizenSkillHandler().getSkills().get(Skill.Stamina).getLevel(),
-            citizenData.getCitizenSkillHandler().getSkills().get(Skill.Mana).getLevel(),
-            citizenData.getCitizenSkillHandler().getSkills().get(Skill.Adaptability).getLevel(),
-            citizenData.getCitizenSkillHandler().getSkills().get(Skill.Focus).getLevel(),
-            citizenData.getCitizenSkillHandler().getSkills().get(Skill.Creativity).getLevel(),
-            citizenData.getCitizenSkillHandler().getSkills().get(Skill.Knowledge).getLevel(),
-            citizenData.getCitizenSkillHandler().getSkills().get(Skill.Intelligence).getLevel()};
-        context.getSource().sendSuccess(() -> Component.translatable(CommandTranslationConstants.COMMAND_CITIZEN_INFO_SKILLS, skills), true);
-
         if (citizenData.getJob() == null)
         {
-            context.getSource().sendSuccess(() -> Component.translatable(CommandTranslationConstants.COMMAND_CITIZEN_INFO_NO_JOB), true);
-            context.getSource().sendSuccess(() -> Component.translatable(CommandTranslationConstants.COMMAND_CITIZEN_INFO_NO_ACTIVITY), true);
+            context.getSource().sendSuccess(() -> Component.translatable(CommandTranslationConstants.COMMAND_CITIZEN_INFO_NO_JOB), false);
+            context.getSource().sendSuccess(() -> Component.translatable(CommandTranslationConstants.COMMAND_CITIZEN_INFO_NO_ACTIVITY), false);
         }
         else if (citizenData.getWorkBuilding() != null && citizenData.getWorkBuilding().hasModule(WorkerBuildingModule.class))
         {
             context.getSource()
               .sendSuccess(() -> Component.translatable(CommandTranslationConstants.COMMAND_CITIZEN_INFO_JOB,
-                citizenData.getWorkBuilding().getFirstModuleOccurance(WorkerBuildingModule.class).getJobEntry().getTranslationKey()), true);
+                  citizenData.getWorkBuilding().getFirstModuleOccurance(WorkerBuildingModule.class).getJobEntry().getTranslationKey()), false);
+
+            if (optionalEntityCitizen.isPresent())
+            {
+                final AbstractEntityCitizen entityCitizen = optionalEntityCitizen.get();
+                context.getSource()
+                    .sendSuccess(() -> Component.translatable(CommandTranslationConstants.COMMAND_CITIZEN_INFO_ACTIVITY,
+                        ((EntityCitizen) entityCitizen).getCitizenAI().getState(),
+                        entityCitizen.getCitizenJobHandler().getColonyJob().getNameTagDescription(),
+                        entityCitizen.getCitizenJobHandler().getWorkAI().getState()), false);
+            }
+        }
 
+        if (optionalEntityCitizen.isPresent())
+        {
+            final AbstractEntityCitizen entityCitizen = optionalEntityCitizen.get();
             context.getSource()
-              .sendSuccess(() -> Component.translatable(CommandTranslationConstants.COMMAND_CITIZEN_INFO_ACTIVITY,
-                ((EntityCitizen) entityCitizen).getCitizenAI().getState(),
-                entityCitizen.getCitizenJobHandler().getColonyJob().getNameTagDescription(),
-                entityCitizen.getCitizenJobHandler().getWorkAI().getState()), true);
+                .sendSuccess(() -> Component.literal("Stuck level: " + ((IMinecoloniesNavigator) entityCitizen.getNavigation()).getStuckHandler().getStuckLevel()), false);
         }
 
         return 1;
     }
 
+    /**
+     * Creates a style with clickable teleport
+     *
+     * @param pos
+     * @return
+     */
+    private static Style styleWithTeleport(final BlockPos pos)
+    {
+        return Style.EMPTY.withClickEvent(new ClickEvent(ClickEvent.Action.RUN_COMMAND, "/tp " + pos.getX() + " " + pos.getY() + " " + pos.getZ()));
+    }
+
     /**
      * Name string of the command.
      */
diff --git a/src/main/java/com/minecolonies/core/entity/ai/minimal/EntityAISleep.java b/src/main/java/com/minecolonies/core/entity/ai/minimal/EntityAISleep.java
index c2d9bb88344..bbbca8fa91a 100755
--- a/src/main/java/com/minecolonies/core/entity/ai/minimal/EntityAISleep.java
+++ b/src/main/java/com/minecolonies/core/entity/ai/minimal/EntityAISleep.java
@@ -201,7 +201,7 @@ private void findBedAndTryToSleep()
                 usedBed = homePos;
             }
 
-            if (EntityNavigationUtils.walkToPosInBuilding(citizen, usedBed, citizen.getCitizenData().getHomeBuilding(), 5))
+            if (EntityNavigationUtils.walkToPosInBuilding(citizen, usedBed, citizen.getCitizenData().getHomeBuilding(), 12))
             {
                 bedTicks++;
                 if (!citizen.getCitizenSleepHandler().trySleep(usedBed))
diff --git a/src/main/java/com/minecolonies/core/entity/ai/workers/AbstractEntityAIBasic.java b/src/main/java/com/minecolonies/core/entity/ai/workers/AbstractEntityAIBasic.java
index a01c9644ed4..2998d0822cc 100755
--- a/src/main/java/com/minecolonies/core/entity/ai/workers/AbstractEntityAIBasic.java
+++ b/src/main/java/com/minecolonies/core/entity/ai/workers/AbstractEntityAIBasic.java
@@ -36,10 +36,10 @@
 import com.minecolonies.core.colony.jobs.JobDeliveryman;
 import com.minecolonies.core.colony.requestsystem.resolvers.StationRequestResolver;
 import com.minecolonies.core.entity.pathfinding.navigation.EntityNavigationUtils;
-import com.minecolonies.core.util.citizenutils.CitizenItemUtils;
 import com.minecolonies.core.entity.pathfinding.proxy.EntityCitizenWalkToProxy;
 import com.minecolonies.core.tileentities.TileEntityRack;
 import com.minecolonies.core.util.WorkerUtil;
+import com.minecolonies.core.util.citizenutils.CitizenItemUtils;
 import com.mojang.authlib.GameProfile;
 import net.minecraft.core.BlockPos;
 import net.minecraft.core.Direction;
@@ -286,7 +286,7 @@ public void setWalkTo(final BlockPos walkto)
      */
     private IAIState walkToState()
     {
-        if (!walkWithProxy(walkTo, DEFAULT_RANGE_FOR_DELAY))
+        if (!walkToSafePos(walkTo))
         {
             return getState();
         }
@@ -900,6 +900,8 @@ protected final boolean walkWithProxy(@NotNull final BlockPos stand, final int r
             workOnBlock(null, DELAY_RECHECK);
             return true;
         }
+
+        worker.getNavigation().stop();
         return false;
     }
 
diff --git a/src/main/java/com/minecolonies/core/entity/mobs/aitasks/RaiderWalkAI.java b/src/main/java/com/minecolonies/core/entity/mobs/aitasks/RaiderWalkAI.java
index abef1781cbc..fca51630365 100644
--- a/src/main/java/com/minecolonies/core/entity/mobs/aitasks/RaiderWalkAI.java
+++ b/src/main/java/com/minecolonies/core/entity/mobs/aitasks/RaiderWalkAI.java
@@ -102,7 +102,7 @@ else if (raider.blockPosition().distSqr(targetBlock) < 25)
                 walkTimer = raider.level.getGameTime() + TICKS_SECOND * 30;
                 walkInBuilding = raider.getColony().getBuildingManager().getBuilding(targetBlock);
             }
-            else if (raider.getNavigation().isDone() || raider.getNavigation().getDesiredPos() == null)
+            else if (raider.getNavigation().isDone())
             {
                 final List<BlockPos> wayPoints = ((IColonyRaidEvent) event).getWayPoints();
                 final BlockPos moveToPos = ShipBasedRaiderUtils.chooseWaypointFor(wayPoints, raider.blockPosition(), targetBlock);
diff --git a/src/main/java/com/minecolonies/core/entity/pathfinding/PathfindingUtils.java b/src/main/java/com/minecolonies/core/entity/pathfinding/PathfindingUtils.java
index c58ce47c8c4..8db034fcbe3 100644
--- a/src/main/java/com/minecolonies/core/entity/pathfinding/PathfindingUtils.java
+++ b/src/main/java/com/minecolonies/core/entity/pathfinding/PathfindingUtils.java
@@ -81,11 +81,40 @@ public static void syncDebugReachedPositions(final HashSet<BlockPos> reached, fi
      */
     public static BlockPos prepareStart(@NotNull final LivingEntity entity)
     {
-        @NotNull BlockPos.MutableBlockPos pos = new BlockPos.MutableBlockPos(Mth.floor(entity.getX()),
+        final BlockPos.MutableBlockPos pos = new BlockPos.MutableBlockPos(Mth.floor(entity.getX()),
             Mth.floor(entity.getY()),
             Mth.floor(entity.getZ()));
         final Level level = entity.level;
         BlockState bs = level.getBlockState(pos);
+
+        // Check if the entity is standing ontop of another block with part of its bb
+        final BlockPos.MutableBlockPos below = new BlockPos.MutableBlockPos(pos.getX(), pos.getY() - 1, pos.getZ());
+        if (entity.onGround() && SurfaceType.getSurfaceType(level, level.getBlockState(below), below) != SurfaceType.WALKABLE)
+        {
+            int minX = Mth.floor(entity.getBoundingBox().minX);
+            int minZ = Mth.floor(entity.getBoundingBox().minZ);
+            int maxX = Mth.floor(entity.getBoundingBox().maxX);
+            int maxZ = Mth.floor(entity.getBoundingBox().maxZ);
+
+            for (int x = minX; x <= maxX; x++)
+            {
+                for (int z = minZ; z <= maxZ; z++)
+                {
+                    final BlockPos toCheck = new BlockPos(x, below.getY(), z);
+                    // Only check other positions than the current
+                    if ((x != pos.getX() || z != pos.getZ())
+                        && SurfaceType.getSurfaceType(level, level.getBlockState(toCheck), toCheck) == SurfaceType.WALKABLE
+                        && Math.abs(ShapeUtil.max(level.getBlockState(toCheck).getCollisionShape(level, toCheck), Direction.Axis.Y) + toCheck.getY() - entity.getY()) < 0.1)
+                    {
+                        pos.setX(x);
+                        pos.setZ(z);
+                        below.setX(x);
+                        below.setZ(z);
+                    }
+                }
+            }
+        }
+
         // 1 Up when we're standing within this collision shape
         final VoxelShape collisionShape = bs.getCollisionShape(level, pos);
         final boolean isFineToStandIn = canStandInSolidBlock(bs);
diff --git a/src/main/java/com/minecolonies/core/entity/pathfinding/navigation/AbstractAdvancedPathNavigate.java b/src/main/java/com/minecolonies/core/entity/pathfinding/navigation/AbstractAdvancedPathNavigate.java
index 8b2a8688af2..6125fb6464b 100644
--- a/src/main/java/com/minecolonies/core/entity/pathfinding/navigation/AbstractAdvancedPathNavigate.java
+++ b/src/main/java/com/minecolonies/core/entity/pathfinding/navigation/AbstractAdvancedPathNavigate.java
@@ -23,8 +23,6 @@ public abstract class AbstractAdvancedPathNavigate extends GroundPathNavigation
 {
     //  Parent class private members
     protected final Mob    ourEntity;
-    @Nullable
-    protected       BlockPos     destination;
     protected       double       walkSpeedFactor = 1.0D;
     @Nullable
     protected       BlockPos     originalDestination;
@@ -42,17 +40,6 @@ public AbstractAdvancedPathNavigate(
         this.ourEntity = mob;
     }
 
-    /**
-     * Get the destination from the path.
-     *
-     * @return the destination position.
-     */
-    @Nullable
-    public BlockPos getDestination()
-    {
-        return destination;
-    }
-
     /**
      * Used to path away from a position.
      *
@@ -210,19 +197,12 @@ public Mob getOurEntity()
         return ourEntity;
     }
 
-    /**
-     * Gets the desired to go position
-     *
-     * @return desired go to pos
-     */
-    public abstract BlockPos getDesiredPos();
-
     /**
      * Sets the desired position to reach, used also for stuckhandling
      *
      * @param pos
      */
-    public abstract void setDesiredPos(BlockPos pos);
+    public abstract void setSafeDestinationPos(BlockPos pos);
 
     /**
      * Sets the stuck handler for this navigator
diff --git a/src/main/java/com/minecolonies/core/entity/pathfinding/navigation/MinecoloniesAdvancedPathNavigate.java b/src/main/java/com/minecolonies/core/entity/pathfinding/navigation/MinecoloniesAdvancedPathNavigate.java
index a071975cc6a..959961b3d9d 100644
--- a/src/main/java/com/minecolonies/core/entity/pathfinding/navigation/MinecoloniesAdvancedPathNavigate.java
+++ b/src/main/java/com/minecolonies/core/entity/pathfinding/navigation/MinecoloniesAdvancedPathNavigate.java
@@ -5,6 +5,8 @@
 import com.minecolonies.api.entity.ModEntities;
 import com.minecolonies.api.entity.citizen.AbstractEntityCitizen;
 import com.minecolonies.api.entity.other.MinecoloniesMinecart;
+import com.minecolonies.api.entity.pathfinding.IDynamicHeuristicNavigator;
+import com.minecolonies.api.entity.pathfinding.IMinecoloniesNavigator;
 import com.minecolonies.api.entity.pathfinding.IStuckHandler;
 import com.minecolonies.api.util.*;
 import com.minecolonies.core.entity.pathfinding.PathFindingStatus;
@@ -20,7 +22,6 @@
 import net.minecraft.util.Mth;
 import net.minecraft.world.entity.Entity;
 import net.minecraft.world.entity.Mob;
-import net.minecraft.world.entity.Pose;
 import net.minecraft.world.entity.ai.attributes.Attributes;
 import net.minecraft.world.level.BlockGetter;
 import net.minecraft.world.level.ChunkPos;
@@ -49,7 +50,7 @@
  * Minecolonies async PathNavigate.
  */
 // TODO: Rework
-public class MinecoloniesAdvancedPathNavigate extends AbstractAdvancedPathNavigate implements IDynamicHeuristicNavigator
+public class MinecoloniesAdvancedPathNavigate extends AbstractAdvancedPathNavigate implements IDynamicHeuristicNavigator, IMinecoloniesNavigator
 {
     private static final double ON_PATH_SPEED_MULTIPLIER = 1.3D;
     public static final  double MIN_Y_DISTANCE           = 0.001;
@@ -67,17 +68,12 @@ public class MinecoloniesAdvancedPathNavigate extends AbstractAdvancedPathNaviga
     /**
      * Desired position to reach
      */
-    private BlockPos desiredPos;
-
-    /**
-     * Timeout for the desired pos, resets when its no longer wanted
-     */
-    private int desiredPosTimeout = 0;
+    private BlockPos safeDestinationPos;
 
     /**
      * The stuck handler to use
      */
-    private IStuckHandler stuckHandler;
+    private IStuckHandler<MinecoloniesAdvancedPathNavigate> stuckHandler;
 
     /**
      * Whether we did set sneaking
@@ -99,6 +95,11 @@ public class MinecoloniesAdvancedPathNavigate extends AbstractAdvancedPathNaviga
      */
     private int pauseTicks = 0;
 
+    /**
+     * Increasing amount for pause times, each time a path fails
+     */
+    private int pauseTickBackupAmount = 10;
+
     /**
      * Temporary block position
      */
@@ -109,6 +110,16 @@ public class MinecoloniesAdvancedPathNavigate extends AbstractAdvancedPathNaviga
      */
     private Vec3 wantedPosition = null;
 
+    /**
+     * The recheck delay for checking stuck
+     */
+    private int checkStuckDelay = 10;
+
+    /**
+     * Time at which a path finished
+     */
+    private long finishTime = Long.MAX_VALUE;
+
     /**
      * Instantiates the navigation of an ourEntity.
      *
@@ -131,12 +142,6 @@ public MinecoloniesAdvancedPathNavigate(@NotNull final Mob entity, final Level w
         stuckHandler = PathingStuckHandler.createStuckHandler().withTakeDamageOnStuck(0.2f).withTeleportSteps(6).withTeleportOnFullStuck();
     }
 
-    @Override
-    public BlockPos getDestination()
-    {
-        return destination;
-    }
-
     @Nullable
     protected PathResult<PathJobMoveAwayFromLocation> walkAwayFrom(final BlockPos avoid, final double range, final double speedFactor, final boolean safeDestination)
     {
@@ -238,6 +243,7 @@ protected PathResult<PathJobMoveCloseToXNearY> walkCloseToXNearY(
     }
 
     @Nullable
+    @Override
     public <T extends AbstractPathJob> PathResult<T> setPathJob(
       @NotNull final AbstractPathJob job,
       final BlockPos dest,
@@ -248,7 +254,14 @@ public <T extends AbstractPathJob> PathResult<T> setPathJob(
             return null;
         }
 
-        stop();
+        if (pathResult != null)
+        {
+            pathResult.cancel();
+            pathResult.setStatus(PathFindingStatus.CANCELLED);
+            pathResult = null;
+        }
+        super.stop();
+
         if (dest != null && !dest.equals(BlockPos.ZERO))
         {
             if (job.getStart().distSqr(dest) > 500 * 500)
@@ -262,15 +275,11 @@ public <T extends AbstractPathJob> PathResult<T> setPathJob(
             }
         }
 
-        this.destination = dest;
+        finishTime = Long.MAX_VALUE;
         this.originalDestination = dest;
         if (safeDestination)
         {
-            desiredPos = dest;
-            if (dest != null)
-            {
-                desiredPosTimeout = 50 * 20;
-            }
+            safeDestinationPos = dest;
         }
 
         this.walkSpeedFactor = speedFactor;
@@ -296,12 +305,10 @@ public boolean isDone()
     @Override
     public void tick()
     {
-        if (desiredPosTimeout > 0)
+        if (checkStuckDelay-- < 0)
         {
-            if (--desiredPosTimeout <= 0)
-            {
-                desiredPos = null;
-            }
+            checkStuckDelay = 10;
+            stuckHandler.checkStuck(this);
         }
 
         if (pauseTicks > 0)
@@ -328,7 +335,6 @@ else if (pathResult.getStatus() == PathFindingStatus.CALCULATION_COMPLETE)
         if (handleLadders(oldIndex))
         {
             followThePath();
-            stuckHandler.checkStuck(this);
             return;
         }
 
@@ -340,7 +346,6 @@ else if (pathResult.getStatus() == PathFindingStatus.CALCULATION_COMPLETE)
 
         if (handleRails())
         {
-            stuckHandler.checkStuck(this);
             return;
         }
 
@@ -393,9 +398,13 @@ else if (this.path != null && !this.path.isDone())
         if (pathResult != null && isDone())
         {
             pathResult.setStatus(PathFindingStatus.COMPLETE);
-        }
 
-        stuckHandler.checkStuck(this);
+            // Cleanup pathresult if the entity forgot about it
+            if (ourEntity.level.getGameTime() - finishTime > TICKS_SECOND * 20 + pauseTickBackupAmount)
+            {
+                pathResult = null;
+            }
+        }
     }
 
     /**
@@ -470,44 +479,6 @@ protected PathFinder createPathFinder(final int p_179679_1_)
     @Override
     protected boolean canUpdatePath()
     {
-        if (ourEntity.getPose() != Pose.STANDING)
-        {
-            ourEntity.setPose(Pose.STANDING);
-        }
-
-        // Auto dismount when trying to path.
-        if (ourEntity.vehicle != null)
-        {
-            @NotNull final PathPointExtended pEx = (PathPointExtended) this.getPath().getNode(this.getPath().getNextNodeIndex());
-            if (pEx.isRailsExit())
-            {
-                final Entity entity = ourEntity.vehicle;
-                ourEntity.stopRiding();
-                entity.remove(Entity.RemovalReason.DISCARDED);
-            }
-            else if (!pEx.isOnRails())
-            {
-                if (ourEntity.vehicle instanceof MinecoloniesMinecart)
-                {
-                    final Entity entity = ourEntity.vehicle;
-                    ourEntity.stopRiding();
-                    entity.remove(Entity.RemovalReason.DISCARDED);
-                }
-                else
-                {
-                    if (destination == null || mob.distanceToSqr(destination.getX(), destination.getY(), destination.getZ()) > 2)
-                    {
-                        ourEntity.stopRiding();
-                    }
-                }
-            }
-            else if ((Math.abs(pEx.x - mob.getX()) > 7 || Math.abs(pEx.z - mob.getZ()) > 7) && ourEntity.vehicle != null)
-            {
-                final Entity entity = ourEntity.vehicle;
-                ourEntity.stopRiding();
-                entity.remove(Entity.RemovalReason.DISCARDED);
-            }
-        }
         return true;
     }
 
@@ -615,9 +586,6 @@ private Path convertPath(final Path path)
             }
 
             tempPath = new Path(Arrays.asList(newPoints), path.getTarget(), path.canReach());
-
-            final PathPointExtended finalPoint = newPoints[pathLength - 1];
-            destination = new BlockPos(finalPoint.x, finalPoint.y, finalPoint.z);
         }
 
         return tempPath == null ? path : tempPath;
@@ -648,11 +616,18 @@ private void processCompletedCalculationResult()
 
         if (pathResult.failedToReachDestination())
         {
+            pauseTicks = pauseTickBackupAmount;
+            pauseTickBackupAmount += 10;
+
             if (pathResult.searchedNodes >= MAX_NODES)
             {
-                pauseTicks = 50;
+                pauseTicks += 50;
             }
         }
+        else
+        {
+            pauseTickBackupAmount = 10;
+        }
 
         moveTo(pathResult.getPath(), getSpeedFactor());
     }
@@ -935,6 +910,7 @@ private boolean handleEntityInWater(int oldIndex, final PathPointExtended pEx)
     @Override
     protected void followThePath()
     {
+        // TODO: Rework pathfollow
         getSpeedFactor();
         final int curNode = path.getNextNodeIndex();
         final int curNodeNext = curNode + 1;
@@ -960,6 +936,13 @@ protected void followThePath()
                 }
                  return;
             }
+
+            if (!pEx.isOnRails() && ourEntity.vehicle != null)
+            {
+                final Entity entity = ourEntity.vehicle;
+                ourEntity.stopRiding();
+                entity.remove(Entity.RemovalReason.DISCARDED);
+            }
         }
 
         this.maxDistanceToWaypoint = 0.5F;
@@ -1048,6 +1031,7 @@ else if (isTracking)
      */
     private void onPathFinish()
     {
+        finishTime = ourEntity.level.getGameTime();
         super.stop();
     }
 
@@ -1062,6 +1046,9 @@ protected void doStuckDetection(@NotNull final Vec3 positionVec3)
         // Do nothing, unstuck is checked on tick, not just when we have a path
     }
 
+    /**
+     * Stop indicates that the entity no longer desires to move.
+     */
     @Override
     public void stop()
     {
@@ -1072,8 +1059,30 @@ public void stop()
             pathResult = null;
         }
 
-        desiredPos = BlockPos.ZERO;
-        destination = null;
+        safeDestinationPos = BlockPos.ZERO;
+        stuckHandler.resetGlobalStuckTimers();
+
+        if (ourEntity.vehicle != null)
+        {
+            final Entity entity = ourEntity.vehicle;
+            ourEntity.stopRiding();
+            entity.remove(Entity.RemovalReason.DISCARDED);
+        }
+
+        super.stop();
+    }
+
+    /**
+     * Triggers an indirect recalc, isDone() returns true now
+     */
+    @Override
+    public void recalc()
+    {
+        if (pathResult != null)
+        {
+            pathResult.cancel();
+            pathResult.setStatus(PathFindingStatus.CANCELLED);
+        }
         super.stop();
     }
 
@@ -1109,7 +1118,7 @@ public TreePathResult walkToTree(final int range, final double speed, final List
         @NotNull BlockPos start = PathfindingUtils.prepareStart(ourEntity);
         final BlockPos buildingPos = ((AbstractEntityCitizen) mob).getCitizenColonyHandler().getWorkBuilding().getPosition();
 
-        if (BlockPosUtil.getDistance2D(buildingPos, ((AbstractEntityCitizen) mob).blockPosition()) > range * 4)
+        if (BlockPosUtil.getDistance2D(buildingPos, mob.blockPosition()) > range * 4)
         {
             start = buildingPos;
         }
@@ -1140,15 +1149,15 @@ public void setCanFloat(boolean canSwim)
     }
 
     @Override
-    public BlockPos getDesiredPos()
+    public BlockPos getSafeDestination()
     {
-        return desiredPos;
+        return safeDestinationPos;
     }
 
     @Override
-    public void setDesiredPos(final BlockPos pos)
+    public void setSafeDestinationPos(final BlockPos pos)
     {
-        desiredPos = pos;
+        safeDestinationPos = pos;
     }
 
     /**
@@ -1193,4 +1202,10 @@ public PathResult getPathResult()
     {
         return pathResult;
     }
+
+    @Override
+    public IStuckHandler<MinecoloniesAdvancedPathNavigate> getStuckHandler()
+    {
+        return stuckHandler;
+    }
 }
diff --git a/src/main/java/com/minecolonies/core/entity/pathfinding/navigation/PathingStuckHandler.java b/src/main/java/com/minecolonies/core/entity/pathfinding/navigation/PathingStuckHandler.java
index b58fac9dd67..00244a91bcf 100644
--- a/src/main/java/com/minecolonies/core/entity/pathfinding/navigation/PathingStuckHandler.java
+++ b/src/main/java/com/minecolonies/core/entity/pathfinding/navigation/PathingStuckHandler.java
@@ -2,16 +2,19 @@
 
 import com.ldtteam.structurize.util.BlockUtils;
 import com.minecolonies.api.entity.ai.workers.util.IBuilderUndestroyable;
+import com.minecolonies.api.entity.pathfinding.IMinecoloniesNavigator;
 import com.minecolonies.api.entity.pathfinding.IStuckHandler;
 import com.minecolonies.api.entity.pathfinding.IStuckHandlerEntity;
 import com.minecolonies.api.items.ModTags;
 import com.minecolonies.api.util.BlockPosUtil;
 import com.minecolonies.api.util.DamageSourceKeys;
+import com.minecolonies.api.util.Log;
 import com.minecolonies.api.util.constant.ColonyConstants;
 import com.minecolonies.core.entity.pathfinding.SurfaceType;
 import net.minecraft.core.BlockPos;
 import net.minecraft.core.Direction;
 import net.minecraft.world.entity.Mob;
+import net.minecraft.world.entity.ai.navigation.PathNavigation;
 import net.minecraft.world.level.Level;
 import net.minecraft.world.level.block.Block;
 import net.minecraft.world.level.block.Blocks;
@@ -19,7 +22,9 @@
 import net.minecraft.world.level.block.state.BlockState;
 import net.minecraft.world.level.pathfinder.Node;
 import net.minecraft.world.phys.Vec3;
+import net.minecraftforge.fml.loading.FMLEnvironment;
 
+import java.util.Objects;
 import java.util.Random;
 
 import static com.minecolonies.api.util.BlockPosUtil.HORIZONTAL_DIRS;
@@ -27,7 +32,7 @@
 /**
  * Stuck handler for pathing
  */
-public class PathingStuckHandler implements IStuckHandler
+public class PathingStuckHandler<NAV extends PathNavigation & IMinecoloniesNavigator> implements IStuckHandler<NAV>
 {
     /**
      * The distance at which we consider a target to arrive
@@ -136,9 +141,9 @@ private PathingStuckHandler()
      *
      * @return new stuck handler
      */
-    public static PathingStuckHandler createStuckHandler()
+    public static <NAV extends PathNavigation & IMinecoloniesNavigator> PathingStuckHandler<NAV> createStuckHandler()
     {
-        return new PathingStuckHandler();
+        return new PathingStuckHandler<>();
     }
 
     /**
@@ -147,10 +152,9 @@ public static PathingStuckHandler createStuckHandler()
      * @param navigator navigator to check
      */
     @Override
-    public void checkStuck(final AbstractAdvancedPathNavigate navigator)
+    public void checkStuck(final NAV navigator)
     {
-        // TODO: rework to allow paths to nonsafe locations still benefit from non-teleport options(skip ahead, reset path etc)
-        if (navigator.getDesiredPos() == null || navigator.getDesiredPos().equals(BlockPos.ZERO))
+        if (navigator.getPathResult() == null)
         {
             resetGlobalStuckTimers();
             return;
@@ -162,25 +166,25 @@ public void checkStuck(final AbstractAdvancedPathNavigate navigator)
             return;
         }
 
-        final double distanceToGoal =
-          navigator.getOurEntity().position().distanceTo(new Vec3(navigator.getDesiredPos().getX(), navigator.getDesiredPos().getY(), navigator.getDesiredPos().getZ()));
-
-        // Close enough to be considered at the goal
-        if (distanceToGoal < MIN_TARGET_DIST)
-        {
-            resetGlobalStuckTimers();
-            return;
-        }
-
         // Global timeout check
-        if (prevDestination.equals(navigator.getDesiredPos()))
+        if (Objects.equals(prevDestination, navigator.getSafeDestination()))
         {
-            globalTimeout++;
-
+            globalTimeout += 10;
             // Try path first, if path fits target pos
-            if (globalTimeout > Math.max(MIN_TP_DELAY, timePerBlockDistance * Math.max(MIN_DIST_FOR_TP, distanceToGoal)))
+            if (globalTimeout > MIN_TP_DELAY)
             {
-                completeStuckAction(navigator);
+                if (navigator.getSafeDestination() != null && navigator.getSafeDestination() != BlockPos.ZERO)
+                {
+                    final int distance = Math.max(MIN_DIST_FOR_TP, BlockPosUtil.distManhattan(navigator.getSafeDestination(), navigator.getOurEntity().blockPosition()));
+                    if (globalTimeout > timePerBlockDistance * distance)
+                    {
+                        completeStuckAction(navigator);
+                    }
+                }
+                else
+                {
+                    completeStuckAction(navigator);
+                }
             }
         }
         else
@@ -188,8 +192,24 @@ public void checkStuck(final AbstractAdvancedPathNavigate navigator)
             resetGlobalStuckTimers();
         }
 
-        delayToNextUnstuckAction--;
-        prevDestination = navigator.getDesiredPos();
+        prevDestination = navigator.getSafeDestination();
+
+        if (prevDestination != null && prevDestination != BlockPos.ZERO)
+        {
+            final double distanceToGoal =
+                navigator.getOurEntity()
+                    .position()
+                    .distanceTo(new Vec3(navigator.getSafeDestination().getX(), navigator.getSafeDestination().getY(), navigator.getSafeDestination().getZ()));
+
+            // Close enough to be considered at the goal
+            if (distanceToGoal < MIN_TARGET_DIST)
+            {
+                resetGlobalStuckTimers();
+                return;
+            }
+        }
+
+        delayToNextUnstuckAction -= 10;
 
         if (navigator.getPath() == null || navigator.getPath().isDone())
         {
@@ -225,7 +245,7 @@ else if (lastPathIndex < 2 && navigator.getPath().getNodeCount() > 2)
                         navigator.getPath().setNextNodeIndex(2);
                     }
 
-                    if ((stuckLevel == 0 || navigator.getPath().getTarget().distSqr(prevDestination) < 25))
+                    if ((stuckLevel == 0 || (prevDestination != null && prevDestination != BlockPos.ZERO && navigator.getPath().getTarget().distSqr(prevDestination) < 25)))
                     {
                         progressedNodes = navigator.getPath().getNextNodeIndex() > lastPathIndex ? progressedNodes + 1 : progressedNodes - 1;
 
@@ -247,7 +267,8 @@ else if (lastPathIndex < 2 && navigator.getPath().getNodeCount() > 2)
     /**
      * Resets global stuck timers
      */
-    private void resetGlobalStuckTimers()
+    @Override
+    public void resetGlobalStuckTimers()
     {
         globalTimeout = 0;
         prevDestination = BlockPos.ZERO;
@@ -257,13 +278,20 @@ private void resetGlobalStuckTimers()
     /**
      * Final action when completly stuck before resetting stuck handler and path
      */
-    private void completeStuckAction(final AbstractAdvancedPathNavigate navigator)
+    private void completeStuckAction(final NAV navigator)
     {
-        final BlockPos desired = navigator.getDesiredPos();
+        final BlockPos desired = navigator.getSafeDestination();
         final Level world = navigator.getOurEntity().level();
         final Mob entity = navigator.getOurEntity();
 
-        if (canTeleportGoal)
+        if (!FMLEnvironment.production)
+        {
+            Log.getLogger()
+                .warn("Entity complete stuck action stuck:" + navigator.getOurEntity() + " desired:" + navigator.getSafeDestination() + " stuckLevel:" + stuckLevel + " teleport:"
+                    + canTeleportGoal);
+        }
+
+        if (canTeleportGoal && desired != null && desired != BlockPos.ZERO)
         {
             final BlockPos tpPos = BlockPosUtil.findAround(world, desired, 10, 10,
               (posworld, pos) -> SurfaceType.getSurfaceType(posworld, posworld.getBlockState(pos.below()), pos.below()) == SurfaceType.WALKABLE
@@ -274,6 +302,7 @@ private void completeStuckAction(final AbstractAdvancedPathNavigate navigator)
                 entity.teleportTo(tpPos.getX() + 0.5, tpPos.getY(), tpPos.getZ() + 0.5);
             }
         }
+
         if (takeDamageOnCompleteStuck)
         {
             entity.hurt(world.damageSources().source(DamageSourceKeys.STUCK_DAMAGE), entity.getMaxHealth() * damagePct);
@@ -281,8 +310,10 @@ private void completeStuckAction(final AbstractAdvancedPathNavigate navigator)
 
         if (completeStuckBlockBreakRange > 0)
         {
-            final Direction facing = BlockPosUtil.getFacing(BlockPos.containing(entity.position()), navigator.getDesiredPos());
-
+            final BlockPos neighbour = prevDestination != null && prevDestination != BlockPos.ZERO
+                ? prevDestination
+                : navigator.getPath() != null ? navigator.getPath().getTarget() : entity.blockPosition().east();
+            final Direction facing = BlockPosUtil.getFacing(BlockPos.containing(entity.position()), neighbour);
             for (int i = 1; i <= completeStuckBlockBreakRange; i++)
             {
                 if (!world.isEmptyBlock(BlockPos.containing(entity.position()).relative(facing, i)) || !world.isEmptyBlock(BlockPos.containing(entity.position())
@@ -302,32 +333,31 @@ private void completeStuckAction(final AbstractAdvancedPathNavigate navigator)
     /**
      * Tries unstuck options depending on the level
      */
-    private void tryUnstuck(final AbstractAdvancedPathNavigate navigator)
+    private void tryUnstuck(final NAV navigator)
     {
         if (delayToNextUnstuckAction > 0)
         {
             return;
         }
-        delayToNextUnstuckAction = 100;
+        delayToNextUnstuckAction = 50;
 
         // Clear path
         if (stuckLevel == 0)
         {
             stuckLevel++;
-            delayToNextUnstuckAction = 100;
+            delayToNextUnstuckAction = 200;
             navigator.getOurEntity().stopRiding();
-            BlockPos desired = navigator.getDesiredPos();
-            navigator.stop();
-            navigator.setDesiredPos(desired);
+            navigator.recalc();
             return;
         }
 
+        final int lastStuckLevel = this.stuckLevel;
+        chanceStuckLevel(navigator);
+
         // Move away, with chance to skip this.
-        if (stuckLevel == 1 && rand.nextDouble() > chanceToByPassMovingAway
-              || ((stuckLevel >= 3 && stuckLevel <= 8) && !(canBreakBlocks || canBuildLeafBridges || canPlaceLadders) && rand.nextBoolean()))
+        if (rand.nextDouble() < chanceToByPassMovingAway ||
+            (lastStuckLevel == 1 || ((lastStuckLevel >= 3 && lastStuckLevel <= 8) && !(canBreakBlocks || canBuildLeafBridges || canPlaceLadders) && rand.nextBoolean())))
         {
-            delayToNextUnstuckAction = 600;
-
             if (navigator.getPath() != null)
             {
                 moveAwayStartPos = navigator.getPath().getNodePos(navigator.getPath().getNextNodeIndex());
@@ -337,52 +367,47 @@ private void tryUnstuck(final AbstractAdvancedPathNavigate navigator)
                 moveAwayStartPos = navigator.getOurEntity().blockPosition().above();
             }
 
-            navigator.stop();
-            final int range = ColonyConstants.rand.nextInt(20) + Math.min(100, Math.max(20, BlockPosUtil.distManhattan(navigator.ourEntity.blockPosition(), prevDestination)));
-            navigator.walkTowards(navigator.getOurEntity().blockPosition().relative(movingAwayDir, 40), range, 1.0f);
+            final int range = ColonyConstants.rand.nextInt(20) + 20;
+            navigator.setPauseTicks(0);
+            ((MinecoloniesAdvancedPathNavigate) navigator).walkTowards(navigator.getOurEntity().blockPosition().relative(movingAwayDir, 40), range, 1.0f);
             movingAwayDir = movingAwayDir.getClockWise();
             navigator.setPauseTicks(range * TICKS_PER_BLOCK);
+            delayToNextUnstuckAction = (int) (range * TICKS_PER_BLOCK * 1.5);
             return;
         }
 
         // Skip ahead
-        if (stuckLevel == 2 && teleportRange > 0 && hadPath)
+        if (lastStuckLevel == 2 && teleportRange > 0 && hadPath)
         {
             int index = Math.min(navigator.getPath().getNextNodeIndex() + teleportRange, navigator.getPath().getNodeCount() - 1);
             final Node togo = navigator.getPath().getNode(index);
             navigator.getOurEntity().teleportTo(togo.x + 0.5d, togo.y, togo.z + 0.5d);
-            delayToNextUnstuckAction = 300;
+            delayToNextUnstuckAction = 200;
         }
 
         // Place ladders & leaves
-        if (stuckLevel >= 3 && stuckLevel <= 5)
+        if (lastStuckLevel >= 3 && lastStuckLevel <= 5)
         {
-            delayToNextUnstuckAction = 200;
             if (canPlaceLadders && rand.nextBoolean())
             {
+                delayToNextUnstuckAction = 200;
                 placeLadders(navigator);
             }
-            else if (canBuildLeafBridges && rand.nextBoolean())
+            else if (canBuildLeafBridges)
             {
                 delayToNextUnstuckAction = 100;
                 placeLeaves(navigator);
             }
-            else if (canPlaceLadders || canBuildLeafBridges)
-            {
-                return;
-            }
         }
 
         // break blocks
-        if (stuckLevel >= 6 && stuckLevel <= 8 && canBreakBlocks)
+        if (lastStuckLevel >= 6 && lastStuckLevel <= 8 && canBreakBlocks)
         {
             delayToNextUnstuckAction = 200;
             breakBlocks(navigator);
         }
 
-        chanceStuckLevel();
-
-        if (stuckLevel == 9)
+        if (lastStuckLevel == 9)
         {
             completeStuckAction(navigator);
             resetStuckTimers();
@@ -392,7 +417,7 @@ else if (canPlaceLadders || canBuildLeafBridges)
     /**
      * Random chance to decrease to a previous level of stuck
      */
-    private void chanceStuckLevel()
+    private void chanceStuckLevel(NAV nav)
     {
         stuckLevel++;
         // 20 % to decrease to the previous level again
@@ -475,7 +500,7 @@ private void setAirIfPossible(final Level world, final BlockPos pos)
      *
      * @param navigator navigator to use
      */
-    private void placeLadders(final AbstractAdvancedPathNavigate navigator)
+    private void placeLadders(final NAV navigator)
     {
         final Level world = navigator.getOurEntity().level;
         final Mob entity = navigator.getOurEntity();
@@ -497,12 +522,12 @@ private void placeLadders(final AbstractAdvancedPathNavigate navigator)
      *
      * @param navigator navigator to use
      */
-    private void placeLeaves(final AbstractAdvancedPathNavigate navigator)
+    private void placeLeaves(final NAV navigator)
     {
         final Level world = navigator.getOurEntity().level();
         final Mob entity = navigator.getOurEntity();
 
-        final Direction badFacing = BlockPosUtil.getFacing(BlockPos.containing(entity.position()), navigator.getDesiredPos()).getOpposite();
+        final Direction badFacing = BlockPosUtil.getFacing(BlockPos.containing(entity.position()), navigator.getSafeDestination()).getOpposite();
 
         for (final Direction dir : HORIZONTAL_DIRS)
         {
@@ -543,12 +568,12 @@ private boolean tryPlaceLeaveOnPos(final Level world, final BlockPos pos)
      *
      * @param navigator navigator to use
      */
-    private void breakBlocks(final AbstractAdvancedPathNavigate navigator)
+    private void breakBlocks(final NAV navigator)
     {
         final Level world = navigator.getOurEntity().level();
         final Mob entity = navigator.getOurEntity();
 
-        final Direction facing = BlockPosUtil.getFacing(BlockPos.containing(entity.position()), navigator.getDesiredPos());
+        final Direction facing = BlockPosUtil.getFacing(BlockPos.containing(entity.position()), navigator.getSafeDestination());
 
         if (breakBlocksAhead(world, entity.blockPosition(), facing) && entity.getHealth() >= entity.getMaxHealth() / 3)
         {
@@ -664,4 +689,10 @@ public PathingStuckHandler withCompleteStuckBlockBreak(int range)
         completeStuckBlockBreakRange = range;
         return this;
     }
+
+    @Override
+    public int getStuckLevel()
+    {
+        return stuckLevel;
+    }
 }
diff --git a/src/main/java/com/minecolonies/core/entity/pathfinding/pathjobs/AbstractPathJob.java b/src/main/java/com/minecolonies/core/entity/pathfinding/pathjobs/AbstractPathJob.java
index 84890d07d08..566c9a29362 100644
--- a/src/main/java/com/minecolonies/core/entity/pathfinding/pathjobs/AbstractPathJob.java
+++ b/src/main/java/com/minecolonies/core/entity/pathfinding/pathjobs/AbstractPathJob.java
@@ -5,6 +5,7 @@
 import com.ldtteam.domumornamentum.block.decorative.ShingleBlock;
 import com.ldtteam.domumornamentum.block.decorative.ShingleSlabBlock;
 import com.minecolonies.api.blocks.decorative.AbstractBlockMinecoloniesConstructionTape;
+import com.minecolonies.api.entity.pathfinding.IDynamicHeuristicNavigator;
 import com.minecolonies.api.entity.pathfinding.IPathJob;
 import com.minecolonies.api.util.BlockPosUtil;
 import com.minecolonies.api.util.Log;
@@ -14,7 +15,6 @@
 import com.minecolonies.core.Network;
 import com.minecolonies.core.blocks.BlockDecorationController;
 import com.minecolonies.core.entity.pathfinding.*;
-import com.minecolonies.core.entity.pathfinding.navigation.IDynamicHeuristicNavigator;
 import com.minecolonies.core.entity.pathfinding.pathresults.PathResult;
 import com.minecolonies.core.entity.pathfinding.world.CachingBlockLookup;
 import com.minecolonies.core.entity.pathfinding.world.ChunkCache;