Aeronautics Autopilot

-- ==========================================
-- MULTI-MODE AERONAUTICS FLIGHT CONTROLLER
-- ==========================================

-- TRANSPORT MODE: "quad", "steer", or "prop"
local transport_mode = "prop"

-- Configuration
local config = {
    invert_analog = true,

    -- Base Outputs
    lift_side = "bottom",
    forward_side = "front",
    reverse_side = "back",
    steer_left = "left",
    steer_right = "right",

    -- Quadcopter Outputs (X-Configuration)
    quad_fl = "left",
    quad_fr = "front",
    quad_bl = "back",
    quad_br = "right",

    -- Logical Lift States (0 = Off, 15 = Max Power)
    lift_takeoff = 15,
    lift_hover = 12,
    lift_land = 8,
    lift_off = 0,

    -- Flight Parameters
    min_altitude = 150,
    max_descent_rate = -1.5,
    loop_delay = 0.5,

    -- Propeller / Steering Tuning
    p_gain = 0.3,
    d_gain = 0.1,         -- Braking strength against spins
    deadzone = 3,
    max_steer_power = 4,  -- Max turn speed (1-15)

    -- Quadcopter Tuning
    max_tilt = 15,
    gimbal_p = 0.4
}

-- Flight States
local STATE_STANDBY = 0
local STATE_TAKEOFF = 1
local STATE_TRANSIT = 2
local STATE_ARRIVED = 3
local STATE_LANDING = 4
local STATE_HOVER = 5

local current_state = STATE_STANDBY
local last_altitude = 0
local target_hover_alt = config.min_altitude

-- ==========================================
-- PERIPHERAL INITIALIZATION
-- ==========================================
local nav_name, alt_name, gimbal_name = nil, nil, nil

for _, name in ipairs(peripheral.getNames()) do
    if string.find(name, "navigation_table") then nav_name = name
    elseif string.find(name, "altitude_sensor") then alt_name = name
    elseif string.find(name, "gimbal_sensor") then gimbal_name = name end
end

if not nav_name or not alt_name then
    error("CRITICAL: Navigation Table or Altitude Sensor missing!")
end
if transport_mode == "quad" and not gimbal_name then
    error("CRITICAL: Quad mode requires a Gimbal Sensor on the network!")
end

local nav = peripheral.wrap(nav_name)
local alt = peripheral.wrap(alt_name)
local gimbal = gimbal_name and peripheral.wrap(gimbal_name) or nil

-- ==========================================
-- HELPER FUNCTIONS
-- ==========================================
local function setOutput(side, power)
    power = math.max(0, math.min(15, math.floor(power)))
    if config.invert_analog then
        redstone.setAnalogOutput(side, 15 - power)
    else
        redstone.setAnalogOutput(side, power)
    end
end

local function hardStopThrust()
    if transport_mode == "quad" then
        setOutput(config.quad_fl, 0)
        setOutput(config.quad_fr, 0)
        setOutput(config.quad_bl, 0)
        setOutput(config.quad_br, 0)
    else
        setOutput(config.lift_side, 0)
        setOutput(config.forward_side, 0)
        setOutput(config.reverse_side, 0)
        setOutput(config.steer_left, 0)
        setOutput(config.steer_right, 0)
    end
end

local function applyStandardThrust(current_angle, last_angle, distance_flag)
    if distance_flag == "ARRIVED" then
        setOutput(config.forward_side, 0)
        setOutput(config.reverse_side, 5)
        setOutput(config.steer_left, 0)
        setOutput(config.steer_right, 0)
        return
    end

    setOutput(config.forward_side, 15)
    setOutput(config.reverse_side, 0)

    local turn_rate = 0
    if last_angle ~= nil then
        local delta_angle = (current_angle - last_angle + 180) % 360 - 180
        turn_rate = delta_angle / config.loop_delay
    end

    -- PD MATH: Steer towards the target (P) but fight against spinning too fast (D)
    local steer_effort = (current_angle * config.p_gain) - (turn_rate * config.d_gain)
    local power = math.min(config.max_steer_power, math.abs(steer_effort))

    if steer_effort > config.deadzone then
        setOutput(config.steer_right, power)
        setOutput(config.steer_left, 0)
    elseif steer_effort < -config.deadzone then
        setOutput(config.steer_left, power)
        setOutput(config.steer_right, 0)
    else
        setOutput(config.steer_left, 0)
        setOutput(config.steer_right, 0)
    end
end

local function applyQuadThrust(target_angle, current_pitch, current_roll, base_lift, is_transit)
    local target_pitch = 0
    local target_roll = 0

    if is_transit and target_angle then
        local rad_angle = math.rad(target_angle)
        target_pitch = math.cos(rad_angle) * config.max_tilt
        target_roll = math.sin(rad_angle) * config.max_tilt
    end

    local pitch_error = current_pitch - target_pitch
    local roll_error = current_roll - target_roll

    local pitch_adj = pitch_error * config.gimbal_p
    local roll_adj = roll_error * config.gimbal_p

    local out_fl = base_lift + pitch_adj + roll_adj
    local out_fr = base_lift + pitch_adj - roll_adj
    local out_bl = base_lift - pitch_adj + roll_adj
    local out_br = base_lift - pitch_adj - roll_adj

    setOutput(config.quad_fl, out_fl)
    setOutput(config.quad_fr, out_fr)
    setOutput(config.quad_bl, out_bl)
    setOutput(config.quad_br, out_br)
end

