-- AD Field Jobs
-- Markarbejdets job-løkke (hvert sekund): opdater klar-andel, start marker, send sæt ud, følg dem
-- gennem AutoDrive-kørsel -> Courseplay-arbejde -> hjem, og gå videre til næste trin. decide() er ren
-- og testet; resten oversætter til AutoDrive/Courseplay og er pcall'et pr. mark og pr. sæt.

ADFieldJobs = {}

ADFieldJobs.LOG_PREFIX = "ADSmartPickup: "
ADFieldJobs.TICK_MS = 1000
ADFieldJobs.ARRIVE_DISTANCE = 30
ADFieldJobs.IDLE_CONFIRM_MS = 5000
ADFieldJobs.START_TIMEOUT_MS = 60000
-- Courseplay kan efterlade hjørner på skæve marker: efter så mange færdige slå-ture går marken videre.
ADFieldJobs.MAX_MOW_PASSES = 2
ADFieldJobs.state = {fields = {}, fleet = {}, assignments = {}}
ADFieldJobs.timer = 0
ADFieldJobs.now = 0
ADFieldJobs.loaded = false

local ACTIVE_PHASES = {driving = true, starting = true, working = true}

-- ---- ren beslutning ----

function ADFieldJobs.decide(field, info, assignments, now)
    if field.stage == "ready" then
        if not ADFieldModel.shouldStart(field) then return {action = "none"} end
        if info == nil or info.markerId == nil then return {action = "wait"} end
        return {action = "start"}
    end
    if not ADFieldModel.isWorking(field) then return {action = "none"} end
    local active, finished = 0, 0
    for _, assignment in ipairs(assignments) do
        if ACTIVE_PHASES[assignment.phase] then active = active + 1 end
        if assignment.phase == "finished" or assignment.phase == "returning" then finished = finished + 1 end
    end
    -- slået mark uden sæt på (sluppet undervejs, genindlæst): slåningen er gjort, videre til næste trin
    if active == 0 and field.stage == "mowing" and info ~= nil and info.mowable and (info.readyRatio or 1) < ADFieldModel.CUT_RATIO then
        return {action = "advance"}
    end
    if active == 0 and finished > 0 then
        local stillStanding = field.stage == "mowing" and info ~= nil and (info.readyRatio or 0) >= ADFieldModel.CUT_RATIO
        if not stillStanding or finished >= ADFieldJobs.MAX_MOW_PASSES then return {action = "advance"} end
    end
    if info == nil or info.markerId == nil then return {action = "wait"} end
    local role = ADFieldModel.wantsRig(field, active, now)
    if role ~= nil then return {action = "dispatch", role = role} end
    return {action = "none"}
end

-- ---- tilstand ----

local function log(fieldId, fmt, ...)
    Logging.info(ADFieldJobs.LOG_PREFIX .. "F%s " .. fmt, tostring(fieldId), ...)
end

function ADFieldJobs.getField(id)
    return ADFieldJobs.state.fields[id] or ADFieldModel.newField(id)
end

local function setField(field)
    ADFieldJobs.state.fields[field.id] = field
end

function ADFieldJobs.assignmentsFor(fieldId)
    local list = {}
    for _, assignment in pairs(ADFieldJobs.state.assignments) do
        if assignment.fieldId == fieldId then table.insert(list, assignment) end
    end
    return list
end

function ADFieldJobs.save()
    local ok, err = pcall(ADFieldStorage.save, ADFieldJobs.state)
    if not ok then Logging.warning(ADFieldJobs.LOG_PREFIX .. "marker kunne ikke gemmes: %s", tostring(err)) end
end

function ADFieldJobs.load()
    local ok, loaded = pcall(ADFieldStorage.load)
    if ok then
        ADFieldJobs.state.fields, ADFieldJobs.state.fleet = loaded.fields, loaded.fleet
    else
        Logging.warning(ADFieldJobs.LOG_PREFIX .. "marker kunne ikke indlæses: %s", tostring(loaded))
    end
    ADFieldJobs.loaded = true
end

function ADFieldJobs.setPlan(id, changes)
    setField(ADFieldModel.with(ADFieldJobs.getField(id), {plan = changes}))
    ADFieldJobs.save()
