FA_Courseplay = {}

local specName = "spec_fahrgassenAutomatik"

------------------------------------------------
-- BASIC CHECK
------------------------------------------------
function FA_Courseplay.hasCourseplay(self)
    local root = self.rootVehicle or self

    return root ~= nil
        and root.hasCpCourse ~= nil
        and root.getFieldWorkCourse ~= nil
end

------------------------------------------------
-- GENERIC GETTERS
------------------------------------------------
function FA_Courseplay.cpGetAttr(wp)
    if wp == nil then return nil end

    if wp.getAttributes ~= nil then
        local ok, attr = pcall(wp.getAttributes, wp)
        if ok and attr ~= nil then return attr end
    end

    return wp.attributes
end

function FA_Courseplay.cpReadField(obj, name)
    if obj == nil then return nil end
    return obj[name]
end

function FA_Courseplay.cpCallNoArg(obj, fnName)
    if obj ~= nil and obj[fnName] ~= nil then
        local ok, value = pcall(obj[fnName], obj)
        if ok then return value end
    end
    return nil
end

function FA_Courseplay.cpGetValue(wp, ...)
    local attr = FA_Courseplay.cpGetAttr(wp)
    local names = {...}

    for _, name in ipairs(names) do
        local v = FA_Courseplay.cpCallNoArg(wp, name)
        if v ~= nil then return v end

        v = FA_Courseplay.cpCallNoArg(attr, name)
        if v ~= nil then return v end

        v = FA_Courseplay.cpReadField(wp, name)
        if v ~= nil then return v end

        v = FA_Courseplay.cpReadField(attr, name)
        if v ~= nil then return v end
    end

    return nil
end

function FA_Courseplay.cpGetBool(wp, ...)
    for _, name in ipairs({...}) do
        local v = FA_Courseplay.cpGetValue(wp, name)
        if v ~= nil then return v == true end
    end
    return false
end

function FA_Courseplay.cpGetString(wp, ...)
    for _, name in ipairs({...}) do
        local v = FA_Courseplay.cpGetValue(wp, name)
        if type(v) == "string" and v ~= "" then return v end
    end
    return nil
end

function FA_Courseplay.cpGetNumber(wp, ...)
    for _, name in ipairs({...}) do
        local v = FA_Courseplay.cpGetValue(wp, name)
        if type(v) == "number" then return v end
    end
    return nil
end


------------------------------------------------
-- WAYPOINT INFO
------------------------------------------------
function FA_Courseplay.cpGetRowNumber(wp)
    return FA_Courseplay.cpGetNumber(wp, "getRowNumber", "rowNumber")
end

function FA_Courseplay.cpGetHeadlandPassNumber(wp)
    return FA_Courseplay.cpGetNumber(wp, "getHeadlandPassNumber", "headlandPassNumber")
end

function FA_Courseplay.cpIsHeadland(wp)
    if FA_Courseplay.cpGetBool(wp, "isHeadlandTurn", "headlandTurn") then
        return true
    end

    return FA_Courseplay.cpGetHeadlandPassNumber(wp) ~= nil
end

function FA_Courseplay.cpIsConnecting(wp)
    return FA_Courseplay.cpGetBool(wp, "isOnConnectingPath", "onConnectingPath")
end

