ai_utils.lua

-- AI_UTILS.lua - Utility functions for GroundAI FSM system
AI_UTILS = {}

AI_UTILS.general={}

--- Deep copy a table or value (recursive).
---@param object any
---@return any
function AI_UTILS.general.deepCopy(object)
    local lookup_table = {}
    local function _copy(object)
      if type(object) ~= "table" then
        return object
      elseif lookup_table[object] then
        return lookup_table[object]
      end
      local new_table = {}
      lookup_table[object] = new_table
      for index, value in pairs(object) do
        new_table[_copy(index)] = _copy(value)
      end
      return setmetatable(new_table, getmetatable(object))
    end
    return _copy(object)
end

-- Vector operations
AI_UTILS.vector = {}

--- Calculate 2D distance between two points (Vec2 or Vec3).
---@param point1 table
---@param point2 table
---@return number
function AI_UTILS.vector.distance2D(point1, point2)
    -- Accepts Vec2 or Vec3 for either argument (mix allowed)
    -- Vec3: {x, y, z}, Vec2: {x, y}; Vec3.z is Vec2.y
    local function getXZ(p)
        if p.z ~= nil then
            -- Vec3: x, z
            return p.x, p.z
        else
            -- Vec2: x, y
            return p.x, p.y
        end
    end
    local x1, z1 = getXZ(point1)
    local x2, z2 = getXZ(point2)
    return math.sqrt((x2 - x1)^2 + (z2 - z1)^2)
end

--- Calculate 3D distance between two points (Vec3).
---@param point1 table
---@param point2 table
---@return number
function AI_UTILS.vector.distance3D(point1, point2)
    return math.sqrt((point2.x - point1.x)^2 + (point2.y - point1.y)^2 + (point2.z - point1.z)^2)
end

--- Get heading from point1 to point2 (in radians).
---@param point1 table
---@param point2 table
---@return number
function AI_UTILS.vector.heading(point1, point2)
    return math.atan2(point2.z - point1.z, point2.x - point1.x)
end

--- Convert radians to degrees.
---@param radians number
---@return number
function AI_UTILS.vector.toDegrees(radians)
    return radians * (180 / math.PI)
end

--- Convert degrees to radians.
---@param degrees number
---@return number
function AI_UTILS.vector.toRadians(degrees)
    return degrees * (math.PI / 180)
end

--- Normalize a vector to unit length.
---@param vec table
---@return table
function AI_UTILS.vector.normalize(vec)
    local length = math.sqrt(vec.x^2 + vec.y^2 + vec.z^2)
    if length > 0 then
        return {x = vec.x / length, y = vec.y / length, z = vec.z / length}
    else
        return {x = 0, y = 0, z = 0}
    end
end

--- Multiply a vector by a scalar.
---@param vec table
---@param scalar number
---@return table
function AI_UTILS.vector.scale(vec, scalar)
    return {x = vec.x * scalar, y = vec.y * scalar, z = vec.z * scalar}
end

--- Add two vectors.
---@param vec1 table
---@param vec2 table
---@return table
function AI_UTILS.vector.add(vec1, vec2)
    return {x = vec1.x + vec2.x, y = vec1.y + vec2.y, z = vec1.z + vec2.z}
end

--- Subtract vec2 from vec1.
---@param vec1 table
---@param vec2 table
---@return table
function AI_UTILS.vector.subtract(vec1, vec2)
    return {x = vec1.x - vec2.x, y = vec1.y - vec2.y, z = vec1.z - vec2.z}
end

--- Calculate dot product of two vectors.
---@param vec1 table
---@param vec2 table
---@return number
function AI_UTILS.vector.dot(vec1, vec2)
    return vec1.x * vec2.x + vec1.y * vec2.y + vec1.z * vec2.z
end

--- Rotate formation offset by heading (2D).
---@param offset table
---@param heading number
---@return table
function AI_UTILS.vector.rotateOffset2D (offset, heading)
    local cosH = math.cos(heading)
    local sinH = math.sin(heading)
    return {
        x = offset.x * cosH - (offset.z or 0) * sinH,
        y = offset.y or 0,
        z = offset.x * sinH + (offset.z or 0) * cosH
    }