end

function ADFieldJobs.requestStart(id)
    setField(ADFieldModel.with(ADFieldJobs.getField(id), {startRequested = true}))
end

local function note(fieldId, changes)
    setField(ADFieldModel.with(ADFieldJobs.getField(fieldId), changes))
end

function ADFieldJobs.forceReady(id)
    setField(ADFieldModel.forceReady(ADFieldJobs.getField(id)))
end

function ADFieldJobs.toggleFleet(vehicleId)
    ADFieldJobs.state.fleet = ADFieldFleet.toggle(ADFieldJobs.state.fleet, vehicleId)
    ADFieldJobs.save()
end

-- ---- AutoDrive ----

local function stateModule(vehicle)
    return vehicle.ad ~= nil and vehicle.ad.stateModule or nil
end

local function isAdActive(vehicle)
    local module = stateModule(vehicle)
    return module ~= nil and module:isActive()
end

local function stopAd(vehicle)
    if isAdActive(vehicle) then vehicle:stopAutoDrive() end
end

local function startAd(adEnv, vehicle, mode, firstId, secondId, withCp)
    local module = stateModule(vehicle)
    stopAd(vehicle)
    module:setMode(mode)
    module:setFirstMarkerByWayPointId(firstId)
    if secondId ~= nil then module:setSecondMarkerByWayPointId(secondId) end
    module:setStartHelper(withCp == true)
    if withCp then
        module.usedHelper = adEnv.ADStateModule ~= nil and adEnv.ADStateModule.HELPER_CP or 1
        if vehicle.ad.settings ~= nil and vehicle.ad.settings.enableParkAtJobFinished ~= nil then
            vehicle.ad.settings.enableParkAtJobFinished.current = 2
            vehicle.ad.settings.enableParkAtJobFinished.new = 2
        end
    end
    local farmId = vehicle:getOwnerFarmId()
    if module.setActualFarmId ~= nil then module:setActualFarmId(farmId) end
    module:raiseDirtyFlag()
    local current = module:getCurrentMode()
    if current ~= nil and current.start ~= nil then
        current:start(adEnv.AutoDrive.USER_PLAYER or 1)
    else
        vehicle:startAutoDrive()
    end
end

local function parkMarkerId(adEnv, vehicle)
    local module = stateModule(vehicle)
    local index = module ~= nil and module.getParkDestinationAtJobFinished ~= nil and module:getParkDestinationAtJobFinished() or nil
    if index == nil or index < 1 then return nil end
    local marker = adEnv.ADGraphManager:getMapMarkerById(index)
    return marker ~= nil and marker.id or nil
end

local function restLoad(adEnv, vehicle)
    local ok, level = pcall(function()
        return (adEnv.AutoDrive.getAllFillLevels(adEnv.AutoDrive.getAllUnits(vehicle)))
    end)
    return ok and tonumber(level) or 0
end

local function vehiclePosition(vehicle)
    local x, _, z = getWorldTranslation(vehicle.rootNode)
    return x, z
end

local function isControlledByPlayer(vehicle)
    return (vehicle.getIsControlled ~= nil and vehicle:getIsControlled()) or false
end

-- ---- flåde ----

function ADFieldJobs.getRigs(adEnv)
    local rigs = {}
    for _, vehicle in ipairs(ADRunsController.getFarmVehicles(adEnv)) do
        local role = ADFieldFleet.roleFromSpecs(ADCpBridge.getSpecs(vehicle))
        local id = ADRunsController.getVehicleId(vehicle)
        if role ~= nil and id ~= nil then
            local x, z = vehiclePosition(vehicle)
            table.insert(rigs, {
                id = id, vehicle = vehicle, role = role, label = ADRuns.vehicleLabel(vehicle),
                enabled = ADFieldJobs.state.fleet[id] == true,
                busy = ADFieldJobs.state.assignments[id] ~= nil or ADRunsController.getRunForVehicle(vehicle) ~= nil,
                controlled = isControlledByPlayer(vehicle),
                width = ADCpBridge.getWorkWidth(vehicle), x = x, z = z,
            })
        end
    end
    return rigs
