-- AD Hitch
-- Kobler redskaber på og af med AutoDrive.
-- "Connect <navn>" (eller "Kobl <navn>"): markøren ligger lige under vognens trækøje med et bakkespor ned til
-- den. Når en traktor har kørt AD "Kør til" dertil 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).
-- "Afkobl <navn>" (eller "Kobl af", "Detach", "Disconnect"): ved ankomst kobles redskaberne BAG traktoren af;
-- frontvægt og frontlæsser bliver siddende.
-- ADHitch.fetch()/ADHitch.drop() er indgangen for flows/markarbejde: kør til markøren, kobl på/af, meld tilbage.

ADHitch = {}
ADHitch.LOG_PREFIX = "ADSmartPickup: kobling: "
-- afkobling tjekkes først, så "Kobl af X" ikke læses som "Kobl <af X>"
ADHitch.MARKER_PREFIXES = {
    {prefix = "afkobl", action = "detach"},
    {prefix = "kobl af", action = "detach"},
    {prefix = "detach", action = "detach"},
    {prefix = "disconnect", action = "detach"},
    {prefix = "connect", action = "attach"},
    {prefix = "kobl", action = "attach"},
}
ADHitch.TIMEOUT_REASON = {attach = "noAttachable", detach = "nothingToDetach", arrive = "notArrived"}
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/drop
ADHitch.ACT_TIMEOUT_MS = 8000      -- så længe forsøges kobling/afkobling efter ankomst
ADHitch.TICK_MS = 250
ADHitch.ROUTE_WAIT_MS = 10000     -- pause ved almindelige markører midt i en tur

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

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

-- implements = {{z = position i traktorens rum, ...}} -> kun dem bag traktoren
function ADHitch.rearImplements(implements)
    local rear = {}
    for _, implement in ipairs(implements or {}) do
        if implement.z ~= nil and implement.z < 0 then table.insert(rear, implement) end
    end
    return rear
end

-- konsol-argumenter "Connect Bredal > Start > Afkobl Bredal > Start" -> {"Connect Bredal", "Start", ...}
function ADHitch.parseTrip(...)
    local steps = {}
    for part in (table.concat({...}, " ") .. ">"):gmatch("([^>]*)>") do
        local name = part:match("^%s*(.-)%s*$")
        if name ~= "" then table.insert(steps, name) end
    end
    return steps
end

-- markørnavn -> attach / detach / arrive (almindelig markør: bare køre dertil)
function ADHitch.stepAction(name)
    return ADHitch.parseMarker(name) or "arrive"
end

-- I en tur: en almindelig markør, der er AD-parkeringen for redskabet bag traktoren, er stedet vognen sættes af.
function ADHitch.resolveAction(action, isImplementPark)
    if action == "arrive" and isImplementPark then return "detach" end
    return action
end

-- pause efter et trin: ved almindelige markører undervejs holdes der, ikke efter sidste trin
function ADHitch.waitAfter(action, index, count)
    if action == "arrive" and index < count then return ADHitch.ROUTE_WAIT_MS end
    return 0
end

-- markers = {{name, id}} -> markøren med navnet (store/små bogstaver ligegyldigt)
function ADHitch.findMarker(markers, name)
    if type(name) ~= "string" then return nil end
    local wanted = name:lower()
    for _, marker in pairs(markers or {}) do
        if type(marker.name) == "string" and marker.name:lower() == wanted then return marker 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, action)
    return {markerId = markerId, markerName = markerName, action = action, state = "driving", elapsed = 0,
        actElapsed = 0, seenActive = false}
end

-- Et skridt i jobbet. succeeded = lykkedes kobling/afkobling i denne tick (kun relevant i "acting").
-- Tilstande: driving -> acting -> done | failed (reason: stoppedAway / noAttachable / nothingToDetach)
function ADHitch.step(job, state, dt, succeeded)
    if job.state == "driving" then
        job.elapsed = job.elapsed + dt
        if state.adActive then
            job.seenActive = true
        elseif ADHitch.hasArrived(state) then
            job.state = "acting"
        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 == "acting" then
        if succeeded then
            job.state = "done"
        else
            job.actElapsed = job.actElapsed + dt
            if job.actElapsed >= ADHitch.ACT_TIMEOUT_MS then
                job.state, job.reason = "failed", ADHitch.TIMEOUT_REASON[job.action]
            end
        end
    end
    return job
end

-- ---- spil ----

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

