-- PalletStacker.lua
-- Robust pallet stacker for RH2460-based receiving hopper (FS25)
-- Stacks full cropBox pallets in a 3x3x2 grid behind stackRoot with physics-safe movement


_G.PalletStacker = _G.PalletStacker or {}
local PalletStacker = _G.PalletStacker

PalletStacker.MOD_NAME = g_currentModName or "FS25_PalletizerBoxStacker"

-- Local to this file only (cannot be overridden globally)
local PS_DEBUG = false -- true = enable debug logging

local function PS_LOG(...)
    if PS_DEBUG then
        print(...)
    end
end

PS_LOG(nil, "### PalletStacker.lua LOADED BY FS25")
--------------------------------------------------------
-- REQUIRED BY GIANTS SPECIALIZATION SYSTEM
--------------------------------------------------------
function PalletStacker.prerequisitesPresent(specializations)
    return true
end

function PalletStacker.registerFunctions(vehicleType)
    PS_LOG(nil, "### PalletStacker.registerFunctions for type: " .. tostring(vehicleType.name) .. " ")
    SpecializationUtil.registerFunction(vehicleType, "ps_findNextFreeSlot",       PalletStacker.ps_findNextFreeSlot)
    SpecializationUtil.registerFunction(vehicleType, "ps_movePalletToSlot",       PalletStacker.ps_movePalletToSlot)
    SpecializationUtil.registerFunction(vehicleType, "ps_checkAndStack",          PalletStacker.ps_checkAndStack)
    SpecializationUtil.registerFunction(vehicleType, "ps_cleanupRemovedPallets",  PalletStacker.ps_cleanupRemovedPallets)
    SpecializationUtil.registerFunction(vehicleType, "ps_detectSpawnedBox",       PalletStacker.ps_detectSpawnedBox)
end

function PalletStacker.registerEventListeners(vehicleType)
    PS_LOG(nil, "### PalletStacker.registerEventListeners for type: " .. tostring(vehicleType.name) .. " ")
    SpecializationUtil.registerEventListener(vehicleType, "onLoad",   PalletStacker)
    SpecializationUtil.registerEventListener(vehicleType, "onDelete", PalletStacker)
    SpecializationUtil.registerEventListener(vehicleType, "onUpdate", PalletStacker)
end

--------------------------------------------------------
-- LOCAL HELPERS (physics & mounting)
--------------------------------------------------------
local function ps_getObjectRootNode(object)
    if object == nil then
        return nil
    end

    local node = nil

    if object.components ~= nil and object.components[1] ~= nil then
        node = object.components[1].node
    elseif object.nodeId ~= nil then
        node = object.nodeId
    elseif object.rootNode ~= nil then
        node = object.rootNode
    end

    if node == nil or node == 0 or not entityExists(node) then
        return nil
    end

    return node
end

local function ps_removeFromPhysics(object)
    if object == nil then
        return
    end

    if object.isRoundbale ~= nil or object.isSplitShape then
        local node = ps_getObjectRootNode(object)
        if node ~= nil then
            removeFromPhysics(node)
        end
    elseif object.isAddedToPhysics then
        if object.removeFromPhysics ~= nil then
            object:removeFromPhysics()
        end
    end
end

local function ps_addToPhysics(object)
    if object == nil then
        return
    end

    if object.isRoundbale ~= nil or object.isSplitShape then
        local node = ps_getObjectRootNode(object)
        if node ~= nil then
            addToPhysics(node)
        end
    else
        if object.addToPhysics ~= nil then
            object:addToPhysics()
        end
    end

    if object.raiseActive ~= nil then
        object:raiseActive()
        if object.networkTimeInterpolator ~= nil
        and object.networkTimeInterpolator.reset ~= nil then
            object.networkTimeInterpolator:reset()
        end
    end

    if object.raiseDirtyFlags ~= nil then
        if object.physicsObjectDirtyFlag ~= nil then
            object:raiseDirtyFlags(object.physicsObjectDirtyFlag)
        elseif object.vehicleDirtyFlag ~= nil then
            object:raiseDirtyFlags(object.vehicleDirtyFlag)
        end
    end