------------------------------------------------
-- 🔥 DEINE HAUPTFUNKTIONEN (1:1 verschieben)
------------------------------------------------
function FA_Courseplay.getCourseplayRowInfo(self)
 local spec = self[specName]
    local root = self.rootVehicle or self

    if not FA_Courseplay.hasCourseplay(self) or not root:hasCpCourse() then
        return nil
    end

    local course = root:getFieldWorkCourse()
    if not course or not course.getNumberOfWaypoints or not course.getWaypoint then
        return nil
    end

    local now = g_currentMission.time or 0

    ------------------------------------------------
    -- 🔥 THROTTLE (max 2x pro Sekunde)
    ------------------------------------------------
    if spec._cpRowInfoCache ~= nil then
        if now - (spec._cpRowInfoLastTime or 0) < 200 then
            return spec._cpRowInfoCache
        end
    end
    ------------------------------------------------
    -- 🔥 HEADLAND TIME LOCK
    ------------------------------------------------
    local now = g_currentMission.time or 0
    
    if spec._cpHeadlandLockUntil ~= nil and now < spec._cpHeadlandLockUntil then
        return spec._cpHeadlandLockData
    end

    ------------------------------------------------
    -- 1) Nächsten WP finden
    ------------------------------------------------
    -- ⭐ Implement-Position (Sämaschine) statt Traktor verwenden!
    -- Grund: Traktor ragt am Bahnende ins Vorgewende, Sämaschine noch nicht → falscher Pass.
    local implementNode = self.components and self.components[1] and self.components[1].node
    local tractorNode   = root.components  and root.components[1]  and root.components[1].node
    local posNode = implementNode or tractorNode
    local vx, _, vz = getWorldTranslation(posNode)
    local numWp = course:getNumberOfWaypoints()

    local nearestIx = nil
    local nearestDist2 = math.huge

    for i = 1, numWp do
        local wp = course:getWaypoint(i)
        if wp and wp.getPosition then
            local x, _, z = wp:getPosition()
            local dx = vx - x
            local dz = vz - z
            local d2 = dx * dx + dz * dz

            if d2 < nearestDist2 then
                nearestDist2 = d2
                nearestIx = i
            end
        end
    end

    if nearestIx == nil then
        spec._cpRowInfoCache = nil
        spec._cpRowInfoLastTime = now
        return nil
    end

    ------------------------------------------------
    -- 2) Infos vom nächsten WP
    ------------------------------------------------
    local nearestWp = course:getWaypoint(nearestIx)

    local nearestRow = nil
    local nearestHeadlandPass = nil
    local isConnecting = false

    if nearestWp then
        nearestRow = FA_Courseplay.cpGetRowNumber(nearestWp)
        nearestHeadlandPass = FA_Courseplay.cpGetHeadlandPassNumber(nearestWp)
        isConnecting = FA_Courseplay.cpIsConnecting(nearestWp)
    end

    ------------------------------------------------
    -- 3) firstRow / lastRow bestimmen
    ------------------------------------------------
    local firstRow = nil
    local lastRow = nil

    for i = 1, numWp do
        local wp = course:getWaypoint(i)
        if wp then
            local row = FA_Courseplay.cpGetRowNumber(wp)
            if row ~= nil then
                if firstRow == nil or row < firstRow then
                    firstRow = row
                end
                if lastRow == nil or row > lastRow then
                    lastRow = row
                end
            end
        end
    end

    ------------------------------------------------
    -- 🔥 4) LOKALE HEADLAND-ERKENNUNG (¼ Arbeitsbreite)
    ------------------------------------------------
    local laneWidth = course:getWorkWidth()
    local radius = math.max(1.2, laneWidth * 0.25)
    local radius2 = radius * radius

    local bestHeadland = nil
    local bestHeadlandDist2 = math.huge

    for i = 1, numWp do
        local wp = course:getWaypoint(i)
        if wp and wp.getPosition then
            local x, _, z = wp:getPosition()
            local dx = vx - x
            local dz = vz - z
            local d2 = dx * dx + dz * dz

            if d2 <= radius2 then
                local headlandPass = FA_Courseplay.cpGetHeadlandPassNumber(wp)
                local wpIsHeadland = FA_Courseplay.cpIsHeadland(wp)
                local wpIsConnecting = FA_Courseplay.cpIsConnecting(wp)

                if wpIsHeadland and not wpIsConnecting and headlandPass ~= nil then
                    if d2 < bestHeadlandDist2 then
                        bestHeadlandDist2 = d2
                        bestHeadland = {
                            row = FA_Courseplay.cpGetRowNumber(wp),
                            headlandPass = headlandPass,
                            isHeadland = true,
                            wpIx = i,
                            dist2 = d2,
                            isConnecting = false,
                            firstRow = firstRow,
                            lastRow = lastRow
                        }
                    end
                end
            end
        end
    end

    ------------------------------------------------
    -- 🔥 PRIORITÄT: HEADLAND
    ------------------------------------------------
   if bestHeadland ~= nil then

    -- 🔥 LOCK für 800 ms
    spec._cpHeadlandLockUntil = now + 1000
    spec._cpHeadlandLockData = bestHeadland

    spec._cpRowInfoCache = bestHeadland
    spec._cpRowInfoLastTime = now

   --print(string.format("[FA_DEBUG] HEADLAND erkannt: headlandPass=%s isConnecting=%s dist=%.2f",
   --    tostring(bestHeadland.headlandPass), tostring(bestHeadland.isConnecting), math.sqrt(bestHeadland.dist2 or 0)))

    return bestHeadland