end

-- ---- silo ----

local function chooseStore(adEnv, field, info, vehicle)
    local fillType = field.plan.product == "hay" and info.hay or info.windrow
    local isReachable = function(markerId) return ADUnloadWait.hasRouteTo(adEnv, vehicle, markerId) end
    if #field.plan.storeBuildingIds > 0 then
        local markerIds = {}
        for _, buildingId in ipairs(field.plan.storeBuildingIds) do
            local building = ADBuildings.find(adEnv, vehicle:getOwnerFarmId(), buildingId)
            for _, marker in ipairs(building ~= nil and ADBuildings.getMarkers(building, "unload") or {}) do
                table.insert(markerIds, marker.id)
            end
        end
        return ADLoadSwap.findForcedDumpMarker(adEnv, vehicle, fillType, 1, markerIds, isReachable)
    end
    return ADLoadSwap.findDumpMarker(adEnv, vehicle, fillType, 1, info.markerId, isReachable, nil, nil, false)
end

-- ---- udsendelse og opfølgning ----

-- Færdige sæt for en mark glemmes, når marken går videre eller stoppes. Listen er begrænset: en mark
-- der sendes ud igen uden at gå videre (hjørner), går videre efter MAX_MOW_PASSES færdige ture.
local function forgetFinished(fieldId)
    local remaining = {}
    for _, done in ipairs(ADFieldJobs.finished or {}) do
        if done.fieldId ~= fieldId then table.insert(remaining, done) end
    end
    ADFieldJobs.finished = remaining
end

local function release(assignment, phase)
    assignment.phase = phase or "released"
    ADFieldJobs.state.assignments[assignment.vehicleId] = nil
end

local function dispatch(adEnv, field, info, role)
    local candidates = ADFieldJobs.getRigs(adEnv)
    local rig, reason, args = ADFieldFleet.pick(candidates, role, info.x, info.z, field.mowWidth)
    if rig == nil then
        note(field.id, {reason = {key = "spu_field_reason_" .. reason, role = role, args = args}})
        return
    end
    local assignment = {vehicleId = rig.id, vehicle = rig.vehicle, fieldId = field.id, role = role, phase = "driving", since = ADFieldJobs.now, width = rig.width}
    ADFieldJobs.state.assignments[rig.id] = assignment
    startAd(adEnv, rig.vehicle, adEnv.AutoDrive.MODE_DRIVETO, info.markerId, nil, false)
    log(field.id, "%s '%s' sendt til %s (%.1f m)", role, rig.label, tostring(info.markerName), rig.width)
    note(field.id, {reason = false})
end

local function beginWork(adEnv, field, info, assignment)
    local vehicle = assignment.vehicle
    if assignment.role == "collector" then
        local store, why = chooseStore(adEnv, field, info, vehicle)
        if store == nil then
            release(assignment)
            note(field.id, {retryAt = ADFieldJobs.now + ADFieldModel.RETRY_MS, reason = {text = tostring(why)}})
            log(field.id, "intet lager: %s", tostring(why))
            return
        end
        assignment.storeId = store.id
        -- AD overtager ved "fuld": Hent og aflever F -> silo, hjælper = Courseplay
        local module = stateModule(vehicle)
        module:setMode(adEnv.AutoDrive.MODE_PICKUPANDDELIVER)
        module:setFirstMarkerByWayPointId(info.markerId)
        module:setSecondMarkerByWayPointId(store.id)
        module:setStartHelper(true)
        module.usedHelper = adEnv.ADStateModule ~= nil and adEnv.ADStateModule.HELPER_CP or 1
        if vehicle.ad.settings ~= nil and vehicle.ad.settings.enableParkAtJobFinished ~= nil then
            vehicle.ad.settings.enableParkAtJobFinished.current = 2
            vehicle.ad.settings.enableParkAtJobFinished.new = 2
        end
        module:raiseDirtyFlag()
        log(field.id, "læssevogn '%s' aflæsser i %s", ADRuns.vehicleLabel(vehicle), tostring(store.name))
    end
    local point = info.points[1] or {x = info.x, z = info.z}
    assignment.phase = "starting"
    assignment.since = ADFieldJobs.now
    local handle, err = ADCpBridge.start(vehicle, point.x, point.z, function(ok, reason)
        if ok then
            assignment.phase = "working"
            assignment.since = ADFieldJobs.now
            if assignment.role == "mower" then
                note(field.id, {mowWidth = assignment.width})
            end
            log(field.id, "Courseplay startet (%s)", assignment.role)
        else
            release(assignment)
            note(field.id, {retryAt = ADFieldJobs.now + ADFieldModel.RETRY_MS, reason = {text = tostring(reason)}})
            log(field.id, "Courseplay fejlede: %s", tostring(reason))
        end
    end)
    if handle == nil then
        release(assignment)
        log(field.id, "Courseplay kunne ikke startes: %s", tostring(err))
        note(field.id, {retryAt = ADFieldJobs.now + ADFieldModel.RETRY_MS, reason = {text = tostring(err)}})
        return
    end
    assignment.handle = handle