end

-- Target acquisition and visibility checks
AI_UTILS.target = {}

--- Check if target is visible based on terrain and range.
---@param fromUnit Unit
---@param toUnit Unit
---@param maxRange? number
---@return boolean
function AI_UTILS.target.isVisible(fromUnit, toUnit, maxRange)
    maxRange = maxRange or 4000 -- Default maximum visibility range in meters

    -- Get positions
    local fromPos = fromUnit:getPosition().p
    local toPos = toUnit:getPosition().p

    -- Check if target is within maximum range
    local distance = AI_UTILS.vector.distance3D(fromPos, toPos)
    if distance > maxRange then
        return false
    end

    -- Elevate positions by 1 meter to simulate eye level
    fromPos = {x = fromPos.x, y = fromPos.y + 1, z = fromPos.z}
    toPos = {x = toPos.x, y = toPos.y + 1, z = toPos.z}

    -- Check line of sight using DCS terrain raycast
    local visible = land.isVisible(fromPos, toPos)

    if visible then
        -- Flatter range curve for more reliable detection
        local rangeModifier = 1 - (distance / maxRange)^0.8  -- Much flatter curve

        -- Target speed modifier
        local targetVel = toUnit:getVelocity()
        local speed = math.sqrt(targetVel.x^2 + targetVel.y^2 + targetVel.z^2)
        local speedModifier = math.min(1, speed / 20) * 0.4 -- Max 40% boost for fast targets

        -- Target size modifier
        local sizeModifier = 0.8 -- Higher base for vehicles
        local unitType = toUnit:getTypeName()
        if string.find(unitType, "Tank") or string.find(unitType, "IFV") then
            sizeModifier = 1.0
        elseif string.find(unitType, "APC") or string.find(unitType, "Truck") then
            sizeModifier = 0.9
        elseif string.find(unitType, "MANPADS") or string.find(unitType, "Infantry") then
            sizeModifier = 0.5
        end

        -- Calculate final visibility score
        local visibilityScore = rangeModifier * (1 + speedModifier) * sizeModifier

        -- Add randomness
        local randomFactor = math.random() * 0.2 -- Up to 20% random variation
        visibilityScore = visibilityScore * (0.9 + randomFactor)

        -- Debug log for visibility scoring
        if AI_UTILS and AI_UTILS.debug and AI_UTILS.debug.log then
            AI_UTILS.debug.log(string.format(
                "isVisible: dist=%.1f, rangeMod=%.2f, speed=%.1f, speedMod=%.2f, sizeMod=%.2f, score=%.2f", 
                distance, rangeModifier, speed, speedModifier, sizeModifier, visibilityScore
            ), "DEBUG")
        end

        return visibilityScore > 0.15 -- Lowered threshold for easier detection
    end

    return false
end

--- Calculate lead for moving targets (master function).
---@param shooterPos table
---@param targetUnit Unit
---@param projectileSpeed number
---@return table
function AI_UTILS.target.calculateLead(shooterPos, targetUnit, projectileSpeed)
    local targetPos = targetUnit:getPosition().p
    local targetVel = targetUnit:getVelocity()
    
    -- Check if target is airborne
    local targetDesc = targetUnit:getDesc()
    local isAirborne = targetDesc and (targetDesc.category == Unit.Category.AIRPLANE or 
                                       targetDesc.category == Unit.Category.HELICOPTER)
    
    if isAirborne then
        return AI_UTILS.target.calculateAirLead(shooterPos, targetPos, targetVel, projectileSpeed, targetDesc)
    else
        return AI_UTILS.target.calculateGroundLead(shooterPos, targetPos, targetVel, projectileSpeed)
    end
end

