-- AD Hitch Creep
-- Bak de sidste meter hen til vognen: når AD "Kør til" når en Connect-markør, og vognens trækøje står 3-8 m
-- lige bag traktorens krog, lægges en lille bakke-opgave i AD's kø før stop. Traktoren bakker i krybefart og
-- standser, så snart spillet ser vognen inden for koblingsrækkevidde; ADHitch kobler derefter på som før.
-- AD's egen bak-kollisionssensor bruges ikke: den ser vognen selv som forhindring.

ADHitchCreep = {}
ADHitchCreep.LOG_PREFIX = "ADSmartPickup: kobling: "
ADHitchCreep.ROOMY_ATTACH_DISTANCE = 3  -- m: herinden for kobler ADHitch direkte (samme som ADHitch.ROOMY_ATTACH_DISTANCE)
ADHitchCreep.CREEP_MAX_DISTANCE = 8     -- m: længere væk end det bakkes ikke — markøren ligger forkert
ADHitchCreep.CREEP_MAX_LATERAL = 1.5    -- m til siden: mere end det rammer en lige bakning ikke trækøjet
ADHitchCreep.CREEP_TIMEOUT_MS = 25000
ADHitchCreep.CREEP_SPEED = 2            -- km/t
ADHitchCreep.AWAY_TOLERANCE = 1         -- m: længere væk end ved start = den bakker forkert
ADHitchCreep.RANGE_CHECK_MS = 250

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

-- offset = {x, z}: vognens trækøje i traktorens rum set fra krogen (z < 0 = bagved) -> "attach" | "creep" | "none"
function ADHitchCreep.plan(offset)
    if offset == nil then return "none" end
    local distance = math.sqrt(offset.x ^ 2 + offset.z ^ 2)
    if distance <= ADHitchCreep.ROOMY_ATTACH_DISTANCE then return "attach" end
    if offset.z >= 0 or distance > ADHitchCreep.CREEP_MAX_DISTANCE or math.abs(offset.x) > ADHitchCreep.CREEP_MAX_LATERAL then
        return "none"
    end
    return "creep"
end

-- s = {inRange, distance, startDistance, elapsedMs, passed} -> "done" | "drive" | "giveUp"
function ADHitchCreep.step(s)
    if s.inRange then return "done" end
    if (s.elapsedMs or 0) >= ADHitchCreep.CREEP_TIMEOUT_MS then return "giveUp" end
    if s.passed then return "giveUp" end
    if s.distance > s.startDistance + ADHitchCreep.AWAY_TOLERANCE then return "giveUp" end
    return "drive"
end

-- ---- spil ----

local function worldPos(node)
    local x, y, z = getWorldTranslation(node)
    return {x = x, y = y, z = z}
end

local function vehicleName(vehicle)
    return vehicle ~= nil and vehicle.getName ~= nil and vehicle:getName() or "?"
end

-- Traktorens ledige bagerste krog-noder.
local function freeRearJoints(vehicle)
    local spec = vehicle.spec_attacherJoints
    if spec == nil or spec.attacherJoints == nil or vehicle.rootNode == nil then return {} end
    local used = {}
    for _, implement in ipairs(spec.attachedImplements or {}) do
        if implement.jointDescIndex ~= nil then used[implement.jointDescIndex] = true end
    end
    local joints = {}
    for index, joint in ipairs(spec.attacherJoints) do
        if not used[index] and joint.jointTransform ~= nil then
            local p = worldPos(joint.jointTransform)
            local _, _, lz = worldToLocal(vehicle.rootNode, p.x, p.y, p.z)
            if lz < 0 then table.insert(joints, joint.jointTransform) end
        end
    end
    return joints
end

-- Vognens trækøje i traktorens rum set fra krogen.
local function offsetOf(vehicle, jointNode, targetNode)
    local j, t = worldPos(jointNode), worldPos(targetNode)
    local x, _, z = worldDirectionToLocal(vehicle.rootNode, t.x - j.x, 0, t.z - j.z)
    return {x = x, z = z}
end

-- Nærmeste frakoblede redskab (samme gård) med et trækøje bag en ledig krog. -> {wagon, joint, target, offset, plan} | nil
function ADHitchCreep.findTarget(vehicle)
    local joints = freeRearJoints(vehicle)
    if #joints == 0 or g_currentMission == nil or g_currentMission.vehicleSystem == nil then return nil end
    local farmId = vehicle.getOwnerFarmId ~= nil and vehicle:getOwnerFarmId() or nil
    local best, bestDistance = nil, math.huge
    for _, object in pairs(g_currentMission.vehicleSystem.vehicles or {}) do
        local attachable = object.spec_attachable
        if object ~= vehicle and attachable ~= nil and object.spec_motorized == nil and attachable.inputAttacherJoints ~= nil
            and (object.getAttacherVehicle == nil or object:getAttacherVehicle() == nil)
            and (farmId == nil or object.getOwnerFarmId == nil or object:getOwnerFarmId() == farmId) then
            for _, input in ipairs(attachable.inputAttacherJoints) do
                if input.node ~= nil then
                    for _, joint in ipairs(joints) do
                        local offset = offsetOf(vehicle, joint, input.node)
                        local distance = math.sqrt(offset.x ^ 2 + offset.z ^ 2)
                        if distance < bestDistance and distance <= ADHitchCreep.CREEP_MAX_DISTANCE + 2 then
                            bestDistance = distance
                            best = {wagon = object, joint = joint, target = input.node, offset = offset}
                        end
                    end
                end
            end
        end
    end
    if best ~= nil then best.plan = ADHitchCreep.plan(best.offset) end
    return best
