-- AD Unload Wait
-- Holder en traktor med rest-læs ved en fuld stald AutoDrive i dag stille ved
-- aflæsningspunktet "til den er tømt på en eller anden måde" — og spærrer for
-- de næste i køen. Her omdirigeres den i stedet til nærmeste ledige Wait-punkt
-- (markør med ordet "Wait" i navnet eller i mappen "Wait"), og når målet igen
-- har plads til HELE resten, sendes den tilbage og læsser af.
--
-- Kun den aktuelle aflæsningsopgaves destination ændres; AutoDrives mål,
-- rotation og loop-tæller røres ikke.

ADUnloadWait = {}

-- Under dette regnes vognen som tom (AutoDrive selv bruger 0,1 l).
ADUnloadWait.MIN_REST_LITERS = 100
-- Kan målet tage mindre end dette, regnes det som fuldt (aflæsning starter ikke).
ADUnloadWait.MIN_FREE_TO_UNLOAD = 100
ADUnloadWait.LOG_PREFIX = "ADSmartPickup: "

-- køretøj -> {task, homeDestinationID, waitMarker, fillType, rest}
local waits = setmetatable({}, {__mode = "k"})
-- markør-id -> køretøj der har reserveret Wait-punktet
local claims = setmetatable({}, {__mode = "v"})
local warnedNoWaitPoint = setmetatable({}, {__mode = "k"})
local isInstalled = false

-- Den vare og mængde der er tilbage på vognen (største fyldte aflæsselige enhed).
local function getRestLoad(adEnv, vehicle)
    local bestFillType, bestLevel = nil, 0
    for _, unit in ipairs(adEnv.AutoDrive.getAllDischargeableUnits(vehicle) or {}) do
        local object = unit.object
        if object ~= nil and object.getFillUnitFillLevel ~= nil then
            local level = object:getFillUnitFillLevel(unit.fillUnitIndex) or 0
            if level > bestLevel then
                bestLevel = level
                bestFillType = object.getFillUnitFillType ~= nil and object:getFillUnitFillType(unit.fillUnitIndex) or nil
            end
        end
    end
    return bestFillType, bestLevel
end

local function getWaitCandidates(adEnv)
    local candidates = {}
    for _, marker in pairs(adEnv.ADGraphManager:getMapMarkers()) do
        if ADSupplyPlanner.isWaitMarker(marker) then
            local wayPoint = adEnv.ADGraphManager:getWayPointById(marker.id)
            if wayPoint ~= nil then
                table.insert(candidates, {marker = marker, x = wayPoint.x, z = wayPoint.z})
            end
        end
    end
    return candidates
end

local function getVehiclePosition(vehicle)
    if vehicle.rootNode == nil then
        return nil
    end
    local x, _, z = getWorldTranslation(vehicle.rootNode)
    return x, z
end

local function redirect(task, destinationID)
    task.destinationID = destinationID
    task.state = ADUnloadWait.STATE_DRIVING
    task.vehicle.ad.drivePathModule:setPathTo(destinationID)
end

local function release(vehicle)
    local wait = waits[vehicle]
    if wait ~= nil and claims[wait.waitMarker.id] == vehicle then
        claims[wait.waitMarker.id] = nil
    end
    waits[vehicle] = nil
end