--- Calculate lead for airborne targets.
---@param shooterPos table
---@param targetPos table
---@param targetVel table
---@param projectileSpeed number
---@param targetDesc table
---@return table
function AI_UTILS.target.calculateAirLead(shooterPos, targetPos, targetVel, projectileSpeed, targetDesc)
    -- Calculate relative position
    local relPos = AI_UTILS.vector.subtract(targetPos, shooterPos)
    
    -- Determine if target is helicopter or fixed-wing
    local isHelicopter = targetDesc.category == Unit.Category.HELICOPTER
    
    -- Get 3D speed
    local speed = math.sqrt(targetVel.x^2 + targetVel.y^2 + targetVel.z^2)
    
    -- Adjust predictions based on aircraft type
    local predictMultiplier = 1.0
    local maneuverFactor = 0.0
    
    if isHelicopter then
        -- Helicopters can hover and change direction quickly but are slower
        predictMultiplier = 0.8
        maneuverFactor = 0.3
    else
        -- Fixed-wing aircraft are faster but have more momentum
        predictMultiplier = 1.2
        maneuverFactor = 0.1
    end
    
    -- Calculate distance and basic time to intercept
    local distance = AI_UTILS.vector.distance3D(targetPos, shooterPos)
    local timeToIntercept = distance / projectileSpeed * predictMultiplier
    
    -- For fast moving targets at long distances, account for projectile travel time
    local iterations = 1
    if speed > 100 and distance > 3000 then
        iterations = 3  -- Use iterative refinement for fast targets
    end
    
    local leadPos = {x = targetPos.x, y = targetPos.y, z = targetPos.z}
    
    -- Iteratively refine the lead calculation
    for i = 1, iterations do
        -- Calculate where target will be after timeToIntercept
        leadPos = {
            x = targetPos.x + (targetVel.x * timeToIntercept),
            y = targetPos.y + (targetVel.y * timeToIntercept),
            z = targetPos.z + (targetVel.z * timeToIntercept)
        }
        
        -- Recalculate distance to updated lead position
        local newDistance = AI_UTILS.vector.distance3D(leadPos, shooterPos)
        timeToIntercept = newDistance / projectileSpeed * predictMultiplier
    end
    
    -- Add some maneuver prediction for agile aircraft
    -- Assume target might slightly alter course during projectile flight
    if maneuverFactor > 0 then
        -- Predict potential evasive maneuver based on current velocity direction
        local normVel = AI_UTILS.vector.normalize(targetVel)
        local perpVel = {
            x = -normVel.z, 
            y = 0,
            z = normVel.x
        }
        
        -- Add small random component to account for possible maneuvers
        local randomDir = math.random() > 0.5 and 1 or -1
        leadPos.x = leadPos.x + perpVel.x * speed * timeToIntercept * maneuverFactor * randomDir
        leadPos.z = leadPos.z + perpVel.z * speed * timeToIntercept * maneuverFactor * randomDir
    end
    
    -- Return calculated lead position without accuracy variance
    return leadPos
end

--- Calculate lead for ground targets.
---@param shooterPos table
---@param targetPos table
---@param targetVel table
---@param projectileSpeed number
---@return table
function AI_UTILS.target.calculateGroundLead(shooterPos, targetPos, targetVel, projectileSpeed)
    -- Calculate 2D velocity (ignore y component for ground units)
    local vel2D = {x = targetVel.x, z = targetVel.z}
    local speed2D = math.sqrt(vel2D.x^2 + vel2D.z^2)
    
    -- Calculate basic time to intercept
    local distance = AI_UTILS.vector.distance3D(targetPos, shooterPos)
    local timeToIntercept = distance / projectileSpeed
    
    -- Calculate 2D position after projectile flight time
    local leadPos2D = {
        x = targetPos.x + (vel2D.x * timeToIntercept),
        z = targetPos.z + (vel2D.z * timeToIntercept)
    }
    
    -- Get terrain height at predicted position
    local terrainHeight = land.getHeight({x=leadPos2D.x, y=leadPos2D.z})
    
    -- Calculate final lead position with correct height based on terrain
    local leadPos = {
        x = leadPos2D.x,
        y = terrainHeight + 1.5, -- Add small offset to hit center of unit instead of ground
        z = leadPos2D.z
    }
    
    -- Return calculated lead position without accuracy variance
    return leadPos
end