end

    ------------------------------------------------
    -- 🔥 ORIGINAL LOGIK (UNVERÄNDERT)
    ------------------------------------------------

    if nearestHeadlandPass ~= nil then
        local result = {
            row = nearestRow,
            headlandPass = nearestHeadlandPass,
            isHeadland = true,
            wpIx = nearestIx,
            dist2 = nearestDist2,
            isConnecting = isConnecting,
            firstRow = firstRow,
            lastRow = lastRow
        }

       -- print(string.format("[FA_DEBUG] NEAREST ist Headland: wpIx=%d headlandPass=%s isConnecting=%s dist=%.2f",
       --     nearestIx, tostring(nearestHeadlandPass), tostring(isConnecting), math.sqrt(nearestDist2 or 0)))

        spec._cpRowInfoCache = result
        spec._cpRowInfoLastTime = now
        return result
    end

    if nearestRow ~= nil then
        local result = {
            row = nearestRow,
            headlandPass = nil,
            isHeadland = false,
            wpIx = nearestIx,
            dist2 = nearestDist2,
            isConnecting = isConnecting,
            firstRow = firstRow,
            lastRow = lastRow
        }

        --print(string.format("[FA_DEBUG] NEAREST ist FELD: wpIx=%d row=%s isConnecting=%s dist=%.2f",
        --    nearestIx, tostring(nearestRow), tostring(isConnecting), math.sqrt(nearestDist2 or 0)))

        spec._cpRowInfoCache = result
        spec._cpRowInfoLastTime = now
        return result
    end

    ------------------------------------------------
    -- 🔁 Fallback links/rechts
    ------------------------------------------------
    for offset = 1, numWp do
        local leftIx = nearestIx - offset
        local rightIx = nearestIx + offset

        if leftIx >= 1 then
            local wp = course:getWaypoint(leftIx)
            if wp then
                local row = FA_Courseplay.cpGetRowNumber(wp)
                local headlandPass = FA_Courseplay.cpGetHeadlandPassNumber(wp)

                if row ~= nil or headlandPass ~= nil then
                    local result = {
                        row = row,
                        headlandPass = headlandPass,
                        wpIx = leftIx,
                        dist2 = nearestDist2,
                        isHeadland = (headlandPass ~= nil),
                        isConnecting = FA_Courseplay.cpIsHeadland(wp),
                        firstRow = firstRow,
                        lastRow = lastRow
                    }

                    spec._cpRowInfoCache = result
                    spec._cpRowInfoLastTime = now
                    return result
                end
            end
        end

        if rightIx <= numWp then
            local wp = course:getWaypoint(rightIx)
            if wp then
                local row = FA_Courseplay.cpGetRowNumber(wp)
                local headlandPass = FA_Courseplay.cpGetHeadlandPassNumber(wp)

                if row ~= nil or headlandPass ~= nil then
                    local result = {
                        row = row,
                        headlandPass = headlandPass,
                        wpIx = rightIx,
                        dist2 = nearestDist2,
                        isHeadland = (headlandPass ~= nil),
                        isConnecting = FA_Courseplay.cpIsHeadland(wp),
                        firstRow = firstRow,
                        lastRow = lastRow
                    }

                    spec._cpRowInfoCache = result
                    spec._cpRowInfoLastTime = now
                    return result
                end
            end
        end
    end

    spec._cpRowInfoCache = nil
    spec._cpRowInfoLastTime = now
    return nil