end

local function ps_unmountDynamicMount(object)
    if object == nil then
        return
    end

    if object.mountObject ~= nil then
        if object.mountObject.removeMountedObject ~= nil then
            object.mountObject:removeMountedObject(object, true)
        end
        if object.mountObject.onUnmountObject ~= nil then
            object.mountObject:onUnmountObject(object)
        end
    end

    if object.dynamicMountObject ~= nil then
        local vehicle = object.dynamicMountObject
        if vehicle.removeDynamicMountedObject ~= nil then
            vehicle:removeDynamicMountedObject(object, true)
        end
    end

    if object.unmountDynamic ~= nil then
        if object.dynamicMountType == MountableObject.MOUNT_TYPE_DYNAMIC then
            object:unmountDynamic()
        elseif object.dynamicMountType == MountableObject.MOUNT_TYPE_KINEMATIC then
            if object.unmountKinematic ~= nil then
                object:unmountKinematic()
            end
        end

        pcall(function() object:unmountDynamic(true) end)

        if object.additionalDynamicMountJointNode ~= nil then
            delete(object.additionalDynamicMountJointNode)
            object.additionalDynamicMountJointNode = nil
        end
    end
end

--------------------------------------------------------
-- ON LOAD
--------------------------------------------------------
function PalletStacker:onLoad(savegame)
    Logging.info("### PalletStacker:onLoad() for vehicle: %s", self.configFileName)

    self.ps = {}
    self.ps.debug       = true      
    self.ps.checkTimer  = 0
    self.ps.detectTimer = 0

    if self.i3dMappings == nil then
        Logging.error("!!! PalletStacker ERROR: self.i3dMappings is nil")
        self.ps = nil
        return
    end

    local mapping = self.i3dMappings["stackRoot"]
    if mapping == nil then
        Logging.error("!!! PalletStacker ERROR: i3dMapping 'stackRoot' not found")
        self.ps = nil
        return
    end

    local stackRootNode = mapping
    if type(mapping) == "table" then
        if mapping.node ~= nil then
            stackRootNode = mapping.node
        elseif mapping.nodeId ~= nil then
            stackRootNode = mapping.nodeId
        elseif mapping.index ~= nil and self.components ~= nil and I3DUtil ~= nil then
            local ok, node = pcall(I3DUtil.indexToObject, self.components, mapping.index)
            if ok and node ~= nil then
                stackRootNode = node
            end
        end
    end

    if type(stackRootNode) ~= "number" then
        Logging.error("!!! PalletStacker ERROR: stackRootNode is not a nodeId (got %s)", tostring(stackRootNode))
        self.ps = nil
        return
    end

    self.ps.stackRoot = stackRootNode
    Logging.info("### PalletStacker: resolved stackRoot nodeId = %d", stackRootNode)

    self.ps.boxSpawnPlace = nil
    local sp = self.i3dMappings["boxSpawnPlace"]
    if sp ~= nil then
        if sp.node ~= nil then
            self.ps.boxSpawnPlace = sp.node
        elseif sp.nodeId ~= nil then
            self.ps.boxSpawnPlace = sp.nodeId
        elseif sp.index ~= nil and I3DUtil ~= nil then
            local ok, node = pcall(I3DUtil.indexToObject, self.components, sp.index)
            if ok and node ~= nil then
                self.ps.boxSpawnPlace = node
            end
        end
    end

    if self.ps.boxSpawnPlace ~= nil then
        PS_LOG(self, "### PalletStacker: boxSpawnPlace node = " .. tostring(self.ps.boxSpawnPlace))
    else
        PS_LOG(self, "### PalletStacker: boxSpawnPlace not found (optional)")
    end

    self.ps.slots = {}
    local slotIndex = 1

    local cfg = self.ps_config or {}
    local xOffsets = cfg.x or { -1.75, 0.0, 1.75 }
    local zOffsets = cfg.z or {  0.0, -1.35, -2.70 }
    local yOffsets = cfg.y or {  0.10, 1.40 }

    for yi = 1, #yOffsets do
        for zi = 1, #zOffsets do
            for xi = 1, #xOffsets do
                local slotNode = createTransformGroup(string.format("ps_slot_%02d", slotIndex))
                link(self.ps.stackRoot, slotNode)
                setTranslation(slotNode, xOffsets[xi], yOffsets[yi], zOffsets[zi])
                setRotation(slotNode, 0, 0, 0)

                self.ps.slots[slotIndex] = {
                    node   = slotNode,
                    pallet = nil
                }

                if self.ps.debug then
                    local sx, sy, sz = getWorldTranslation(slotNode)
                    PS_LOG(self, string.format(
                        "### PalletStacker: created slot %d at (%.2f, %.2f, %.2f)",
                        slotIndex, sx, sy, sz
                    ))
                end

                slotIndex = slotIndex + 1
            end
        end
    end

    self.ps.numSlots = #self.ps.slots
    PS_LOG(self, "### PalletStacker: initialized with " .. tostring(self.ps.numSlots) .. " slots")
