-- 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.
-- Kun "til punktet": ved aflæsning af (forkert) last i en silo er næste tur AD's egen
-- rute fra siloen, så "fra siloen hjem" er ikke et krav.
local function hasRouteTo(adEnv, vehicle, wayPointId)
    local graph = adEnv.ADGraphManager
    if graph.getPathTo == nil then
        return true
    end
    return pathReaches(graph:getPathTo(vehicle, wayPointId), wayPointId)
end

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

-- køretøj -> AD's oprindelige 2. markør (waypoint-id) mens der læsses af i en silo
local pendingTargetRestore = setmetatable({}, {__mode = "k"})

-- Aflæsning ved en ANDEN destination end AD's mål: AD tipper kun når vognen er tæt på
-- AD's egen 2. markør (getDistanceToUnloadPosition), så den skiftes midlertidigt til
-- siloen og gendannes når opgaven er færdig.
local function redirectForUnload(task, destinationID)
    local vehicle = task.vehicle
    local stateModule = vehicle.ad.stateModule
    local current = stateModule.getSecondMarker ~= nil and stateModule:getSecondMarker() or nil
    if current ~= nil and current.id ~= destinationID and stateModule.setSecondMarkerByWayPointId ~= nil then
        pendingTargetRestore[vehicle] = pendingTargetRestore[vehicle] or current.id
        stateModule:setSecondMarkerByWayPointId(destinationID)
    end
    redirect(task, destinationID)
end

local function restoreTarget(vehicle)
    local homeId = pendingTargetRestore[vehicle]
    if homeId == nil then
        return
    end
    pendingTargetRestore[vehicle] = nil
    local stateModule = vehicle.ad ~= nil and vehicle.ad.stateModule or nil
    if stateModule ~= nil and stateModule.setSecondMarkerByWayPointId ~= nil then
        stateModule:setSecondMarkerByWayPointId(homeId)
    end
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

