From 61b09d3c71dc91f5ddee73dbb7634e1484d3f3fd Mon Sep 17 00:00:00 2001
From: schwiti6190 <58079399+schwiti6190@users.noreply.github.com>
Date: Tue, 10 Oct 2023 12:01:46 +0200
Subject: [PATCH 1/7] WIP Pathfinder controller to unify the pathfinder
 handling

---
 scripts/ai/AIDriveStrategyCourse.lua |   6 +-
 scripts/ai/PathfinderController.lua  | 364 +++++++++++++++++++++++++++
 2 files changed, 369 insertions(+), 1 deletion(-)
 create mode 100644 scripts/ai/PathfinderController.lua

diff --git a/scripts/ai/AIDriveStrategyCourse.lua b/scripts/ai/AIDriveStrategyCourse.lua
index f53cdbd49..6b467c1ec 100644
--- a/scripts/ai/AIDriveStrategyCourse.lua
+++ b/scripts/ai/AIDriveStrategyCourse.lua
@@ -111,6 +111,9 @@ function AIDriveStrategyCourse:setAIVehicle(vehicle, jobParameters)
     self:initializeImplementControllers(vehicle)
     self.ppc = PurePursuitController(vehicle)
     self.ppc:registerListeners(self, 'onWaypointPassed', 'onWaypointChange')
+
+    self.pathfinderController = PathfinderController(vehicle)
+
     self.storage = vehicle.spec_cpAIWorker
 
     self.settings = vehicle:getCpSettings()
@@ -428,8 +431,9 @@ function AIDriveStrategyCourse:getCurrentCourse()
     return self.ppc:getCourse() or self.course
 end
 
-function AIDriveStrategyCourse:update()
+function AIDriveStrategyCourse:update(dt)
     self.ppc:update()
+    self.pathfinderController:update(dt)
     self:updatePathfinding()
     self:updateInfoTexts()
 end