end

--------------------------------------------------------
-- ON DELETE
--------------------------------------------------------
function PalletStacker:onDelete()
    if self.ps ~= nil and self.ps.slots ~= nil then
        for _, slot in ipairs(self.ps.slots) do
            if slot.node ~= nil and entityExists(slot.node) then
                delete(slot.node)
            end
        end
    end
    self.ps = nil
end

--------------------------------------------------------
-- CLEANUP REMOVED PALLETS
--------------------------------------------------------
function PalletStacker:ps_cleanupRemovedPallets()
    if self.ps == nil or self.ps.slots == nil then
        return
    end

    for i, slot in ipairs(self.ps.slots) do
        local pallet = slot.pallet

        if pallet ~= nil then
            local root = ps_getObjectRootNode(pallet)
            local freeThisSlot = false

            if root == nil or not entityExists(root) then
                freeThisSlot = true
            else
                local sx, sy, sz = getWorldTranslation(slot.node)
                local px, py, pz = getWorldTranslation(root)

                local dx = px - sx
                local dz = pz - sz
                local distXZ = math.sqrt(dx*dx + dz*dz)

                if distXZ > 0.65 then
                    freeThisSlot = true
                end
            end

            if freeThisSlot then
                if self.ps.debug then
                    PS_LOG(self, string.format(
                        "### PalletStacker: freeing slot %d (pallet removed/moved)", i
                    ))
                end
                slot.pallet = nil
                if pallet.ps_stacked ~= nil then
                    pallet.ps_stacked = nil
                end
            end
        end
    end
end

--------------------------------------------------------
-- SPAWNPLACE DETECTION FOR RECOVERY
--------------------------------------------------------
function PalletStacker:ps_detectSpawnedBox()
    if not self.isServer then
        return
    end

    local specRH = self.spec_receivingHopper
    if specRH == nil then
        return
    end

    if specRH.lastBox ~= nil then
        return
    end

    if not specRH.spawnBoxesAutomatically then
        return
    end

    local spawnNode = self.ps.boxSpawnPlace
    if spawnNode == nil or not entityExists(spawnNode) then
        return
    end

    local x, y, z = getWorldTranslation(spawnNode)
    local radius = 1.5

    for _, object in pairs(g_currentMission.vehicles) do
        local cfg = object.configFileName and string.lower(object.configFileName) or ""
        if cfg:match("cropbox%.xml$") then
            local root = ps_getObjectRootNode(object)
            if root and entityExists(root) then
                local px, py, pz = getWorldTranslation(root)
                local dx = px - x
                local dy = py - y
                local dz = pz - z
                local dist = math.sqrt(dx*dx + dy*dy + dz*dz)

                if dist < radius then
                    PS_LOG(self, "### PalletStacker: recovered lastBox near spawn area")
                    specRH.lastBox = object
                    return
                end
            end
        end
    end
end

