8 files changed, 486 insertions, 335 deletions

diff --git a/src/Mobs/CMakeLists.txt b/src/Mobs/CMakeLists.txt
index 5c374f9ee..14c7a8ca3 100644
--- a/src/Mobs/CMakeLists.txt
+++ b/src/Mobs/CMakeLists.txt
@@ -25,6 +25,7 @@ SET (SRCS
 	PassiveAggressiveMonster.cpp
 	PassiveMonster.cpp
 	Path.cpp
+	PathFinder.cpp
 	Pig.cpp
 	Rabbit.cpp
 	Sheep.cpp
@@ -39,7 +40,7 @@ SET (SRCS
 	Wolf.cpp
 	Zombie.cpp
 	ZombiePigman.cpp)
-	
+
 SET (HDRS
 	AggressiveMonster.h
 	Bat.h
@@ -64,6 +65,7 @@ SET (HDRS
 	PassiveAggressiveMonster.h
 	PassiveMonster.h
 	Path.h
+	PathFinder.h
 	Pig.h
 	Rabbit.h
 	Sheep.h
diff --git a/src/Mobs/Monster.cpp b/src/Mobs/Monster.cpp
index d1173c41c..3f7153fb3 100644
--- a/src/Mobs/Monster.cpp
+++ b/src/Mobs/Monster.cpp
@@ -14,7 +14,7 @@
 #include "../Chunk.h"
 #include "../FastRandom.h"
 
-#include "Path.h"
+#include "PathFinder.h"
 
 
 
@@ -75,11 +75,8 @@ cMonster::cMonster(const AString & a_ConfigName, eMonsterType a_MobType, const A
 	, m_EMState(IDLE)
 	, m_EMPersonality(AGGRESSIVE)
 	, m_Target(nullptr)
-	, m_Path(nullptr)
-	, m_IsFollowingPath(false)
+	, m_PathFinder(a_Width, a_Height)
 	, m_PathfinderActivated(false)
-	, m_GiveUpCounter(0)
-	, m_TicksSinceLastPathReset(1000)
 	, m_LastGroundHeight(POSY_TOINT)
 	, m_JumpCoolDown(0)
 	, m_IdleInterval(0)
@@ -125,127 +122,20 @@ void cMonster::SpawnOn(cClientHandle & a_Client)
 
 
 
-bool cMonster::TickPathFinding(cChunk & a_Chunk)
+void cMonster::MoveToWayPoint(cChunk & a_Chunk)
 {
-	if (!m_PathfinderActivated)
-	{
-		return false;
-	}
-	if (m_TicksSinceLastPathReset < 1000)
-	{
-		// No need to count beyond 1000. 1000 is arbitary here.
-		++m_TicksSinceLastPathReset;
-	}
-
-	if (ReachedFinalDestination())
+	if ((m_NextWayPointPosition - GetPosition()).SqrLength() < WAYPOINT_RADIUS * WAYPOINT_RADIUS)
 	{
-		StopMovingToPosition();
-		return false;
-	}
-
-	if ((m_FinalDestination - m_PathFinderDestination).Length() > 0.25)  // if the distance between where we're going and where we should go is too big.
-	{
-		/* If we reached the last path waypoint,
-		Or if we haven't re-calculated for too long.
-		Interval is proportional to distance squared, and its minimum is 10.
-		(Recalculate lots when close, calculate rarely when far) */
-		if (
-			((GetPosition() - m_PathFinderDestination).Length() < 0.25) ||
-			((m_TicksSinceLastPathReset > 10) && (m_TicksSinceLastPathReset > (0.4 * (m_FinalDestination - GetPosition()).SqrLength())))
-		)
-		{
-			/* Re-calculating is expensive when there's no path to target, and it results in mobs freezing very often as a result of always recalculating.
-			This is a workaround till we get better path recalculation. */
-			if (!m_NoPathToTarget)
-			{
-				ResetPathFinding();
-			}
-		}
-	}
-
-	if (m_Path == nullptr)
-	{
-		if (!EnsureProperDestination(a_Chunk))
-		{
-			StopMovingToPosition();  // Invalid chunks, probably world is loading or something, cancel movement.
-			return false;
-		}
-		m_GiveUpCounter = 40;
-		m_NoPathToTarget = false;
-		m_NoMoreWayPoints = false;
-		m_PathFinderDestination = m_FinalDestination;
-		m_Path = new cPath(a_Chunk, GetPosition(), m_PathFinderDestination, 20, GetWidth(), GetHeight());
-	}
-
-	switch (m_Path->Step(a_Chunk))
-	{
-		case ePathFinderStatus::NEARBY_FOUND:
-		{
-			m_NoPathToTarget = true;
-			m_PathFinderDestination = m_Path->AcceptNearbyPath();
-			break;
-		}
-
-		case ePathFinderStatus::PATH_NOT_FOUND:
-		{
-			StopMovingToPosition();  // Try to calculate a path again.
-			// Note that the next time may succeed, e.g. if a player breaks a barrier.
-			break;
-		}
-		case ePathFinderStatus::CALCULATING:
-		{
-			// Pathfinder needs more time
-			break;
-		}
-		case ePathFinderStatus::PATH_FOUND:
-		{
-			if (m_NoMoreWayPoints || (--m_GiveUpCounter == 0))
-			{
-				if (m_EMState == ATTACKING)
-				{
-					ResetPathFinding();  // Try to calculate a path again.
-					// This results in mobs hanging around an unreachable target (player).
-				}
-				else
-				{
-					StopMovingToPosition();  // Find a different place to go to.
-				}
-				return false;
-			}
-			else if (!m_Path->IsLastPoint())  // Have we arrived at the next cell, as denoted by m_NextWayPointPosition?
-			{
-				if ((m_Path->IsFirstPoint() || ReachedNextWaypoint()))
-				{
-					m_NextWayPointPosition = m_Path->GetNextPoint();
-					m_GiveUpCounter = 40;  // Give up after 40 ticks (2 seconds) if failed to reach m_NextWayPointPosition.
-				}
-			}
-			else
-			{
-				m_NoMoreWayPoints = true;
-			}
-
-			m_IsFollowingPath = true;
-			return true;
-		}
+		return;
 	}
 
-	return false;
-}
-
-
-
-
 
-void cMonster::MoveToWayPoint(cChunk & a_Chunk)
-{
 	if (m_JumpCoolDown == 0)
 	{
 		if (DoesPosYRequireJump(FloorC(m_NextWayPointPosition.y)))
 		{
 			if (
-				(IsOnGround() && (GetSpeedX() == 0.0f) && (GetSpeedY() == 0.0f)) ||
-				(IsSwimming() && (m_GiveUpCounter < 15))
+				(IsOnGround() && (GetSpeedX() == 0.0f) && (GetSpeedY() == 0.0f))  // TODO water handling?
 			)
 			{
 				m_bOnGround = false;
@@ -296,98 +186,7 @@ void cMonster::MoveToWayPoint(cChunk & a_Chunk)
 
 
 
-bool cMonster::EnsureProperDestination(cChunk & a_Chunk)
-{
-	cChunk * Chunk = a_Chunk.GetNeighborChunk(FloorC(m_FinalDestination.x), FloorC(m_FinalDestination.z));
-	BLOCKTYPE BlockType;
-	NIBBLETYPE BlockMeta;
-
-	if ((Chunk == nullptr) || !Chunk->IsValid())
-	{
-		return false;
-	}
-
-	int RelX = FloorC(m_FinalDestination.x) - Chunk->GetPosX() * cChunkDef::Width;
-	int RelZ = FloorC(m_FinalDestination.z) - Chunk->GetPosZ() * cChunkDef::Width;
-
-	// If destination in the air, first try to go 1 block north, or east, or west.
-	// This fixes the player leaning issue.
-	// If that failed, we instead go down to the lowest air block.
-	Chunk->GetBlockTypeMeta(RelX, FloorC(m_FinalDestination.y) - 1, RelZ, BlockType, BlockMeta);
-	if (!cBlockInfo::IsSolid(BlockType))
-	{
-		bool InTheAir = true;
-		int x, z;
-		for (z = -1; z <= 1; ++z)
-		{
-			for (x = -1; x <= 1; ++x)
-			{
-				if ((x == 0) && (z == 0))
-				{
-					continue;
-				}
-				Chunk = a_Chunk.GetNeighborChunk(FloorC(m_FinalDestination.x+x), FloorC(m_FinalDestination.z+z));
-				if ((Chunk == nullptr) || !Chunk->IsValid())
-				{
-					return false;
-				}
-				RelX = FloorC(m_FinalDestination.x + x) - Chunk->GetPosX() * cChunkDef::Width;
-				RelZ = FloorC(m_FinalDestination.z + z) - Chunk->GetPosZ() * cChunkDef::Width;
-				Chunk->GetBlockTypeMeta(RelX, FloorC(m_FinalDestination.y) - 1, RelZ, BlockType, BlockMeta);
-				if (cBlockInfo::IsSolid(BlockType))
-				{
-					m_FinalDestination.x += x;
-					m_FinalDestination.z += z;
-					InTheAir = false;
-					goto breakBothLoops;
-				}
-			}
-		}
-		breakBothLoops:
-
-		// Go down to the lowest air block.
-		if (InTheAir)
-		{
-			while (m_FinalDestination.y > 0)
-			{
-				Chunk->GetBlockTypeMeta(RelX, FloorC(m_FinalDestination.y) - 1, RelZ, BlockType, BlockMeta);
-				if (cBlockInfo::IsSolid(BlockType))
-				{
-					break;
-				}
-				m_FinalDestination.y -= 1;
-			}
-		}
-	}
-
-	// If destination in water, go up to the highest water block.
-	// If destination in solid, go up to first air block.
-	bool InWater = false;
-	while (m_FinalDestination.y < cChunkDef::Height)
-	{
-		Chunk->GetBlockTypeMeta(RelX, FloorC(m_FinalDestination.y), RelZ, BlockType, BlockMeta);
-		if (BlockType == E_BLOCK_STATIONARY_WATER)
-		{
-			InWater = true;
-		}
-		else if (cBlockInfo::IsSolid(BlockType))
-		{
-			InWater = false;
-		}
-		else
-		{
-			break;
-		}
-		m_FinalDestination.y += 1;
-	}
-	if (InWater)
-	{
-		m_FinalDestination.y -= 1;
-	}
-
 
-	return true;
-}
 
 
 
@@ -406,22 +205,6 @@ void cMonster::MoveToPosition(const Vector3d & a_Position)
 void cMonster::StopMovingToPosition()
 {
 	m_PathfinderActivated = false;
-	ResetPathFinding();
-}
-
-
-
-
-
-void cMonster::ResetPathFinding(void)
-{
-	m_TicksSinceLastPathReset = 0;
-	m_IsFollowingPath = false;
-	if (m_Path != nullptr)
-	{
-		delete m_Path;
-		m_Path = nullptr;
-	}
 }
 
 
@@ -435,7 +218,7 @@ void cMonster::Tick(std::chrono::milliseconds a_Dt, cChunk & a_Chunk)
 
 	if (m_Health <= 0)
 	{
-		// The mob is dead, but we're still animating the "puff" they leave when they die.
+		// The mob is dead, but we're still animating the "puff" they leave when they die
 		m_DestroyTimer += a_Dt;
 		if (m_DestroyTimer > std::chrono::seconds(1))
 		{
@@ -453,34 +236,57 @@ void cMonster::Tick(std::chrono::milliseconds a_Dt, cChunk & a_Chunk)
 		m_Target = nullptr;
 	}
 
-	if (GetPosY() >= 0)
+	// Process the undead burning in daylight.
+	HandleDaylightBurning(*Chunk, WouldBurnAt(GetPosition(), *Chunk));
+
+	bool a_IsFollowingPath = false;
+	if (m_PathfinderActivated)
 	{
-		// Process the undead burning in daylight.
-		HandleDaylightBurning(*Chunk, WouldBurnAt(GetPosition(), *Chunk));
-		if (TickPathFinding(*Chunk))
+		if (ReachedFinalDestination())
 		{
-			/* If I burn in daylight, and I won't burn where I'm standing, and I'll burn in my next position, and at least one of those is true:
-			1. I am idle
-			2. I was not hurt by a player recently.
-			Then STOP. */
-			if (
-				m_BurnsInDaylight && ((m_TicksSinceLastDamaged >= 100) || (m_EMState == IDLE)) &&
-				WouldBurnAt(m_NextWayPointPosition, *Chunk) &&
-				!WouldBurnAt(GetPosition(), *Chunk)
-			)
-			{
-				// If we burn in daylight, and we would burn at the next step, and we won't burn where we are right now, and we weren't provoked recently:
-				StopMovingToPosition();
-				m_GiveUpCounter = 40;  // This doesn't count as giving up, keep the giveup timer as is.
-			}
-			else
+			StopMovingToPosition();  // Simply sets m_PathfinderActivated to false.
+		}
+		else
+		{
+			// Note that m_NextWayPointPosition is actually returned by GetNextWayPoint)
+			switch (m_PathFinder.GetNextWayPoint(*Chunk, GetPosition(), &m_FinalDestination, &m_NextWayPointPosition, m_EMState == IDLE ? true : false))
 			{
-				MoveToWayPoint(*Chunk);
+				case ePathFinderStatus::PATH_FOUND:
+				{
+					/* If I burn in daylight, and I won't burn where I'm standing, and I'll burn in my next position, and at least one of those is true:
+					1. I am idle
+					2. I was not hurt by a player recently.
+					Then STOP. */
+					if (
+						m_BurnsInDaylight && ((m_TicksSinceLastDamaged >= 100) || (m_EMState == IDLE)) &&
+						WouldBurnAt(m_NextWayPointPosition, *Chunk) &&
+						!WouldBurnAt(GetPosition(), *Chunk)
+					)
+					{
+						// If we burn in daylight, and we would burn at the next step, and we won't burn where we are right now, and we weren't provoked recently:
+						StopMovingToPosition();
+					}
+					else
+					{
+						a_IsFollowingPath = true;  // Used for proper body / head orientation only.
+						MoveToWayPoint(*Chunk);
+					}
+					break;
+				}
+				case ePathFinderStatus::PATH_NOT_FOUND:
+				{
+					StopMovingToPosition();
+					break;
+				}
+				default:
+				{
+
+				}
 			}
 		}
 	}
 
-	SetPitchAndYawFromDestination();
+	SetPitchAndYawFromDestination(a_IsFollowingPath);
 	HandleFalling();
 
 	switch (m_EMState)
@@ -522,24 +328,11 @@ void cMonster::Tick(std::chrono::milliseconds a_Dt, cChunk & a_Chunk)
 
 
 
-void cMonster::SetPitchAndYawFromDestination()
+void cMonster::SetPitchAndYawFromDestination(bool a_IsFollowingPath)
 {
-	Vector3d FinalDestination = m_FinalDestination;
-	if (m_Target != nullptr)
-	{
-		if (m_Target->IsPlayer())
-		{
-			FinalDestination.y = static_cast<cPlayer *>(m_Target)->GetStance() - 1;
-		}
-		else
-		{
-			FinalDestination.y = m_Target->GetPosY() + GetHeight();
-		}
-	}
-
-
+	/* Todo Buggy */
 	Vector3d BodyDistance;
-	if (!m_IsFollowingPath && (m_Target != nullptr))
+	if (!a_IsFollowingPath && (m_Target != nullptr))
 	{
 		BodyDistance = m_Target->GetPosition() - GetPosition();
 	}
@@ -552,22 +345,39 @@ void cMonster::SetPitchAndYawFromDestination()
 	VectorToEuler(BodyDistance.x, BodyDistance.y, BodyDistance.z, BodyRotation, BodyPitch);
 	SetYaw(BodyRotation);
 
-	Vector3d Distance = FinalDestination - GetPosition();
+	Vector3d HeadDistance;
+	if (m_Target != nullptr)
 	{
-		double HeadRotation, HeadPitch;
-		Distance.Normalize();
-		VectorToEuler(Distance.x, Distance.y, Distance.z, HeadRotation, HeadPitch);
-		if (std::abs(BodyRotation - HeadRotation) < 90)
+		if (m_Target->IsPlayer())  // Look at a player
 		{
-			SetHeadYaw(HeadRotation);
-			SetPitch(-HeadPitch);
+			HeadDistance = m_Target->GetPosition() - GetPosition();
+			// HeadDistance.y = static_cast<cPlayer *>(m_Target)->GetStance() - 1;
 		}
-		else  // We're not an owl. If it's more than 120, don't look behind and instead look at where you're walking.
+		else  // Look at some other entity
 		{
-			SetHeadYaw(BodyRotation);
-			SetPitch(-BodyPitch);
+			HeadDistance = m_Target->GetPosition() - GetPosition();
+			// HeadDistance.y = m_Target->GetPosY() + GetHeight();
 		}
 	}
+	else  // Look straight
+	{
+		HeadDistance = BodyDistance;
+		HeadDistance.y = 0;
+	}
+
+	double HeadRotation, HeadPitch;
+	HeadDistance.Normalize();
+	VectorToEuler(HeadDistance.x, HeadDistance.y, HeadDistance.z, HeadRotation, HeadPitch);
+	if (std::abs(BodyRotation - HeadRotation) < 90)
+	{
+		SetHeadYaw(HeadRotation);
+		SetPitch(-HeadPitch);
+	}
+	else
+	{
+		SetHeadYaw(BodyRotation);
+		SetPitch(-BodyPitch);
+	}
 }
 
 
@@ -806,7 +616,7 @@ void cMonster::EventLosePlayer(void)
 
 void cMonster::InStateIdle(std::chrono::milliseconds a_Dt)
 {
-	if (m_IsFollowingPath)
+	if (m_PathfinderActivated)
 	{
 		return;  // Still getting there
 	}
diff --git a/src/Mobs/Monster.h b/src/Mobs/Monster.h
index 7b6c0c488..1e1012f57 100644
--- a/src/Mobs/Monster.h
+++ b/src/Mobs/Monster.h
@@ -7,17 +7,12 @@
 #include "../Item.h"
 #include "../Enchantments.h"
 #include "MonsterTypes.h"
-
+#include "PathFinder.h"
 
 
 class cClientHandle;
 class cWorld;
 
-// Fwd: cPath
-enum class ePathFinderStatus;
-class cPath;
-
-
 
 // tolua_begin
 class cMonster :
@@ -168,34 +163,19 @@ protected:
 
 	/** A pointer to the entity this mobile is aiming to reach */
 	cEntity * m_Target;
-	cPath *  m_Path;  // TODO unique ptr
 
-	/** Stores if mobile is currently moving towards the ultimate, final destination */
-	bool m_IsFollowingPath;
+	/** The pathfinder instance handles pathfinding for this monster. */
+	cPathFinder m_PathFinder;
 
 	/** Stores if pathfinder is being used - set when final destination is set, and unset when stopped moving to final destination */
 	bool m_PathfinderActivated;
 
-	/* If 0, will give up reaching the next m_NextWayPointPosition and will re-compute path. */
-	int m_GiveUpCounter;
-	int m_TicksSinceLastPathReset;
-
 	/** Coordinates of the next position that should be reached */
 	Vector3d m_NextWayPointPosition;
 
 	/** Coordinates for the ultimate, final destination. */
 	Vector3d m_FinalDestination;
 
-	/** Coordinates for the ultimate, final destination last given to the pathfinder. */
-	Vector3d m_PathFinderDestination;
-
-	/** True if there's no path to target and we're walking to an approximated location. */
-	bool m_NoPathToTarget;
-
-	/** Whether The mob has finished their path, note that this does not imply reaching the destination,
-	the destination may sometimes differ from the current path. */
-	bool m_NoMoreWayPoints;
-
 	/** Finds the lowest non-air block position (not the highest, as cWorld::GetHeight does)
 	If current Y is nonsolid, goes down to try to find a solid block, then returns that + 1
 	If current Y is solid, goes up to find first nonsolid block, and returns that.
@@ -203,48 +183,25 @@ protected:
 	int FindFirstNonAirBlockPosition(double a_PosX, double a_PosZ);
 
 	/** Returns if the ultimate, final destination has been reached. */
-	bool ReachedFinalDestination(void) { return ((m_FinalDestination - GetPosition()).Length() < GetWidth()/2); }
+	bool ReachedFinalDestination(void) { return ((m_FinalDestination - GetPosition()).SqrLength() < WAYPOINT_RADIUS * WAYPOINT_RADIUS); }
 
 	/** Returns whether or not the target is close enough for attack. */
 	bool TargetIsInRange(void) { return ((m_FinalDestination - GetPosition()).SqrLength() < (m_AttackRange * m_AttackRange)); }
 
-	/** Returns if the intermediate waypoint of m_NextWayPointPosition has been reached */
-	bool ReachedNextWaypoint(void) { return ((m_NextWayPointPosition - GetPosition()).SqrLength() < 0.25); }
-
 	/** Returns if a monster can reach a given height by jumping. */
 	inline bool DoesPosYRequireJump(int a_PosY)
 	{
 		return ((a_PosY > POSY_TOINT) && (a_PosY == POSY_TOINT + 1));
 	}
 
-	/** Finds the next place to go by calculating a path and setting the m_NextWayPointPosition variable for the next block to head to
-	This is based on the ultimate, final destination and the current position, as well as the A* algorithm, and any environmental hazards
-	Returns if a path is ready, and therefore if the mob should move to m_NextWayPointPosition
-	*/
-	bool TickPathFinding(cChunk & a_Chunk);
-
 	/** Move in a straight line to the next waypoint in the path, will jump if needed. */
 	void MoveToWayPoint(cChunk & a_Chunk);
 
-	/** Ensures the destination is not buried underground or under water. Also ensures the destination is not in the air.
-	Only the Y coordinate of m_FinalDestination might be changed.
-	1. If m_FinalDestination is the position of a water block, m_FinalDestination's Y will be modified to point to the heighest water block in the pool in the current column.
-	2. If m_FinalDestination is the position of a solid, m_FinalDestination's Y will be modified to point to the first airblock above the solid in the current column.
-	3. If m_FinalDestination is the position of an air block, Y will keep decreasing until hitting either a solid or water.
-	Now either 1 or 2 is performed. */
-	bool EnsureProperDestination(cChunk & a_Chunk);
-
-	/** Resets a pathfinding task, be it due to failure or something else
-	Resets the pathfinder. If m_IsFollowingPath is true, TickPathFinding starts a brand new path.
-	Should only be called by the pathfinder, cMonster::Tick or StopMovingToPosition. */
-	void ResetPathFinding(void);
-
-	/** Stops pathfinding
-	Calls ResetPathFinding and sets m_IsFollowingPath to false */
+	/** Stops pathfinding. Calls ResetPathFinding and sets m_IsFollowingPath to false */
 	void StopMovingToPosition();
 
-	/** Sets the body yaw and head yaw / pitch based on next / ultimate destinations */
-	void SetPitchAndYawFromDestination(void);
+	/** Sets the body yaw and head yaw */
+	void SetPitchAndYawFromDestination(bool a_IsFollowingPath);
 
 	virtual void HandleFalling(void);
 	int m_LastGroundHeight;
@@ -297,5 +254,4 @@ protected:
 	/** Adds weapon that is equipped with the chance saved in m_DropChance[...] (this will be greter than 1 if picked up or 0.085 + (0.01 per LootingLevel) if born with) to the drop */
 	void AddRandomWeaponDropItem(cItems & a_Drops, unsigned int a_LootingLevel);
 
-
 } ;  // tolua_export
diff --git a/src/Mobs/Path.cpp b/src/Mobs/Path.cpp
index b98dd0d10..c0cffbeb4 100644
--- a/src/Mobs/Path.cpp
+++ b/src/Mobs/Path.cpp
@@ -34,6 +34,7 @@ cPath::cPath(
 	int a_MaxUp, int a_MaxDown
 ) :
 	m_StepsLeft(a_MaxSteps),
+	m_IsValid(true),
 	m_CurrentPoint(0),  // GetNextPoint increments this to 1, but that's fine, since the first cell is always a_StartingPoint
 	m_Chunk(&a_Chunk),
 	m_BadChunkFound(false)
@@ -68,11 +69,14 @@ cPath::cPath(
 	ProcessCell(GetCell(m_Source), nullptr, 0);
 }
 
+cPath::cPath() : m_IsValid(false)
+{
 
+}
 
 
 
-ePathFinderStatus cPath::Step(cChunk & a_Chunk)
+ePathFinderStatus cPath::CalculationStep(cChunk & a_Chunk)
 {
 	m_Chunk = &a_Chunk;
 	if (m_Status != ePathFinderStatus::CALCULATING)
@@ -287,11 +291,12 @@ void cPath::AttemptToFindAlternative()
 void cPath::BuildPath()
 {
 	cPathCell * CurrentCell = GetCell(m_Destination);
-	do
+	while (CurrentCell->m_Parent != nullptr)
 	{
-		m_PathPoints.push_back(CurrentCell->m_Location);  // Populate the cPath with points.
+		m_PathPoints.push_back(CurrentCell->m_Location);  // Populate the cPath with points. All midpoints are added. Destination is added. Source is excluded.
 		CurrentCell = CurrentCell->m_Parent;
-	} while (CurrentCell != nullptr);
+	}
+
 }
 
 
diff --git a/src/Mobs/Path.h b/src/Mobs/Path.h
index 69a05f651..410d6fec5 100644
--- a/src/Mobs/Path.h
+++ b/src/Mobs/Path.h
@@ -72,12 +72,15 @@ public:
 		double a_BoundingBoxWidth, double a_BoundingBoxHeight,
 		int a_MaxUp = 1, int a_MaxDown = 1
 	);
-	
+
+	/** Creates a dummy path which does nothing except returning false when isValid is called. */
+	cPath();
+
 	/** Performs part of the path calculation and returns the appropriate status.
 	If NEARBY_FOUND is returned, it means that the destination is not reachable, but a nearby destination
 	is reachable. If the user likes the alternative destination, they can call AcceptNearbyPath to treat the path as found,
 	and to make consequent calls to step return PATH_FOUND */
-	ePathFinderStatus Step(cChunk & a_Chunk);
+	ePathFinderStatus CalculationStep(cChunk & a_Chunk);
 
 	/** Called after the PathFinder's step returns NEARBY_FOUND.
 	Changes the PathFinder status from NEARBY_FOUND to PATH_FOUND, returns the nearby destination that
@@ -90,24 +93,41 @@ public:
 	inline Vector3d GetNextPoint()
 	{
 		ASSERT(m_Status == ePathFinderStatus::PATH_FOUND);
-		Vector3i Point = m_PathPoints[m_PathPoints.size() - 1 - (++m_CurrentPoint)];
+		ASSERT(m_CurrentPoint < m_PathPoints.size());
+		Vector3i Point = m_PathPoints[m_PathPoints.size() - 1 - m_CurrentPoint];
+		++m_CurrentPoint;
 		return Vector3d(Point.x + m_HalfWidth, Point.y, Point.z + m_HalfWidth);
 	}
 
 
-	/** Checks whether this is the last point or not. Never call getnextPoint when this is true. */
-	inline bool IsLastPoint() const
+	/** Checks if we have no more waypoints to return. Never call getnextPoint when this is true. */
+	inline bool NoMoreWayPoints() const
 	{
 		ASSERT(m_Status == ePathFinderStatus::PATH_FOUND);
-		return (m_CurrentPoint == m_PathPoints.size() - 1);
+		return (m_CurrentPoint == m_PathPoints.size());
 	}
 
+	/** Returns true if GetNextPoint() was never called for this Path. */
 	inline bool IsFirstPoint() const
 	{
 		ASSERT(m_Status == ePathFinderStatus::PATH_FOUND);
 		return (m_CurrentPoint == 0);
 	}
 
+	/** Returns true if this path is properly initialized.
+	Returns false if this path was initialized with an empty constructor.
+	If false, the path is unusable and you should not call any methods. */
+	inline bool IsValid() const
+	{
+		return m_IsValid;
+	}
+
+	/** The amount of waypoints left to return. */
+	inline size_t WayPointsLeft() const
+	{
+		ASSERT(m_Status == ePathFinderStatus::PATH_FOUND);
+		return m_PathPoints.size() - m_CurrentPoint;
+	}
 
 
 
@@ -145,6 +165,7 @@ private:
 
 	/* Control fields */
 	ePathFinderStatus m_Status;
+	bool m_IsValid;
 
 	/* Final path fields */
 	size_t m_CurrentPoint;
diff --git a/src/Mobs/PathFinder.cpp b/src/Mobs/PathFinder.cpp
new file mode 100644
index 000000000..fbc82fcfd
--- /dev/null
+++ b/src/Mobs/PathFinder.cpp
@@ -0,0 +1,261 @@
+#include "Globals.h"
+#include "PathFinder.h"
+#include "../Chunk.h"
+
+
+
+
+
+cPathFinder::cPathFinder(double a_MobWidth, double a_MobHeight) :
+	m_Path(),
+	m_GiveUpCounter(0),
+	m_NotFoundCooldown(0)
+{
+	m_Width = a_MobWidth;
+	m_Height = a_MobHeight;
+}
+
+
+
+
+
+ePathFinderStatus cPathFinder::GetNextWayPoint(cChunk & a_Chunk, const Vector3d & a_Source, Vector3d * a_Destination, Vector3d * a_OutputWaypoint, bool a_DontCare)
+{
+	m_FinalDestination = *a_Destination;
+	m_Source = a_Source;
+
+	// If a recent PATH_NOT_FOUND was returned, we rest for a few ticks.
+	if (m_NotFoundCooldown > 0)
+	{
+		m_NotFoundCooldown -= 1;
+		return ePathFinderStatus::CALCULATING;
+	}
+
+	// Tweak the destination. If something is wrong with the destination or the chunk, rest for a while.
+	if (!EnsureProperDestination(a_Chunk))
+	{
+		m_NotFoundCooldown = 20;
+		return ePathFinderStatus::PATH_NOT_FOUND;
+	}
+
+	// Rest is over. Prepare m_Path by calling ResetPathFinding.
+	if (m_NotFoundCooldown == 0)
+	{
+		m_NotFoundCooldown = -1;
+		ResetPathFinding(a_Chunk);
+	}
+
+	// If m_Path has not been initialized yet, initialize it.
+	if (!m_Path.IsValid())
+	{
+		ResetPathFinding(a_Chunk);
+	}
+
+	switch (m_Path.CalculationStep(a_Chunk))
+	{
+		case ePathFinderStatus::NEARBY_FOUND:
+		{
+			m_NoPathToTarget = true;
+			m_PathDestination = m_Path.AcceptNearbyPath();
+			if (a_DontCare)
+			{
+				m_FinalDestination = m_PathDestination;
+				*a_Destination = m_FinalDestination;  // Modify the mob's final destination because it doesn't care about reaching an exact spot
+			}
+			else
+			{
+				m_DeviationOrigin = m_FinalDestination;  // This is the only case in which m_DeviationOrigin != m_PathDestination
+			}
+			return ePathFinderStatus::CALCULATING;
+			// The next call will trigger the PATH_FOUND case
+		}
+
+		case ePathFinderStatus::PATH_NOT_FOUND:
+		{
+			m_NotFoundCooldown = 20;
+			return ePathFinderStatus::PATH_NOT_FOUND;
+		}
+		case ePathFinderStatus::CALCULATING:
+		{
+			return ePathFinderStatus::CALCULATING;
+		}
+		case ePathFinderStatus::PATH_FOUND:
+		{
+			m_GiveUpCounter -= 1;
+
+			if ((m_GiveUpCounter == 0) || PathIsTooOld())
+			{
+				ResetPathFinding(a_Chunk);
+				return ePathFinderStatus::CALCULATING;
+			}
+
+			if (m_Path.NoMoreWayPoints())
+			{
+				// We're always heading towards m_PathDestination.
+				// If m_PathDestination is exactly m_FinalDestination, then we're about to reach the destination.
+				if (m_PathDestination == m_FinalDestination)
+				{
+					*a_OutputWaypoint = m_FinalDestination;
+					return ePathFinderStatus::PATH_FOUND;
+
+				}
+				else
+				{
+					// Otherwise, we've finished our approximate path and time to recalc.
+					ResetPathFinding(a_Chunk);
+					return ePathFinderStatus::CALCULATING;
+				}
+			}
+
+
+			if (m_Path.IsFirstPoint() || ((m_WayPoint - m_Source).SqrLength() < WAYPOINT_RADIUS))
+			{
+				// if the mob has just started or if the mob reached a waypoint, give them a new waypoint.
+				m_WayPoint = m_Path.GetNextPoint();
+				m_GiveUpCounter = 40;
+				return ePathFinderStatus::PATH_FOUND;
+			}
+			else
+			{
+				// Otherwise, the mob is still walking towards its waypoint, we'll patiently wait. We won't update m_WayPoint.
+				*a_OutputWaypoint = m_WayPoint;
+				return ePathFinderStatus::PATH_FOUND;
+			}
+		}
+		#ifndef __clang__
+		default:
+		{
+			return ePathFinderStatus::PATH_FOUND;
+			// Fixes GCC warning: "control reaches end of non-void function".
+		}
+		#endif
+	}
+}
+
+
+
+
+
+void cPathFinder::ResetPathFinding(cChunk &a_Chunk)
+{
+	m_GiveUpCounter = 40;
+	m_NoPathToTarget = false;
+	m_PathDestination = m_FinalDestination;
+	m_DeviationOrigin = m_PathDestination;
+	m_Path = cPath(a_Chunk, m_Source, m_PathDestination, 20, m_Width, m_Height);
+}
+
+
+
+
+
+bool cPathFinder::EnsureProperDestination(cChunk & a_Chunk)
+{
+	cChunk * Chunk = a_Chunk.GetNeighborChunk(FloorC(m_FinalDestination.x), FloorC(m_FinalDestination.z));
+	BLOCKTYPE BlockType;
+	NIBBLETYPE BlockMeta;
+
+	if ((Chunk == nullptr) || !Chunk->IsValid())
+	{
+		return false;
+	}
+
+	int RelX = FloorC(m_FinalDestination.x) - Chunk->GetPosX() * cChunkDef::Width;
+	int RelZ = FloorC(m_FinalDestination.z) - Chunk->GetPosZ() * cChunkDef::Width;
+
+	// If destination in the air, first try to go 1 block north, or east, or west.
+	// This fixes the player leaning issue.
+	// If that failed, we instead go down to the lowest air block.
+	Chunk->GetBlockTypeMeta(RelX, FloorC(m_FinalDestination.y) - 1, RelZ, BlockType, BlockMeta);
+	if (!cBlockInfo::IsSolid(BlockType))
+	{
+		bool InTheAir = true;
+		int x, z;
+		for (z = -1; z <= 1; ++z)
+		{
+			for (x = -1; x <= 1; ++x)
+			{
+				if ((x == 0) && (z == 0))
+				{
+					continue;
+				}
+				Chunk = a_Chunk.GetNeighborChunk(FloorC(m_FinalDestination.x+x), FloorC(m_FinalDestination.z+z));
+				if ((Chunk == nullptr) || !Chunk->IsValid())
+				{
+					return false;
+				}
+				RelX = FloorC(m_FinalDestination.x+x) - Chunk->GetPosX() * cChunkDef::Width;
+				RelZ = FloorC(m_FinalDestination.z+z) - Chunk->GetPosZ() * cChunkDef::Width;
+				Chunk->GetBlockTypeMeta(RelX, FloorC(m_FinalDestination.y) - 1, RelZ, BlockType, BlockMeta);
+				if (cBlockInfo::IsSolid(BlockType))
+				{
+					m_FinalDestination.x += x;
+					m_FinalDestination.z += z;
+					InTheAir = false;
+					goto breakBothLoops;
+				}
+			}
+		}
+		breakBothLoops:
+
+		// Go down to the lowest air block.
+		if (InTheAir)
+		{
+			while (m_FinalDestination.y > 0)
+			{
+				Chunk->GetBlockTypeMeta(RelX, FloorC(m_FinalDestination.y) - 1, RelZ, BlockType, BlockMeta);
+				if (cBlockInfo::IsSolid(BlockType))
+				{
+					break;
+				}
+				m_FinalDestination.y -= 1;
+			}
+		}
+	}
+
+	// If destination in water, go up to the highest water block.
+	// If destination in solid, go up to first air block.
+	bool InWater = false;
+	while (m_FinalDestination.y < cChunkDef::Height)
+	{
+		Chunk->GetBlockTypeMeta(RelX, FloorC(m_FinalDestination.y), RelZ, BlockType, BlockMeta);
+		if (BlockType == E_BLOCK_STATIONARY_WATER)
+		{
+			InWater = true;
+		}
+		else if (cBlockInfo::IsSolid(BlockType))
+		{
+			InWater = false;
+		}
+		else
+		{
+			break;
+		}
+		m_FinalDestination.y += 1;
+	}
+	if (InWater)
+	{
+		m_FinalDestination.y -= 1;
+	}
+
+
+	return true;
+}
+
+
+
+
+
+bool cPathFinder::PathIsTooOld() const
+{
+	size_t acceptableDeviation = m_Path.WayPointsLeft() / 2;
+	if (acceptableDeviation == 0)
+	{
+		acceptableDeviation = 1;
+	}
+	if ((m_FinalDestination - m_DeviationOrigin).SqrLength() > acceptableDeviation * acceptableDeviation)
+	{
+		return true;
+	}
+	return false;
+}
diff --git a/src/Mobs/PathFinder.h b/src/Mobs/PathFinder.h
new file mode 100644
index 000000000..1570679bf
--- /dev/null
+++ b/src/Mobs/PathFinder.h
@@ -0,0 +1,96 @@
+
+#pragma once
+#include "Path.h"
+
+#define WAYPOINT_RADIUS 0.5
+/*
+TODO DOXY style
+
+This class wraps cPath.
+cPath is a "dumb device" - You give it point A and point B, and it returns a full path path.
+cPathFinder - You give it a constant stream of point A (where you are) and point B (where you want to go),
+and it tells you where to go next. It manages path recalculation internally, and is much more efficient that calling cPath every step.
+
+*/
+
+class cPathFinder
+{
+
+public:
+	/** Creates a cPathFinder instance. Each mob should have one cPathFinder throughout its lifetime.
+	@param a_MobWidth The mob width.
+	@param a_MobWidth The mob height.
+	*/
+	cPathFinder(double a_MobWidth, double a_MobHeight);
+
+	/** Updates the PathFinder's internal state and returns a waypoint.
+	A waypoint is a coordinate which the mob can safely move to from its current position in a straight line.
+	The mob is expected to call this function tick as long as it is following a path.
+	@param a_Chunk The chunk in which the mob is currently at.
+	@param a_Source The mob's position. a_Source's coordinates are expected to be within the chunk given in a_Chunk.
+	@param a_Destination The position the mob would like to reach. If a_ExactPath is true, the PathFinder may modify this.
+	@param a_OutputWaypoint An output parameter: The next waypoint to go to.
+	@param a_DontCare If true, the mob doesn't care where to go, and the Pathfinder may modify a_Destination.
+	This should usually be false. An exception is a wandering idle mob which doesn't care about its final destination.
+	In the future, idle mobs shouldn't use A* at all.
+
+	Returns an ePathFinderStatus.
+	ePathFinderStatus:CALCULATING - The PathFinder is still processing a path. Nothing was written to a_OutputWaypoint. The mob should probably not move.
+	ePathFinderStatus:PATH_FOUND - The PathFinder has found a path to the target. The next waypoint was written a_OutputWaypoint. The mob should probably move to a_OutputWaypoint.
+	ePathFinderStatus:NEARBY_FOUND - The PathFinder did not find a destination to the target but did find a nearby spot. The next waypoint was written a_OutputWaypoint. The mob should probably move to a_OutputWaypoint.
+	ePathFinderStatus:PATH_NOT_FOUND - The PathFinder did not find a destination to the target. Nothing was written to a_OutputWaypoint. The mob should probably not move.
+
+	Note: Once NEARBY_FOUND is returned once, subsequent calls return PATH_FOUND. */
+	ePathFinderStatus GetNextWayPoint(cChunk & a_Chunk, const Vector3d & a_Source, Vector3d * a_Destination, Vector3d * a_OutputWaypoint, bool a_DontCare = false);
+
+private:
+
+	/** The width of the Mob which owns this PathFinder. */
+	double m_Width;
+
+	/** The height of the Mob which owns this PathFinder. */
+	double m_Height;
+
+	/** The current cPath instance we have. This is discarded and recreated when a path recalculation is needed. */
+	cPath m_Path;
+
+	/** If 0, will give up reaching the next m_WayPoint and will recalculate path. */
+	int m_GiveUpCounter;
+
+	/** Coordinates of the next position that should be reached. */
+	Vector3d m_WayPoint;
+
+	/** Coordinates for where we should go. This is out ultimate, final destination. */
+	Vector3d m_FinalDestination;
+
+	/** Coordinates for where we are practically going. */
+	Vector3d m_PathDestination;
+
+	/** When FinalDestination is too far from this, we recalculate.
+	This usually equals PathDestination. Except when m_NoPathToTarget is true. */
+	Vector3d m_DeviationOrigin;
+
+
+	/** Coordinates for where the mob is currently at. */
+	Vector3d m_Source;
+
+	/** True if there's no path to target and we're walking to a nearby location instead. */
+	bool m_NoPathToTarget;
+
+	/** When a path is not found, this cooldown prevents any recalculations for several ticks. */
+	int m_NotFoundCooldown;
+
+	/** Ensures the destination is not buried underground or under water. Also ensures the destination is not in the air.
+	Only the Y coordinate of m_FinalDestination might be changed by this call.
+	1. If m_FinalDestination is the position of a water block, m_FinalDestination's Y will be modified to point to the heighest water block in the pool in the current column.
+	2. If m_FinalDestination is the position of a solid, m_FinalDestination's Y will be modified to point to the first airblock above the solid in the current column.
+	3. If m_FinalDestination is the position of an air block, Y will keep decreasing until hitting either a solid or water.
+	Now either 1 or 2 is performed. */
+	bool EnsureProperDestination(cChunk & a_Chunk);
+
+	/** Resets a pathfinding task, typically because m_FinalDestination has deviated too much from m_DeviationOrigin. */
+	void ResetPathFinding(cChunk &a_Chunk);
+
+	/** Is the path too old and should be recalculated? When this is true ResetPathFinding() is called. */
+	bool PathIsTooOld() const;
+};
diff --git a/src/Mobs/Villager.cpp b/src/Mobs/Villager.cpp
index 2dc753f52..41807e335 100644
--- a/src/Mobs/Villager.cpp
+++ b/src/Mobs/Villager.cpp
@@ -158,7 +158,7 @@ void cVillager::HandleFarmerPrepareFarmCrops()
 void cVillager::HandleFarmerTryHarvestCrops()
 {
 	// Harvest the crops if the villager isn't moving and if the crops are closer then 2 blocks.
-	if (!m_IsFollowingPath && (GetPosition() - m_CropsPos).Length() < 2)
+	if (!m_PathfinderActivated && (GetPosition() - m_CropsPos).Length() < 2)
 	{
 		// Check if the blocks didn't change while the villager was walking to the coordinates.
 		BLOCKTYPE CropBlock = m_World->GetBlock(m_CropsPos.x, m_CropsPos.y, m_CropsPos.z);