end

local function follow(adEnv, assignment)
    local vehicle = assignment.vehicle
    -- Kun når spilleren selv KØRER (hverken Courseplay eller AutoDrive styrer): at sætte sig ind for at
    -- kigge på HUD'en mens CP slår, må ikke slippe sættet (in-game 25/9: F22 blev aldrig sendt hjem).
    local steeredByAi = ADCpBridge.isWorking(vehicle) or isAdActive(vehicle) or assignment.phase == "starting"
    if isControlledByPlayer(vehicle) and not steeredByAi then
        if assignment.handle ~= nil then assignment.handle.cancelled = true end
        log(assignment.fieldId, "'%s' frigivet — spilleren kører det", ADRuns.vehicleLabel(vehicle))
        return release(assignment)
    end
    local field = ADFieldJobs.getField(assignment.fieldId)
    local info = ADFields.getInfo(assignment.fieldId)
    if assignment.phase == "driving" then
        if not isAdActive(vehicle) and ADFieldJobs.now - assignment.since > 2000 then
            local x, z = vehiclePosition(vehicle)
            local wayPoint = adEnv.ADGraphManager:getWayPointById(info.markerId)
            local distance = wayPoint ~= nil and math.sqrt((wayPoint.x - x) ^ 2 + (wayPoint.z - z) ^ 2) or math.huge
            if distance <= ADFieldJobs.ARRIVE_DISTANCE then
                beginWork(adEnv, field, info, assignment)
            else
                log(assignment.fieldId, "'%s' nåede ikke %s (%.0f m væk)", ADRuns.vehicleLabel(vehicle), tostring(info.markerName), distance)
                release(assignment)
                note(assignment.fieldId, {retryAt = ADFieldJobs.now + ADFieldModel.RETRY_MS,
                    reason = {text = string.format("%s nåede ikke %s", ADRuns.vehicleLabel(vehicle), tostring(info.markerName))}})
            end
        end
    elseif assignment.phase == "starting" then
        if ADFieldJobs.now - assignment.since > ADFieldJobs.START_TIMEOUT_MS then
            if assignment.handle ~= nil then assignment.handle.cancelled = true end
            release(assignment)
            note(assignment.fieldId, {retryAt = ADFieldJobs.now + ADFieldModel.RETRY_MS, reason = {text = "Courseplay svarede ikke (60 s)"}})
            log(assignment.fieldId, "Courseplay svarede ikke inden 60 s")
        end
    elseif assignment.phase == "working" then
        local busy = ADCpBridge.isWorking(vehicle) or isAdActive(vehicle)
        if busy then
            assignment.idleSince = nil
        else
            assignment.idleSince = assignment.idleSince or ADFieldJobs.now
            local rest = assignment.role == "collector" and assignment.storeId ~= nil and not assignment.deliveredRest and restLoad(adEnv, vehicle) or 0
            if rest > 0 and ADFieldJobs.now - assignment.idleSince >= ADFieldJobs.IDLE_CONFIRM_MS then
                -- CP færdig med læs på: AD tømmer kun selv med parkering sat, så vi sender resten til silo
                assignment.deliveredRest = true
                assignment.idleSince = nil
                startAd(adEnv, vehicle, adEnv.AutoDrive.MODE_DELIVERTO, assignment.storeId, nil, false)
                log(assignment.fieldId, "'%s' kører rest-læs (%.0f l) til silo", ADRuns.vehicleLabel(vehicle), rest)
            elseif ADFieldJobs.now - assignment.idleSince >= ADFieldJobs.IDLE_CONFIRM_MS then
                log(assignment.fieldId, "%s '%s' færdig", assignment.role, ADRuns.vehicleLabel(vehicle))
                local park = assignment.role ~= "collector" and parkMarkerId(adEnv, vehicle) or nil
                if park ~= nil then startAd(adEnv, vehicle, adEnv.AutoDrive.MODE_DRIVETO, park, nil, false) end
                assignment.phase = "finished"
                ADFieldJobs.finished = ADFieldJobs.finished or {}
                table.insert(ADFieldJobs.finished, assignment)
                ADFieldJobs.state.assignments[assignment.vehicleId] = nil
            end
        end
    end