--------------------------------------------------------
-- ON UPDATE
--------------------------------------------------------
function PalletStacker:onUpdate(dt)
    if self.ps == nil then
        return
    end

    if not self.isServer then
        return
    end

    self:ps_cleanupRemovedPallets()

    self.ps.detectTimer = (self.ps.detectTimer or 0) + dt
    if self.ps.detectTimer > 500 then
        self.ps.detectTimer = 0
        self:ps_detectSpawnedBox()
    end

    self.ps.checkTimer = (self.ps.checkTimer or 0) + dt
    if self.ps.checkTimer < 250 then
        return
    end
    self.ps.checkTimer = 0

    self:ps_checkAndStack()
end

--------------------------------------------------------
-- FIND NEXT FREE SLOT
--------------------------------------------------------
function PalletStacker:ps_findNextFreeSlot()
    if self.ps == nil or self.ps.slots == nil then
        return nil
    end

    for i, slot in ipairs(self.ps.slots) do
        if slot.pallet == nil then
            return i
        end
    end
    return nil
end

--------------------------------------------------------
-- MOVE PALLET TO SLOT
--------------------------------------------------------
function PalletStacker:ps_movePalletToSlot(pallet, slotIndex)
    if pallet == nil or self.ps == nil or self.ps.slots == nil then
        return
    end

    if not self.isServer then
        return
    end

    local slot = self.ps.slots[slotIndex]
    if slot == nil or slot.node == nil then
        PS_LOG(self, "!!! PalletStacker ERROR: invalid slot index " .. tostring(slotIndex))
        return
    end

    PS_LOG(self, "### PalletStacker: stacking pallet to slot " .. tostring(slotIndex) .. "")

    ps_unmountDynamicMount(pallet)
    ps_removeFromPhysics(pallet)

    local node = ps_getObjectRootNode(pallet)
    if node == nil then
        PS_LOG(self, "### PalletStacker ERROR: pallet root node not found")
        ps_addToPhysics(pallet)
        return
    end

    local x, y, z    = getWorldTranslation(slot.node)
    local rx, ry, rz = getWorldRotation(slot.node)

    setWorldTranslation(node, x, y, z)
    setWorldRotation(node, rx, ry, rz)

    ps_addToPhysics(pallet)

    slot.pallet       = pallet
    pallet.ps_stacked = true

    PS_LOG(self, string.format(
        "### PalletStacker: moved pallet to slot %d → (%.2f, %.2f, %.2f)",
        slotIndex, x, y, z
    ))
end

--------------------------------------------------------
-- CHECK AND STACK
--------------------------------------------------------
function PalletStacker:ps_checkAndStack()
    local specRH = self.spec_receivingHopper
    if specRH == nil then
        if self.ps ~= nil and self.ps.debug then
            PS_LOG(self, "### PalletStacker: spec_receivingHopper == nil → skipping")
        end
        return
    end

    local pallet = specRH.lastBox
    if pallet == nil then
        return
    end

    if pallet.ps_stacked then
        return
    end

    local cfg = pallet.configFileName or ""
    local cfgLower = string.lower(cfg)
    if not cfgLower:match("cropbox%.xml$") then
        if self.ps ~= nil and self.ps.debug then
            PS_LOG(self, "### PalletStacker: ignoring non-cropBox pallet: " .. tostring(cfg))
        end
        return
    end

    local isFull    = false
    local debugText = ""

    if pallet.getFillUnitFreeCapacity ~= nil then
        local freeCap = pallet:getFillUnitFreeCapacity(1) or 0
        local cap     = 0
        if pallet.getFillUnitCapacity ~= nil then
            cap = pallet:getFillUnitCapacity(1) or 0
        end
        local lvl = cap - freeCap

        debugText = string.format("lvl=%.2f cap=%.2f free=%.2f", lvl, cap, freeCap)

        if freeCap <= 0.01 then
            isFull = true
        end
    else
        debugText = "no getFillUnitFreeCapacity API"
    end

    if self.ps ~= nil and self.ps.debug then
        PS_LOG(self, "### PalletStacker: fullnessCheck → " .. debugText .. " ")
    end

    if not isFull then
        return
    end

    local freeSlot = self:ps_findNextFreeSlot()
    if not freeSlot then
        PS_LOG(self, "### PalletStacker: all stacking slots FULL")
        return
    end

    self:ps_movePalletToSlot(pallet, freeSlot)
end

return PalletStacker