diff --git a/scripts/ai/PathfinderController.lua b/scripts/ai/PathfinderController.lua
new file mode 100644
index 000000000..a85bed4e5
--- /dev/null
+++ b/scripts/ai/PathfinderController.lua
@@ -0,0 +1,364 @@
+--[[
+This file is part of Courseplay (https://github.com/Courseplay/Courseplay_FS22)
+Copyright (C) 2023 Courseplay Dev Team
+This program is free software: you can redistribute it and/or modify
+it under the terms of the GNU General Public License as published by
+the Free Software Foundation, either version 3 of the License, or
+(at your option) any later version.
+This program is distributed in the hope that it will be useful,
+but WITHOUT ANY WARRANTY; without even the implied warranty of
+MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+GNU General Public License for more details.
+You should have received a copy of the GNU General Public License
+along with this program.  If not, see <http://www.gnu.org/licenses/>.
+]]
+
+--[[
+PathfinderController for easy access to the pathfinder.
+- Enables retrying with adjustable parameters compared to the last try, like fruit allowed and so..
+- Handles the pathfinder coroutines if needed.
+- One callback when the path finding finished.
+	- Triggered if a valid path was found.
+	- Gets triggered if the goal node is invalid.
+	- Also gets triggered if no valid path was found and all retry attempts are used.
+- Every time the path finding failed a callback gets triggered, if there are retry attempts left over.
+	- Enabled the changing of the pathfinder context and restart with the new context.
+
+PathfinderControllerContext implements all pathfinder parameters and the number of retries allowed.
+- Other Context Classes could be derived for commonly used contexts. 
+
+
+
+Example implementations: 
+
+function Strategy:startPathfindingToGoal()
+	local numRetries = 2
+	local context = PathfinderControllerContext(self.vehicle, numRetries)
+	context:set(
+		... 
+	)
+	self.pathfinderController:setCallbacks(self, self.onPathfingFinished, self.onPathfindingFailed)
+
+	self.pathfinderController:findPathToNode(context, ...)
+
+end
+
+function Strategy:onPathfingFinished(controller : PathfinderController, success : boolean, 
+	path : table, goalNodeInvalid : boolean|nil)
+	if success then
+		// Path finding finished successfully
+		local course = self.pathfinderController:getTemporaryCourseFromPath(path)
+		...
+	else 
+		if goalNodeInvalid then 
+			// Goal position can't be reached!
+		else 
+			// Num retries reached without path!
+		end
+	end
+end
+
+function Strategy:onPathfindingFailed(controller : PathfinderController, lastContext : PathfinderControllerContext, 
+	wasLastRetry : boolean, currentRetryAttempt : number)
+	if currentRetryAttempt == 1 then 
+		// Reduced fruit impact:
+		lastContext:ignoreFruit()
+		self.pathfinderController:findPathToNode(lastContext, ...)
+	else 
+		// Something else ...
+		self.pathfinderController:findPathToNode(lastContext, ...)
+	end
+end
+
+]]
+
+---@class PathfinderControllerContext
+PathfinderControllerContext = CpObject()
+PathfinderControllerContext.defaultNumRetries = 0
+function PathfinderControllerContext:init(vehicle, numRetries)
+	self.vehicle = vehicle
+	self.numRetries = numRetries or self.defaultNumRetries
+	--- Percentage of fruit allowed, math.hug means no fruit avoidance
+	self.maxFruitPercent = nil
+	--- Penalty sticking on the field and avoiding outside of the field
+	self.offFieldPenalty = nil
+	--- Should none owned field be avoid?
+	self.useFieldNum = false
+	--- A given area that has to be avoided.
+	self.areaToAvoid = nil
+	--- Is reverse driving allowed?
+	self.allowReverse = false
+	--- Vehicle Collisions that are ignored.
+	self.vehiclesToIgnore = nil
+	--- Is a accurate pathfinder goal position needed?
+	self.mustBeAccurate = false
+	self.areaToIgnoreFruit = nil
+end
+
+--- Sets the Pathfinder context
+---@param mustBeAccurate boolean|nil
+---@param allowReverse boolean|nil
+---@param maxFruitPercent number|nil
+---@param offFieldPenalty number|nil
+---@param useFieldNum boolean|nil
+---@param areaToAvoid table|nil
+---@param vehiclesToIgnore table|nil
+---@param areaToIgnoreFruit table|nil
+function PathfinderControllerContext:set(
+	mustBeAccurate, allowReverse, 
+	maxFruitPercent, offFieldPenalty, useFieldNum, 
+	areaToAvoid, vehiclesToIgnore, areaToIgnoreFruit)
+	self.maxFruitPercent = maxFruitPercent
+	self.offFieldPenalty = offFieldPenalty
+	self.useFieldNum = useFieldNum
+	self.areaToAvoid = areaToAvoid
+	self.allowReverse = allowReverse
+	self.vehiclesToIgnore = vehiclesToIgnore
+	self.mustBeAccurate = mustBeAccurate
+	self.areaToIgnoreFruit = areaToIgnoreFruit
+end
+
+--- Disables the fruit avoidance
+function PathfinderControllerContext:ignoreFruit()
+	self.maxFruitPercent = math.huge
+end
+
+function PathfinderControllerContext:getNumRetriesAllowed()
+	return self.numRetries
+end
+
+function PathfinderControllerContext:__tostring()
+	local str = "PathfinderControllerContext(vehicle name=%s, maxFruitPercent=%s, "+ 
+		"offFieldPenalty=%s, useFieldNum=%s, areaToAvoid=%s, allowReverse=%s, "+
+		"vehiclesToIgnore=%s, mustBeAccurate=%s, areaToIgnoreFruit=%s)"
+	return string.format(str,
+		CpUtil.getName(self.vehicle),
+		tostring(self.maxFruitPercent),
+		tostring(self.offFieldPenalty),
+		tostring(self.useFieldNum),
+		tostring(self.areaToAvoid),
+		tostring(self.allowReverse),
+		tostring(self.vehiclesToIgnore),
+		tostring(self.mustBeAccurate),
+		tostring(self.areaToIgnoreFruit))
+end
+
+---@class DefaultFieldPathfinderControllerContext : PathfinderControllerContext
+DefaultFieldPathfinderControllerContext = CpObject(PathfinderControllerContext)
+
+function DefaultFieldPathfinderControllerContext:init(...)
+	PathfinderControllerContext.init(self, ...)
+end
+
+---@class PathfinderController 
+PathfinderController= CpObject()
+
+PathfinderController.SUCCESS_FOUND_VALID_PATH = 0
+PathfinderController.ERROR_NO_PATH_FOUND = 1
+PathfinderController.ERROR_INVALID_GOAL_NODE = 2
+function PathfinderController:init(vehicle)
+	self.vehicle = vehicle
+	---@type PathfinderInterface
+	self.pathfinder = nil
+	---@type PathfinderControllerContext
+	self.lastContext = nil
+	self:reset()
+end
+
+function PathfinderController:__tostring()
+	return string.format("PathfinderController(failCount=%d, numRetries=%s, active=%s)",
+		self.failCount, self.numRetries, tostring(self.pathfinder ~= nil))
+end
+
+function PathfinderController:reset()
+	self.numRetries = 0
+	self.failCount = 0
+	self.startedAt = 0
+	self.timeTakenMs = 0
+	self.callbackClass = nil
+	self.callbackSuccessFunction = nil 
+	self.callbackRetryFunction = nil
+	self.lastContext = nil
+end
+
+function PathfinderController:update(dt)
+	if self:isActive() then
+		--- Applies coroutine for path finding
+		local done, path, goalNodeInvalid = self.pathfinder:resume()
+        if done then
+			self:finished(path, goalNodeInvalid)
+        end
+	end
+end
+
+function PathfinderController:isActive()
+	return self.pathfinder and self.pathfinder:isActive()
+end
+
+---@return PathfinderControllerContext
+function PathfinderController:getLastContext()
+	return self.lastContext
+end
+
+--- Sets the callbacks which get called on the pathfinder finish.
+---@param class table
+---@param successFunc function func(PathfinderController, success, path, goalNodeInvalid)
+---@param retryFunc function func(PathfinderController, last context, was last retry, retry attempt number)
+function PathfinderController:setCallbacks(class, successFunc, retryFunc)
+	self.callbackClass = class
+	self.callbackSuccessFunction = successFunc 
+	self.callbackRetryFunction = retryFunc
+end
+
+--- Pathfinder was started
+---@param context PathfinderControllerContext
+function PathfinderController:started(context)
+	self:debug("Started pathfinding with context: %s.", tostring(context))
+	self.startedAt = g_time
+	self.lastContext = context
+	self.numRetries = context:getNumRetriesAllowed()
+end
+
+function PathfinderController:callCallback(callbackFunc, ...)
+	if self.callbackClass then 
+		callbackFunc(self.callbackClass, self, ...)
+	else 
+		callbackFunc(self, ...)
+	end
+end
+
+--- Path finding has finished
+---@param path table|nil
+---@param goalNodeInvalid boolean|nil
+function PathfinderController:finished(path, goalNodeInvalid)
+	self.pathfinder = nil
+	self.timeTakenMs = g_time - self.startedAt
+	local retValue = self:isValidPath(path, goalNodeInvalid)
+	if retValue == self.ERROR_NO_PATH_FOUND then 
+		if self.callbackRetryFunction then
+			--- Retry is allowed, so check if any tries are leftover
+			if self.failCount < self.numRetries then 
+				self:debug("Failed with try %d of %d.", self.failCount, self.numRetries)
+				--- Retrying the path finding
+				self.failCount = self.failCount + 1
+				self:callCallback(self.callbackRetryFunction, self.lastContext, self.failCount == self.numRetries, self.failCount)
+				return
+			end
+		end
+	end
+	self:callCallback(self.callbackSuccessFunction, retValue == self.SUCCESS_FOUND_VALID_PATH, path, goalNodeInvalid)
+	self:reset()
+end
+
+--- Is the path found and valid?
+---@param path table|nil
+---@param goalNodeInvalid boolean|nil
+---@return integer
+function PathfinderController:isValidPath(path, goalNodeInvalid)
+	if path and #path > 2 then
+        self:debug('Found a path (%d waypoints, after %d ms)', #path, self.timeTakenMs)
+        return self.SUCCESS_FOUND_VALID_PATH
+    end
+	if goalNodeInvalid then
+		self:error('No path found, goal node is invalid')
+		return self.ERROR_INVALID_GOAL_NODE
+	end 
+	self:error("No path found after %d ms", self.timeTakenMs)
+	return self.ERROR_NO_PATH_FOUND
+end
+
+--- Finds a path to given goal node
+---@param context PathfinderControllerContext
+---@param goalNode number
+---@param xOffset number
+---@param zOffset number
+---@return boolean Was path finding started?
+function PathfinderController:findPathToNode(context, 
+	goalNode, xOffset, zOffset)
+
+	if not self.callbackSuccessFunction then
+		self:error("No valid success callback was given!") 
+		return false
+	end
+	self:started(context)
+	local pathfinder, done, path, goalNodeInvalid = PathfinderUtil.startPathfindingFromVehicleToNode(
+		context.vehicle,
+		goalNode,
+		xOffset,
+		zOffset,
+		context.allowReverse, 
+		context.useFieldNum and 1,
+		context.vehiclesToIgnore,
+		context.maxFruitPercent,
+		context.offFieldPenalty,
+		context.areaToAvoid,
+		context.mustBeAccurate
+	)
+	if done then 
+		self:finished(path, goalNodeInvalid)
+	else 
+		self:debug("Continuing as coroutine...")
+		self.pathfinder = pathfinder
+	end
+	return true
+end
+
+--- Finds a path to a waypoint of a course.
+---@param context PathfinderControllerContext
+---@param course Course
+---@param waypointIndex number
+---@param xOffset number
+---@param zOffset number
+---@return boolean Was path finding started?
+function PathfinderController:findPathToWaypoint(context, 
+	course, waypointIndex, xOffset, zOffset)
+
+	if not self.callbackSuccessFunction then 
+		self:error("No valid success callback was given!") 
+		return false
+	end
+	self:started(context)
+	local pathfinder, done, path, goalNodeInvalid = PathfinderUtil.startPathfindingFromVehicleToWaypoint(
+		context.vehicle, 
+		course, 
+		waypointIndex,
+		xOffset,
+		zOffset,
+		context.allowReverse,
+		context.useFieldNum and CpFieldUtil.getFieldNumUnderVehicle(context.vehicle),
+		context.vehiclesToIgnore, 
+		context.maxFruitPercent,
+		context.offFieldPenalty, 
+		context.areaToAvoid, 
+		context.areaToIgnoreFruit)
+	if done then 
+		self:finished(path, goalNodeInvalid)
+	else 
+		self:debug("Continuing as coroutine...")
+		self.pathfinder = pathfinder
+	end
+end
+
+function PathfinderController:getTemporaryCourseFromPath(path)
+	return Course(self.vehicle, CourseGenerator.pointsToXzInPlace(path), true)
+end
+
+--------------------------------------------
+--- Debug functions
+--------------------------------------------
+
+function PathfinderController:debugStr(str, ...)
+	return "Pathfinder controller: " .. str, ...
+end
+
+function PathfinderController:debug(...)
+	CpUtil.debugVehicle(CpDebug.DBG_PATHFINDER, self.vehicle, self:debugStr(...))	
+end
+
+function PathfinderController:info(...)
+	CpUtil.infoVehicle(self.vehicle, self:debugStr(...))	
+end
+
+function PathfinderController:error(...)
+	self:info(...)
+end
\ No newline at end of file

From f3d7745f15230649cfd6dd09e442b60a7c58bb7c Mon Sep 17 00:00:00 2001
From: schwiti6190 <58079399+schwiti6190@users.noreply.github.com>
Date: Fri, 13 Oct 2023 12:43:42 +0200
Subject: [PATCH 2/7] PR adjustments

---
 modDesc.xml                           |   1 +
 scripts/CpObject.lua                  |  31 ++++
 scripts/ai/PathfinderController.lua   | 212 +++++++++++++-------------
 scripts/pathfinder/PathfinderUtil.lua |   4 +-
 4 files changed, 140 insertions(+), 108 deletions(-)

diff --git a/modDesc.xml b/modDesc.xml
index 73f81840d..f8510e82c 100644
--- a/modDesc.xml
+++ b/modDesc.xml
@@ -276,6 +276,7 @@ Changelog 7.1.0.0:
 
 		<sourceFile filename="scripts/ai/AIUtil.lua"/>
 		<sourceFile filename="scripts/ai/ImplementUtil.lua"/>
+		<sourceFile filename="scripts/ai/PathfinderController.lua"/>
 		<sourceFile filename="scripts/ai/ProximityController.lua"/>
 		<sourceFile filename="scripts/ai/FieldWorkerProximityController.lua"/>
 		<sourceFile filename="scripts/ai/CollisionAvoidanceController.lua"/>
diff --git a/scripts/CpObject.lua b/scripts/CpObject.lua
index 64cda0a00..fab7e9dde 100644
--- a/scripts/CpObject.lua
+++ b/scripts/CpObject.lua
@@ -59,10 +59,41 @@ function CpObject(base, init)
 		end
 		return false
 	end
+	c.__tostring = function (self)
+		-- Default tostring function for printing all attributes and assigned functions.
+		local str = '[ '
+		for attribute, value in pairs(self) do
+			str = str .. string.format('%s: %s ', attribute, value)
+		end
+		str = str .. ']'
+		return str
+	end
+
 	setmetatable(c, mt)
 	return c
 end
 
+---@class CpObjectUtil
+CpObjectUtil = {}
+
+--- Registers a builder api for a class.
+--- The attributes are set as private variables with "_" before the variable name 
+--- and the builder functions are named like the attribute.
+---@param class table
+---@param attributesToDefault table<attributeName, any>
+function CpObjectUtil.registerBuilderAPI(class, attributesToDefault)
+	for attributeName, default in pairs(attributesToDefault) do 
+		--- Applies the default value to the private variable
+		class["_" .. attributeName] = default
+		--- Creates the builder functions/ setters with the public variable name
+		class[attributeName] = function(self, value)
+			self["_" .. attributeName] = value	
+			return self		
+		end
+	end
+end
+
+
 --- Object that holds a value temporarily. You can tell when to set the value and how long it should keep that
 --- value, in milliseconds. Great for timers.
 ---@class CpTemporaryObject
diff --git a/scripts/ai/PathfinderController.lua b/scripts/ai/PathfinderController.lua
index a85bed4e5..91ff2af39 100644
--- a/scripts/ai/PathfinderController.lua
+++ b/scripts/ai/PathfinderController.lua
@@ -14,6 +14,11 @@ along with this program.  If not, see <http://www.gnu.org/licenses/>.
 ]]
 
 --[[
+
+--------------------------------------------
+--- Pathfinder controller
+--------------------------------------------
+
 PathfinderController for easy access to the pathfinder.
 - Enables retrying with adjustable parameters compared to the last try, like fruit allowed and so..
 - Handles the pathfinder coroutines if needed.
@@ -24,11 +29,6 @@ PathfinderController for easy access to the pathfinder.
 - Every time the path finding failed a callback gets triggered, if there are retry attempts left over.
 	- Enabled the changing of the pathfinder context and restart with the new context.
 
-PathfinderControllerContext implements all pathfinder parameters and the number of retries allowed.
-- Other Context Classes could be derived for commonly used contexts. 
-
-
-
 Example implementations: 
 
 function Strategy:startPathfindingToGoal()
@@ -44,10 +44,9 @@ function Strategy:startPathfindingToGoal()
 end
 
 function Strategy:onPathfingFinished(controller : PathfinderController, success : boolean, 
-	path : table, goalNodeInvalid : boolean|nil)
+	course : Course, goalNodeInvalid : boolean|nil)
 	if success then
 		// Path finding finished successfully
-		local course = self.pathfinderController:getTemporaryCourseFromPath(path)
 		...
 	else 
 		if goalNodeInvalid then 
@@ -70,77 +69,65 @@ function Strategy:onPathfindingFailed(controller : PathfinderController, lastCon
 	end
 end
 
+--------------------------------------------
+--- Pathfinder controller context
+--------------------------------------------
+
+
+PathfinderControllerContext implements all pathfinder parameters and the number of retries allowed.
+- Other Context Classes could be derived for commonly used contexts. 
+- Only the attributesToDefaultValue table has to be changed in the derived class.
+
+Example usage of the builder api:
+
+local context = PathfinderControllerContext():maxFruitPercent(100):useFieldNum(true):vehiclesToIgnore({vehicle})
+
+
 ]]
 
 ---@class PathfinderControllerContext