end

function ADFieldJobs.stopField(adEnv, id)
    for _, assignment in ipairs(ADFieldJobs.assignmentsFor(id)) do
        if assignment.handle ~= nil then assignment.handle.cancelled = true end
        pcall(stopAd, assignment.vehicle)
        pcall(function() if assignment.vehicle.stopCurrentAIJob ~= nil and ADCpBridge.isWorking(assignment.vehicle) then assignment.vehicle:stopCurrentAIJob(AIMessageSuccessStoppedByUser.new()) end end)
        release(assignment)
    end
    forgetFinished(id)
    setField(ADFieldModel.stop(ADFieldJobs.getField(id)))
    ADFieldJobs.save()
    log(id, "stoppet fra menuen")
end

local function finishedFor(fieldId)
    local list = {}
    for _, assignment in ipairs(ADFieldJobs.finished or {}) do
        if assignment.fieldId == fieldId then table.insert(list, assignment) end
    end
    return list
end

local function stepField(adEnv, info)
    local field = ADFieldModel.onReadiness(ADFieldJobs.getField(info.id), info.readyRatio or 0)
    setField(field)
    local assignments = ADFieldJobs.assignmentsFor(info.id)
    for _, done in ipairs(finishedFor(info.id)) do table.insert(assignments, done) end
    local decision = ADFieldJobs.decide(field, info, assignments, ADFieldJobs.now)
    if decision.action == "start" then
        setField(ADFieldModel.start(field))
        log(info.id, "starter (%s)", field.plan.product)
        ADFieldJobs.save()
    elseif decision.action == "advance" then
        local nextField = ADFieldModel.advance(field)
        setField(nextField)
        forgetFinished(info.id)
        log(info.id, "%s -> %s", field.stage, nextField.stage)
        ADFieldJobs.save()
    elseif decision.action == "dispatch" then
        dispatch(adEnv, field, info, decision.role)
    end
end

function ADFieldJobs.tick(adEnv, dt)
    ADFieldJobs.now = ADFieldJobs.now + dt
    ADFieldJobs.timer = ADFieldJobs.timer + dt
    if ADFieldJobs.timer < ADFieldJobs.TICK_MS then return end
    ADFieldJobs.timer = 0
    if not ADFieldJobs.loaded then ADFieldJobs.load() end
    local farmId = g_currentMission:getFarmId()
    ADFields.scanNext(adEnv, farmId)
    for _, assignment in pairs(ADFieldJobs.state.assignments) do
        local ok, err = pcall(follow, adEnv, assignment)
        if not ok then
            Logging.warning(ADFieldJobs.LOG_PREFIX .. "F%s opfølgning fejlede: %s", tostring(assignment.fieldId), tostring(err))
            release(assignment)
        end
    end
    for _, info in ipairs(ADFields.list()) do
        if info.mowable or ADFieldModel.isWorking(ADFieldJobs.getField(info.id)) then
            local ok, err = pcall(stepField, adEnv, info)
            if not ok then Logging.warning(ADFieldJobs.LOG_PREFIX .. "F%s fejl: %s", tostring(info.id), tostring(err)) end
        end
    end
end