-- Group and waypoint management
AI_UTILS.group = {}

--- Extract waypoints from original mission for a given group.
---@param groupName string
---@return table|nil
function AI_UTILS.group.getOriginalWaypoints(groupName)
    local mission = env.mission
    if not mission or not mission.coalition then
        return nil
    end
    
    -- Search through all coalitions and countries
    for _, coalition in pairs(mission.coalition) do
        if type(coalition) == "table" and coalition.country then
            for _, country in pairs(coalition.country) do
                if type(country) == "table" then
                    -- Search ground groups
                    if country.vehicle and country.vehicle.group then
                        for _, group in pairs(country.vehicle.group) do
                            if group.name == groupName then
                                return group.route.points
                            end
                        end
                    end
                    -- Search ship groups
                    if country.ship and country.ship.group then
                        for _, group in pairs(country.ship.group) do
                            if group.name == groupName then
                                return group.route.points
                            end
                        end
                    end
                end
            end
        end
    end
    
    return nil
end

--- Select a group leader based on unit positions and types.
---@param units table
---@return Unit|nil
function AI_UTILS.group.selectLeader(units)
    -- Default to first unit if no better criteria
    if #units == 0 then
        return nil
    end
    
    -- Prefer command vehicles if available
    for _, unit in ipairs(units) do
        local unitType = unit:getTypeName()
        if string.find(unitType, "Command") or 
           string.find(unitType, "HQ") or 
           string.find(unitType, "CP") then
            return unit
        end
    end
    
    -- Otherwise, select the most capable unit
    local bestUnit = units[1]
    local highestCapability = 0
    
    for _, unit in ipairs(units) do
        local unitType = unit:getTypeName()
        local capability = 1 -- Default value
        
        -- Assign capability values based on unit type
        if string.find(unitType, "Tank") then
            capability = 5
        elseif string.find(unitType, "IFV") or string.find(unitType, "BMP") or string.find(unitType, "Bradley") then
            capability = 4
        elseif string.find(unitType, "APC") then
            capability = 3
        elseif string.find(unitType, "MLRS") or string.find(unitType, "Artillery") then
            capability = 2
        end
        
        if capability > highestCapability then
            highestCapability = capability
            bestUnit = unit
        end
    end
    
    return bestUnit
end

-- Formation calculations for unit positioning
AI_UTILS.formation = {}

-- Formation types
AI_UTILS.formation.FORMATIONS = {
    LINE = "LINE",
    COLUMN = "COLUMN",
    WEDGE = "WEDGE",
    DIAMOND = "DIAMOND",
    VEE = "VEE"
}

--- Calculate relative offsets for units in a line formation.
---@param numUnits number
---@param spacing? number
---@return table
function AI_UTILS.formation.line(numUnits, spacing)
    if numUnits % 2 == 0 then numUnits = numUnits + 1 end
    spacing = spacing or 50
    local offsets = {}
    offsets[1] = {x = 0, y = 0, z = 0} -- Leader always at center
    if numUnits == 1 then return offsets end
    local followerIndex = 2
    local half = math.floor((numUnits - 1) / 2)
    for i = 1, half do
        offsets[followerIndex] = {x = 0, y = 0, z = i * spacing}
        followerIndex = followerIndex + 1
        offsets[followerIndex] = {x = 0, y = 0, z = -i * spacing}
        followerIndex = followerIndex + 1
    end
    -- If even number of units, one extra follower will be on the positive side
    if (numUnits - 1) % 2 == 1 then
        offsets[followerIndex] = {x = 0, y = 0, z = (half + 1) * spacing}
    end
    return offsets
end

