-- REAtrailerDynamics.lua
-- Feature 3: Anhänger-Dynamik & Deichselspiel (Ultra C, aber nach Masse skaliert)

REAtrailerDynamics = {}
REAtrailerDynamics.version = "1.0.0"

REAtrailerDynamics.CONFIG = {
    yawGain        = 0.9,    -- wie stark Knickwinkel in Schaukeln übersetzt wird
    yawDamping     = 0.28,   -- Dämpfung (klein = mehr Nachschwingen)
    brakeGain      = 0.6,    -- Bremsen triggert Schaukeln
    accelGain      = 0.4,    -- Gasgeben triggert Schaukeln
    maxSwayAngle   = 0.5,    -- max. Schwenkwinkel (rad)
    tipThreshold   = 0.85,   -- ab wann Kippimpuls gesetzt wird
    minSpeedKmh    = 12.0,
}

local function clamp(v, a, b)
    if v < a then return a end
    if v > b then return b end
    return v
end

function REAtrailerDynamics:loadMap(name)
    print(string.format("REAtrailerDynamics v%s geladen (Deichselspiel Ultra, skaliert nach Masse)", REAtrailerDynamics.version))
end

function REAtrailerDynamics:update(dt)
    if g_currentMission == nil or g_currentMission.vehicles == nil then
        return
    end

    for _, tractor in pairs(g_currentMission.vehicles) do
        if tractor ~= nil and tractor.getAttachedImplements ~= nil and tractor.isAddedToPhysics then
            self:updateVehicle(tractor, dt)
        end
    end
end

function REAtrailerDynamics:getMassFactor(tractor, trailer)
    local tMass = tractor.getTotalMass and tractor:getTotalMass() or (tractor.mass or 8000)
    local trMass = trailer.getTotalMass and trailer:getTotalMass() or (trailer.mass or 5000)
    local ratio = trMass / math.max(tMass, 1)
    return clamp(ratio, 0.4, 2.0)
end

function REAtrailerDynamics:updateVehicle(tractor, dt)
    local implements = tractor:getAttachedImplements()
    if implements == nil or #implements == 0 then return end

    local speedKmh = math.abs((tractor.lastSpeedReal or tractor.lastSpeed or 0) * 3600)
    if speedKmh < REAtrailerDynamics.CONFIG.minSpeedKmh then return end

    for _, impl in pairs(implements) do
        if impl.object ~= nil and impl.object.isAddedToPhysics then
            self:updateTrailerPair(tractor, impl.object, dt, speedKmh)
        end
    end
end

function REAtrailerDynamics:updateTrailerPair(tractor, trailer, dt, speedKmh)
    local dtSec = dt / 1000

    if trailer.rootNode == nil or tractor.rootNode == nil then return end

    local tx, ty, tz = localToLocal(trailer.rootNode, tractor.rootNode, 0, 0, 0)
    local yawError = math.atan2(tx, tz)

    trailer.REA_swayState = trailer.REA_swayState or 0.0

    local longAcc = (tractor.lastSpeedAcceleration or 0) * 60
    local trigger = 0
    if longAcc < -0.05 then
        trigger = trigger + (-longAcc) * REAtrailerDynamics.CONFIG.brakeGain
    elseif longAcc > 0.05 then
        trigger = trigger + longAcc * REAtrailerDynamics.CONFIG.accelGain
    end

    local massFactor = REAtrailerDynamics:getMassFactor(tractor, trailer)

    local targetSway = (yawError * REAtrailerDynamics.CONFIG.yawGain + trigger) * massFactor
    trailer.REA_swayState = trailer.REA_swayState + (targetSway - trailer.REA_swayState) * (dtSec * 4.0)

    trailer.REA_swayState = trailer.REA_swayState * (1.0 - REAtrailerDynamics.CONFIG.yawDamping * dtSec * 10)

    trailer.REA_swayState = clamp(trailer.REA_swayState,
        -REAtrailerDynamics.CONFIG.maxSwayAngle,
         REAtrailerDynamics.CONFIG.maxSwayAngle)

    local rx, ry, rz = getRotation(trailer.rootNode)
    setRotation(trailer.rootNode, rx, ry + trailer.REA_swayState * 0.18, rz)

    local swayNorm = math.abs(trailer.REA_swayState) / (REAtrailerDynamics.CONFIG.maxSwayAngle + 0.001)
    if swayNorm > REAtrailerDynamics.CONFIG.tipThreshold and speedKmh > 25 then
        local tipDir = (trailer.REA_swayState > 0) and 1 or -1
        setRotation(trailer.rootNode, rx, ry, rz + tipDir * 0.022 * massFactor)
    end
end

addModEventListener(REAtrailerDynamics)