-- ==========================================
-- THREAD 1: KEYBOARD LISTENER
-- ==========================================
local function keyboardThread()
    local last_s_time = 0
    local last_x_time = 0
    local x_press_count = 0

    while true do
        local event, key = os.pullEvent("char")
        local now = os.clock()

        if key == "s" then
            if current_state == STATE_STANDBY then
                print(">>> MANUAL START: TAKEOFF INITIATED <<<")
                current_state = STATE_TAKEOFF
                last_s_time = 0
            elseif current_state > STATE_STANDBY and current_state ~= STATE_HOVER then
                if now - last_s_time < 0.5 then
                    print(">>> PAUSE: HOVER ENGAGED <<<")
                    local ok, current_alt = pcall(alt.getHeight)
                    target_hover_alt = ok and math.max(current_alt, 10) or config.min_altitude
                    current_state = STATE_HOVER
                end
            end
            last_s_time = now

        elseif key == "r" and current_state == STATE_HOVER then
            print(">>> RESUMING FLIGHT <<<")
            current_state = STATE_TAKEOFF

        elseif key == "l" and current_state > STATE_STANDBY and current_state ~= STATE_LANDING then
            print(">>> MANUAL SAFE LANDING INITIATED <<<")
            current_state = STATE_LANDING

        elseif key == "x" then
            if now - last_x_time < 0.6 then
                x_press_count = x_press_count + 1
            else
                x_press_count = 1
            end
            last_x_time = now

            if x_press_count >= 3 then
                print(">>> EMERGENCY KILL SWITCH TRIGGERED! ENGINES CUT <<<")
                current_state = STATE_STANDBY
                x_press_count = 0
            end
        end
    end
end

-- ==========================================
-- THREAD 2: MAIN FLIGHT LOGIC
-- ==========================================
local function flightLogicThread()
    local last_angle = nil

    while true do
        local alt_ok, current_alt = pcall(alt.getHeight)
        local nav_ok, current_angle = pcall(nav.getRelativeAngle)

        current_angle = current_angle or 0

        local gim_ok, angles = true, {0, 0}
        if transport_mode == "quad" then
            gim_ok, angles = pcall(gimbal.getAngles)
        end

        if not alt_ok or not nav_ok or not gim_ok then
            sleep(config.loop_delay)
        else
            if current_state == STATE_STANDBY then
                hardStopThrust()
            else
                local current_pitch = angles[1] or 0
                local current_roll = angles[2] or 0
                local base_lift = config.lift_off

                if current_state == STATE_TAKEOFF then
                    base_lift = config.lift_takeoff
                    if current_alt >= config.min_altitude then current_state = STATE_TRANSIT end

                elseif current_state == STATE_TRANSIT then
                    base_lift = config.lift_hover

                    -- NEW BULLETPROOF ARRIVAL DETECTION
                    if last_angle ~= nil then
                        local was_facing_target = math.abs(last_angle) < 60
                        local is_target_behind = math.abs(current_angle) > 120
                        if was_facing_target and is_target_behind then
                            current_state = STATE_ARRIVED
                        end
                    end

                elseif current_state == STATE_ARRIVED then
                    base_lift = config.lift_hover

                elseif current_state == STATE_HOVER then
                    local descent_rate = (current_alt - last_altitude) / config.loop_delay
                    if current_alt < target_hover_alt - 2 or descent_rate < -0.5 then
                        base_lift = config.lift_takeoff
                    elseif current_alt > target_hover_alt + 2 then
                        base_lift = config.lift_land
                    else
                        base_lift = config.lift_hover
                    end

                elseif current_state == STATE_LANDING then
                    local descent_rate = (current_alt - last_altitude) / config.loop_delay
                    if descent_rate < config.max_descent_rate then
                        base_lift = config.lift_hover -- Fixed Trampoline Bug
                    else
                        base_lift = config.lift_land
                    end
                    if current_alt <= 1 or (descent_rate == 0 and current_alt < config.min_altitude - 10) then
                        print("Touchdown. Systems standing by.")
                        current_state = STATE_STANDBY
                        hardStopThrust()
                    end
                end

                if transport_mode == "quad" then
                    local is_transit = (current_state == STATE_TRANSIT)
                    applyQuadThrust(current_angle, current_pitch, current_roll, base_lift, is_transit)
                else
                    setOutput(config.lift_side, base_lift)
                    -- HAS_TARGET BUG REMOVED HERE
                    if current_state == STATE_TRANSIT then
                        applyStandardThrust(current_angle, last_angle, "TRANSIT")
                    elseif current_state == STATE_ARRIVED then
                        applyStandardThrust(current_angle, last_angle, "ARRIVED")
                        sleep(2)
                        current_state = STATE_LANDING
                    elseif current_state == STATE_HOVER or current_state == STATE_LANDING then
                        setOutput(config.forward_side, 0)
                        setOutput(config.reverse_side, 0)
                        setOutput(config.steer_left, 0)
                        setOutput(config.steer_right, 0)
                    end
                end
            end

            last_altitude = current_alt
            last_angle = current_angle
            sleep(config.loop_delay)
        end
    end
end

-- ==========================================
-- BOOT SEQUENCE
-- ==========================================
term.clear()
term.setCursorPos(1,1)
print("Aeronautics OS v6.2")
print("-------------------------")
print("[S]    Start Navigation")
print("[S x2] Pause / Hover in place")
print("[R]    Resume Navigation")
print("[L]    Safe Land Here")
print("[X x3] KILL SWITCH")
print("-------------------------")
parallel.waitForAny(keyboardThread, flightLogicThread)