end

function FA_Courseplay.getCourseplayHeadlandCount(self)
      local root = self.rootVehicle or self

    if not FA_Courseplay.hasCourseplay(self) or not root:hasCpCourse() then
        return 0
    end

    local course = root:getFieldWorkCourse()
    if not course or not course.getNumberOfWaypoints or not course.getWaypoint then
        return 0
    end

    local numWp = course:getNumberOfWaypoints()
    local maxHeadlandPass = 0

    for i = 1, numWp do
        local wp = course:getWaypoint(i)
        if wp ~= nil then
            local hp = FA_Courseplay.cpGetHeadlandPassNumber(wp)
            if hp ~= nil and hp > maxHeadlandPass then
                maxHeadlandPass = hp
            end
        end
    end
    return maxHeadlandPass
end

function FA_Courseplay.GetWaypointPos2D(wp)
    if wp ~= nil and wp.getPosition ~= nil then
        local x, _, z = wp:getPosition()
        return x, z
    end
    return nil
end
function FA_Courseplay.cpBuildReferenceLine(self)
    local root = self.rootVehicle or self

    if not FA_Courseplay.hasCourseplay(self) or not root:hasCpCourse() then
        return nil
    end

    local course = root:getFieldWorkCourse()
    if not course or not course.getNumberOfWaypoints or not course.getWaypoint then
        return nil
    end

    local spec = self[specName]
    spec.cpHeadlandCount = FA_Courseplay.getCourseplayHeadlandCount(self)

    local numWp = course:getNumberOfWaypoints()

    ------------------------------------------------
    -- NUR BLOCK 1 / ROW 1 ALS REFERENZ HOLEN
    ------------------------------------------------
    local row1Points = {}
    local firstRow = nil
    local row1Started = false

    for i = 1, numWp do
        local wp = course:getWaypoint(i)

        if wp and wp.getPosition then
            local row = FA_Courseplay.cpGetRowNumber(wp)

            if row ~= nil and not FA_Courseplay.cpIsHeadland(wp) and not FA_Courseplay.cpIsConnecting(wp) then
                local x, z = FA_Courseplay.GetWaypointPos2D(wp)

                if x ~= nil then
                    -- erste gültige Feld-Row merken
                    if firstRow == nil then
                        firstRow = row
                    end

                    -- nur zusammenhängenden ERSTEN row1-Block sammeln
                    if row == firstRow then
                        -- Winkel-Check: bei >= 90 Grad Knick abbrechen (Wende)
                        -- Der letzte Punkt (pB) gehoert bereits zur Wende
                        -- und wird ebenfalls entfernt.
                        if #row1Points >= 2 then
                            local pA = row1Points[#row1Points - 1]
                            local pB = row1Points[#row1Points]
                            local dx1 = pB.x - pA.x
                            local dz1 = pB.z - pA.z
                            local dx2 = x - pB.x
                            local dz2 = z - pB.z
                            local len1 = math.sqrt(dx1*dx1 + dz1*dz1)
                            local len2 = math.sqrt(dx2*dx2 + dz2*dz2)
                            if len1 > 0.001 and len2 > 0.001 then
                                local dot = (dx1*dx2 + dz1*dz2) / (len1 * len2)
                                -- dot <= 0 bedeutet >= 90 Grad
                                if dot <= 0 then
                                    -- Wendepunkt (pB) auch entfernen
                                    table.remove(row1Points)
                                    break
                                end
                            end
                        end

                        row1Started = true

                        table.insert(row1Points, {
                            x = x,
                            z = z,
                            ix = i
                        })
                    elseif row1Started then
                        -- sobald nach dem ersten row1-Block eine andere Row kommt:
                        -- sofort fertig, NICHT weitere row1-Blöcke sammeln
                        break
                    end
                end
            end
        end
    end

    if #row1Points < 2 then
    --    print("[FA] Row1Ref FAILED: zu wenig Punkte")
        return nil
    end

  -- print(string.format(
  --     "[FA] Row1Ref OK: row=%s points=%d wp=%d-%d",
  --     tostring(firstRow),
  --     #row1Points,
  --     row1Points[1].ix or -1,
  --     row1Points[#row1Points].ix or -1
  -- ))

    return row1Points
end

function FA_Courseplay.cpRebuildSegmentPassMap(self)
    local spec = self.spec_fahrgassenAutomatik
    local root = self.rootVehicle or self

    if not spec then return end

    spec.cpSegments = {}

    if not FA_Courseplay.hasCourseplay(self) or not root:hasCpCourse() then
       -- print("[FA] Kein CP Kurs")
        return
    end

    local course = root:getFieldWorkCourse()
    local laneWidth = course:getWorkWidth()

    local refPoints = FA_Courseplay.cpBuildReferenceLine(self)
    if not refPoints or #refPoints < 2 then
      --  print("[FA] Keine Referenzlinie")
        return
    end

    ------------------------------------------------
    -- 🔥 REF VERLÄNGERN (gegen Start/End Bug)
    ------------------------------------------------
    local extendedRef = {}

    -- Anfang
    do
        local a = refPoints[1]
        local b = refPoints[2]

        local dx = b.x - a.x
        local dz = b.z - a.z
        local len = math.sqrt(dx*dx + dz*dz)

        if len > 0.0001 then
            dx = dx / len
            dz = dz / len

            table.insert(extendedRef, {
                x = a.x - dx * 10000,
                z = a.z - dz * 10000
            })
        end
    end

    -- Original
    for i=1, #refPoints do
        table.insert(extendedRef, refPoints[i])
    end

    -- Ende
    do
        local a = refPoints[#refPoints-1]
        local b = refPoints[#refPoints]

        local dx = b.x - a.x
        local dz = b.z - a.z
        local len = math.sqrt(dx*dx + dz*dz)

        if len > 0.0001 then
            dx = dx / len
            dz = dz / len

            table.insert(extendedRef, {
                x = b.x + dx * 10000,
                z = b.z + dz * 10000
            })
        end
    end

    ------------------------------------------------
    -- SEGMENT BUILD
    ------------------------------------------------
    local segments = {}
    local current = nil

    function FA_Courseplay.closeCurrent()
        if current then table.insert(segments, current) end
        current = nil
    end

    local numWp = course:getNumberOfWaypoints()

    for i=1, numWp do
        local wp = course:getWaypoint(i)

        local isField = false
        local row, x, z = nil, nil, nil

        if wp and wp.getPosition then
            local wx, _, wz = wp:getPosition()
            x, z = wx, wz
            row = FA_Courseplay.cpGetRowNumber(wp)

            isField =
                (not FA_Courseplay.cpIsHeadland(wp)) and
                (not FA_Courseplay.cpIsConnecting(wp)) and
                (row ~= nil)
        end

        if isField then
            if not current then
                current = { startIx=i, endIx=i, row=row, points={} }
            end

            if i - current.endIx > 1 or current.row ~= row then
                FA_Courseplay.closeCurrent()
                current = { startIx=i, endIx=i, row=row, points={} }
            end

            current.endIx = i
            table.insert(current.points, {x=x, z=z})
        else
            FA_Courseplay.closeCurrent()
        end
    end

    FA_Courseplay.closeCurrent()

    ------------------------------------------------
    -- 🔥 DIST (90° PROJECTION AUF POLYLINE)
    ------------------------------------------------
    for segId, seg in ipairs(segments) do

        local p2, p3

        if #seg.points >= 2 then
            p2 = seg.points[1]
            p3 = seg.points[2]
        elseif #seg.points == 1 then
            p2 = seg.points[1]
            p3 = seg.points[1]
        end

        local bestDist = math.huge

        if p2 and p3 then
            local mx = 0.5 * (p2.x + p3.x)
            local mz = 0.5 * (p2.z + p3.z)

            -- Segment-Richtung
            local sx = p3.x - p2.x
            local sz = p3.z - p2.z
            local lenS = math.sqrt(sx*sx + sz*sz)

            if lenS > 0.0001 then
                sx = sx / lenS
                sz = sz / lenS
            end

            -- 90° Normalen
            local nx = -sz
            local nz = sx

            local bestT = math.huge

            local dirs = {
                {nx, nz},
                {-nx, -nz}
            }

            for _, dir in ipairs(dirs) do
                local dxN = dir[1]
                local dzN = dir[2]

                for i=1, #extendedRef-1 do
                    local a = extendedRef[i]
                    local b = extendedRef[i+1]

                    local rx = b.x - a.x
                    local rz = b.z - a.z

                    local denom = dxN * rz - dzN * rx

                    if math.abs(denom) > 0.0001 then
                        local dx = a.x - mx
                        local dz = a.z - mz

                        local t = (dx * rz - dz * rx) / denom
                        local u = (dx * dzN - dz * dxN) / denom

                        if t > 0 and u >= 0 and u <= 1 then
                            if t < bestT then
                                bestT = t
                            end
                        end
                    end
                end
            end

            -- Fallback
            if bestT ~= math.huge then
                bestDist = bestT
            else
                local bestFallback = math.huge

                for i=1, #extendedRef-1 do
                    local a = extendedRef[i]
                    local b = extendedRef[i+1]

                    local vx = b.x - a.x
                    local vz = b.z - a.z
                    local len2 = vx*vx + vz*vz

                    if len2 > 0.0001 then
                        local t = ((mx - a.x)*vx + (mz - a.z)*vz) / len2
                        t = math.max(0, math.min(1, t))

                        local qx = a.x + t * vx
                        local qz = a.z + t * vz

                        local dx = mx - qx
                        local dz = mz - qz
                        local d = math.sqrt(dx*dx + dz*dz)

                        if d < bestFallback then
                            bestFallback = d
                        end
                    end
                end

                bestDist = bestFallback
            end
        end

        ------------------------------------------------
        -- 🔥 SEG DATA (WICHTIG!)
        ------------------------------------------------
        seg.id = segId
        seg.rawDist = bestDist
        seg.absLane = math.floor((bestDist / laneWidth) + 0.5)
        seg.absDist = seg.absLane * laneWidth
    end

    ------------------------------------------------
    -- 🔥 PASS SYSTEM (WICHTIG!)
    ------------------------------------------------
    local minAbsLane = math.huge

    for _, seg in ipairs(segments) do
        if seg.absLane < minAbsLane then
            minAbsLane = seg.absLane
        end
    end

    local startPass = 1
    if spec.cpHeadlandCount and spec.N and spec.N > 0 then
        startPass = (spec.cpHeadlandCount % spec.N) + 1
    end

    for _, seg in ipairs(segments) do
        local laneIndex = (seg.absLane - minAbsLane) + 1

        if laneIndex < 1 then laneIndex = 1 end

        seg.laneIndex = laneIndex
        seg.localDist = (seg.absLane - minAbsLane) * laneWidth
        seg.pass = ((startPass + laneIndex - 2) % spec.N) + 1
    end

    ------------------------------------------------
    -- 🔥 SAVE
    ------------------------------------------------
    spec.cpSegments = segments
    spec.cpRefPoints = refPoints
    spec.cpSegmentLaneWidth = laneWidth
    spec.cpSegmentMinAbsLane = minAbsLane
    spec.cpSegmentStartPass = startPass
    spec.cpSegmentMapValid = true

 --   print(string.format(
 --       "[FA] CP segment map built: segments=%d",
 --       #segments
 --   ))
    return true
end
