turret2

-- Constants for cannon and base positions
local cannon = {x = 157.5, y = 74.5, z = -13.5}
local defaultYawOffset = 0 -- Adjust this value based on the cannon's rotation (e.g., 90° for West)
local overshootTolerance = 1 -- Allowed overshoot tolerance for yaw and pitch
local detectionRange = 30 -- Detection range for player detector

-- Peripheral assignments
local yaw = peripheral.wrap("left") -- Rotation Speed Controller for Yaw
local pitch = peripheral.wrap("right") -- Rotation Speed Controller for Pitch
local entDet = peripheral.wrap("playerDetector_0") -- Player Detector
local blockReader = peripheral.wrap("blockReader_2") -- Block Reader
local mon = peripheral.find("monitor")

-- Relative angles for target
local relYaw = 0
local relPitch = 0

-- Helper function: Dynamically adjust speed for pitch based on distance to target
local function adjustSpeedPitch(current, target)
    local difference = target - current
    local speed = math.min(math.abs(difference) * 5, 60) -- Scales speed dynamically
    return speed * (difference > 0 and -1 or 1) -- Adjust direction
end

-- Helper function: Dynamically adjust speed for yaw based on distance to target
local function adjustSpeedYaw(current, target)
    local difference = target - current
    print("Target "..target.." Current "..current)
    local speed = math.min(math.abs(difference) * 5, 60) -- Scales speed dynamically
    print("Diff "..(difference > 0 and -1 or 1))
    print("Speed "..speed)
    return speed * (difference > 0 and -1 or 1) -- Adjust direction
end

-- Function to handle Yaw adjustment
local function setYaw()
    local canYaw = blockReader.getBlockData().CannonYaw
    print("relYaw "..relYaw)
    print("canYaw "..canYaw)
    while math.abs(canYaw - relYaw) >= overshootTolerance do
        local speed = adjustSpeedYaw(canYaw, relYaw)
        yaw.setTargetSpeed(speed)
        sleep(0.1) -- Prevent CPU overload
        canYaw = blockReader.getBlockData().CannonYaw
    end
    yaw.setTargetSpeed(0) -- Stop rotation
end

-- Function to handle Pitch adjustment
local function setPitch()
    local canPitch = blockReader.getBlockData().CannonPitch
    while math.abs(canPitch - relPitch) >= overshootTolerance do
        local speed = adjustSpeedPitch(canPitch, relPitch)
        pitch.setTargetSpeed(speed)
        sleep(0.1)
        canPitch = blockReader.getBlockData().CannonPitch
    end
    pitch.setTargetSpeed(0)
end

-- Function to detect and assign a target
local function checkForNewTarget()
    for _, entity in ipairs(entDet.getPlayersInRange(detectionRange)) do
        if entity ~= "NeuGoga1" then
            return entity
        end
    end
    return nil
end

-- Function to calculate angles for target
local function calculateAngles(targetX, targetY, targetZ)
    local dx, dy, dz = targetX - cannon.x, targetY - cannon.y, targetZ - cannon.z
    local distance = math.sqrt(dx^2 + dy^2 + dz^2)

    relPitch = math.deg(math.asin(dy / distance)) -- Calculate relative pitch
    local rawYaw = math.deg(math.atan2(dz, dx)) -- Calculate relative yaw
    -- Adjust for the cannon's default orientation (facing south)
    relYaw = (rawYaw - 90 + 360) % 360

    -- Normalize the yaw to [-180, 180]
    if relYaw > 180 then relYaw = relYaw - 360 end
    if relYaw < -180 then relYaw = relYaw + 360 end

    print("Calculated Yaw: " .. relYaw)
    print("Calculated Pitch: " .. relPitch)
end

-- Main loop to track and move turret
while true do
    local target = checkForNewTarget() -- Check for a new target
    if target then
        local pos = entDet.getPlayerPos(target) -- Get target position
        print(pos.x, pos.y)
        if pos then
            calculateAngles(pos.x, pos.y, pos.z) -- Update angles
            parallel.waitForAll(setYaw, setPitch) -- Move both pitch and yaw simultaneously
        end
    else
        pitch.setTargetSpeed(0)
        yaw.setTargetSpeed(0)
        print("No targets detected.") -- Log when no target is found
        sleep(1) -- Add delay to reduce polling frequency
    end
end