+---@field maxFruitPercent function
+---@field offFieldPenalty function
+---@field useFieldNum function
+---@field areaToAvoid function
+---@field allowReverse function
+---@field vehiclesToIgnore function
+---@field mustBeAccurate function
+---@field areaToIgnoreFruit function
+---@field _maxFruitPercent number
+---@field _offFieldPenalty number
+---@field _useFieldNum boolean
+---@field _areaToAvoid PathfinderUtil.NodeArea
+---@field _allowReverse boolean
+---@field _vehiclesToIgnore table[]
+---@field _mustBeAccurate boolean
+---@field _areaToIgnoreFruit table[]
 PathfinderControllerContext = CpObject()
 PathfinderControllerContext.defaultNumRetries = 0
-function PathfinderControllerContext:init(vehicle, numRetries)
-	self.vehicle = vehicle
-	self.numRetries = numRetries or self.defaultNumRetries
-	--- Percentage of fruit allowed, math.hug means no fruit avoidance
-	self.maxFruitPercent = nil
-	--- Penalty sticking on the field and avoiding outside of the field
-	self.offFieldPenalty = nil
-	--- Should none owned field be avoid?
-	self.useFieldNum = false
-	--- A given area that has to be avoided.
-	self.areaToAvoid = nil
-	--- Is reverse driving allowed?
-	self.allowReverse = false
-	--- Vehicle Collisions that are ignored.
-	self.vehiclesToIgnore = nil
-	--- Is a accurate pathfinder goal position needed?
-	self.mustBeAccurate = false
-	self.areaToIgnoreFruit = nil
-end
+PathfinderControllerContext.attributesToDefaultValue = {
+	["maxFruitPercent"] = 50,
+	["offFieldPenalty"] = 50,
+	["useFieldNum"] = false,
+	["areaToAvoid"] = {},
+	["allowReverse"] = false,
+	["vehiclesToIgnore"] = {},
+	["mustBeAccurate"] = false,
+	["areaToIgnoreFruit"] = {}
+}
 