--- Calculate relative offsets for units in a wedge formation.
---@param numUnits number
---@param spacing? number
---@return table
function AI_UTILS.formation.wedge(numUnits, spacing)
    spacing = spacing or 50
    local offsets = {}
    offsets[1] = {x = 0, y = 0, z = 0} -- Leader at the front
    if numUnits <= 1 then return offsets end

    local placed = 2
    local leftRank = 1
    local rightRank = 1

    while placed <= numUnits do
        -- Right side: first is half spacing behind, then full spacing steps
        if placed <= numUnits then
            if rightRank == 1 then
                offsets[placed] = {x = -0.5 * spacing, y = 0, z = spacing}
            else
                local prev = offsets[placed - 2] -- previous right-side tank
                offsets[placed] = {
                    x = prev.x - spacing,
                    y = 0,
                    z = prev.z + spacing
                }
            end
            placed = placed + 1
            rightRank = rightRank + 1
        end
        -- Left side: always full spacing behind and left from previous left-side tank
        if placed <= numUnits then
            if leftRank == 1 then
                offsets[placed] = {x = -spacing, y = 0, z = -spacing}
            else
                local prev = offsets[placed - 2] -- previous left-side tank
                offsets[placed] = {
                    x = prev.x - spacing,
                    y = 0,
                    z = prev.z - spacing
                }
            end
            placed = placed + 1
            leftRank = leftRank + 1
        end
    end
    return offsets
end

--- Calculate relative offsets for units in a column formation.
---@param numUnits number
---@param spacing? number
---@return table
function AI_UTILS.formation.column(numUnits, spacing)
    spacing = spacing or 50
    local offsets = {}
    for i = 1, numUnits do
        offsets[i] = {x = -(i-1)*spacing, y = 0, z = 0}
    end
    return offsets
end

--- Calculate relative offsets for units in a diamond formation.
---@param numUnits number
---@param spacing? number
---@return table
function AI_UTILS.formation.diamond(numUnits, spacing)
    spacing = spacing or 50
    local offsets = {}
    if numUnits < 1 then return offsets end

    -- Always put leader at the tip (origin)
    offsets[1] = {x = 0, y = 0, z = 0}
    if numUnits == 1 then return offsets end

    -- Determine how many segments per edge (units per edge minus 1)
    local edgeSegments = 1
    if numUnits > 4 then
        edgeSegments = math.ceil((numUnits - 4) / 4) + 1
    end
    local edgeLength = edgeSegments * spacing

    -- Calculate the three other corners relative to the leader at (0,0,0)
    local left  = {x = -edgeLength/2, y = 0, z = edgeLength/2}
    local rear  = {x = -edgeLength, y = 0, z = 0}
    local right = {x = -edgeLength/2, y = 0, z = -edgeLength/2}

    -- Place corners: left, rear, right
    local placed = 2
    offsets[placed] = left;  placed = placed + 1
    if placed > numUnits then return offsets end
    offsets[placed] = rear;  placed = placed + 1
    if placed > numUnits then return offsets end
    offsets[placed] = right; placed = placed + 1
    if placed > numUnits then return offsets end

    -- Place edge units (between corners)
    local corners = {offsets[1], left, rear, right}
    local edgeMap = {
        {1,2}, -- Tip to Left
        {2,3}, -- Left to Rear
        {3,4}, -- Rear to Right
        {4,1}, -- Right to Tip
    }
    for edgeIdx = 1, 4 do
        local fromIdx, toIdx = edgeMap[edgeIdx][1], edgeMap[edgeIdx][2]
        local fromCorner, toCorner = corners[fromIdx], corners[toIdx]
        for seg = 1, edgeSegments-1 do
            if placed > numUnits then break end
            local t = seg / edgeSegments
            local x = fromCorner.x + (toCorner.x - fromCorner.x) * t
            local z = fromCorner.z + (toCorner.z - fromCorner.z) * t
            offsets[placed] = {x = x, y = 0, z = z}
            placed = placed + 1
        end
        if placed > numUnits then break end
    end

    -- Place any remaining units inside the diamond
    local remaining = numUnits - (placed - 1)
    if remaining > 0 then
        if remaining == 1 then
            offsets[placed] = {x = 0, y = 0, z = edgeLength/2}
        elseif remaining == 2 then
            offsets[placed] = {x = -edgeLength/4, y = 0, z = edgeLength/4}
            offsets[placed+1] = {x = edgeLength/4, y = 0, z = edgeLength/4}
        elseif remaining == 3 then
            offsets[placed] = {x = 0, y = 0, z = edgeLength/2}
            offsets[placed+1] = {x = -edgeLength/6, y = 0, z = edgeLength/3}
            offsets[placed+2] = {x = edgeLength/6, y = 0, z = edgeLength/3}
        else
            for i = 0, remaining-1 do
                local frac = (i+1)/(remaining+1)
                offsets[placed+i] = {x = (frac-0.5)*edgeLength/1.5, y = 0, z = edgeLength*frac}
            end
        end
    end

    return offsets
