CBC control

--!/rom/programs/startup
-- Main program for controlling a Create Big Cannon with ComputerCraft.
-- This script uses CC: Create Bridge to interact with Sequenced Gearshifts
-- for aiming, and bundled cables for building and firing.

-- Configuration Parameters (YOU MUST EDIT THESE!)
-- These values will depend on your specific cannon build and Minecraft world.
local config = {
    -- Peripheral names for your Sequenced Gearshifts
    -- Use the name given when you wrap it, e.g., "Create_SequencedGearshift_3"
    yawGearshiftSide = "Create_SequencedGearshift_3",
    pitchGearshiftSide = "Create_SequencedGearshift_2",

    -- Bundled cable side for ALL Redstone signals (build/fire/etc.)
    bundledCableSide = "left",

    -- Bundled cable colors (channels) for specific actions
    buildSignalColor = colors.black, -- The color of the channel for building/resetting (on/off)
    fireSignalColor = colors.red,    -- The color of the channel for firing (pulse)

    -- Cannon specific parameters
    barrelLength = 2,           -- Length of the cannon barrel in blocks

    -- Gear Ratio: How many degrees the gearshift needs to rotate for 1 degree of cannon rotation.
    -- If 1 gearshift degree = 0.125 cannon degrees, then 1 cannon degree = 1 / 0.125 = 8 gearshift degrees.
    gearRatio = 8.0,             -- (1.0 / 0.125) Every 1 degree of cannon movement requires 8 degrees of gearshift rotation.

    -- Default cannon orientation offset (for built-in direction)
    -- This value should be the yaw angle (in degrees, 0-360) that your cannon faces
    -- when it's in its default, 'built' orientation (e.g., 0 for North, 90 for East, etc.)
    defaultYawOffset = 180,        -- Example: 0 for North, 90 for East, 180 for South, 270 for West

    -- Offsets in blocks from the COMPUTER'S position to the CANNON'S BARREL TIP
    -- These are crucial for accurate GPS-based trajectory calculations.
    xOffset = 0.0,               -- X-offset (East/West)
    yOffset = 1.0,               -- Y-offset (Up/Down)
    zOffset = 0.0,               -- Z-offset (North/South)

    -- --- PHYSICS PARAMETERS (REQUIRED FOR AUTOMATIC AIMING) ---
    -- You MUST fill these in based on your Create Big Cannons setup.
    -- These are crucial for accurate trajectory calculation.
    projectileVelocity = 40.0,   -- 💥 Initial speed of the projectile in blocks per tick (e.g., 50)
                                 --    (SET THIS VALUE TO YOUR CANNON'S ACTUAL MUZZLE VELOCITY!)
    gravity = 0.08,              -- ⬇️ Minecraft's default gravity, blocks per tick^2 (0.08)
                                 --    Adjust if you have mods changing gravity.
    airResistance = 0.00,        -- 🌬️ Simple drag coefficient (e.g., 0.01-0.05). Set to 0 for no drag.
                                 --    (SET THIS VALUE based on your observation, or leave 0 for no drag!)
    maxPitch = 90.0,             -- ⬆️ Maximum physical pitch angle your cannon can achieve (degrees)
    minPitch = -30,              -- ⬇️ Minimum physical pitch angle your cannon can achieve (degrees)
}

-- Global peripheral variables
local yawGearshift = nil
local pitchGearshift = nil
-- 'redstone' is a global API in CC:Tweaked, no need to peripheral.wrap it.

-- Internal state to track current angles
-- IMPORTANT: These values are tracked by the program, the physical gearshifts
-- will rotate relatively. It's critical that your physical cannon is aligned
-- to its 'defaultYawOffset' and 0 pitch when the program starts or `resetcannon` is used.
local currentYaw = 0.0
local currentPitch = 0.0

-- Internal state for build status
local isCannonBuilt = false

--- Initializes all required peripherals.
-- @return boolean True if all peripherals are found, false otherwise.
local function initializePeripherals()
    print("Initializing peripherals...")
    yawGearshift = peripheral.wrap(config.yawGearshiftSide)
    pitchGearshift = peripheral.wrap(config.pitchGearshiftSide)

    if not yawGearshift then print("Error: Yaw Gearshift '" .. config.yawGearshiftSide .. "' not found. Check name/side and connection.") end
    if not pitchGearshift then print("Error: Pitch Gearshift '" .. config.pitchGearshiftSide .. "' not found. Check name/side and connection.") end

    if yawGearshift and pitchGearshift then
        print("All required peripherals initialized successfully.")
        return true
    else
        print("Failed to initialize one or both gearshift peripherals. Please check your config and in-game setup.")
        return false
    end
end

--- Sends a brief pulse on a specific bundled cable channel.
-- This function uses bitwise operations to safely toggle a single color channel
-- without affecting others on the same bundled cable.
-- @param color The color constant (e.g., `colors.red`) for the channel.
local function pulseBundled(color)
    -- 'redstone' is a global API, so no need for a 'redstonePeripheral' variable.
    local currentOutput = redstone.getBundledOutput(config.bundledCableSide)
    redstone.setBundledOutput(config.bundledCableSide, bit.bor(currentOutput, color))
    os.sleep(0.2) -- Keep the signal active briefly to ensure it registers in Create
    currentOutput = redstone.getBundledOutput(config.bundledCableSide) -- Re-read in case others changed
    redstone.setBundledOutput(config.bundledCableSide, bit.band(currentOutput, bit.bnot(color)))
end

--- Sets the state of a bundled cable channel (on/off).
-- This is used for the 'build' signal which is not a pulse.
-- @param color The color constant for the channel.
-- @param state True for on, false for off.
local function setBundledState(color, state)
    local currentOutput = redstone.getBundledOutput(config.bundledCableSide)
    if state then
        redstone.setBundledOutput(config.bundledCableSide, bit.bor(currentOutput, color))
    else
        redstone.setBundledOutput(config.bundledCableSide, bit.band(currentOutput, bit.bnot(color)))
    end
end

--- Normalizes an angle to be within the -180 to 180 degree range.
-- This helps in calculating the shortest rotation path.
-- @param angle The angle in degrees.
-- @return The normalized angle.
local function normalizeAngle(angle)
    angle = angle % 360
    if angle > 180 then
        angle = angle - 360
    elseif angle <= -180 then
        angle = angle + 360
    end
    return angle
end

--- Gets the cannon's current GPS location, applying configured offsets.
-- The returned coordinates represent the theoretical firing point (barrel tip).
-- @return table A table with x, y, z coordinates, or nil if GPS fails.
local function getCannonLocation()
    print("Attempting to get cannon GPS location...")
    local x, y, z = gps.locate()
    if x then
        -- Apply configured offsets to the base GPS location for precise barrel position
        x = x + config.xOffset
        y = y + config.yOffset
        z = z + config.zOffset
        print(string.format("Cannon Location (with offsets): X:%.2f Y:%.2f Z:%.2f", x, y, z))
        return {x = x, y = y, z = z}
    else
        print("Could not get GPS location. Ensure wireless modems are set up and in range.")
        return nil
    end
end

--- Sets the yaw (horizontal) angle of the cannon using the Sequenced Gearshift.
-- Calculates the relative rotation needed from the current tracked angle to the target.
-- Compensates for the gear ratio: target_cannon_angle * config.gearRatio = gearshift_angle_to_rotate.
-- This function is blocking and waits for the gearshift to stop moving.
-- @param targetCannonAngle The desired absolute yaw angle in degrees (for the cannon).
-- @return boolean True if the command was sent successfully and completed, false otherwise.
local function setYaw(targetCannonAngle)
    if not yawGearshift then
        print("Yaw Gearshift not initialized. Cannot set yaw.")
        return false
    end

    -- Adjust target angle by the cannon's fixed orientation offset
    local adjustedTargetAngle = normalizeAngle(targetCannonAngle - config.defaultYawOffset)

    local deltaCannonAngle = adjustedTargetAngle - currentYaw
    -- Adjust deltaCannonAngle for the shortest path if crossing the -180/180 boundary
    if math.abs(deltaCannonAngle) > 180 then
        deltaCannonAngle = -(360 - math.abs(deltaCannonAngle)) * math.sign(deltaCannonAngle)
    end

    local modifier = 1 -- Default forward rotation for gearshift
    if deltaCannonAngle < 0 then
        modifier = -1 -- Reverse rotation for gearshift
    end

    local cannonAngleToRotate = math.abs(deltaCannonAngle)

    -- If the cannon angle to rotate is very small, we consider it already at target.
    if cannonAngleToRotate < 0.01 then -- Using a small epsilon for floating point comparison
        print(string.format("Yaw already at target %.2f degrees (actual cannon angle).", targetCannonAngle))
        currentYaw = adjustedTargetAngle -- Ensure internal state is precise
        return true
    end

    -- Apply the gear ratio to get the required gearshift rotation
    local gearshiftAngleToRotate = cannonAngleToRotate * config.gearRatio

    -- The 'rotate' method expects a positive integer angle.
    -- Round to the nearest integer, ensuring a minimum of 1 if rotation is needed.
    local roundedGearshiftAngleToRotate = math.max(1, math.floor(gearshiftAngleToRotate + 0.5))

    print(string.format("Setting yaw from %.2f (relative) to %.2f (relative) (rotating gearshift by %d with modifier %d)...",
                        currentYaw, adjustedTargetAngle, roundedGearshiftAngleToRotate, modifier))

    yawGearshift.rotate(roundedGearshiftAngleToRotate, modifier) -- No return value expected

    -- Wait for the gearshift to finish rotating (blocking call)
    while yawGearshift.isRunning() do
        os.sleep(0.1) -- Small delay to avoid busy-waiting
    end
    print("Yaw Gearshift stopped.")

    currentYaw = adjustedTargetAngle -- Update internal state after physical movement is done
    return true
end

--- Sets the pitch (vertical) angle of the cannon using the Sequenced Gearshift.
-- Compensates for the gear ratio. Clamps the target angle to physical min/max pitch.
-- This function is blocking and waits for the gearshift to stop moving.
-- @param targetCannonAngle The desired absolute pitch angle in degrees (for the cannon).
-- @return boolean True if the command was sent successfully and completed, false otherwise.
local function setPitch(targetCannonAngle)
    if not pitchGearshift then
        print("Pitch Gearshift not initialized. Cannot set pitch.")
        return false
    end

    -- Clamp pitch angle to the configured physical range
    targetCannonAngle = math.max(config.minPitch, math.min(config.maxPitch, targetCannonAngle))

    local deltaCannonAngle = targetCannonAngle - currentPitch

    local modifier = 1
    if deltaCannonAngle < 0 then
        modifier = -1
    end

    local cannonAngleToRotate = math.abs(deltaCannonAngle)

    if cannonAngleToRotate < 0.01 then
        print(string.format("Pitch already at target %.2f degrees (actual cannon angle).", targetCannonAngle))
        currentPitch = targetCannonAngle
        return true
    end

    -- Apply the gear ratio to get the required gearshift rotation
    local gearshiftAngleToRotate = cannonAngleToRotate * config.gearRatio

    local roundedGearshiftAngleToRotate = math.max(1, math.floor(gearshiftAngleToRotate + 0.5))

    print(string.format("Setting pitch from %.2f to %.2f (rotating gearshift by %d with modifier %d)...",
                        currentPitch, targetCannonAngle, roundedGearshiftAngleToRotate, modifier))

    pitchGearshift.rotate(roundedGearshiftAngleToRotate, modifier) -- No return value expected

    -- Wait for the gearshift to finish rotating (blocking call)
    while pitchGearshift.isRunning() do
        os.sleep(0.1) -- Small delay to avoid busy-waiting
    end
    print("Pitch Gearshift stopped.")

    currentPitch = targetCannonAngle -- Update internal state after physical movement is done
    return true
end

--- Resets the program's internal angle tracking and physically resets the cannon.
-- This function will toggle the 'build' signal off then on, which you've stated
-- causes the cannon to reset to the default position.
local function resetCannon()
    print("Performing full cannon reset sequence...")
    -- First, set the build signal OFF (to un-build/reset state)
    setBundledState(config.buildSignalColor, false)
    isCannonBuilt = false
    os.sleep(1.0) -- Give Minecraft/Create time to process the un-build/reset

    -- Then, set the build signal ON (to build state, which defaults position)
    setBundledState(config.buildSignalColor, true)
    isCannonBuilt = true
    os.sleep(1.0) -- Give Minecraft/Create time to process the build

    -- Reset internal angle tracking based on the cannon's default orientation
    currentYaw = normalizeAngle(0.0) -- Relative to defaultYawOffset, so 0 is the baseline
    currentPitch = 0.0 -- Assuming 0 pitch is default for reset

    print(string.format("Cannon reset sequence complete. Internal angles reset to Yaw: %.2f (relative), Pitch: %.2f.", currentYaw, currentPitch))
    print("Ensure your physical cannon is aligned with its default (initial) orientation.")
end

--- Toggles the cannon's 'built' state (on/off).
-- If the cannon is currently built, it will un-build it. If not built, it will build it.
-- Toggling from un-built to built should reset its position.
local function toggleBuildState()
    if isCannonBuilt then
        print("Un-building cannon (Black Bundled Cable channel OFF)...")
        setBundledState(config.buildSignalColor, false)
        isCannonBuilt = false
        print("Cannon un-built.")
    else
        print("Building cannon (Black Bundled Cable channel ON)...")
        setBundledState(config.buildSignalColor, true)
        isCannonBuilt = true
        print("Cannon built.")
        -- When building, we assume it snaps to default position
        currentYaw = normalizeAngle(0.0) -- Relative to defaultYawOffset, so 0 is the baseline
        currentPitch = 0.0
        print(string.format("Internal angles reset to Yaw: %.2f (relative), Pitch: %.2f after building.", currentYaw, currentPitch))
    end
end

--- Triggers the cannon to fire.
-- This sends a brief pulse on the red bundled cable channel.
-- @return boolean True if the signal was sent.
local function fireCannon()
    print("Activating cannon fire signal (Red Bundled Cable channel)...")
    pulseBundled(config.fireSignalColor)
    print("Fire signal sent. Expect explosion!")
    return true
end

--- Calculates the required yaw and pitch to hit a target.
-- This is where the physics-based trajectory calculation happens.
-- Uses the provided physics parameters from the config.
-- @param targetX Target X coordinate.
-- @param targetY Target Y coordinate.
-- @param targetZ Target Z coordinate.
-- @return table A table {yaw = angle, pitch = angle} if successful, nil otherwise.
local function calculateTrajectory(targetX, targetY, targetZ)
    local cannonLoc = getCannonLocation()
    if not cannonLoc then
        print("Cannot calculate trajectory: Cannon location unknown.")
        return nil
    end

    local dx = targetX - cannonLoc.x
    local dy = targetY - cannonLoc.y
    local dz = targetZ - cannonLoc.z

    print(string.format("Target relative to cannon (DX:%.2f, DY:%.2f, DZ:%.2f)", dx, dy, dz))

    -- --- ADVANCED PHYSICS CALCULATION ---
    -- Yaw Calculation: Simple arctan2 for horizontal aim.
    -- Assuming Minecraft's coordinate system where +X is East, +Z is South.
    -- Yaw 0 is North, 90 is East, 180 is South, 270 (or -90) is West, increasing clockwise.
    -- math.atan2(y, x) returns angle in radians from positive X axis counter-clockwise.
    -- For Minecraft yaw (0=North, clockwise):
    -- atan2(dx, dz) where dx is targetX-originX (East/West diff), dz is targetZ-originZ (North/South diff)
    -- This gives angle from +Z (South), positive towards +X (East). Need to adjust for North.
    -- angle from North clockwise: if dz > 0 (South), use atan2(dx, dz)
    -- if dz < 0 (North), use atan2(-dx, -dz) + 180 or handle sign change.
    -- Simpler: map dx, dz to standard math.atan2 arguments to get desired orientation.
    -- If 0 degrees is North, 90 East, 180 South, 270 West:
    -- atan2(east_component, north_component) where east is positive X, north is positive Z (if 0 north).
    -- If your map's +Z is South, and +X is East, and 0 yaw is North:
    local calculatedYaw = math.deg(math.atan2(dx, dz)) -- Angle from Z-axis, positive towards X.
    -- Adjust to make 0 degrees true North, increasing clockwise:
    -- math.atan2(y, x) gives angle from +x axis counter-clockwise.
    -- If +Z is North, +X is East, then (X,Z) plane maps to (y,x) in atan2.
    -- To get angle from +Z (North) increasing clockwise: use atan2(dx, dz) and then adjust.
    calculatedYaw = 90 - calculatedYaw -- Adjust for 0 being North and clockwise
    calculatedYaw = normalizeAngle(calculatedYaw) -- Ensure it's in -180 to 180 range for setYaw

    -- Pitch Calculation: Projectile motion in 2D (horizontal distance vs vertical distance)
    local horizontalDistance = math.sqrt(dx^2 + dz^2)

    local calculatedPitch = nil -- Will store the pitch in degrees

    if horizontalDistance < 0.1 then -- Too close, pitch straight up or down (handle edge case)
        if dy > 0 then calculatedPitch = 90.0 else calculatedPitch = config.minPitch end -- Or a negative pitch if cannon allows
    else
        -- Check if projectileVelocity is set to a non-zero value
        if config.projectileVelocity == 0 then
            print("Error: projectileVelocity is 0. Cannot calculate trajectory with physics. Please set 'config.projectileVelocity'.")
            return nil
        end

        -- Quadratic formula for projectile motion (simplified, ignoring air resistance for the first pass):
        -- dy = v0y * t - 0.5 * g * t^2
        -- dx_horizontal = v0x * t
        -- v0y = v0 * sin(theta)
        -- v0x = v0 * cos(theta)
        -- horizontalDistance = v0 * cos(theta) * t  =>  t = horizontalDistance / (v0 * cos(theta))

        -- Substitute t into dy equation:
        -- dy = v0 * sin(theta) * (horizontalDistance / (v0 * cos(theta))) - 0.5 * g * (horizontalDistance / (v0 * cos(theta)))^2
        -- dy = horizontalDistance * tan(theta) - (0.5 * g * horizontalDistance^2) / (v0^2 * cos(theta)^2)
        -- dy = horizontalDistance * tan(theta) - (g * horizontalDistance^2) / (2 * v0^2 * (1/cos(theta)^2))  (since 1/cos^2 = sec^2 = 1 + tan^2)
        -- dy = horizontalDistance * tan(theta) - (g * horizontalDistance^2 / (2 * v0^2)) * (1 + tan(theta)^2)

        -- Rearrange into a quadratic equation in terms of tan(theta):
        -- (g * horizontalDistance^2 / (2 * v0^2)) * tan(theta)^2 - horizontalDistance * tan(theta) + dy + (g * horizontalDistance^2 / (2 * v0^2)) = 0

        -- Let A = (config.gravity * horizontalDistance^2) / (2 * config.projectileVelocity^2)
        -- Let B = -horizontalDistance
        -- Let C = dy + (config.gravity * horizontalDistance^2) / (2 * config.projectileVelocity^2)

        -- A_quad * tan(theta)^2 + B_quad * tan(theta) + C_quad = 0
        local A_quad = config.gravity * horizontalDistance^2 / (2 * config.projectileVelocity^2)
        local B_quad = -horizontalDistance
        local C_quad = dy + A_quad -- This is the corrected C from the derivation

        local discriminant = B_quad^2 - 4 * A_quad * C_quad

        if discriminant < 0 then
            print("Error: Target unreachable with current velocity/gravity. Try adjusting target or velocity.")
            return nil
        else
            local tan_theta_1 = (-B_quad + math.sqrt(discriminant)) / (2 * A_quad)
            local tan_theta_2 = (-B_quad - math.sqrt(discriminant)) / (2 * A_quad)

            local pitch1 = math.deg(math.atan(tan_theta_1))
            local pitch2 = math.deg(math.atan(tan_theta_2))

            local validPitches = {}
            if pitch1 >= config.minPitch and pitch1 <= config.maxPitch then
                table.insert(validPitches, pitch1)
            end
            if pitch2 >= config.minPitch and pitch2 <= config.maxPitch then
                table.insert(validPitches, pitch2)
            end

            if #validPitches == 0 then
                print("Error: No valid pitch found within cannon's physical limits for this trajectory.")
                return nil
            elseif #validPitches == 1 then
                calculatedPitch = validPitches[1]
            else -- Two valid pitches, prefer the lower (flatter) trajectory
                calculatedPitch = math.min(validPitches[1], validPitches[2])
            end
        end
    end

    -- --- END PHYSICS CALCULATION ---

    print(string.format("Calculated Trajectory (Cannon Angles): Yaw: %.2f, Pitch: %.2f", calculatedYaw, calculatedPitch))

    -- Return the calculated angles
    return {yaw = calculatedYaw, pitch = calculatedPitch}
end


--- Main program execution loop.
local function main()
    -- Attempt to initialize peripherals. Exit if critical ones are missing.
    if not initializePeripherals() then
        return -- Program stops if peripherals cannot be wrapped
    end

    -- Initialize build state by reading the bundled cable on startup
    isCannonBuilt = (bit.band(redstone.getBundledOutput(config.bundledCableSide), config.buildSignalColor) ~= 0)

    print("\n--- Cannon Control Program Initialized ---")
    print("Current tracked Yaw (relative to cannon's default): " .. string.format("%.2f", currentYaw) .. " degrees")
    print("Current tracked Pitch: " .. string.format("%.2f", currentPitch) .. " degrees")
    print("Cannon Built Status: " .. (isCannonBuilt and "BUILT" or "NOT BUILT"))
    print("Type 'help' for available commands.")

    -- Main command processing loop
    while true do
        term.write("> ")
        local input = read()

        local args = {}
        for s in string.gmatch(input, "%S+") do
            table.insert(args, s)
        end
        local command = args[1] and string.lower(args[1])

        if command == "help" then
            print("Commands:")
            print("  location                     - Get current cannon GPS location (with configured offsets).")
            print("  getangles                    - Display the program's current tracked yaw and pitch angles.")
            print("  setyaw <angle>               - Set the absolute yaw angle of the cannon (degrees, -180 to 180).")
            print("  setpitch <angle>             - Set the absolute pitch angle of the cannon (degrees, clamped " .. config.minPitch .. "-" .. config.maxPitch .. ").")
            print("  aim <x> <y> <z>              - Calculate and set yaw/pitch to hit target XYZ coordinates.")
            print("  fire                         - Trigger the cannon to fire (via Red bundled cable).")
            print("  togglebuild                  - Toggle cannon build/un-build state (via Black bundled cable).")
            print("  resetcannon                  - Perform a full reset sequence (un-build -> build) which aligns cannon to default.")
            print("  exit                         - Exit the program.")
        elseif command == "location" then
            getCannonLocation()
        elseif command == "getangles" then
            print(string.format("Current Program-Tracked Angles: Yaw: %.2f (relative to default), Pitch: %.2f", currentYaw, currentPitch))
        elseif command == "setyaw" then
            local angle = tonumber(args[2])
            if angle ~= nil then
                setYaw(angle)
            else
                print("Usage: setyaw <angle> (angle must be a number)")
            end
        elseif command == "setpitch" then
            local angle = tonumber(args[2])
            if angle ~= nil then
                setPitch(angle)
            else
                print("Usage: setpitch <angle> (angle must be a number)")
            end
        elseif command == "aim" then
            local targetX = tonumber(args[2])
            local targetY = tonumber(args[3])
            local targetZ = tonumber(args[4])
            if targetX and targetY and targetZ then
                print(string.format("Attempting to aim at target: X:%d Y:%d Z:%d", targetX, targetY, targetZ))
                local angles = calculateTrajectory(targetX, targetY, targetZ)
                if angles then
                    if setYaw(angles.yaw) and setPitch(angles.pitch) then
                        print("Cannon aimed successfully!")
                    else
                        print("Failed to set cannon angles.")
                    end
                else
                    print("Could not calculate trajectory.")
                end
            else
                print("Usage: aim <x> <y> <z> (coordinates must be numbers)")
            end
        elseif command == "fire" then
            fireCannon()
        elseif command == "togglebuild" then
            toggleBuildState()
        elseif command == "resetcannon" then
            resetCannon()
        elseif command == "exit" then
            print("Exiting program. Goodbye!")
            break
        else
            print("Unknown command. Type 'help' for available commands.")
        end
        print("")
    end
end

-- Run the main program when the script is executed
main()