---- Sets the Pathfinder context
----@param mustBeAccurate boolean|nil
----@param allowReverse boolean|nil
----@param maxFruitPercent number|nil
----@param offFieldPenalty number|nil
----@param useFieldNum boolean|nil
----@param areaToAvoid table|nil
----@param vehiclesToIgnore table|nil
----@param areaToIgnoreFruit table|nil
-function PathfinderControllerContext:set(
-	mustBeAccurate, allowReverse, 
-	maxFruitPercent, offFieldPenalty, useFieldNum, 
-	areaToAvoid, vehiclesToIgnore, areaToIgnoreFruit)
-	self.maxFruitPercent = maxFruitPercent
-	self.offFieldPenalty = offFieldPenalty
-	self.useFieldNum = useFieldNum
-	self.areaToAvoid = areaToAvoid
-	self.allowReverse = allowReverse
-	self.vehiclesToIgnore = vehiclesToIgnore
-	self.mustBeAccurate = mustBeAccurate
-	self.areaToIgnoreFruit = areaToIgnoreFruit
+function PathfinderControllerContext:init(vehicle, numRetries)
+	self._vehicle = vehicle
+	self._numRetries = numRetries or self.defaultNumRetries
+	CpObjectUtil.registerBuilderAPI(self, self.attributesToDefaultValue)
 end
 
 --- Disables the fruit avoidance
 function PathfinderControllerContext:ignoreFruit()