end

--- Calculate relative offsets for units in a vee formation.
---@param numUnits number
---@param spacing? number
---@return table
function AI_UTILS.formation.vee(numUnits, spacing)
    if numUnits % 2 == 0 then numUnits = numUnits + 1 end
    spacing = spacing or 50
    local offsets = {}
    offsets[1] = {x = 0, y = 0, z = 0} -- Leader at the rear (vee points forward)
    if numUnits == 1 then return offsets end
    local placed = 2
    local rank = 1
    while placed <= numUnits do
        -- Left side (ahead and left)
        if placed <= numUnits then
            offsets[placed] = {x = rank * spacing, y = 0, z = -rank * spacing}
            placed = placed + 1
        end
        -- Right side (ahead and right)
        if placed <= numUnits then
            offsets[placed] = {x = rank * spacing, y = 0, z = rank * spacing}
            placed = placed + 1
        end
        rank = rank + 1
    end
    return offsets
end

--- Get offsets for any formation type.
---@param formationType string
---@param numUnits number
---@param spacing? number
---@return table
function AI_UTILS.formation.getOffsets(formationType, numUnits, spacing)
    if formationType == 'LINE' then
        return AI_UTILS.formation.line(numUnits, spacing)
    elseif formationType == 'COLUMN' then
        return AI_UTILS.formation.column(numUnits, spacing)
    elseif formationType == 'WEDGE' then
        return AI_UTILS.formation.wedge(numUnits, spacing)
    elseif formationType == 'DIAMOND' then
        return AI_UTILS.formation.diamond(numUnits, spacing)
    elseif formationType == 'VEE' then
        return AI_UTILS.formation.vee(numUnits, spacing)
    else
        GroundAI.log("Invalid formation type: " .. tostring(formationType), 2, "FORMATION")
        return AI_UTILS.formation.wedge(numUnits, spacing)
    end
end

function AI_UTILS.formation.isOnRoadWaypoint(wp)
    return wp and wp.action == "On Road"
end

AI_UTILS.terrain = {}

function AI_UTILS.terrain.getClosestRoadPoint(pos)
    local x, y
    if pos.z then
        x,y = land.getClosestPointOnRoads("roads",pos.x,pos.z) 
    else
        x,y = land.getClosestPointOnRoads("roads",pos.x,pos.y) 
    end
    if x and y then
        return {x = x, y = y}
    end

    return pos
end


-- Tactical position finding for combat
AI_UTILS.tactics = {}

