-- AD Wait Pool
-- Fælles pulje af Wait-punkter (markører i mapper med ordet "Wait" i navnet) med
-- reservationer, nærmeste-ledige-opslag, rutetjek og omdirigering af en AutoDrive-opgave.
-- Bruges af adUnloadWait (rest-læs ved fuldt mål) og adOutbound (hold uden tur).

ADWaitPool = {}

ADWaitPool.LOG_PREFIX = "ADSmartPickup: "
-- sættes af den der installerer opgave-hooks (task.STATE_DRIVING); 2 er AD's værdi
ADWaitPool.STATE_DRIVING = 2

-- markør-id -> køretøj der har reserveret Wait-punktet
ADWaitPool.claims = setmetatable({}, {__mode = "v"})

-- poolGroup: run'ets Wait-mappe; tom/nil = alle Wait-mapper.
function ADWaitPool.getCandidates(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

function ADWaitPool.claim(markerId, vehicle)
    ADWaitPool.claims[markerId] = vehicle
end

function ADWaitPool.getOwner(markerId)
    return ADWaitPool.claims[markerId]
end

function ADWaitPool.releaseVehicle(vehicle)
    for markerId, owner in pairs(ADWaitPool.claims) do
        if owner == vehicle then
            ADWaitPool.claims[markerId] = nil
        end
    end
end

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

-- Nærmeste ledige (eller eget reserverede) Wait-punkt fra vognens position; isReachable(candidate)
-- valgfri. nil uden position eller uden brugbart punkt. Reserverer IKKE selv.
ADWaitPool.OCCUPIED_RADIUS = 6

-- Positioner for ANDRE motoriserede køretøjer (ikke vognen selv eller dens redskaber) — til at se om et
-- Wait-punkt fysisk er optaget, også uden reservation (fx lige efter en omplanlægning).
local function otherVehiclePositions(vehicle)
    local positions = {}
    local system = g_currentMission ~= nil and g_currentMission.vehicleSystem or nil
    local own = {}
    if vehicle.getChildVehicles ~= nil then
        local ok, children = pcall(vehicle.getChildVehicles, vehicle)
        for _, child in pairs(ok and children or {}) do own[child] = true end
    end
    own[vehicle] = true
    for _, other in pairs(system ~= nil and system.vehicles or {}) do
        if not own[other] and other.spec_motorized ~= nil and other.rootNode ~= nil then
            local ok, x, _, z = pcall(getWorldTranslation, other.rootNode)
            if ok and x ~= nil then table.insert(positions, {x = x, z = z}) end
        end
    end
    return positions
end

function ADWaitPool.pickFree(adEnv, vehicle, poolGroup, isReachable)
    local x, z = getVehiclePosition(vehicle)
    if x == nil then
        return nil
    end
    local candidates = ADWaitPool.getCandidates(adEnv, poolGroup)
    local occupied = ADSupplyPlanner.markOccupiedWaitPoints(candidates, otherVehiclePositions(vehicle), ADWaitPool.OCCUPIED_RADIUS)
    return ADSupplyPlanner.pickNearestFreeWaitPoint(candidates, ADWaitPool.claims, x, z, vehicle, isReachable, occupied)
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

-- Sendes AutoDrive mod et punkt uden rute, viser den "cannot reach" og slukker sig selv.
-- getPathTo/pathFromTo er rene opslag uden bivirkninger.
-- Reserve: bredde-først gennem vejnettets udgange fra traktorens nærmeste waypoint. AD's getPathTo kan
-- sige nej trods en vej (set ved et Wait-punkt: ruten fandtes fra alle omkringliggende waypoints).
ADWaitPool.GRAPH_SEARCH_LIMIT = 50000
local loggedFallback = {}

local function graphReaches(adEnv, vehicle, targetId)
    local graph = adEnv.ADGraphManager
    if vehicle.getClosestWayPoint == nil or graph.getWayPointById == nil then
        return false
    end
    local ok, startId = pcall(vehicle.getClosestWayPoint, vehicle)
    if not ok or startId == nil then
        return false
    end
    local seen, queue, head = {[startId] = true}, {startId}, 1
    while queue[head] ~= nil and head <= ADWaitPool.GRAPH_SEARCH_LIMIT do
        local id = queue[head]
        head = head + 1
        if id == targetId then
            return true
        end
        local wayPoint = graph:getWayPointById(id)
        for _, nextId in ipairs(wayPoint ~= nil and wayPoint.out or {}) do
            if not seen[nextId] then
                seen[nextId] = true
                table.insert(queue, nextId)
            end
        end
    end
    return false
end

function ADWaitPool.hasRouteTo(adEnv, vehicle, wayPointId)
    local graph = adEnv.ADGraphManager
    if graph.getPathTo == nil then
        return true
    end
    if pathReaches(graph:getPathTo(vehicle, wayPointId), wayPointId) then
        return true
    end
    if graphReaches(adEnv, vehicle, wayPointId) then
        if not loggedFallback[wayPointId] and Logging ~= nil and Logging.info ~= nil then
            loggedFallback[wayPointId] = true
            Logging.info("ADSmartPickup: AutoDrives rutesøgning fandt ingen vej til markør %s, men vejnettet har en — godtages", tostring(wayPointId))
        end
        return true
    end
    return false
end

function ADWaitPool.hasRouteBothWays(adEnv, vehicle, toWayPointId, homeWayPointId)
    local graph = adEnv.ADGraphManager
    if graph.getPathTo == nil or graph.pathFromTo == nil then
        return true
    end
    return pathReaches(graph:getPathTo(vehicle, toWayPointId), toWayPointId)
        and pathReaches(graph:pathFromTo(toWayPointId, homeWayPointId), homeWayPointId)
end

-- Står AD midt i en (blokeret) af-/pålæsning, stoppes den pænt først — ellers kører
-- traktoren væk med tippet vogn. Kun opgavens destination ændres.
function ADWaitPool.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 = ADWaitPool.STATE_DRIVING
    task.vehicle.ad.drivePathModule:setPathTo(destinationID)
end

-- Hvem holder hvor (GUI'ets Wait-puljer-visning). Sorteret på mappe, dernæst markør-id.
function ADWaitPool.getClaims(adEnv)
    local result = {}
    local graph = adEnv ~= nil and adEnv.ADGraphManager or (FS25_AutoDrive ~= nil and FS25_AutoDrive.ADGraphManager or nil)
    for markerId, vehicle in pairs(ADWaitPool.claims) do
        local marker = graph ~= nil and graph.getMapMarkerByWayPointId ~= nil and graph:getMapMarkerByWayPointId(markerId) or nil
        if marker ~= nil then
            table.insert(result, {marker = marker, vehicle = vehicle})
        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