-- AD Hitch
-- Kobler redskaber på med AutoDrive. En markør der hedder "Connect <navn>" (eller "Kobl <navn>") ligger lige
-- under vognens trækøje, med et bakkespor ned til den. Når en traktor har kørt AD "Kør til" til sådan en markør
-- og står stille, kobles det redskab på, som spillet selv finder inden for koblingsafstand (samme kald som
-- Courseplay bruger til skærebord: updateVehiclesInAttachRange + attachImplementFromInfo).
-- ADHitch.fetch() er indgangen for flows/markarbejde: kør til markøren, kobl på, meld tilbage.

ADHitch = {}
ADHitch.LOG_PREFIX = "ADSmartPickup: kobling: "
ADHitch.PREFIXES = {"connect", "kobl"}
ADHitch.ARRIVE_DISTANCE = 5        -- m fra markøren, AD standser traktoren tæt på waypointet
ADHitch.STILL_SPEED = 1            -- km/t
ADHitch.START_GRACE_MS = 3000      -- så længe må AD være om at komme i gang efter fetch
ADHitch.ATTACH_TIMEOUT_MS = 8000   -- så længe forsøges koblingen efter ankomst
ADHitch.TICK_MS = 250

-- ---- ren logik ----

-- "Connect Bredal" -> "Bredal"; "Connect" -> ""; andre navne -> nil
function ADHitch.connectTarget(name)
    if type(name) ~= "string" then return nil end
    local trimmed = name:match("^%s*(.-)%s*$")
    local lower = trimmed:lower()
    for _, prefix in ipairs(ADHitch.PREFIXES) do
        if lower == prefix then return "" end
        if lower:sub(1, #prefix + 1) == prefix .. " " then
            return (trimmed:sub(#prefix + 2):match("^%s*(.-)$"))
        end
    end
    return nil
end

-- state = {adActive, distance, speed}
function ADHitch.hasArrived(state)
    return not state.adActive and state.distance <= ADHitch.ARRIVE_DISTANCE and state.speed <= ADHitch.STILL_SPEED
end

function ADHitch.newJob(markerId, markerName)
    return {markerId = markerId, markerName = markerName, state = "driving", elapsed = 0, attachElapsed = 0,
        seenActive = false}
end

-- Et skridt i jobbet. attached = lykkedes koblingsforsøget i denne tick (kun relevant i "attaching").
-- Tilstande: driving -> attaching -> done | failed (reason: stoppedAway / noAttachable)
function ADHitch.step(job, state, dt, attached)
    if job.state == "driving" then
        job.elapsed = job.elapsed + dt
        if state.adActive then
            job.seenActive = true
        elseif ADHitch.hasArrived(state) then
            job.state = "attaching"
        elseif state.distance > ADHitch.ARRIVE_DISTANCE and (job.seenActive or job.elapsed >= ADHitch.START_GRACE_MS) then
            job.state, job.reason = "failed", "stoppedAway"
        end
    elseif job.state == "attaching" then
        if attached then
            job.state = "done"
        else
            job.attachElapsed = job.attachElapsed + dt
            if job.attachElapsed >= ADHitch.ATTACH_TIMEOUT_MS then job.state, job.reason = "failed", "noAttachable" end
        end
    end
    return job
end

-- ---- spil ----

local jobs = setmetatable({}, {__mode = "k"}) -- [vehicle] = job (+ onDone)
local tickTimer = 0

local function vehicleName(vehicle)
    return vehicle.getFullName ~= nil and vehicle:getFullName() or tostring(vehicle.configFileName)
end

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

local function vehicleState(adEnv, vehicle, markerId)
    local module = stateModule(vehicle)
    local wayPoint = adEnv.ADGraphManager:getWayPointById(markerId)
    local distance = math.huge
    if wayPoint ~= nil and vehicle.rootNode ~= nil then
        local x, _, z = getWorldTranslation(vehicle.rootNode)
        distance = math.sqrt((x - wayPoint.x) ^ 2 + (z - wayPoint.z) ^ 2)
    end
    return {
        adActive = module ~= nil and module:isActive(),
        distance = distance,
        speed = vehicle.getLastSpeed ~= nil and vehicle:getLastSpeed() or 0,
    }
end

-- Kobler det redskab på, som spillet finder inden for koblingsafstand. -> ok, redskab eller årsag
function ADHitch.tryAttach(vehicle)
    local spec = vehicle.spec_attacherJoints
    if spec == nil or AttacherJoints == nil or AttacherJoints.updateVehiclesInAttachRange == nil then
        return false, "noJoints"
    end
    AttacherJoints.updateVehiclesInAttachRange(vehicle, AttacherJoints.MAX_ATTACH_DISTANCE_SQ,
        AttacherJoints.MAX_ATTACH_ANGLE, true)
    local info = spec.attachableInfo
    if info == nil or info.attachable == nil then return false, "noAttachable" end
    local attacher = info.attacherVehicle or vehicle
    local farmId = vehicle.getActiveFarm ~= nil and vehicle:getActiveFarm() or vehicle:getOwnerFarmId()
    local allowed, warning = info.attachable:isAttachAllowed(farmId, attacher)
    if not allowed then return false, tostring(warning or "notAllowed") end
    attacher:attachImplementFromInfo(info)
    return true, info.attachable
end

-- Kør til en Connect-markør og kobl på. onDone(vehicle, ok, implementOrReason) kaldes én gang.
function ADHitch.fetch(adEnv, vehicle, markerId, onDone)
    local marker = adEnv.ADGraphManager:getMapMarkerByWayPointId(markerId)
    local job = ADHitch.newJob(markerId, marker ~= nil and marker.name or tostring(markerId))
    job.onDone = onDone
    jobs[vehicle] = job
    ADFieldWork.startAd(adEnv, vehicle, adEnv.AutoDrive.MODE_DRIVETO, markerId)
    Logging.info(ADHitch.LOG_PREFIX .. "'%s' kører til '%s'", vehicleName(vehicle), job.markerName)
    return job
end

-- Spilleren sender selv en traktor til en Connect-markør med AD "Kør til": den kobles også på.
local function watchManualTrips(adEnv)
    for _, vehicle in ipairs(ADRunsController.getFarmVehicles(adEnv)) do
        local module = stateModule(vehicle)
        if jobs[vehicle] == nil and vehicle.spec_attacherJoints ~= nil and module ~= nil and module:isActive()
            and module:getMode() == adEnv.AutoDrive.MODE_DRIVETO then
            local marker = module:getFirstMarker()
            if marker ~= nil and ADHitch.connectTarget(marker.name) ~= nil then
                jobs[vehicle] = ADHitch.newJob(marker.id, marker.name)
            end
        end
    end
end

local function finish(vehicle, job, ok, detail)
    jobs[vehicle] = nil
    if ok then
        Logging.info(ADHitch.LOG_PREFIX .. "'%s' koblede '%s' på ved '%s'", vehicleName(vehicle), vehicleName(detail),
            job.markerName)
    else
        Logging.warning(ADHitch.LOG_PREFIX .. "'%s' kunne ikke koble på ved '%s': %s", vehicleName(vehicle),
            job.markerName, tostring(detail))
    end
    if job.onDone ~= nil then pcall(job.onDone, vehicle, ok, detail) end
end

function ADHitch.tick(adEnv, dt)
    if adEnv == nil or adEnv.ADGraphManager == nil then return end
    tickTimer = tickTimer + dt
    if tickTimer < ADHitch.TICK_MS then return end
    local elapsed = tickTimer
    tickTimer = 0
    watchManualTrips(adEnv)
    for vehicle, job in pairs(jobs) do
        local attached, detail = false, nil
        if job.state == "attaching" then
            attached, detail = ADHitch.tryAttach(vehicle)
            if not attached then job.lastReason = detail end
        end
        ADHitch.step(job, vehicleState(adEnv, vehicle, job.markerId), elapsed, attached)
        if job.state == "done" then
            finish(vehicle, job, true, detail)
        elseif job.state == "failed" then
            finish(vehicle, job, false, job.lastReason or job.reason)
        end
    end
end

-- Konsol: "spuConnect" kobler det nærmeste redskab på den maskine spilleren sidder i.
function ADHitch:consoleConnect()
    local vehicle = g_localPlayer ~= nil and g_localPlayer.getCurrentVehicle ~= nil and g_localPlayer:getCurrentVehicle() or nil
    if vehicle == nil then return "spuConnect: sæt dig i en traktor først" end
    local ok, detail = ADHitch.tryAttach(vehicle)
    if ok then return "spuConnect: koblede " .. vehicleName(detail) .. " på" end
    return "spuConnect: intet koblet på (" .. tostring(detail) .. ")"
end

if addConsoleCommand ~= nil then
    addConsoleCommand("spuConnect", "Kobler nærmeste redskab på den maskine du sidder i", "consoleConnect", ADHitch)
end