-- FS25_AD_CP_Zubringer - ADCPSupplyTask
--
-- Diese Task wird in vehicle.ad.taskModule des AD-Zubringers eingehängt,
-- genau wie AutoDrive es bei EmptyHarvesterTask fuer Mähdrescher-Abfahrer
-- macht (siehe FS25_AutoDrive/scripts/Tasks/EmptyHarvesterTask.lua).
-- Statt zum Rohr des Mähdreschers navigieren wir direkt zum wartenden
-- CP-Fahrzeug (vehicle.ad.pathFinderModule:startPathPlanningToVehicle),
-- und statt Getreide abzusaugen, schieben wir Fuellstand vom Zubringer
-- in den Ausbringer.
--
-- HINWEIS: Diese Task setzt voraus, dass vehicle.ad.stateModule bereits
-- aktiv ist (der Zubringer also "unter AD-Kontrolle" faehrt). Das stellt
-- ADCP_Bridge.lua vor dem Einhaengen der Task sicher.

-- Eigene, schlanke Timer-Klasse statt AutoDriveTON (AD's eigene Klasse) -
-- aus demselben Grund wie oben bei ADInheritsFrom/AbstractTask: bare
-- AD-Globals sind aus unserem Mod-Skript heraus zur Laufzeit nicht
-- zuverlaessig sichtbar (siehe Log: "AutoDrive-Global"/"ADGraphManager-Global
-- ist an dieser Stelle nil"). Verhalten 1:1 wie AutoDriveTON nachgebaut.
ADCPTimer = {}
ADCPTimer.__index = ADCPTimer

function ADCPTimer:new()
    local o = setmetatable({}, ADCPTimer)
    o.elapsedTime = 0
    o.time = 0
    return o
end

function ADCPTimer:timer(signal, time, dt)
    if time ~= nil then
        self.time = time
    end
    if dt ~= nil then
        self.elapsedTime = self.elapsedTime + dt
    end
    if signal then
        return self.elapsedTime > self.time
    else
        self.elapsedTime = 0
    end
    return false
end

function ADCPTimer:done()
    return self.elapsedTime > self.time
end
ADCPSupplyTask = {}
ADCPSupplyTask.__index = ADCPSupplyTask

ADCPSupplyTask.STATE_PATHPLANNING = {}
ADCPSupplyTask.STATE_DRIVING = {}
ADCPSupplyTask.STATE_TRANSFER = {}
ADCPSupplyTask.STATE_CLEARING_FORWARD = {}
ADCPSupplyTask.STATE_FINISHED_OK = {}
ADCPSupplyTask.STATE_FAILED = {}

function ADCPSupplyTask:new(vehicle, targetVehicle)
    local o = setmetatable({}, ADCPSupplyTask)
    o.vehicle = vehicle
    o.target = targetVehicle
    o.state = ADCPSupplyTask.STATE_PATHPLANNING
    o.stuckTimer = ADCPTimer:new()
    o.totalTimer = ADCPTimer:new()
    o.result = nil -- "ok" | "failed" wird von ADCP_Bridge ausgewertet
    return o
end

function ADCPSupplyTask:setUp()
    local mode = self.vehicle.ad.stateModule:getCurrentMode()
    ADCP.diagPrint("SupplyTask:setUp vehicle=%s target=%s mode=%s modeState=%s activeTask=%s",
        tostring(self.vehicle:getName()), tostring(self.target and self.target:getName()), tostring(mode),
        tostring(mode and mode.state), tostring(mode and mode.activeTask))
    ADCP.debugPrint("SupplyTask:setUp fuer %s -> Ziel %s", tostring(self.vehicle.ad.stateModule:getName()), tostring(self.target))

    -- Keine Geschwindigkeitsbegrenzung durch dieses Mod: AutoDrive darf
    -- die vom Benutzer eingestellten Strassen-/Feldgeschwindigkeiten nutzen.

    -- Seitliche Anfahrposition statt direkt hinter dem Ziel: AD's eigene
    -- startPathPlanningToVehicle() zielt bewusst auf einen Punkt HINTER dem
    -- Zielfahrzeug (fuer das "hinter dem fahrenden Maehdrescher herfahren"-
    -- Szenario) - fuer unser stationaeres Andocken wollen wir stattdessen
    -- seitlich neben den Ausbringer, daher hier selbst berechnet.
    local targetNode = self.target.components[1].node
    local worldX, worldY, worldZ = getWorldTranslation(targetNode)
    local fwdX, _, fwdZ = localDirectionToWorld(targetNode, 0, 0, 1)
    local sideX, _, sideZ = localDirectionToWorld(targetNode, 1, 0, 0)

    -- Auf der Seite anfahren, auf der sich der Zubringer bereits befindet.
    -- So kreuzt er nicht hinter dem Ausbringer und wird nicht auf die
    -- gegenueberliegende Feldseite bzw. aus dem Feld heraus geschickt.
    local supplierX, _, supplierZ = getWorldTranslation(self.vehicle.components[1].node)
    local localSupplierX = select(1, worldToLocal(targetNode, supplierX, worldY, supplierZ))
    local sideSign = localSupplierX >= 0 and 1 or -1
    self.approachSideSign = sideSign

    -- Ziel bleibt exakt seitlich neben dem Ausbringer. Der in 0.1.8
    -- verwendete Vorlauf entlang der Fahrtrichtung wird bewusst entfernt,
    -- weil er den Zubringer ueber die Feldgrenze hinaus schicken konnte.
    local approachPoint = {
        x = worldX + ADCP.DOCKING_DISTANCE * sideSign * sideX,
        y = worldY,
        z = worldZ + ADCP.DOCKING_DISTANCE * sideSign * sideZ,
    }
    local approachDirection = {x = fwdX, z = fwdZ}

    self.vehicle.ad.pathFinderModule:reset()
    self.vehicle.ad.pathFinderModule:startPathPlanningTo(approachPoint, approachDirection)
end

function ADCPSupplyTask:update(dt)
    -- Zielfahrzeug verschwunden (verkauft, entladen, etc.) -> abbrechen
    if self.target == nil or self.target.components == nil or g_currentMission.nodeToObject[self.target.components[1].node] == nil then
        ADCP.debugPrint("SupplyTask: Zielfahrzeug nicht mehr vorhanden -> Abbruch")
        self.result = "failed"
        self:finished()
        return
    end

    self.totalTimer:timer(true, ADCP.MAX_DOCKING_TIME_MS, dt)
    if self.totalTimer:done() and self.state ~= ADCPSupplyTask.STATE_TRANSFER then
        ADCP.debugPrint("SupplyTask: Zeitlimit erreicht -> Abbruch")
        self.result = "failed"
        self:finished()
        return
    end

    local checkForStuck = (self.vehicle.lastSpeedReal <= 0.0002) and (self.state == ADCPSupplyTask.STATE_DRIVING)
    self.stuckTimer:timer(checkForStuck, 45000, dt)
    if self.stuckTimer:done() then
        ADCP.debugPrint("SupplyTask: Fahrzeug haengt fest -> Abbruch")
        self.result = "failed"
        self:finished()
        return
    end

    if self.state == ADCPSupplyTask.STATE_PATHPLANNING then
        if self.vehicle.ad.pathFinderModule:hasFinished() then
            local wayPoints = self.vehicle.ad.pathFinderModule:getPath()
            if wayPoints == nil or #wayPoints == 0 then
                ADCP.debugPrint("SupplyTask: keine Route zum Ausbringer gefunden -> Abbruch")
                self.result = "failed"
                self:finished()
                return
            end
            self.vehicle.ad.drivePathModule:setWayPoints(wayPoints)
            self.state = ADCPSupplyTask.STATE_DRIVING
        else
            self.vehicle.ad.pathFinderModule:update(dt)
            self.vehicle.ad.specialDrivingModule:stopVehicle()
            self.vehicle.ad.specialDrivingModule:update(dt)
        end
        return
    elseif self.state == ADCPSupplyTask.STATE_DRIVING then
        if self.vehicle.ad.drivePathModule:isTargetReached() then
            ADCP.debugPrint("SupplyTask: beim Ausbringer angekommen -> Umladen")
            self.state = ADCPSupplyTask.STATE_TRANSFER
        else
            self.drivingDiagTimer = (self.drivingDiagTimer or 0) + dt
            if self.drivingDiagTimer >= 3000 then
                self.drivingDiagTimer = 0
                local x, _, z = getWorldTranslation(self.vehicle.components[1].node)
                local nextWp = self.vehicle.ad.drivePathModule.getNextWayPointId
                    and self.vehicle.ad.drivePathModule:getNextWayPointId()
                local reversing = self.vehicle.ad.drivePathModule.getIsReversing
                    and self.vehicle.ad.drivePathModule:getIsReversing()
                ADCP.debugPrint(
                    "FAHRT-DIAGNOSE: %s pos=(%.1f,%.1f) speed=%.5f naechsterWP=%s rueckwaerts=%s",
                    tostring(self.vehicle:getName()), x, z, self.vehicle.lastSpeedReal or -1,
                    tostring(nextWp), tostring(reversing)
                )
            end
            self.vehicle.ad.drivePathModule:update(dt)
        end
        return
    elseif self.state == ADCPSupplyTask.STATE_TRANSFER then
        self.vehicle.ad.specialDrivingModule.motorShouldNotBeStopped = true
        self.vehicle.ad.specialDrivingModule:stopVehicle()
        self.vehicle.ad.specialDrivingModule:update(dt)

        local transferred, sourceEmpty, targetFull = ADCP.transferFill(dt, self.vehicle, self.target)

        if targetFull or sourceEmpty then
            local node = self.vehicle.components[1].node
            local x, _, z = getWorldTranslation(node)
            self.clearStartX = x
            self.clearStartZ = z

            -- Freifahrt-Ziel diagonal nach vorn und von der Ausbringerseite
            -- weg. Dadurch bleibt die Ueberladeposition unveraendert, waehrend
            -- der Abstand unmittelbar nach dem Umladen sichtbar groesser wird.
            local fwdX, _, fwdZ = localDirectionToWorld(node, 0, 0, 1)
            local sideX, _, sideZ = localDirectionToWorld(node, 1, 0, 0)
            local sideSign = self.approachSideSign or 1
            self.clearTargetX = x
                + fwdX * ADCP.SUPPLIER_CLEAR_FORWARD_DISTANCE_M
                + sideX * sideSign * ADCP.SUPPLIER_CLEAR_SIDE_OFFSET_M
            self.clearTargetZ = z
                + fwdZ * ADCP.SUPPLIER_CLEAR_FORWARD_DISTANCE_M
                + sideZ * sideSign * ADCP.SUPPLIER_CLEAR_SIDE_OFFSET_M
            self.state = ADCPSupplyTask.STATE_CLEARING_FORWARD
            self.vehicle.ad.specialDrivingModule.motorShouldNotBeStopped = false
            ADCP.debugPrint("SupplyTask: Umladen fertig (targetFull=%s, sourceEmpty=%s) -> %.1f m geradeaus freifahren", tostring(targetFull), tostring(sourceEmpty), ADCP.SUPPLIER_CLEAR_FORWARD_DISTANCE_M)
        end
        return
    elseif self.state == ADCPSupplyTask.STATE_CLEARING_FORWARD then
        local node = self.vehicle.components[1].node
        local x, _, z = getWorldTranslation(node)
        local distance = MathUtil.vector2Length(x - self.clearStartX, z - self.clearStartZ)
        if distance >= ADCP.SUPPLIER_CLEAR_FORWARD_DISTANCE_M then
            self.result = "ok"
            ADCP.debugPrint("SupplyTask: Freifahrt beendet (%.2f m) -> normale AutoDrive-Route uebernimmt", distance)
            self:finished()
            return
        end

        -- AD's eigene driveForward()-Methode nutzen statt eines rohen
        -- AIVehicleUtil-Aufrufs: die kuemmert sich zuverlaessig um
        -- Motor-Start/Fahrzeug-Freigabe/Anhaenger-Handling, bevor sie den
        -- eigentlichen Fahrbefehl gibt. Der rohe Aufruf uebersprang das und
        -- fuehrte zu "attempt to index nil with 'setSpeedLimit'" (self.motor
        -- war nicht gesetzt, weil das Fahrzeug nicht sauber "freigegeben" war).
        -- Sanft zum diagonal versetzten Freifahrt-Ziel lenken. driveForward()
        -- bleibt fuer Motor/Fahrzeugfreigabe zustaendig; der Lenkwert wird von
        -- unserem Mod aus der lokalen Lage des Zielpunkts berechnet.
        if self.clearTargetX ~= nil and self.vehicle.setSteeringInput ~= nil then
            local localX, _, localZ = worldToLocal(node, self.clearTargetX, 0, self.clearTargetZ)
            local steering = localX / math.max(math.abs(localZ), 4)
            -- MathUtil.clamp existiert in dieser FS25-Version nicht.
            -- Lokal begrenzen, damit der Task nicht jeden Tick mit einem
            -- Lua-Fehler abbricht und die Freigabe nie an die Bridge meldet.
            steering = math.max(-0.35, math.min(0.35, steering))
            pcall(self.vehicle.setSteeringInput, self.vehicle, steering, false)
        end
        self.vehicle.ad.specialDrivingModule:driveForward(dt)
        return
    end
end

function ADCPSupplyTask:abort()
    self.result = "failed"
end

function ADCPSupplyTask:finished(propagate)
    local tm = self.vehicle.ad.taskModule
    local mode = self.vehicle.ad.stateModule:getCurrentMode()
    ADCP.diagPrint("SupplyTask:finished begin result=%s modeState=%s activeTask=%s",
        tostring(self.result), tostring(mode and mode.state), tostring(tm and tm.activeTask))

    self.vehicle.ad.specialDrivingModule.motorShouldNotBeStopped = false
    self.vehicle.adcpTaskResult = self.result

    -- Den unterbrochenen Pickup-&-Deliver-Zyklus nicht weiterverwenden.
    -- Nach dem Feld-Umladen wird die Fremd-Task ohne Propagation entfernt
    -- und der komplette AD-Modus wie bei einem frischen Benutzerstart neu
    -- initialisiert. Die Marker bleiben im StateModule erhalten, waehrend
    -- Trailer-/Dolly-Erkennung, Triggerstatus und die interne Zustandsmaschine
    -- vollstaendig neu aufgebaut werden.
    if mode ~= nil and mode.start ~= nil then
        -- 1 = ADTaskModule.DONT_PROPAGATE
        tm:setCurrentTaskFinished(1)

        -- Falls die alte Taskreferenz im Modus noch gesetzt ist, vor dem
        -- Neustart entfernen. mode:start() ruft anschliessend reset() auf,
        -- liest alle angehaengten Einheiten neu ein und erzeugt anhand des
        -- aktuellen (leeren) Fuellstands eine neue LoadAtDestinationTask.
        mode.activeTask = nil

        -- AutoDrive behandelt ein Fahrzeug mit aktivem "Helfer neu starten"
        -- im Pickup-&-Deliver-Modus nicht als normalen Ladetransport: Die
        -- LoadAtDestinationTask aktualisiert dann waehrend der Anfahrt das
        -- TrailerModule nicht und stoppt am Ziel AD fuer einen CP-Handover.
        -- Der Zubringer ist hier aber ein reines AD-Fahrzeug. Deshalb den
        -- verwendeten Helfer vor dem Neustart explizit deaktivieren, damit
        -- Triggererkennung und automatisches Beladen wieder aktiv sind.
        local stateModule = self.vehicle.ad.stateModule
        if stateModule ~= nil then
            -- getCanRestartHelper() prueft startHelper UND usedHelper. Der vorige
            -- Patch setzte usedHelper wegen einer nicht global verfuegbaren
            -- ADStateModule-Klassentabelle gar nicht. Deshalb beide Werte ueber
            -- die echte StateModule-Instanz direkt setzen.
            if stateModule.setStartHelper ~= nil then
                stateModule:setStartHelper(false)
            end
            if stateModule.setUsedHelper ~= nil then
                -- ADStateModule.HELPER_NONE ist in AutoDrive 3.0.1.3 RC der Wert 5.
                stateModule:setUsedHelper(5)
            end
            ADCP.debugPrint("SupplyTask: AD-Helferneustart deaktiviert (startHelper=false, usedHelper=NONE); Lade-Triggererkennung aktiv")
        end

        mode:start(nil)

        ADCP.debugPrint("SupplyTask: Pickup-&-Deliver komplett neu gestartet")
    else
        -- Sicherheits-Fallback fuer unerwartete AD-Modi.
        tm:setCurrentTaskFinished(propagate)
    end

    ADCP.diagPrint("SupplyTask:finished end modeState=%s modeTask=%s queue=%s",
        tostring(mode and mode.state), tostring(mode and mode.activeTask),
        tostring(tm and tm.getNumberOfTasks and tm:getNumberOfTasks() or "nil"))
end

function ADCPSupplyTask:getExcludedVehiclesForCollisionCheck()
    local excluded = {}
    if (self.state == ADCPSupplyTask.STATE_DRIVING or self.state == ADCPSupplyTask.STATE_CLEARING_FORWARD) and self.target ~= nil then
        table.insert(excluded, self.target:getRootVehicle())
    end
    -- Andere geparkte/wartende Zubringer nicht als Kollisionshindernis
    -- werten - sonst haelt AD an, sobald ein zweiter Zubringer in der Naehe
    -- steht (siehe ADCP.getOtherSupplyVehicles in ADCP_Bridge.lua). Gilt in
    -- allen Zustaenden, nicht nur waehrend der Fahrt, da dies auch die
    -- Pfadplanung betrifft.
    if ADCP.getOtherSupplyVehicles ~= nil then
        for _, v in ipairs(ADCP.getOtherSupplyVehicles(self.vehicle)) do
            table.insert(excluded, v)
        end
    end
    return excluded
end

function ADCPSupplyTask:getI18nInfo()
    if self.state == ADCPSupplyTask.STATE_PATHPLANNING then
        return "Zubringer: Route wird berechnet"
    elseif self.state == ADCPSupplyTask.STATE_DRIVING then
        return "Zubringer: unterwegs zum Ausbringer"
    elseif self.state == ADCPSupplyTask.STATE_TRANSFER then
        return "Zubringer: laedt um"
    elseif self.state == ADCPSupplyTask.STATE_CLEARING_FORWARD then
        return "Zubringer: faehrt gerade frei"
    end
    return "Zubringer"
end
