-- 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 i en mappe med ordet "Wait" i navnet, fx "Cow 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

-- poolGroup: run'ets Wait-mappe; tom/nil = alle Wait-mapper.
local function getWaitCandidates(adEnv, poolGroup)
    local candidates = {}
    for _, marker in pairs(adEnv.ADGraphManager:getMapMarkers()) do
        local isInPool = poolGroup == nil or poolGroup == "" or marker.group == poolGroup
        if isInPool and 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 getWaitPoolGroup(vehicle)
    local run = ADRunsController ~= nil and ADRunsController.getRunForVehicle(vehicle) or nil
    return run ~= nil and run.waitPoolGroup or nil
end

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

-- Ender ruten i målet? (AutoDrive returnerer tom/afkortet liste når der ingen vej er.)
local function pathReaches(path, wayPointId)
    return path ~= nil and #path > 0 and path[#path].id == wayPointId
end

-- Findes der vejnet fra køretøjet til Wait-punktet OG fra Wait-punktet hjem til målet?
-- Sendes AutoDrive mod et punkt uden rute, viser den "cannot reach" og slukker sig selv.
-- getPathTo/pathFromTo er rene opslag uden bivirkninger.
local function hasRouteBothWays(adEnv, vehicle, waitWayPointId, homeWayPointId)
    local graph = adEnv.ADGraphManager
    if graph.getPathTo == nil or graph.pathFromTo == nil then
        return true
    end
    return pathReaches(graph:getPathTo(vehicle, waitWayPointId), waitWayPointId)
        and pathReaches(graph:pathFromTo(waitWayPointId, homeWayPointId), homeWayPointId)
end