--- Find cover position near a given point.
---@param position table
---@param enemyPosition table
---@param radius? number
---@param step? number
---@return table
function AI_UTILS.tactics.findCoverPosition(position, enemyPosition, radius, step)
    radius = radius or 300 -- Default search radius
    step = step or 30      -- Default step size for search grid
    
    local bestPosition = nil
    local bestCoverValue = -1
    
    -- Direction from position to enemy
    local dirToEnemy = AI_UTILS.vector.normalize(AI_UTILS.vector.subtract(enemyPosition, position))
    
    -- Search in a grid pattern
    for x = -radius, radius, step do
        for z = -radius, radius, step do
            -- Skip points too far from search center
            if x*x + z*z <= radius*radius then
                local testPos = {
                    x = position.x + x,
                    y = position.y,
                    z = position.z + z
                }
                
                -- Get surface height at this position
                testPos.y = land.getHeight({x=testPos.x, y=testPos.z}) + 2
                
                -- Test line of sight from this position to enemy
                local visible = land.isVisible(testPos, enemyPosition)
                
                -- If not visible from enemy, this might be good cover
                if not visible then
                    -- Calculate distance from original position
                    local dist = AI_UTILS.vector.distance2D(position, testPos)
                    
                    -- Calculate value as a combination of being close to original position
                    -- but in the opposite direction from the enemy
                    local dirToTest = AI_UTILS.vector.normalize(AI_UTILS.vector.subtract(testPos, position))
                    local dotProduct = AI_UTILS.vector.dot(dirToEnemy, dirToTest)
                    
                    -- Best cover is behind us relative to enemy and close by
                    local coverValue = (1 - dist / radius) * (1 - dotProduct) * 0.5
                    
                    if coverValue > bestCoverValue then
                        bestCoverValue = coverValue
                        bestPosition = testPos
                    end
                end
            end
        end
    end
    
    -- If no cover found, return a position away from the enemy
    if not bestPosition then
        bestPosition = {
            x = position.x - dirToEnemy.x * radius * 0.5,
            y = land.getHeight({x=position.x - dirToEnemy.x * radius * 0.5, y=position.z - dirToEnemy.z * radius * 0.5}),
            z = position.z - dirToEnemy.z * radius * 0.5
        }
    end
    
    return bestPosition
end

--- Find a good firing position.
---@param position table
---@param enemyPosition table
---@param radius? number
---@param step? number
---@return table
function AI_UTILS.tactics.findFiringPosition(position, enemyPosition, radius, step)
    radius = radius or 200 -- Default search radius
    step = step or 25      -- Default step size for search grid
    
    local bestPosition = nil
    local bestValue = -1
    
    -- Search in a grid pattern
    for x = -radius, radius, step do
        for z = -radius, radius, step do
            -- Skip points too far from search center
            if x*x + z*z <= radius*radius then
                local testPos = {
                    x = position.x + x,
                    y = position.y,
                    z = position.z + z
                }
                
                -- Get surface height at this position
                testPos.y = land.getHeight({x=testPos.x, y=testPos.z})
                
                -- Test line of sight to enemy
                local visible = land.isVisible(testPos, enemyPosition)
                
                -- Good firing position needs visibility to enemy
                if visible then
                    -- Calculate distance from original position and enemy
                    local distFromOrigin = AI_UTILS.vector.distance2D(position, testPos)
                    local distToEnemy = AI_UTILS.vector.distance2D(testPos, enemyPosition)
                    
                    -- Calculate elevation advantage (higher is better)
                    local elevationAdvantage = math.max(0, testPos.y - enemyPosition.y) / 50
                    
                    -- Balance between not moving too far, having good range to target, and elevation
                    local positionValue = (1 - distFromOrigin / radius) * 0.3 + 
                                         (1 - math.min(1, distToEnemy / 2000)) * 0.4 + 
                                         elevationAdvantage * 0.3
                    
                    if positionValue > bestValue then
                        bestValue = positionValue
                        bestPosition = testPos
                    end
                end
            end
        end
    end
    
    -- If no good position found, return original position
    if not bestPosition then
        bestPosition = {
            x = position.x,
            y = position.y,
            z = position.z
        }
    end
    
    return bestPosition
end

-- Helper functions for FSM debugging and logging
AI_UTILS.debug = {}

--- Log function with optional log level.
---@param message string
---@param level? string
function AI_UTILS.debug.log(message, level)
    level = level or "INFO"
    
    local logPrefix = "[GroundAI] [" .. level .. "] "
    --trigger.action.outText(logPrefix .. message, 5)
end

--- Format position for logging.
---@param pos table
---@return string
function AI_UTILS.debug.formatPosition(pos)
    if not pos then return "nil" end
    return string.format("(%.1f, %.1f, %.1f)", pos.x, pos.y, pos.z)
end

--- Format vector for logging.
---@param vec table
---@return string
function AI_UTILS.debug.formatVector(vec)
    if not vec then return "nil" end
    return string.format("(%.1f, %.1f, %.1f)", vec.x, vec.y, vec.z)
end