local function vehicleName(vehicle)
    if type(vehicle) ~= "table" then return tostring(vehicle) end
    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)
    -- nærmeste led i sættet: bakker AD med vogn, er det vognen der ender på markøren, ikke traktoren
    -- (in-game 28/9: vognen stod på Bredal, traktoren et vognslæb væk -> "stoppedAway")
    local distance = math.huge
    local parts = {vehicle}
    if vehicle.getChildVehicles ~= nil then
        local ok, children = pcall(vehicle.getChildVehicles, vehicle)
        if ok and children ~= nil then parts = children end
    end
    for _, part in pairs(parts) do
        if wayPoint ~= nil and part.rootNode ~= nil then
            local x, _, z = getWorldTranslation(part.rootNode)
            distance = math.min(distance, math.sqrt((x - wayPoint.x) ^ 2 + (z - wayPoint.z) ^ 2))
        end
    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

-- Redskaber koblet direkte på traktoren, med deres placering langs traktoren (z > 0 foran, z < 0 bagved).
local function attachedImplements(vehicle)
    local list = {}
    if vehicle.getAttachedImplements == nil then return list end
    for _, implement in ipairs(vehicle:getAttachedImplements()) do
        local jointDesc = vehicle.spec_attacherJoints.attacherJoints[implement.jointDescIndex]
        local node = jointDesc ~= nil and (jointDesc.jointTransform or jointDesc.rootNode) or nil
        if implement.object ~= nil and node ~= nil then
            local _, _, z = localToLocal(node, vehicle.rootNode, 0, 0, 0)
            table.insert(list, {object = implement.object, z = z, name = vehicleName(implement.object)})
        end
    end
    return list
end

-- Er markøren (waypoint-id) AD-parkeringen for et af redskaberne bag traktoren?
local function isImplementPark(adEnv, vehicle, markerId)
    for _, implement in ipairs(ADHitch.rearImplements(attachedImplements(vehicle))) do
        local advd = implement.object.advd
        local index = advd ~= nil and advd.parkDestination or nil
        local marker = type(index) == "number" and index >= 1 and adEnv.ADGraphManager:getMapMarkerById(index) or nil
        if marker ~= nil and marker.id == markerId then return true end
    end
    return false
end

-- Kobler redskaberne bag traktoren af. -> ok, navne eller årsag
function ADHitch.tryDetach(vehicle)
    local rear = ADHitch.rearImplements(attachedImplements(vehicle))
    if #rear == 0 then return false, "nothingToDetach" end
    local names = {}
    for _, implement in ipairs(rear) do
        local object = implement.object
        local allowed, warning = true, nil
        if object.isDetachAllowed ~= nil then allowed, warning = object:isDetachAllowed() end
        if not allowed then return false, tostring(warning or "detachNotAllowed") end
        vehicle:detachImplementByObject(object)
        table.insert(names, implement.name)
    end
    return true, table.concat(names, ", ")
end

local function start(adEnv, vehicle, markerId, action, onDone)
    local marker = adEnv.ADGraphManager:getMapMarkerByWayPointId(markerId)
    local job = ADHitch.newJob(markerId, marker ~= nil and marker.name or tostring(markerId), action)
    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

-- Kør til en Connect-markør og kobl på. onDone(vehicle, ok, redskab eller årsag) kaldes én gang.
function ADHitch.fetch(adEnv, vehicle, markerId, onDone)
    return start(adEnv, vehicle, markerId, "attach", onDone)
end

-- Kør til en Afkobl-markør og kobl af. onDone(vehicle, ok, navne eller årsag) kaldes én gang.
function ADHitch.drop(adEnv, vehicle, markerId, onDone)
    return start(adEnv, vehicle, markerId, "detach", onDone)
end

-- Tur: markørerne køres i rækkefølge. Connect = kobl på, Afkobl eller vognens egen AD-parkering = kobl af,
-- andre = kør dertil (og hold
-- ROUTE_WAIT_MS, hvis det ikke er sidste trin). Stopper ved første fejl. onDone(vehicle, ok, årsag) til sidst.
local waits = {} -- {vehicle, leftMs, continue}

