-- AD Stuck Diag
-- Diagnose: et aktivt AutoDrive-køretøj der har holdt stille længe, logges med AutoDrives egen grund
-- (forhindring, AD-trafik på ruten + hvem, pause, mål nået) og om Smart Pickup holder det (Wait/udkørsel).
-- Kun læsning — ændrer intet. Logges én gang pr. ændret tilstand.

ADStuckDiag = {}

ADStuckDiag.CHECK_INTERVAL_MS = 10000
ADStuckDiag.STILL_MS = 20000
ADStuckDiag.STILL_SPEED = 0.00028 -- samme grænse som AD's egen stoppedTimer (m/ms)

local stillMs = setmetatable({}, {__mode = "k"})
local lastText = setmetatable({}, {__mode = "k"})
local timerMs = 0

local function name(vehicle)
    if vehicle == nil then return "-" end
    local ok, text = pcall(vehicle.getName, vehicle)
    return ok and tostring(text) or "?"
end

local function taskName(vehicle)
    local taskModule = vehicle.ad.taskModule
    local task = taskModule ~= nil and taskModule.activeTask or nil
    if task == nil then return "ingen" end
    return tostring(task.taskType or "ukendt")
end

-- Tekst der beskriver hvorfor køretøjet står (nil hvis ukendt/ikke relevant).
function ADStuckDiag.describe(vehicle)
    local ad = vehicle.ad
    local cdm, dpm, sdm = ad.collisionDetectionModule, ad.drivePathModule, ad.specialDrivingModule
    local wayPoints, current = nil, nil
    if dpm ~= nil and dpm.getWayPoints ~= nil then wayPoints, current = dpm:getWayPoints() end
    local parts = {
        string.format("opgave=%s", taskName(vehicle)),
        string.format("mål=%s", tostring(ad.stateModule.getFirstMarkerId ~= nil and ad.stateModule:getFirstMarkerId() or "?")),
        string.format("wp=%s/%s", tostring(current), tostring(wayPoints ~= nil and #wayPoints or nil)),
        string.format("pause=%s", tostring(dpm ~= nil and dpm.isPaused)),
        string.format("målNået=%s", tostring(dpm ~= nil and dpm.isTargetReached ~= nil and dpm:isTargetReached())),
        string.format("forhindring=%s", tostring(cdm ~= nil and cdm.detectedObstable)),
        string.format("trafik=%s", cdm ~= nil and cdm.trafficVehicle ~= nil and name(cdm.trafficVehicle) or "-"),
        string.format("stopper=%s blokeret=%s", tostring(sdm ~= nil and sdm.isStoppingVehicle ~= nil and sdm:isStoppingVehicle()),
            tostring(sdm ~= nil and sdm.isBlocked)),
    }
    local wait = ADUnloadWait ~= nil and ADUnloadWait.getWaitFor ~= nil and ADUnloadWait.getWaitFor(vehicle) or nil
    local hold = ADOutbound ~= nil and ADOutbound.getHoldFor ~= nil and ADOutbound.getHoldFor(vehicle) or nil
    local pickup = ADSmartPickup ~= nil and ADSmartPickup.getActivePickup ~= nil and ADSmartPickup.getActivePickup(vehicle) or nil
    table.insert(parts, string.format("SP: wait=%s hold=%s pickup=%s",
        wait ~= nil and tostring(wait.markerId or wait.wayPointId or "ja") or "-",
        hold ~= nil and tostring(hold.markerId or "ja") or "-",
        pickup ~= nil and tostring(pickup.wayPointId or pickup.markerId or "ja") or "-"))
    return table.concat(parts, " ")
end

function ADStuckDiag.tick(dt)
    timerMs = timerMs + (dt or 0)
    if timerMs < ADStuckDiag.CHECK_INTERVAL_MS then return end
    local elapsed = timerMs
    timerMs = 0
    local vehicles = g_currentMission ~= nil and g_currentMission.vehicleSystem ~= nil and g_currentMission.vehicleSystem.vehicles or {}
    for _, vehicle in pairs(vehicles) do
        local ad = vehicle.ad
        local active = ad ~= nil and ad.stateModule ~= nil and ad.stateModule:isActive()
        if active and (vehicle.lastSpeedReal or 0) < ADStuckDiag.STILL_SPEED then
            stillMs[vehicle] = (stillMs[vehicle] or 0) + elapsed
        else
            stillMs[vehicle], lastText[vehicle] = nil, nil
        end
        if (stillMs[vehicle] or 0) >= ADStuckDiag.STILL_MS then
            local ok, text = pcall(ADStuckDiag.describe, vehicle)
            text = ok and text or ("diagnose fejlede: " .. tostring(text))
            if text ~= lastText[vehicle] then
                lastText[vehicle] = text
                Logging.info("ADSmartPickup: STÅR STILLE %d s '%s': %s", math.floor(stillMs[vehicle] / 1000), name(vehicle), text)
            end
        end
    end
end