--- Dump table for debugging.
---@param tbl table
---@param indent? number
---@return string
function AI_UTILS.debug.dumpTable(tbl, indent)
    if not tbl then return "nil" end
    if type(tbl) ~= "table" then return tostring(tbl) end
    
    indent = indent or 0
    local spaces = string.rep("  ", indent)
    local result = "{\n"
    
    for k, v in pairs(tbl) do
        result = result .. spaces .. "  " .. tostring(k) .. " = "
        
        if type(v) == "table" then
            result = result .. AI_UTILS.debug.dumpTable(v, indent + 1)
        else
            result = result .. tostring(v)
        end
        
        result = result .. ",\n"
    end
    
    result = result .. spaces .. "}"
    return result
end

AI_UTILS.AI = {}
-- Task templates for DCS AI tasks
AI_UTILS.AI.taskTemplates = {
    FireAtPoint = {
        id = 'FireAtPoint',
        params = {
            point = {x = 0, y = 0},
            radius = 0,
            expendQty = 1,
            expendQtyEnabled = false,
            altitude = 0,
            alt_type = 0
        }
    },
    Mission = {
        id = 'Mission',
        params = {
            route = {
                points = {}
            }
        }
    },
    AttackUnit = {
        id = 'AttackUnit',
        params = {
            unitId = 0,
            weaponType = 0,
            groupAttack = true
        }
    },
    Hold = {
        id = 'Hold',
        params = {}
    },
    EmbarkToTransport = {
        id = 'EmbarkToTransport',
        params = {
            transportGroupId = 0
        }
    },
    DisembarkFromTransport = {
        id = 'DisembarkFromTransport',
        params = {}
    },
    -- SetCommand templates
    setInvisible = {
        id = 'SetInvisible',
        params = { value = true }
    },
    setVisible = {
        id = 'SetInvisible',
        params = { value = false }
    },
    setImmortal = {
        id = 'SetImmortal',
        params = { value = true }
    },
    setMortal = {
        id = 'SetImmortal',
        params = { value = false }
    },
    stopRoute = {
        id = 'StopRoute',
        params = { value = true }
    },
    resumeRoute = {
        id = 'StopRoute',
        params = { value = false }
    },
    activateBeacon = {
        id = 'ActivateBeacon',
        params = {
            type = 0, -- e.g. 0 for VOR, 1 for ILS, etc.
            system = 0, -- e.g. 0 for VOR, 1 for ILS, etc.
            channel = 0,
            modeChannel = 0,
            callsign = "",
            frequency = 0
        }
    },
    deactivateBeacon = {
        id = 'DeactivateBeacon',
        params = {}
    }
}

--- Assign a waypoint to a unit (DCS Mission task).
---@param unit Unit
---@param position table
---@param speed? number
---@param onRoad? boolean
function AI_UTILS.AI.assignWaypoint(unit, position, speed, onRoad)
    if not unit:isExist() then return end
    speed = speed or 3
    local action = onRoad and "On Road" or "Off Road"
    -- Debug: log waypoint assignment
    if GroundAI and GroundAI.DEBUG and GroundAI.DEBUG.FORMATION then
        local unitName = unit.getName and unit:getName() or "<unknown>"
        GroundAI.log("assignWaypoint for " .. unitName .. " to (x=" .. (position.x or "nil") .. ", y=" .. (position.y or "nil") .. ", speed=" .. tostring(speed) .. ", action=" .. action .. ")", 4, "FORMATION")
    end
    -- Build a two-point route: current position and target position
    local currentPos = unit:getPosition().p
    local task = {
        id = 'Mission',
        params = {
            route = {
                points = {
                    [1] = {
                        x = currentPos.x,
                        y = currentPos.z, -- DCS: y is north-south
                        type = 'Turning Point',
                        action = action,
                        speed = speed,
                        formation_template = '',
                        name = 'Start'
                    },
                    [2] = {
                        x = position.x,
                        y = position.z or position.y, -- DCS: y is north-south, use z if present, else y
                        type = 'Turning Point',
                        action = action,
                        speed = speed,
                        formation_template = '',
                        name = 'WP1'
                    }
                }
            }
        }
    }
    unit:getController():setTask(task)
end

return AI_UTILS