function ADHitch.route(adEnv, vehicle, markerIds, onDone)
    local function runStep(index)
        if index > #markerIds then
            Logging.info(ADHitch.LOG_PREFIX .. "'%s' er færdig med turen (%d trin)", vehicleName(vehicle), #markerIds)
            if onDone ~= nil then pcall(onDone, vehicle, true) end
            return
        end
        local marker = adEnv.ADGraphManager:getMapMarkerByWayPointId(markerIds[index])
        local action = ADHitch.resolveAction(ADHitch.stepAction(marker ~= nil and marker.name or nil),
            isImplementPark(adEnv, vehicle, markerIds[index]))
        start(adEnv, vehicle, markerIds[index], action, function(_, ok, detail)
            if not ok then
                Logging.warning(ADHitch.LOG_PREFIX .. "'%s' stopper turen ved trin %d", vehicleName(vehicle), index)
                if onDone ~= nil then pcall(onDone, vehicle, false, detail) end
                return
            end
            local waitMs = ADHitch.waitAfter(action, index, #markerIds)
            if waitMs > 0 then
                Logging.info(ADHitch.LOG_PREFIX .. "'%s' holder %d s ved '%s'", vehicleName(vehicle), waitMs / 1000,
                    marker ~= nil and marker.name or "?")
                table.insert(waits, {vehicle = vehicle, leftMs = waitMs, continue = function() runStep(index + 1) end})
            else
                runStep(index + 1)
            end
        end)
    end
    runStep(1)
end

local function tickWaits(elapsed)
    for i = #waits, 1, -1 do
        local wait = waits[i]
        wait.leftMs = wait.leftMs - elapsed
        if wait.leftMs <= 0 then
            table.remove(waits, i)
            wait.continue()
        end
    end
end

-- Spilleren sender selv en traktor til en Connect-/Afkobl-markør med AD "Kør til": den kobles også.
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()
            local action = marker ~= nil and ADHitch.parseMarker(marker.name) or nil
            if action ~= nil then jobs[vehicle] = ADHitch.newJob(marker.id, marker.name, action) end
        end
    end
end

local DONE_TEXT = {attach = "koblede '%s' på", detach = "koblede '%s' af", arrive = "er fremme%s"}
local FAIL_TEXT = {attach = "kunne ikke koble på", detach = "kunne ikke koble af", arrive = "nåede ikke frem"}

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

local ACTIONS = {attach = ADHitch.tryAttach, detach = ADHitch.tryDetach, arrive = function() return true, "" 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
    tickWaits(elapsed)
    watchManualTrips(adEnv)
    -- kopi: et færdigt trin starter næste trin (nyt job på samme maskine) midt i løkken
    local current = {}
    for vehicle, job in pairs(jobs) do current[vehicle] = job end
    for vehicle, job in pairs(current) do
        local succeeded, detail = false, nil
        if job.state == "acting" then
            succeeded, detail = ACTIONS[job.action](vehicle)
            if not succeeded then job.lastReason = detail end
        end
        ADHitch.step(job, vehicleState(adEnv, vehicle, job.markerId), elapsed, succeeded)
        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" / "spuDetach" på den maskine spilleren sidder i.
local function currentVehicle()
    return g_localPlayer ~= nil and g_localPlayer.getCurrentVehicle ~= nil and g_localPlayer:getCurrentVehicle() or nil
end

function ADHitch:consoleConnect()
    local vehicle = currentVehicle()
    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

function ADHitch:consoleDetach()
    local vehicle = currentVehicle()
    if vehicle == nil then return "spuDetach: sæt dig i en traktor først" end
    local ok, detail = ADHitch.tryDetach(vehicle)
    if ok then return "spuDetach: koblede " .. tostring(detail) .. " af" end
    return "spuDetach: intet koblet af (" .. tostring(detail) .. ")"
end

-- "spuTrip Connect Bredal > Start > Bredal > Start": maskinen spilleren sidder i kører turen
-- (spilleren kan blive siddende; AD kører også med spilleren i førerhuset).
function ADHitch:consoleTrip(...)
    local vehicle = currentVehicle()
    if vehicle == nil then return "spuTrip: sæt dig i en traktor først" end
    local adEnv = ADFieldSilos ~= nil and ADFieldSilos.adEnv or nil
    if adEnv == nil then return "spuTrip: AutoDrive er ikke klar endnu" end
    local names = ADHitch.parseTrip(...)
    if #names == 0 then return "spuTrip: brug fx spuTrip Connect Bredal > Start > Bredal > Start" end
    local markers, ids = adEnv.ADGraphManager:getMapMarkers(), {}
    for _, name in ipairs(names) do
        local marker = ADHitch.findMarker(markers, name)
        if marker == nil then return "spuTrip: ukendt markør '" .. name .. "'" end
        table.insert(ids, marker.id)
    end
    ADHitch.route(adEnv, vehicle, ids)
    return "spuTrip: " .. vehicleName(vehicle) .. " kører " .. table.concat(names, " > ")
end

if addConsoleCommand ~= nil then
    addConsoleCommand("spuConnect", "Kobler nærmeste redskab på den maskine du sidder i", "consoleConnect", ADHitch)
    addConsoleCommand("spuTrip", "Kør en tur: spuTrip Connect Bredal > Start > Bredal > Start", "consoleTrip", ADHitch)
    addConsoleCommand("spuDetach", "Kobler redskaberne bag den maskine du sidder i af", "consoleDetach", ADHitch)
end