-	self.maxFruitPercent = math.huge
+	self._maxFruitPercent = math.huge
 end
 
 function PathfinderControllerContext:getNumRetriesAllowed()
-	return self.numRetries
-end
-
-function PathfinderControllerContext:__tostring()
-	local str = "PathfinderControllerContext(vehicle name=%s, maxFruitPercent=%s, "+ 
-		"offFieldPenalty=%s, useFieldNum=%s, areaToAvoid=%s, allowReverse=%s, "+
-		"vehiclesToIgnore=%s, mustBeAccurate=%s, areaToIgnoreFruit=%s)"
-	return string.format(str,
-		CpUtil.getName(self.vehicle),
-		tostring(self.maxFruitPercent),
-		tostring(self.offFieldPenalty),
-		tostring(self.useFieldNum),
-		tostring(self.areaToAvoid),
-		tostring(self.allowReverse),
-		tostring(self.vehiclesToIgnore),
-		tostring(self.mustBeAccurate),
-		tostring(self.areaToIgnoreFruit))
+	return self._numRetries
 end
 
 ---@class DefaultFieldPathfinderControllerContext : PathfinderControllerContext
@@ -175,9 +162,6 @@ function PathfinderController:reset()
 	self.failCount = 0
 	self.startedAt = 0
 	self.timeTakenMs = 0
-	self.callbackClass = nil
-	self.callbackSuccessFunction = nil 
-	self.callbackRetryFunction = nil
 	self.lastContext = nil
 end
 
@@ -186,11 +170,20 @@ function PathfinderController:update(dt)
 		--- Applies coroutine for path finding
 		local done, path, goalNodeInvalid = self.pathfinder:resume()
         if done then
-			self:finished(path, goalNodeInvalid)
+			self:onFinish(path, goalNodeInvalid)
         end
 	end
 end
 
+function PathfinderController:getDriveData()
+	local maxSpeed
+	if self:isActive() then 
+		--- Pathfinder is active, so we stop the driver.
+		maxSpeed = 0
+	end
+	return nil, nil, nil, maxSpeed
+end
+
 function PathfinderController:isActive()
 	return self.pathfinder and self.pathfinder:isActive()
 end
@@ -200,37 +193,30 @@ function PathfinderController:getLastContext()
 	return self.lastContext
 end
 
---- Sets the callbacks which get called on the pathfinder finish.
----@param class table
----@param successFunc function func(PathfinderController, success, path, goalNodeInvalid)
+--- Registers listeners for pathfinder success and failures.
+--- TODO: Decide if multiple registered listeners are needed or not?
+---@param object table
+---@param successFunc function func(PathfinderController, success, Course, goalNodeInvalid)
 ---@param retryFunc function func(PathfinderController, last context, was last retry, retry attempt number)
-function PathfinderController:setCallbacks(class, successFunc, retryFunc)
-	self.callbackClass = class
+function PathfinderController:registerListeners(object, successFunc, retryFunc)
+	self.callbackObject = object
 	self.callbackSuccessFunction = successFunc 
 	self.callbackRetryFunction = retryFunc
 end
 
 --- Pathfinder was started
 ---@param context PathfinderControllerContext
-function PathfinderController:started(context)
+function PathfinderController:onStart(context)
 	self:debug("Started pathfinding with context: %s.", tostring(context))
 	self.startedAt = g_time
 	self.lastContext = context
 	self.numRetries = context:getNumRetriesAllowed()
 end
 
-function PathfinderController:callCallback(callbackFunc, ...)
-	if self.callbackClass then 
-		callbackFunc(self.callbackClass, self, ...)
-	else 
-		callbackFunc(self, ...)
-	end
-end
-
 --- Path finding has finished
 ---@param path table|nil
 ---@param goalNodeInvalid boolean|nil
-function PathfinderController:finished(path, goalNodeInvalid)
+function PathfinderController:onFinish(path, goalNodeInvalid)
 	self.pathfinder = nil
 	self.timeTakenMs = g_time - self.startedAt
 	local retValue = self:isValidPath(path, goalNodeInvalid)
@@ -241,12 +227,17 @@ function PathfinderController:finished(path, goalNodeInvalid)
 				self:debug("Failed with try %d of %d.", self.failCount, self.numRetries)
 				--- Retrying the path finding
 				self.failCount = self.failCount + 1
-				self:callCallback(self.callbackRetryFunction, self.lastContext, self.failCount == self.numRetries, self.failCount)
+				self:callCallback(self.callbackRetryFunction, 
+					self.lastContext, self.failCount == self.numRetries, self.failCount)
 				return
+			elseif self.numRetries > 0 then 
+				self:debug("Max number of retries already reached!")
 			end
 		end
 	end
-	self:callCallback(self.callbackSuccessFunction, retValue == self.SUCCESS_FOUND_VALID_PATH, path, goalNodeInvalid)
+	self:callCallback(self.callbackSuccessFunction, 
+		retValue == self.SUCCESS_FOUND_VALID_PATH, 
+		self:getTemporaryCourseFromPath(path), goalNodeInvalid)
 	self:reset()
 end
 