-- Står køretøjet ved målet med rest-læs, som målet ikke kan tage? Så find et Wait-punkt.
local function startWaiting(adEnv, task)
    local vehicle = task.vehicle
    if not vehicle.ad.drivePathModule:isTargetReached() or vehicle.ad.trailerModule:isActiveAtTrigger() then
        return
    end
    local fillType, rest = getRestLoad(adEnv, vehicle)
    if fillType == nil or rest < ADUnloadWait.MIN_REST_LITERS then
        return
    end
    local free = ADSmartPickup.getTargetFreeCapacity(adEnv, vehicle, fillType)
    if free == nil or free >= ADUnloadWait.MIN_FREE_TO_UNLOAD then
        return
    end

    local x, z = getVehiclePosition(vehicle)
    local candidate = x ~= nil and ADSupplyPlanner.pickNearestFreeWaitPoint(getWaitCandidates(adEnv), claims, x, z, vehicle) or nil
    if candidate == nil then
        if not warnedNoWaitPoint[vehicle] then
            warnedNoWaitPoint[vehicle] = true
            Logging.warning(ADUnloadWait.LOG_PREFIX .. "'%s' har %.0f l rest, og målet er fuldt — intet ledigt Wait-punkt (markør med 'Wait' i navnet), så AutoDrive venter ved målet", tostring(vehicle:getName()), rest)
        end
        return
    end
    warnedNoWaitPoint[vehicle] = nil
    claims[candidate.marker.id] = vehicle
    waits[vehicle] = {task = task, homeDestinationID = task.destinationID, waitMarker = candidate.marker, fillType = fillType, rest = rest}
    redirect(task, candidate.marker.id)
    local fillTypeName = g_fillTypeManager ~= nil and g_fillTypeManager:getFillTypeNameByIndex(fillType) or tostring(fillType)
    Logging.info(ADUnloadWait.LOG_PREFIX .. "'%s' venter ved %s: %.0f l %s rest, målet har kun %.0f l fri", tostring(vehicle:getName()), tostring(candidate.marker.name), rest, tostring(fillTypeName), free)
end

-- Ventende køretøj: er der nu plads til hele resten, køres tilbage til målet.
local function updateWaiting(adEnv, task, wait)
    local vehicle = task.vehicle
    if wait.task ~= task then
        release(vehicle)
        return
    end
    local fillType, rest = getRestLoad(adEnv, vehicle)
    if fillType == nil or rest < ADUnloadWait.MIN_REST_LITERS then
        release(vehicle)
        return
    end
    local free = ADSmartPickup.getTargetFreeCapacity(adEnv, vehicle, fillType)
    if free == nil or free < rest then
        return
    end
    local home = wait.homeDestinationID
    release(vehicle)
    redirect(task, home)
    Logging.info(ADUnloadWait.LOG_PREFIX .. "'%s' kører tilbage fra %s og læsser %.0f l af (målet har %.0f l fri)", tostring(vehicle:getName()), tostring(wait.waitMarker.name), rest, free)
end

function ADUnloadWait.beforeUpdate(adEnv, task)
    local wait = waits[task.vehicle]
    if wait ~= nil then
        updateWaiting(adEnv, task, wait)
    else
        startWaiting(adEnv, task)
    end
end

-- Kaldes fra ADSmartPickup:update — standsede køretøjer slipper deres Wait-punkt.
function ADUnloadWait.releaseInactive()
    for vehicle in pairs(waits) do
        local stateModule = vehicle.ad ~= nil and vehicle.ad.stateModule or nil
        if stateModule == nil or stateModule.isActive == nil or not stateModule:isActive() then
            release(vehicle)
        end
    end
end

function ADUnloadWait.install(adEnv)
    local task, autoDrive = adEnv.UnloadAtDestinationTask, adEnv.AutoDrive
    if isInstalled or task == nil or task.update == nil or autoDrive == nil or autoDrive.getSetting == nil then
        return
    end
    isInstalled = true
    ADUnloadWait.STATE_DRIVING = task.STATE_DRIVING or 2

    local originalUpdate, originalFinished, originalGetSetting = task.update, task.finished, autoDrive.getSetting

    task.update = function(self, dt)
        local ok, err = pcall(ADUnloadWait.beforeUpdate, adEnv, self)
        if not ok then
            Logging.warning(ADUnloadWait.LOG_PREFIX .. "fejl i Wait-punkt-logik, AutoDrive fortsætter selv: %s", tostring(err))
            release(self.vehicle)
        end
        return originalUpdate(self, dt)
    end

    if originalFinished ~= nil then
        task.finished = function(self)
            release(self.vehicle)
            return originalFinished(self)
        end
    end

    -- Ved "rotér aflæsning" kører AutoDrive straks videre fra et mål den ikke kan læsse
    -- af ved — også fra Wait-punktet. Ventende køretøjer ser derfor rotationen som slået fra.
    autoDrive.getSetting = function(settingName, vehicle)
        if settingName == "rotateTargets" and vehicle ~= nil and waits[vehicle] ~= nil then
            return autoDrive.RT_NONE
        end
        return originalGetSetting(settingName, vehicle)
    end
end