-- Står AD midt i en (blokeret) aflæsning, stoppes den pænt først — ellers kører traktoren
-- væk med tippet vogn.
local function redirect(task, destinationID)
    local trailerModule = task.vehicle.ad.trailerModule
    if trailerModule ~= nil and trailerModule.isActiveAtTrigger ~= nil and trailerModule:isActiveAtTrigger() then
        if trailerModule.stopUnloading ~= nil then trailerModule:stopUnloading() end
        if trailerModule.stopLoading ~= nil then trailerModule:stopLoading() end
    end
    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
    if ADLoadSwap ~= nil then
        ADLoadSwap.clearWarning(vehicle)
    end
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
    -- Kun "fremme ved målet". At AD stadig regner vognen som aflæssende (isActiveAtTrigger)
    -- tæller IKKE med: ved et fuldt fodertrug bliver isUnloading stående for evigt, selv om
    -- intet løber — og så blokerede traktoren stalden i stedet for at køre til Wait.
    if not vehicle.ad.drivePathModule:isTargetReached() then
        return
    end
    -- Efter et bytte er opgavens mål en silo, ikke AD's rigtige aflæsningsmål (anden markør);
    -- der skal AD bare læsse af som normalt, uden Wait-logik.
    local target = vehicle.ad.stateModule.getSecondMarker ~= nil and vehicle.ad.stateModule:getSecondMarker() or nil
    if target == nil or task.destinationID ~= target.id then
        return
    end
    local fillType, rest = getRestLoad(adEnv, vehicle)
    if fillType == nil or rest < ADUnloadWait.MIN_REST_LITERS then
        return
    end
    local free, capacity = ADSmartPickup.getTargetFreeCapacity(adEnv, vehicle, fillType)
    -- nil = målet tager slet ikke varen (forkert last fra et tidligere run): som "fuldt"
    local isRejected = free == nil and ADSmartPickup.getTargetFreeCapacity(adEnv, vehicle, fillType, true) == false
    if isRejected then
        free = 0
    end
    if free == nil or free >= ADUnloadWait.MIN_FREE_TO_UNLOAD then
        return
    end

    local x, z = getVehiclePosition(vehicle)
    local isReachable = function(candidate)
        return hasRouteBothWays(adEnv, vehicle, candidate.marker.id, task.destinationID)
    end
    local pool = getWaitPoolGroup(vehicle)
    local candidate = x ~= nil and ADSupplyPlanner.pickNearestFreeWaitPoint(getWaitCandidates(adEnv, pool), claims, x, z, vehicle, isReachable) or nil
    if candidate == nil then
        if not warnedNoWaitPoint[vehicle] then
            warnedNoWaitPoint[vehicle] = true
            local poolText = pool ~= nil and pool ~= "" and (" i puljen '" .. pool .. "'") or ""
            Logging.warning(ADUnloadWait.LOG_PREFIX .. "'%s' har %.0f l rest, og målet er fuldt — intet ledigt Wait-punkt%s med vejnet til OG fra målet (markør i en mappe med 'Wait' i navnet), så AutoDrive venter ved målet", tostring(vehicle:getName()), rest, poolText)
        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 (markør %s): %.0f l %s rest, målet har kun %.0f l fri", tostring(vehicle:getName()), tostring(candidate.marker.name), tostring(candidate.marker.markerIndex or candidate.marker.id), 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, dt)
    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
    -- Friske tal til evt. bytte og til tilbage-loggen — ikke dem fra da ventningen startede.
    wait.fillType, wait.rest = fillType, rest
    local free, capacity = ADSmartPickup.getTargetFreeCapacity(adEnv, vehicle, fillType)
    local isRejected = free == nil and ADSmartPickup.getTargetFreeCapacity(adEnv, vehicle, fillType, true) == false
    if isRejected then
        free = 0
    end
    wait.isRejected = isRejected
    -- run'ets "kør hjem under X %" (fri ≥ (100-X) % af kapaciteten), ellers standardreglen
    local run = ADRunsController ~= nil and ADRunsController.getRunForVehicle(vehicle) or nil
    local freeFraction = run ~= nil and run.returnBelowPercent ~= nil and (1 - run.returnBelowPercent / 100) or nil
    if free == nil or not ADSupplyPlanner.canUnloadRest(free, rest, capacity, freeFraction) then
        if ADLoadSwap ~= nil then
            local isReachable = function(markerId)
                return hasRouteBothWays(adEnv, vehicle, markerId, wait.homeDestinationID)
            end
            ADLoadSwap.tryFromWait(adEnv, task, wait, dt, redirect, release, isReachable)
        end
        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, dt)
    local wait = waits[task.vehicle]
    if wait ~= nil then
        updateWaiting(adEnv, task, wait, dt)
    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

-- Hvem holder hvor (GUI'ets Wait-puljer-visning). Sorteret på mappe, dernæst markør-id.
-- Vognens indhold (største enhed) og evt. Wait-tilstand — til overblikket i fanen.
function ADUnloadWait.hasRouteBothWays(adEnv, vehicle, toWayPointId, homeWayPointId)
    return hasRouteBothWays(adEnv, vehicle, toWayPointId, homeWayPointId)
end

function ADUnloadWait.getRestLoad(adEnv, vehicle)
    return getRestLoad(adEnv, vehicle)
end

function ADUnloadWait.getWaitFor(vehicle)
    local wait = waits[vehicle]
    if wait == nil then
        return nil
    end
    return {markerName = wait.waitMarker ~= nil and wait.waitMarker.name or nil, fillType = wait.fillType, rest = wait.rest}
end

function ADUnloadWait.getClaims()
    local result = {}
    for _, wait in pairs(waits) do
        if wait.waitMarker ~= nil and claims[wait.waitMarker.id] ~= nil then
            table.insert(result, {marker = wait.waitMarker, vehicle = claims[wait.waitMarker.id]})
        end
    end
    table.sort(result, function(a, b)
        if a.marker.group ~= b.marker.group then
            return tostring(a.marker.group) < tostring(b.marker.group)
        end
        return a.marker.id < b.marker.id
    end)
    return result
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, dt)
        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