@@ -267,6 +258,14 @@ function PathfinderController:isValidPath(path, goalNodeInvalid)
 	return self.ERROR_NO_PATH_FOUND
 end
 
+function PathfinderController:callCallback(callbackFunc, ...)
+	if self.callbackObject then 
+		callbackFunc(self.callbackObject, self, ...)
+	else 
+		callbackFunc(self, ...)
+	end
+end
+
 --- Finds a path to given goal node
 ---@param context PathfinderControllerContext
 ---@param goalNode number
@@ -280,22 +279,22 @@ function PathfinderController:findPathToNode(context,
 		self:error("No valid success callback was given!") 
 		return false
 	end
-	self:started(context)
+	self:onStart(context)
 	local pathfinder, done, path, goalNodeInvalid = PathfinderUtil.startPathfindingFromVehicleToNode(
-		context.vehicle,
+		context._vehicle,
 		goalNode,
 		xOffset,
 		zOffset,
-		context.allowReverse, 
-		context.useFieldNum and 1,
-		context.vehiclesToIgnore,
-		context.maxFruitPercent,
-		context.offFieldPenalty,
-		context.areaToAvoid,
-		context.mustBeAccurate
+		context._allowReverse, 
+		context.useFieldNum and CpFieldUtil.getFieldNumUnderVehicle(context._vehicle) or nil,
+		context._vehiclesToIgnore,
+		context._maxFruitPercent,
+		context._offFieldPenalty,
+		context._areaToAvoid,
+		context._mustBeAccurate
 	)
 	if done then 
-		self:finished(path, goalNodeInvalid)
+		self:onFinish(path, goalNodeInvalid)
 	else 
 		self:debug("Continuing as coroutine...")
 		self.pathfinder = pathfinder
@@ -317,26 +316,27 @@ function PathfinderController:findPathToWaypoint(context,
 		self:error("No valid success callback was given!") 
 		return false
 	end
-	self:started(context)
+	self:onStart(context)
 	local pathfinder, done, path, goalNodeInvalid = PathfinderUtil.startPathfindingFromVehicleToWaypoint(
-		context.vehicle, 
+		context._vehicle, 
 		course, 
 		waypointIndex,
 		xOffset,
 		zOffset,
-		context.allowReverse,
-		context.useFieldNum and CpFieldUtil.getFieldNumUnderVehicle(context.vehicle),
-		context.vehiclesToIgnore, 
-		context.maxFruitPercent,
-		context.offFieldPenalty, 
-		context.areaToAvoid, 
-		context.areaToIgnoreFruit)
+		context._allowReverse,
+		context._useFieldNum and CpFieldUtil.getFieldNumUnderVehicle(context._vehicle) or nil,
+		context._vehiclesToIgnore, 
+		context._maxFruitPercent,
+		context._offFieldPenalty, 
+		context._areaToAvoid, 
+		context._areaToIgnoreFruit)
 	if done then 
-		self:finished(path, goalNodeInvalid)
+		self:onFinish(path, goalNodeInvalid)
 	else 
 		self:debug("Continuing as coroutine...")
 		self.pathfinder = pathfinder
 	end
+	return true
 end
 
 function PathfinderController:getTemporaryCourseFromPath(path)
diff --git a/scripts/pathfinder/PathfinderUtil.lua b/scripts/pathfinder/PathfinderUtil.lua
index 32b19e645..55ee89b12 100644
--- a/scripts/pathfinder/PathfinderUtil.lua
+++ b/scripts/pathfinder/PathfinderUtil.lua
@@ -938,8 +938,8 @@ end
 --- Interface function to start the pathfinder in the game. The goal is a point at sideOffset meters from the goal node
 --- (sideOffset > 0 is left)
 ------------------------------------------------------------------------------------------------------------------------
----@param vehicle table, will be used as the start location/heading, turn radius and size
----@param goalNode table The goal node
+---@param vehicle table will be used as the start location/heading, turn radius and size
+---@param goalNode number The goal node
 ---@param xOffset number side offset of the goal from the goal node
 ---@param zOffset number length offset of the goal from the goal node
 ---@param allowReverse boolean allow reverse driving

From 9a61c7698edc7084610baa93d86a1720bad3c54a Mon Sep 17 00:00:00 2001
From: Peter Vaiko <github@vaiko.us>
Date: Fri, 13 Oct 2023 07:49:01 -0400
Subject: [PATCH 3/7] Pathfinder defaults added/documented.

---
 scripts/ai/PathfinderController.lua | 28 +++++++++++++++++++++-------
 1 file changed, 21 insertions(+), 7 deletions(-)

diff --git a/scripts/ai/PathfinderController.lua b/scripts/ai/PathfinderController.lua
index 91ff2af39..924103d4a 100644
--- a/scripts/ai/PathfinderController.lua
+++ b/scripts/ai/PathfinderController.lua
@@ -97,22 +97,36 @@ local context = PathfinderControllerContext():maxFruitPercent(100):useFieldNum(t
 ---@field _maxFruitPercent number
 ---@field _offFieldPenalty number
 ---@field _useFieldNum boolean
----@field _areaToAvoid PathfinderUtil.NodeArea
+---@field _areaToAvoid PathfinderUtil.NodeArea|nil
 ---@field _allowReverse boolean
----@field _vehiclesToIgnore table[]
+---@field _vehiclesToIgnore table[]|nil
 ---@field _mustBeAccurate boolean
 ---@field _areaToIgnoreFruit table[]
 PathfinderControllerContext = CpObject()
 PathfinderControllerContext.defaultNumRetries = 0
 PathfinderControllerContext.attributesToDefaultValue = {
+	-- If an 4 x 4 m area around a pathfinder node has more than this fruit, a penalty of 0.5 * actual fruit
+	-- percentage will be applied to that node.
+	-- TODO: check if the fruitValue returned by FSDensityMapUtil.getFruitArea() really is a percentage
 	["maxFruitPercent"] = 50,
-	["offFieldPenalty"] = 50,
-	["useFieldNum"] = false,
-	["areaToAvoid"] = {},
+	-- This penalty is added to the cost of each step, which is about 12% of the turning radius when using
+	-- the hybrid A* and 3 with the simple A*.
+	-- Simple A* is used for long-range pathfinding, in that case we are willing to drive about 3 times longer
+	-- to stay on the field. Hybrid A* is more restrictive, TODO: review if these should be balanced
+	-- If useFieldNum true, fields that are not owned have a 20% more penalty.
+	["offFieldPenalty"] = 7.5,
+ 	["useFieldNum"] = false,
+	-- Pathfinder nodes in this area have a prohibitive penalty (2000)
+	["areaToAvoid"] = nil,
 	["allowReverse"] = false,
-	["vehiclesToIgnore"] = {},
+	["vehiclesToIgnore"] = nil,
+	-- If false, as we reach the maximum iterations, we relax our criteria to reach the goal: allow for arriving at
+	-- bigger angle differences, trading off accuracy for speed. This usually results in a direction at the goal
+	-- being less then 30º off which in many cases isn't a problem.
+	-- Otherwise, for example when a combine self unloading must accurately find the trailer, set this to true.
 	["mustBeAccurate"] = false,
-	["areaToIgnoreFruit"] = {}
+	-- No fruit penalty in this area (e.g. when we know the goal is in fruit but want to avoid fruit all the way there)
+	["areaToIgnoreFruit"] = nil
 }
 
 function PathfinderControllerContext:init(vehicle, numRetries)

From f2700487c904ce457b956ef35c7adbaa75aa8837 Mon Sep 17 00:00:00 2001
From: schwiti6190 <58079399+schwiti6190@users.noreply.github.com>
Date: Fri, 13 Oct 2023 14:22:00 +0200
Subject: [PATCH 4/7] Added NIL alternative for Builder generation, as nil
 value are optimized away on declaration

---
 scripts/CpObject.lua                | 8 +++++++-
 scripts/ai/PathfinderController.lua | 6 +++---
 2 files changed, 10 insertions(+), 4 deletions(-)

diff --git a/scripts/CpObject.lua b/scripts/CpObject.lua
index fab7e9dde..02f8dccf0 100644
--- a/scripts/CpObject.lua
+++ b/scripts/CpObject.lua
@@ -74,15 +74,21 @@ function CpObject(base, init)
 end
 
 ---@class CpObjectUtil
-CpObjectUtil = {}
+CpObjectUtil = {
+	BUILDER_API_NIL = "nil"
+}
 
 --- Registers a builder api for a class.
 --- The attributes are set as private variables with "_" before the variable name 
 --- and the builder functions are named like the attribute.
+--- Nil values have to be replaced with CpObjectUtil.BUILDER_API_NIL !!
 ---@param class table
 ---@param attributesToDefault table<attributeName, any>
 function CpObjectUtil.registerBuilderAPI(class, attributesToDefault)
 	for attributeName, default in pairs(attributesToDefault) do 
+		if default == CpObjectUtil.BUILDER_API_NIL then 
+			default = nil
+		end
 		--- Applies the default value to the private variable
 		class["_" .. attributeName] = default
 		--- Creates the builder functions/ setters with the public variable name
diff --git a/scripts/ai/PathfinderController.lua b/scripts/ai/PathfinderController.lua
index 924103d4a..d7fc53668 100644
--- a/scripts/ai/PathfinderController.lua
+++ b/scripts/ai/PathfinderController.lua
@@ -117,16 +117,16 @@ PathfinderControllerContext.attributesToDefaultValue = {
 	["offFieldPenalty"] = 7.5,
  	["useFieldNum"] = false,
 	-- Pathfinder nodes in this area have a prohibitive penalty (2000)
-	["areaToAvoid"] = nil,
+	["areaToAvoid"] = CpObjectUtil.BUILDER_API_NIL,
 	["allowReverse"] = false,
-	["vehiclesToIgnore"] = nil,
+	["vehiclesToIgnore"] = CpObjectUtil.BUILDER_API_NIL,
 	-- If false, as we reach the maximum iterations, we relax our criteria to reach the goal: allow for arriving at
 	-- bigger angle differences, trading off accuracy for speed. This usually results in a direction at the goal
 	-- being less then 30º off which in many cases isn't a problem.
 	-- Otherwise, for example when a combine self unloading must accurately find the trailer, set this to true.
 	["mustBeAccurate"] = false,
 	-- No fruit penalty in this area (e.g. when we know the goal is in fruit but want to avoid fruit all the way there)
-	["areaToIgnoreFruit"] = nil
+	["areaToIgnoreFruit"] = CpObjectUtil.BUILDER_API_NIL
 }
 
 function PathfinderControllerContext:init(vehicle, numRetries)

From 3550fde0f8b4ac24929472138f8f1682241c6150 Mon Sep 17 00:00:00 2001
From: schwiti6190 <58079399+schwiti6190@users.noreply.github.com>
Date: Fri, 13 Oct 2023 14:36:11 +0200
Subject: [PATCH 5/7] Changed useFieldNum to an INT

---
 scripts/ai/PathfinderController.lua | 15 ++++++++++-----
 1 file changed, 10 insertions(+), 5 deletions(-)

diff --git a/scripts/ai/PathfinderController.lua b/scripts/ai/PathfinderController.lua
index d7fc53668..8bb9ad5a5 100644
--- a/scripts/ai/PathfinderController.lua
+++ b/scripts/ai/PathfinderController.lua
@@ -96,7 +96,7 @@ local context = PathfinderControllerContext():maxFruitPercent(100):useFieldNum(t
 ---@field areaToIgnoreFruit function
 ---@field _maxFruitPercent number
 ---@field _offFieldPenalty number
----@field _useFieldNum boolean
+---@field _useFieldNum number
 ---@field _areaToAvoid PathfinderUtil.NodeArea|nil
 ---@field _allowReverse boolean
 ---@field _vehiclesToIgnore table[]|nil
@@ -113,9 +113,9 @@ PathfinderControllerContext.attributesToDefaultValue = {
 	-- the hybrid A* and 3 with the simple A*.
 	-- Simple A* is used for long-range pathfinding, in that case we are willing to drive about 3 times longer
 	-- to stay on the field. Hybrid A* is more restrictive, TODO: review if these should be balanced
-	-- If useFieldNum true, fields that are not owned have a 20% more penalty.
 	["offFieldPenalty"] = 7.5,
- 	["useFieldNum"] = false,
+	-- If useFieldNum > 0, fields that are not owned have a 20% greater penalty.
+ 	["useFieldNum"] = 0,
 	-- Pathfinder nodes in this area have a prohibitive penalty (2000)
 	["areaToAvoid"] = CpObjectUtil.BUILDER_API_NIL,
 	["allowReverse"] = false,
@@ -140,6 +140,11 @@ function PathfinderControllerContext:ignoreFruit()
 	self._maxFruitPercent = math.huge
 end
 
+--- Uses the field number of the vehicle to restrict path finding.
+function PathfinderControllerContext:useVehicleFieldNumber()
+	self._useFieldNum = CpFieldUtil.getFieldNumUnderVehicle(self._vehicle)
+end
+
 function PathfinderControllerContext:getNumRetriesAllowed()
 	return self._numRetries
 end
@@ -300,7 +305,7 @@ function PathfinderController:findPathToNode(context,
 		xOffset,
 		zOffset,
 		context._allowReverse, 
-		context.useFieldNum and CpFieldUtil.getFieldNumUnderVehicle(context._vehicle) or nil,
+		context._useFieldNum,
 		context._vehiclesToIgnore,
 		context._maxFruitPercent,
 		context._offFieldPenalty,
@@ -338,7 +343,7 @@ function PathfinderController:findPathToWaypoint(context,
 		xOffset,
 		zOffset,
 		context._allowReverse,
-		context._useFieldNum and CpFieldUtil.getFieldNumUnderVehicle(context._vehicle) or nil,
+		context._useFieldNum,
 		context._vehiclesToIgnore, 
 		context._maxFruitPercent,
 		context._offFieldPenalty, 

From fdc04400ebeae13455716763fbb6b6ead97c9e00 Mon Sep 17 00:00:00 2001
From: schwiti6190 <58079399+schwiti6190@users.noreply.github.com>
Date: Fri, 13 Oct 2023 14:36:48 +0200
Subject: [PATCH 6/7] Minor fix for build generation

---
 scripts/ai/PathfinderController.lua | 2 ++
 1 file changed, 2 insertions(+)

diff --git a/scripts/ai/PathfinderController.lua b/scripts/ai/PathfinderController.lua
index 8bb9ad5a5..b3a874627 100644
--- a/scripts/ai/PathfinderController.lua
+++ b/scripts/ai/PathfinderController.lua
@@ -138,11 +138,13 @@ end
 --- Disables the fruit avoidance
 function PathfinderControllerContext:ignoreFruit()
 	self._maxFruitPercent = math.huge
+	return self
 end
 
 --- Uses the field number of the vehicle to restrict path finding.
 function PathfinderControllerContext:useVehicleFieldNumber()
 	self._useFieldNum = CpFieldUtil.getFieldNumUnderVehicle(self._vehicle)
+	return self
 end
 
 function PathfinderControllerContext:getNumRetriesAllowed()

From cdf3c224c71f6956783c7faca3eb3760af605ace Mon Sep 17 00:00:00 2001
From: schwiti6190 <58079399+schwiti6190@users.noreply.github.com>
Date: Wed, 18 Oct 2023 11:37:14 +0200
Subject: [PATCH 7/7] Registered listeners to strategy

---
 scripts/ai/AIDriveStrategyCourse.lua | 19 +++++++++++++++++++
 1 file changed, 19 insertions(+)

diff --git a/scripts/ai/AIDriveStrategyCourse.lua b/scripts/ai/AIDriveStrategyCourse.lua
index 6b467c1ec..1849d55d6 100644
--- a/scripts/ai/AIDriveStrategyCourse.lua
+++ b/scripts/ai/AIDriveStrategyCourse.lua
@@ -113,6 +113,7 @@ function AIDriveStrategyCourse:setAIVehicle(vehicle, jobParameters)
     self.ppc:registerListeners(self, 'onWaypointPassed', 'onWaypointChange')
 
     self.pathfinderController = PathfinderController(vehicle)
+    self.pathfinderController:registerListeners(self, self.onPathfindingFinished, self.onPathfindingRetry)
 
     self.storage = vehicle.spec_cpAIWorker
 
@@ -583,6 +584,24 @@ end
 function AIDriveStrategyCourse:onWaypointPassed(ix, course)
 end
 
+--- Pathfinding has finished
+---@param controller PathfinderController
+---@param success boolean
+---@param course Course|nil
+---@param goalNodeInvalid boolean|nil
+function AIDriveStrategyCourse:onPathfindingFinished(controller, success, course, goalNodeInvalid)
+    -- override
+end
+
+--- Pathfinding failed, but a retry attempt is leftover.
+---@param controller PathfinderController
+---@param lastContext PathfinderControllerContext
+---@param wasLastRetry boolean
+---@param currentRetryAttempt number
+function AIDriveStrategyCourse:onPathfindingRetry(controller, lastContext, wasLastRetry, currentRetryAttempt)
+    -- override
+end
+
 ------------------------------------------------------------------------------------------------------------------------
 --- Pathfinding
 ---------------------------------------------------------------------------------------------------------------------------