-- "Fremme ved målet": AD stopper vognen så snart den står i aflæsningstriggeren — ofte
-- 5–10 m FØR markøren, så isTargetReached() bliver aldrig sand (set ved sukkerfabrikken).
-- Derfor tæller også AD's eget mål for tipning (afstand til aflæsningsstedet inden for
-- AD's trigger-afstand) og at AD er i gang med at tippe.
local function isAtUnloadTarget(adEnv, vehicle)
    if vehicle.ad.drivePathModule:isTargetReached() then
        return true
    end
    local trailerModule = vehicle.ad.trailerModule
    if trailerModule ~= nil and trailerModule.isUnloading then
        return true
    end
    local autoDrive = adEnv.AutoDrive
    if autoDrive.getDistanceToUnloadPosition ~= nil and autoDrive.getMaxTriggerDistance ~= nil then
        local ok, distance = pcall(autoDrive.getDistanceToUnloadPosition, vehicle)
        local okMax, maxDistance = pcall(autoDrive.getMaxTriggerDistance, vehicle)
        if ok and okMax and type(distance) == "number" and type(maxDistance) == "number" then
            return distance <= maxDistance
        end
    end
    return false
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 isAtUnloadTarget(adEnv, vehicle) 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}
    -- AD tipper (og holder vognen stille) så længe den står nær sit EGET mål — også i små
    -- portioner, når målet forbruger løbende. Derfor peger AD's mål på Wait-punktet imens.
    redirectForUnload(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 home = wait.homeDestinationID
    local free, capacity = ADSmartPickup.getTargetFreeCapacity(adEnv, vehicle, fillType, nil, home)
    local isRejected = free == nil and ADSmartPickup.getTargetFreeCapacity(adEnv, vehicle, fillType, true, home) == 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 hasRouteTo(adEnv, vehicle, markerId)
            end
            ADLoadSwap.tryFromWait(adEnv, task, wait, dt, redirectForUnload, release, isReachable)
        end
        return
    end
    release(vehicle)
    restoreTarget(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

-- Diagnose (midlertidig): mens en traktor holder ved sit mål med last, logges hvert 15. s hvad
-- AD ser — vognens indhold, målets fri plads, AD's aflæsningstilstand og vognens dischargeState.
ADUnloadWait.DEBUG_INTERVAL_MS = 15000
local debugTimers = setmetatable({}, {__mode = "k"})
local function debugAtTarget(adEnv, task, dt)
    local vehicle = task.vehicle
    local targetReached = vehicle.ad.drivePathModule:isTargetReached()
    debugTimers[vehicle] = (debugTimers[vehicle] or ADUnloadWait.DEBUG_INTERVAL_MS) + (dt or 0)
    if debugTimers[vehicle] < ADUnloadWait.DEBUG_INTERVAL_MS then
        return
    end
    debugTimers[vehicle] = 0
    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)
    -- kun den tilstand vi vil se: målet fuldt/afvist, og traktoren venter ikke ved et Wait-punkt
    if waits[vehicle] ~= nil or (free ~= nil and free >= ADUnloadWait.MIN_FREE_TO_UNLOAD) then
        return
    end
    local trailerModule = vehicle.ad.trailerModule
    local states = {}
    for _, unit in ipairs(adEnv.AutoDrive.getAllDischargeableUnits(vehicle) or {}) do
        local object = unit.object
        local state = object ~= nil and object.getDischargeState ~= nil and object:getDischargeState() or "?"
        local canDischarge = "?"
        if object ~= nil and object.getCanDischargeToObject ~= nil and object.getCurrentDischargeNode ~= nil then
            local ok, value = pcall(function() return object:getCanDischargeToObject(object:getCurrentDischargeNode()) end)
            canDischarge = ok and tostring(value) or "err"
        end
        table.insert(states, string.format("dischargeState=%s canDischargeToObject=%s", tostring(state), canDischarge))
    end
    local fillTypeName = g_fillTypeManager ~= nil and g_fillTypeManager:getFillTypeNameByIndex(fillType) or tostring(fillType)
    local distance = adEnv.AutoDrive.getDistanceToUnloadPosition ~= nil and adEnv.AutoDrive.getDistanceToUnloadPosition(vehicle) or -1
    local secondMarker = vehicle.ad.stateModule.getSecondMarker ~= nil and vehicle.ad.stateModule:getSecondMarker() or nil
    Logging.info(ADUnloadWait.LOG_PREFIX .. "DEBUG '%s': targetReached=%s taskDest=%s adMål=%s; %.0f l %s på vognen; målet fri=%s/kap=%s; AD isUnloading=%s isLoading=%s afstandTilAflæsning=%.1f; venter=%s; %s", tostring(vehicle:getName()), tostring(targetReached), tostring(task.destinationID), tostring(secondMarker ~= nil and secondMarker.id or nil), rest, tostring(fillTypeName), tostring(free), tostring(capacity), tostring(trailerModule ~= nil and trailerModule.isUnloading), tostring(trailerModule ~= nil and trailerModule.isLoading), tonumber(distance) or -1, tostring(waits[vehicle] ~= nil), table.concat(states, " | "))
end

function ADUnloadWait.beforeUpdate(adEnv, task, dt)
    pcall(debugAtTarget, 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
    for vehicle in pairs(pendingTargetRestore) do
        local stateModule = vehicle.ad ~= nil and vehicle.ad.stateModule or nil
        if stateModule == nil or stateModule.isActive == nil or not stateModule:isActive() then
            restoreTarget(vehicle)
        end
    end
end

function ADUnloadWait.getPendingTargetRestore(vehicle)
    return pendingTargetRestore[vehicle]
end

-- Ved Start med forkert last: AD's mål peger på siloen indtil aflæsningen er færdig;
-- det oprindelige mål gendannes af finished-hook'en.
function ADUnloadWait.setDumpTarget(vehicle, dumpWayPointId)
    local stateModule = vehicle.ad.stateModule
    local current = stateModule.getSecondMarker ~= nil and stateModule:getSecondMarker() or nil
    if current ~= nil and current.id ~= dumpWayPointId then
        pendingTargetRestore[vehicle] = pendingTargetRestore[vehicle] or current.id
    end
    if stateModule.setSecondMarkerByWayPointId ~= nil then
        stateModule:setSecondMarkerByWayPointId(dumpWayPointId)
    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.hasRouteTo(adEnv, vehicle, wayPointId)
    return hasRouteTo(adEnv, vehicle, wayPointId)
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)
            restoreTarget(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