end

local function isInRange(vehicle, wagon)
    local spec = vehicle.spec_attacherJoints
    if spec == nil or AttacherJoints == nil or AttacherJoints.updateVehiclesInAttachRange == nil then return false end
    AttacherJoints.updateVehiclesInAttachRange(vehicle, ADHitchCreep.ROOMY_ATTACH_DISTANCE ^ 2, AttacherJoints.MAX_ATTACH_ANGLE, true)
    local info = spec.attachableInfo
    return info ~= nil and info.attachable == wagon
end

-- AD-opgave (samme metoder som AD's AbstractTask).
local CreepTask = {}
CreepTask.__index = CreepTask

function CreepTask.new(adEnv, vehicle, target)
    return setmetatable({adEnv = adEnv, vehicle = vehicle, target = target, elapsedMs = 0, checkMs = 0, inRange = false}, CreepTask)
end

function CreepTask:setUp()
    local offset = offsetOf(self.vehicle, self.target.joint, self.target.target)
    self.startDistance = math.sqrt(offset.x ^ 2 + offset.z ^ 2)
    Logging.info(ADHitchCreep.LOG_PREFIX .. "'%s' bakker %.1f m hen til '%s' (%.1f m til siden)", vehicleName(self.vehicle),
        self.startDistance, vehicleName(self.target.wagon), offset.x)
end

function CreepTask:finish(action, distance)
    local sdm = self.vehicle.ad.specialDrivingModule
    if sdm ~= nil and sdm.stopVehicle ~= nil then sdm:stopVehicle() end
    if action == "done" then
        Logging.info(ADHitchCreep.LOG_PREFIX .. "'%s' står ved '%s' — kobler på", vehicleName(self.vehicle), vehicleName(self.target.wagon))
    else
        Logging.warning(ADHitchCreep.LOG_PREFIX .. "'%s' opgav at bakke hen til '%s' (%.1f m tilbage efter %d s)", vehicleName(self.vehicle),
            vehicleName(self.target.wagon), distance, math.floor(self.elapsedMs / 1000))
    end
    local taskModule = self.adEnv.ADTaskModule
    self.vehicle.ad.taskModule:setCurrentTaskFinished(taskModule ~= nil and taskModule.DONT_PROPAGATE or 1)
end

function CreepTask:update(dt)
    self.elapsedMs = self.elapsedMs + dt
    self.checkMs = self.checkMs + dt
    if self.target.wagon.getAttacherVehicle ~= nil and self.target.wagon:getAttacherVehicle() ~= nil then
        return self:finish("giveUp", 0) -- en anden har taget vognen
    end
    local offset = offsetOf(self.vehicle, self.target.joint, self.target.target)
    local distance = math.sqrt(offset.x ^ 2 + offset.z ^ 2)
    if self.checkMs >= ADHitchCreep.RANGE_CHECK_MS then
        self.checkMs = 0
        self.inRange = isInRange(self.vehicle, self.target.wagon)
    end
    local action = ADHitchCreep.step({inRange = self.inRange, distance = distance, startDistance = self.startDistance,
        elapsedMs = self.elapsedMs, passed = offset.z > 0.5})
    if action ~= "drive" then return self:finish(action, distance) end

    local autoDrive, vehicle = self.adEnv.AutoDrive, self.vehicle
    local p = worldPos(self.target.target)
    local lx, lz = autoDrive.getDriveDirection(vehicle, p.x, p.y, p.z)
    local sdm = vehicle.ad.specialDrivingModule
    if sdm ~= nil and sdm.releaseVehicle ~= nil then sdm:releaseVehicle() end
    if vehicle.startMotor ~= nil and not vehicle:getIsMotorStarted() and vehicle:getCanMotorRun() then vehicle:startMotor() end
    -- som AD's ADSpecialDrivingModule:driveReverse (ikke-guidet): retning spejles ved bakning
    autoDrive.driveInDirection(vehicle, dt, 30, 0.3, 0.2, 20, true, false, -lx, -lz, ADHitchCreep.CREEP_SPEED, 1)
end

function CreepTask:abort() end
function CreepTask:finished() end
function CreepTask:getInfoText() return "Bakker hen til vognen" end
function CreepTask:getI18nInfo() return "" end
function CreepTask:getExcludedVehiclesForCollisionCheck() return {self.target.wagon} end

local isInstalled = false

-- DriveToMode:handleFinishedTask lægger StopAndDisableADTask i køen; bakningen lægges ind lige før.
function ADHitchCreep.install(adEnv)
    local mode = adEnv ~= nil and adEnv.DriveToMode or nil
    if isInstalled or mode == nil or mode.handleFinishedTask == nil or ADHitch == nil or ADHitch.pendingAttach == nil then return end
    isInstalled = true
    local original = mode.handleFinishedTask
    mode.handleFinishedTask = function(self)
        local ok, err = pcall(function()
            local job = self.driveToDestinationTask ~= nil and ADHitch.pendingAttach(self.vehicle) or nil
            if job == nil or job.markerId ~= self.destinationID then return end
            local target = ADHitchCreep.findTarget(self.vehicle)
            if target == nil or target.plan ~= "creep" then return end
            job.crept = true
            self.vehicle.ad.taskModule:addTask(CreepTask.new(adEnv, self.vehicle, target))
        end)
        if not ok then Logging.warning(ADHitchCreep.LOG_PREFIX .. "bakning til vognen: %s", tostring(err)) end
        return original(self)
    end
end