pilot.lua

local altitude = peripheral.find("altitude_sensor")
local relay = peripheral.find("redstone_relay")
local speedRotation = peripheral.find("Create_RotationSpeedController")
local swivel = peripheral.find("swivel_bearing")
local ship = _G.sublevel or peripheral.find("ship_reader") or peripheral.find("vessel_info")
local PID = require("pid")
if not altitude then
    error("altitude_sensor not found")
end
if not speedRotation then
    error("Create_RotationSpeedController not found")
end
if not swivel then
    error("swivel_bearing not found")
end
local TARGET_HEIGHT = 150
local TARGET_ANGLE = 0
local state = {
    running = true,
    auto = false,
    clutchLocked = true,
    burnerOn = false,
    poseOk = false,
    x = 0,
    z = 0,
    yaw = 0,
    height = 0,
    verticalSpeed = 0,
    horizontalSpeed = 0,
    distance = nil,
    approachDistance = nil,
    eta = nil,
    targetX = nil,
    targetZ = nil,
    targetName = "",
    previewX = nil,
    previewZ = nil,
    previewName = "",
    targetHeight = 150,
    cruiseHeight = 150,
    landing = false,
    landingRadius = 10,
    landingSpeed = 0,
    landingStage = "cruise",
    navStage = "idle",
    motorOutput = 0,
    burnerSignal = 0,
    currentAngle = 0,
    yawAlignCommand = 0,
    yawAlignSide = 0,
    lastNavPoint = nil,
    log = "Ready",
    skipTakeoff = false,
    takeoff = false,
    shutdownDeadline = nil
}
local ui = {
    screen = "search",
    focus = "search",
    search = "",
    hInput = "150",
    selectedSuggestion = 1,
    selectedAction = "start",
    widgets = {},
    width = 0,
    height = 0,
    fullRedraw = true,
    dirty = true,
    lastScreen = nil,
    lastFrameTime = os.clock(),
    dt = 0.05,
    pulse = 0,
    animationUntil = 0,
    anim = {searchX = nil, searchY = nil, searchW = nil, routeProgress = 0, routePulse = 0},
    layout = {route = nil, stats = nil}
}
local LANDING_PREDICTION_TIME = 2.2
local LANDING_BRAKE_DISTANCE = 65
local LANDING_FINAL_DISTANCE = 8
local LANDING_MIN_SPEED = 0.08
local LANDING_MAX_ANGLE = 18
local FINAL_YAW = 0
local APPROACH_OFFSET_Z = 100
local APPROACH_RADIUS = 20
local places = {
    {name = "Spawn", x = 0, z = 0},
    {name = "Base", x = -546, z = -124},
    {name = "Viasma", x = 115, z = 323 }
}
local drawCache = {}
local function clamp(value, minValue, maxValue)
    return math.max(minValue, math.min(maxValue, value))
end
local function normalizeAngle(angle)
    return ((angle + 180) % 360) - 180
end
local function expLerp(current, target, rate, dt)
    if current == nil then
        return target
    end
    local amount = 1 - math.exp(-rate * dt)
    return current + (target - current) * amount
end
local function trim(text)
    return tostring(text or ""):gsub("^%s+", ""):gsub("%s+$", "")
end
local function parseNumber(text)
    local cleaned = trim(text)
    if cleaned == "" then
        return nil
    end
    local sign = 1
    local first = cleaned:sub(1, 1)
    if first == "-" then
        sign = -1
        cleaned = cleaned:sub(2)
    elseif first == "+" then
        cleaned = cleaned:sub(2)
    end
    if cleaned == "" then
        return nil
    end
    local whole, frac = cleaned:match("^(%d+)%.(%d+)$")
    if not whole then
        whole = cleaned:match("^(%d+)$")
    end
    if not whole then
        return nil
    end
    local value = 0
    for i = 1, #whole do
        value = value * 10 + (string.byte(whole, i) - 48)
    end
    if frac then
        local place = 0.1
        for i = 1, #frac do
            value = value + (string.byte(frac, i) - 48) * place
            place = place * 0.1
        end
    end
    return value * sign
end
local function padRight(text, width)
    text = tostring(text or "")
    if width <= 0 then
        return ""
    end
    if #text >= width then
        return text:sub(1, width)
    end
    return text .. string.rep(" ", width - #text)
end
local function padCenter(text, width)
    text = tostring(text or "")
    if width <= 0 then
        return ""
    end
    if #text >= width then
        return text:sub(1, width)
    end
    local left = math.floor((width - #text) / 2)
    local right = width - #text - left
    return string.rep(" ", left) .. text .. string.rep(" ", right)
end
local function shortNumber(value, digits)
    if value == nil then
        return "--"
    end
    return string.format("%." .. tostring(digits or 1) .. "f", value)
end
local function etaText(seconds)
    if not seconds then
        return "--"
    end
    seconds = math.max(0, math.floor(seconds + 0.5))
    local minutes = math.floor(seconds / 60)
    local secs = seconds % 60
    if minutes >= 60 then
        return string.format("%dh %02dm", math.floor(minutes / 60), minutes % 60)
    end
    return string.format("%d:%02d", minutes, secs)
end
local function writeAt(x, y, text, fg, bg)
    x = math.floor(x + 0.5)
    y = math.floor(y + 0.5)
    fg = fg or colors.white
    bg = bg or colors.black
    text = tostring(text or "")
    local changed = false
    for i = 1, #text do
        local key = tostring(x + i - 1) .. ":" .. tostring(y)
        local ch = text:sub(i, i)
        local cell = drawCache[key]
        if not cell or cell.ch ~= ch or cell.fg ~= fg or cell.bg ~= bg then
            changed = true
            break
        end
    end
    if not changed then
        return
    end
    term.setCursorPos(x, y)
    term.setTextColor(fg)
    term.setBackgroundColor(bg)
    term.write(text)
    for i = 1, #text do
        local key = tostring(x + i - 1) .. ":" .. tostring(y)
        drawCache[key] = {ch = text:sub(i, i), fg = fg, bg = bg}
    end
end
local function fill(x, y, w, h, bg)
    if w <= 0 or h <= 0 then
        return
    end
    for row = 0, h - 1 do
        writeAt(x, y + row, string.rep(" ", w), colors.white, bg)
    end
end
local function clearRect(x, y, w, h, bg)
    if w <= 0 or h <= 0 then
        return
    end
    for row = y, y + h - 1 do
        for col = x, x + w - 1 do
            drawCache[tostring(col) .. ":" .. tostring(row)] = nil
        end
    end
    fill(x, y, w, h, bg)
end
local function markDirty()
    if not ui.dirty then
        ui.dirty = true
        os.queueEvent("ui_dirty")
    else
        ui.dirty = true
    end
end
local function startAnimation(duration)
    ui.animationUntil = math.max(ui.animationUntil or 0, os.clock() + (duration or 0.2))
    markDirty()
end
local function isAnimating()
    return os.clock() < (ui.animationUntil or 0)
end
local function registerWidget(widget)
    ui.widgets[#ui.widgets + 1] = widget
end
local function pointIn(mx, my, widget)
    return mx >= widget.x and mx < widget.x + widget.w and my >= widget.y and my < widget.y + widget.h
end
local function clearIfNeeded()
    local w, h = term.getSize()
    if ui.width ~= w or ui.height ~= h then
        ui.width = w
        ui.height = h
        term.setBackgroundColor(colors.black)
        term.setTextColor(colors.white)
        term.clear()
        drawCache = {}
        ui.fullRedraw = true
        ui.dirty = true
    end
    return w, h
end
local function setLog(text)
    state.log = tostring(text or "")
    markDirty()
end
local function setClutch(flag)
    if state.clutchLocked == flag then
        redstone.setOutput("bottom", flag)
        return
    end
    state.clutchLocked = flag
    redstone.setOutput("bottom", flag)
    markDirty()
end
local function switchScreen(screen)
    if ui.screen ~= screen then
        ui.screen = screen
        ui.fullRedraw = true
        startAnimation(0.25)
        ui.layout.route = nil
        ui.layout.stats = nil
    end
end
local function getPose()
    if not ship or not ship.getLogicalPose then
        return nil
    end
    local ok, pose = pcall(ship.getLogicalPose)
    if ok then
        return pose
    end
    return nil
end
local function getVelocityVector()
    if not ship or not ship.getVelocity then
        return nil
    end
    local ok, velocity = pcall(ship.getVelocity)
    if ok and velocity then
        return velocity
    end
    return nil
end
local function velocityComponents(velocity)
    if not velocity then
        return nil, nil, nil
    end
    return velocity[1], velocity[2], velocity[3]
end
local function yawFromPose(pose)
    local w = pose.orientation.a
    local x = pose.orientation.v.x
    local y = pose.orientation.v.y
    local z = pose.orientation.v.z
    local sinyCosp = 2 * (w * y + x * z)
    local cosyCosp = 1 - 2 * (y * y + z * z)
    return math.deg(math.atan(sinyCosp, cosyCosp))
end
local FORCE_MODEL = {
    burnerPressure = 0.432,
    burnerForce = 53.93,
    kilopond = 9.80665,
    offsetPower = -32.6
}
local function getMass()
    if not ship or not ship.getMass then
        return nil
    end
    local ok, mass = pcall(ship.getMass)
    if ok then
        return mass
    end
    return nil
end
local function getHoverSignalAtHeight(sampleHeight)
    if not aero or not aero.getAirPressure or not aero.getGravity or not vector then
        return nil
    end
    local pose = getPose()
    local mass = getMass()
    if not pose or not mass then
        return nil
    end
    local pressure = aero.getAirPressure(vector.new(pose.position.x, sampleHeight, pose.position.z))
    if not pressure then
        return nil
    end
    local k = FORCE_MODEL.burnerForce / FORCE_MODEL.burnerPressure
    local targetForce = mass * -aero.getGravity().y
    for signal = 1, 15 do
        local force = ((k / 15 * signal) * pressure * FORCE_MODEL.kilopond) + FORCE_MODEL.offsetPower
        if force >= targetForce then
            return signal
        end
    end
    return 15
end
local function setBurnerAnalogOutput(side, signal)
    if relay and relay.setAnalogOutput then
        local ok = pcall(relay.setAnalogOutput, relay, side, signal)
        if ok then
            return
        end
    end
    redstone.setAnalogOutput(side, signal)
end
local function getApproachPoint(targetX, targetZ)
    if not targetX or not targetZ then
        return nil, nil
    end
    return targetX, targetZ - APPROACH_OFFSET_Z
end
local function getActiveNavTarget()
    if not state.targetX or not state.targetZ then
        return nil, nil, nil
    end
    if state.navStage == "target" or state.landing then
        return state.targetX, state.targetZ, "target", 0
    end
    local approachX, approachZ = getApproachPoint(state.targetX, state.targetZ)
    if not state.landing and state.x and state.z then
        local dx = approachX - state.x
        local dz = approachZ - state.z
        local distance = math.sqrt(dx * dx + dz * dz)
        local remaining = math.max(0, distance - APPROACH_RADIUS)
        if remaining > 0 then
            state.navStage = "approach"
            return approachX, approachZ, "approach", remaining
        end
    end
    state.navStage = "target"
    return state.targetX, state.targetZ, "target", 0
end
local function setDestination(x, z, name)
    state.targetX = x
    state.targetZ = z
    state.targetName = name or string.format("%.0f, %.0f", x, z)
    state.previewX = x
    state.previewZ = z
    state.previewName = state.targetName
    state.landing = false
    state.landingStage = "cruise"
    state.navStage = "approach"
    ui.search = state.targetName
    markDirty()
end
local function setPreviewDestination(x, z, name)
    state.previewX = x
    state.previewZ = z
    state.previewName = name or string.format("%.0f, %.0f", x, z)
    ui.search = state.previewName
    switchScreen("preview")
end
local function parseDestination()
    local query = trim(ui.search):lower()
    for i = 1, #places do
        if query == places[i].name:lower() then
            return places[i].x, places[i].z, places[i].name
        end
    end
    local xText, zText = query:match("^(-?%d+%.?%d*)%s*,%s*(-?%d+%.?%d*)$")
    if xText and zText then
        return parseNumber(xText), parseNumber(zText), nil
    end
    return nil, nil, nil
end
local function getSuggestions()
    local query = trim(ui.search):lower()
    local suggestions = {}
    for i = 1, #places do
        local place = places[i]
        if query == "" or place.name:lower():find(query, 1, true) then
            suggestions[#suggestions + 1] = {
                label = place.name,
                detail = string.format("%.0f, %.0f", place.x, place.z),
                x = place.x,
                z = place.z,
                name = place.name
            }
        end
    end
    local xText, zText = query:match("^(-?%d+%.?%d*)%s*,?%s*(-?%d*%.?%d*)$")
    if xText and xText ~= "" then
        local x = parseNumber(xText)
        local z = parseNumber(zText)
        if x and z then
            table.insert(
                suggestions,
                1,
                {label = "Coordinates", detail = string.format("%.0f, %.0f", x, z), x = x, z = z}
            )
        elseif x then
            table.insert(suggestions, 1, {label = "Keep typing", detail = string.format("%.0f, ?", x), partial = true})
        end
    end
    if #suggestions == 0 and query ~= "" then
        suggestions[1] = {label = "No match", detail = "Try a place name or X,Z", partial = true}
    end
    ui.selectedSuggestion = clamp(ui.selectedSuggestion, 1, math.max(1, #suggestions))
    return suggestions
end
local function previewDistance()
    if not state.previewX or not state.previewZ then
        return nil
    end
    local dx = state.previewX - state.x
    local dz = state.previewZ - state.z
    return math.sqrt(dx * dx + dz * dz)
end
local function previewEta(distance)
    if not distance then
        return nil
    end
    local speed = state.horizontalSpeed
    if speed < 0.15 then
        speed = 8
    end
    return distance / speed
end
local function drawInputField(id, x, y, w, label, value, focused)
    fill(x, y, w, 3, colors.gray)
    writeAt(x + 1, y, padRight(label, w - 2), colors.lightGray, colors.gray)
    writeAt(
        x + 1,
        y + 1,
        padRight(value, w - 2),
        focused and colors.black or colors.white,
        focused and colors.white or colors.gray
    )
    registerWidget({id = "focus:" .. id, x = x, y = y, w = w, h = 3})
end
local function drawButton(id, x, y, w, label, active)
    local bg = active and colors.white or colors.gray
    local fg = active and colors.black or colors.white
    writeAt(x, y, " " .. padCenter(label, w - 2) .. " ", fg, bg)
    registerWidget({id = id, x = x, y = y, w = w, h = 1})
end
local function drawSearchBar(x, y, w, text, focused)
    local fillColor = focused and colors.lightBlue or colors.gray
    writeAt(x, y, " " .. padRight(text, w - 2) .. " ", colors.black, fillColor)
    registerWidget({id = "focus:search", x = x, y = y, w = w, h = 1})
end
local function drawCard(x, y, w, h, bg)
    fill(x, y, w, h, bg)
end
local function drawMetricCard(x, y, w, title, value, bg)
    drawCard(x, y, w, 3, bg)
    writeAt(x + 1, y, padRight(title, w - 2), colors.lightGray, bg)
    writeAt(x + 1, y + 1, padRight(value, w - 2), colors.white, bg)
end
local function drawRoutePanel(x, y, w, h, targetX, targetZ, accent)
    if ui.fullRedraw then
        drawCard(x, y, w, h, colors.black)
    end
    if w < 12 or h < 7 then
        return
    end
    if
        ui.fullRedraw or not ui.layout.route or ui.layout.route.x ~= x or ui.layout.route.y ~= y or
            ui.layout.route.w ~= w or
            ui.layout.route.h ~= h
     then
        writeAt(x, y, "+" .. string.rep("-", w - 2) .. "+", colors.gray, colors.black)
        for row = 1, h - 2 do
            writeAt(x, y + row, "|" .. string.rep(" ", w - 2) .. "|", colors.gray, colors.black)
        end
        writeAt(x, y + h - 1, "+" .. string.rep("-", w - 2) .. "+", colors.gray, colors.black)
        writeAt(x + 2, y + 1, padRight("route", w - 4), colors.lightGray, colors.black)
        ui.layout.route = {x = x, y = y, w = w, h = h}
    end
    clearRect(x + 1, y + 1, w - 2, h - 2, colors.black)
    writeAt(x + 2, y + 1, padRight("route", w - 4), colors.lightGray, colors.black)
    local sx = x + 3
    local sy = y + h - 3
    local tx = x + w - 4
    local ty = y + 2
    if targetX and targetZ then
        local dx = targetX - state.x
        local dz = targetZ - state.z
        local span = math.max(math.abs(dx), math.abs(dz), 1)
        tx = clamp(math.floor(x + 2 + ((dx / span) + 1) * (w - 5) * 0.5 + 0.5), x + 2, x + w - 3)
        ty = clamp(math.floor(y + 2 + ((dz / span) + 1) * (h - 5) * 0.5 + 0.5), y + 2, y + h - 3)
    end
    local steps = math.max(math.abs(tx - sx), math.abs(ty - sy))
    if steps < 1 then
        steps = 1
    end
    for i = 0, steps do
        local px = sx + math.floor((tx - sx) * i / steps + 0.5)
        local py = sy + math.floor((ty - sy) * i / steps + 0.5)
        writeAt(px, py, " ", colors.white, accent)
    end
    local headingChar = "A"
    if state.yaw >= -45 and state.yaw < 45 then
        headingChar = "^"
    elseif state.yaw >= 45 and state.yaw < 135 then
        headingChar = ">"
    elseif state.yaw >= -135 and state.yaw < -45 then
        headingChar = "<"
    else
        headingChar = "v"
    end
    writeAt(sx, sy, headingChar, colors.black, colors.lime)
    writeAt(tx, ty, "D", colors.black, colors.white)
end
local function drawFooter(w)
    writeAt(2, ui.height, padRight(state.log, w - 2), colors.lightGray, colors.black)
end
local function drawSearchScene(w, h, accent)
    if ui.fullRedraw then
        fill(1, 1, w, h, colors.black)
    end
    local compact = trim(ui.search) ~= ""
    local targetW = compact and math.max(18, w - 8) or math.min(w - 8, 28)
    local targetX = compact and 4 or math.max(2, math.floor((w - targetW) / 2) + 1)
    local targetY = compact and 3 or math.max(5, math.floor(h * 0.28))
    ui.anim.searchX = expLerp(ui.anim.searchX, targetX, 14, ui.dt)
    ui.anim.searchY = expLerp(ui.anim.searchY, targetY, 14, ui.dt)
    ui.anim.searchW = expLerp(ui.anim.searchW, targetW, 14, ui.dt)
    local searchX = math.floor(ui.anim.searchX + 0.5)
    local searchY = math.floor(ui.anim.searchY + 0.5)
    local searchW = math.floor(ui.anim.searchW + 0.5)
    writeAt(2, 2, padRight("Autopilot", w - 2), colors.lightGray, colors.black)
    writeAt(2, math.max(1, searchY - 2), padRight("Where do you want to go?", w - 2), colors.white, colors.black)
    drawSearchBar(searchX, searchY, searchW, ui.search, ui.focus == "search")
    local suggestions = getSuggestions()
    local listY = searchY + 2
    writeAt(2, listY, padRight(compact and "Suggestions" or "Recent places", w - 2), colors.lightGray, colors.black)
    local maxItems = math.max(1, math.floor((h - listY - 4) / 3))
    local count = math.min(#suggestions, maxItems)
    for i = 1, count do
        local item = suggestions[i]
        local cardY = listY + 1 + (i - 1) * 3
        local selected = i == ui.selectedSuggestion
        local bg = selected and colors.white or colors.gray
        local fg = selected and colors.black or colors.white
        fill(3, cardY, w - 4, 2, bg)
        writeAt(4, cardY, padRight(item.label, w - 6), fg, bg)
        writeAt(4, cardY + 1, padRight(item.detail or "", w - 6), selected and colors.gray or colors.lightGray, bg)
        registerWidget({id = "suggest:" .. tostring(i), x = 3, y = cardY, w = w - 4, h = 2})
    end
    writeAt(2, h - 2, padRight("Enter select Up/Down browse Q quit", w - 2), colors.lightGray, colors.black)
    drawFooter(w)
end
local function drawPreviewScene(w, h, accent)
    if ui.fullRedraw then
        fill(1, 1, w, h, colors.black)
    end
    local title = state.previewName ~= "" and state.previewName or "Destination preview"
    local distance = previewDistance()
    local eta = previewEta(distance)
    writeAt(2, 2, padRight("Preview", w - 2), colors.lightGray, colors.black)
    writeAt(2, 3, padRight(title, w - 2), colors.white, colors.black)
    writeAt(2, 4, padRight("Confirm route and cruise height", w - 2), accent, colors.black)
    local leftW = math.max(24, math.floor(w * 0.34))
    drawMetricCard(2, 6, leftW, "ETA", etaText(eta), colors.gray)
    drawMetricCard(2, 10, leftW, "Distance", shortNumber(distance, 1) .. " m", colors.gray)
    drawInputField("height", 2, 14, leftW, "Cruise height (m)", ui.hInput, ui.focus == "height")
    local buttonY = math.max(18, h - 4)
    local routeBottom = buttonY - 2
    local mapX = leftW + 4
    local mapW = math.max(16, w - mapX - 1)
    drawRoutePanel(mapX, 6, mapW, math.max(7, routeBottom - 5), state.previewX, state.previewZ, accent)
    drawButton("back", 2, buttonY, 12, "Back", ui.selectedAction == "back")
    drawButton("start", 16, buttonY, 18, "Start journey", ui.selectedAction == "start")
    writeAt(
        2,
        h - 2,
        padRight("Tab height/search Left/Right switch Enter start T toggle climb", w - 2),
        colors.lightGray,
        colors.black
    )
    drawFooter(w)
end
local function drawJourneyScene(w, h, accent)
    if ui.fullRedraw then
        fill(1, 1, w, h, colors.black)
    end
    local destination = state.targetName ~= "" and state.targetName or "Destination"
    local statusLine = state.landing and ("Landing " .. state.landingStage) or "Autopilot engaged"
    if not state.landing and state.approachDistance and state.approachDistance > 0 then
        statusLine = "To align zone edge " .. shortNumber(state.approachDistance, 1) .. " m"
    elseif not state.landing and state.navStage == "target" then
        statusLine = "Final approach"
    end
    writeAt(2, 2, padRight("Journey", w - 2), colors.lightGray, colors.black)
    writeAt(2, 3, padRight(destination, w - 2), colors.white, colors.black)
    writeAt(2, 4, padRight(statusLine, w - 2), accent, colors.black)
    local buttonY = h - 2
    local statsTop = h - 9
    drawRoutePanel(2, 6, w - 2, statsTop - 7, state.targetX, state.targetZ, accent)
    clearRect(2, statsTop, w - 2, 6, colors.gray)
    writeAt(
        3,
        statsTop,
        padRight("ETA " .. etaText(state.eta) .. " Distance " .. shortNumber(state.distance, 1) .. " m", w - 4),
        colors.white,
        colors.gray
    )
    writeAt(
        3,
        statsTop + 1,
        padRight(
            "Height " ..
                shortNumber(state.height, 1) ..
                    " / " .. shortNumber(TARGET_HEIGHT, 1) .. " V/S " .. shortNumber(state.verticalSpeed, 1),
            w - 4
        ),
        colors.black,
        colors.white
    )
    writeAt(
        3,
        statsTop + 2,
        padRight(
            "Angle " ..
                shortNumber(TARGET_ANGLE, 1) ..
                    " Motor " .. shortNumber(state.motorOutput, 1) .. " " .. (state.clutchLocked and "LOCKED" or "FREE"),
            w - 4
        ),
        colors.white,
        colors.gray
    )
    writeAt(
        3,
        statsTop + 3,
        padRight("Burner " .. (state.burnerOn and "ON" or "OFF") .. " Pose " .. (state.poseOk and "OK" or "NO"), w - 4),
        colors.white,
        colors.gray
    )
    drawInputField("height", 2, statsTop + 4, 22, "Cruise height (m)", ui.hInput, ui.focus == "height")
    drawButton("stop", w - 15, buttonY, 13, state.landing and "Abort" or "Stop", false)
    drawFooter(w)
end
local function drawUI()
    local w, h = clearIfNeeded()
    ui.widgets = {}
    local now = os.clock()
    ui.dt = now - ui.lastFrameTime
    if ui.dt <= 0 or ui.dt > 0.25 then
        ui.dt = 0.05
    end
    ui.lastFrameTime = now
    local accent = colors.cyan
    ui.fullRedraw = ui.fullRedraw or ui.lastScreen ~= ui.screen or ui.dirty
    ui.lastScreen = ui.screen
    if ui.screen == "search" then
        drawSearchScene(w, h, accent)
    elseif ui.screen == "preview" then
        drawPreviewScene(w, h, accent)
    else
        drawJourneyScene(w, h, accent)
    end
    ui.dirty = false
    ui.fullRedraw = false
end
local function applySelectedSuggestion()
    local suggestions = getSuggestions()
    local suggestion = suggestions[ui.selectedSuggestion]
    if suggestion and not suggestion.partial and suggestion.x and suggestion.z then
        setPreviewDestination(suggestion.x, suggestion.z, suggestion.name)
        setLog("Selected " .. (suggestion.name or suggestion.detail))
        return true
    end
    local x, z, name = parseDestination()
    if x and z then
        setPreviewDestination(x, z, name)
        setLog("Selected " .. (name or string.format("%.0f, %.0f", x, z)))
        return true
    end
    setLog("Type a place name or coordinates")
    return false
end
local function stopFlight()
    state.auto = false
    TARGET_ANGLE = 0
    state.yawAlignCommand = 0
    state.yawAlignSide = 0
    state.landing = false
    state.landingStage = "idle"
    state.navStage = "idle"
    state.approachDistance = nil
    state.takeoff = false
    state.shutdownDeadline = nil
    setClutch(true)
    switchScreen("preview")
    setLog("Autopilot stopped")
end
local function stopFlightAndShutdown()
    stopFlight()
    state.shutdownDeadline = os.clock() + 10
    setLog("Landing complete; shutting down in 10s")
end
local function beginLanding()
    if state.landing then
        return
    end
    state.landing = true
    state.landingStage = "descent"
    state.navStage = "target"
    TARGET_ANGLE = 0
    state.yawAlignCommand = 0
    state.yawAlignSide = 0
    setClutch(true)
    TARGET_HEIGHT = state.height
    state.targetHeight = TARGET_HEIGHT
    setLog("Landing descent")
end
local function startFlight()
    if not ship then
        setLog("No ship reader found")
        return
    end
    local x = state.previewX
    local z = state.previewZ
    local name = state.previewName
    if not x or not z then
        local ok = applySelectedSuggestion()
        if not ok then
            return
        end
        x = state.previewX
        z = state.previewZ
        name = state.previewName
    end
    local height = parseNumber(ui.hInput)
    if not height then
        setLog("Cruise height must be a number")
        return
    end
    TARGET_HEIGHT = height
    state.targetHeight = height
    state.cruiseHeight = height
    setDestination(x, z, name)
    state.auto = true
    state.takeoff = not state.skipTakeoff
    TARGET_ANGLE = 0
    setClutch(true)
    if state.takeoff then
        TARGET_HEIGHT = math.max(state.cruiseHeight, state.height + 35)
        setLog("Vertical takeoff")
    else
        TARGET_HEIGHT = state.height
        setLog("Direct translation")
    end
    state.targetHeight = TARGET_HEIGHT
    switchScreen("journey")
end
local function focusNext()
    if ui.screen == "search" then
        ui.focus = "search"
    elseif ui.focus == "search" then
        ui.focus = "height"
    else
        ui.focus = "search"
    end
    markDirty()
end
local function setFocusedText(text)
    if ui.focus == "search" then
        ui.search = text
        ui.selectedSuggestion = 1
        if ui.screen ~= "journey" then
            switchScreen("search")
        end
    elseif ui.focus == "height" then
        ui.hInput = text
        local parsed = parseNumber(text)
        if parsed and parsed > 0 then
            state.cruiseHeight = parsed
            if state.auto and not state.landing then
                TARGET_HEIGHT = parsed
                state.targetHeight = parsed
            end
        end
    end
    markDirty()
end
local function getFocusedText()
    if ui.focus == "height" then
        return ui.hInput
    end
    return ui.search
end
local function handleWidget(id)
    if type(id) ~= "string" then
        return
    end
    if id == "start" then
        startFlight()
    elseif id == "back" then
        switchScreen("search")
        setLog("Back to search")
    elseif id == "stop" then
        stopFlight()
    elseif id:sub(1, 8) == "suggest:" then
        ui.selectedSuggestion = parseNumber(id:sub(9)) or 1
        applySelectedSuggestion()
    elseif id:sub(1, 6) == "focus:" then
        ui.focus = id:sub(7)
        markDirty()
    end
end
setClutch(true)
parallel.waitForAny(
    function()
        local burnerSide = "top"
        local lastHeight = altitude.getHeight()
        local lastTime = os.clock()
        local hoverSignal = nil
        local lastHoverRefresh = 0
        local wasAuto = false
        local deadzone = 2
        local strongZone = 40
        local predictTime = 6
        local maxBurn = 4
        local minBurn = 0.15
        local signalKp = 0.11
        local signalKd = 0.32
        local function setBurnerSignal(signal)
            local clamped = clamp(math.floor((signal or 0) + 0.5), 0, 15)
            local isOn = clamped > 0
            state.burnerOn = isOn
            state.burnerSignal = clamped
            setBurnerAnalogOutput(burnerSide, clamped)
            if ui.screen == "journey" then
                markDirty()
            end
        end
        local function burner(on)
            setBurnerSignal(on and 15 or 0)
        end
        local function analogHold(height, velocity, localTargetHeight)
            local predictedHeight = height + velocity * predictTime
            local error = localTargetHeight - predictedHeight
            local signal = hoverSignal + (error * signalKp) - (velocity * signalKd)
            if localTargetHeight <= 2 then
                signal = signal - 1.5
            elseif localTargetHeight <= 8 then
                signal = signal - 0.75
            end
            if height > localTargetHeight + 12 and velocity > 1.5 then
                signal = 0
            end
            setBurnerSignal(signal)
        end
        while state.running do
            local now = os.clock()
            local dt = now - lastTime
            if dt <= 0 then
                dt = 0.05
            end
            lastTime = now

            if not state.auto then
                if wasAuto then
                    burner(false)
                    hoverSignal = nil
                    lastHoverRefresh = 0
                    lastHeight = altitude.getHeight()
                    wasAuto = false
                end
                sleep(0.1)
                goto continue_height_loop
            end
            wasAuto = true

            local height = altitude.getHeight()
            local velocity = (height - lastHeight) / dt
            lastHeight = height
            state.height = height
            state.verticalSpeed = velocity
            if now - lastHoverRefresh > 1.5 then
                local hoverSampleHeight = TARGET_HEIGHT
                if state.landing and height < 8 then
                    hoverSampleHeight = height
                end
                hoverSignal = getHoverSignalAtHeight(hoverSampleHeight)
                lastHoverRefresh = now
            end
            local closeLanding = state.landing and ((state.distance or math.huge) <= 4 or state.landingStage == "final" or state.landingStage == "yaw_align")
            if closeLanding then
                if height > 1.5 then
                    burner(false)
                elseif velocity < -0.9 then
                    burner(true)
                    sleep(0.10)
                    burner(false)
                else
                    burner(false)
                end
                sleep(0.05)
            elseif state.landing and state.landingStage ~= "yaw_align" then
                if hoverSignal and height > 3 then
                    analogHold(height, velocity, TARGET_HEIGHT)
                elseif height > 6 then
                    burner(false)
                elseif velocity < -1.2 then
                    burner(true)
                    sleep(0.12)
                    burner(false)
                else
                    burner(false)
                end
                sleep(0.05)
            elseif hoverSignal then
                analogHold(height, velocity, TARGET_HEIGHT)
                sleep(0.05)
            else
                local predictedHeight = height + velocity * predictTime
                local error = TARGET_HEIGHT - predictedHeight
                if velocity > 3 and height > TARGET_HEIGHT then
                    burner(false)
                elseif math.abs(error) <= deadzone then
                    burner(false)
                elseif error > 0 then
                    local burnTime
                    if error > strongZone then
                        burnTime = maxBurn
                    else
                        burnTime = math.max(minBurn, math.min(maxBurn, error * 0.5))
                    end
                    burner(true)
                    sleep(burnTime)
                    burner(false)
                else
                    burner(false)
                end
                sleep(0.05)
            end
            ::continue_height_loop::
        end
    end,
    function()
        local pid = PID:new(0.5, 0.001, 0.5, -5, 5)
        local pulsing = false
        local pulseUntil = 0
        local cooldownUntil = 0
        local pulseSpeed = 0
        local lastShownMotor = nil
        local pulseDuration = 0.08
        local pulseCooldown = 0.18
        local pulseMagnitude = 1
        while state.running do
            local now = os.clock()
            local angle = swivel.getTargetAngle()
            state.currentAngle = angle
            local raw = pid:compute(TARGET_ANGLE, angle, 1)
            local limited = clamp(raw, -20, 20)
            local yawPulse = state.yawAlignCommand or 0
            local yawAlignSideReady = math.abs(math.abs(TARGET_ANGLE) - 90) <= 0.001 and math.abs(angle - TARGET_ANGLE) <= 8
            if state.clutchLocked then
                pulsing = false
                state.motorOutput = 0
                speedRotation.setTargetSpeed(0)
            elseif state.landingStage == "yaw_align" and not yawAlignSideReady then
                pulsing = false
                state.motorOutput = limited
                speedRotation.setTargetSpeed(limited)
            elseif state.landingStage == "yaw_align" and math.abs(yawPulse) > 0.1 then
                if pulsing and now < pulseUntil then
                    state.motorOutput = pulseSpeed
                    speedRotation.setTargetSpeed(pulseSpeed)
                elseif now < cooldownUntil then
                    pulsing = false
                    state.motorOutput = 0
                    speedRotation.setTargetSpeed(0)
                else
                    pulseSpeed = yawPulse > 0 and pulseMagnitude or -pulseMagnitude
                    pulseUntil = now + pulseDuration
                    cooldownUntil = pulseUntil + pulseCooldown
                    pulsing = true
                    state.motorOutput = pulseSpeed
                    speedRotation.setTargetSpeed(pulseSpeed)
                end
            elseif math.abs(limited) > 0.02 and math.abs(limited) < 1 then
                if pulsing and now < pulseUntil then
                    state.motorOutput = pulseSpeed
                    speedRotation.setTargetSpeed(pulseSpeed)
                elseif now < cooldownUntil then
                    pulsing = false
                    state.motorOutput = 0
                    speedRotation.setTargetSpeed(0)
                else
                    pulseSpeed = limited >= 0 and pulseMagnitude or -pulseMagnitude
                    if state.landingStage == "yaw_align" and math.abs(limited) > 6 then
                        pulseSpeed = limited >= 0 and 2 or -2
                    end
                    pulseUntil = now + pulseDuration
                    cooldownUntil = pulseUntil + pulseCooldown
                    pulsing = true
                    state.motorOutput = pulseSpeed
                    speedRotation.setTargetSpeed(pulseSpeed)
                end
            else
                pulsing = false
                state.motorOutput = limited
                speedRotation.setTargetSpeed(limited)
            end
            if state.landingStage == "yaw_align" and (math.abs(yawPulse) <= 0.1 or not yawAlignSideReady) then
                pulsing = false
                if math.abs(yawPulse) <= 0.1 then
                    state.motorOutput = 0
                    speedRotation.setTargetSpeed(0)
                end
            elseif math.abs(TARGET_ANGLE) < 0.35 and math.abs(limited) < 0.35 and now >= cooldownUntil then
                pulsing = false
                state.motorOutput = 0
                speedRotation.setTargetSpeed(0)
            end
            local shownMotor = math.floor((state.motorOutput or 0) * 10 + 0.5)
            if shownMotor ~= lastShownMotor then
                lastShownMotor = shownMotor
                if ui.screen == "journey" then
                    markDirty()
                end
            end
            sleep(0.05)
        end
    end,
    function()
        local turnPid = PID:new(0.5, 0.001, 0.5, -50, 50)
        local lastUiPush = 0
        while state.running do
            local pose = getPose()
            if pose then
                state.poseOk = true
                local curX = pose.position.x
                local curZ = pose.position.z
                local yaw = yawFromPose(pose)
                local now = os.clock()
                local velocity = getVelocityVector()
                local velX, velY, velZ = velocityComponents(velocity)
                if state.lastNavPoint then
                    local dt = now - state.lastNavPoint.t
                    if dt > 0.05 then
                        local dx = curX - state.lastNavPoint.x
                        local dz = curZ - state.lastNavPoint.z
                        local instant = math.sqrt(dx * dx + dz * dz) / dt
                        state.horizontalSpeed = state.horizontalSpeed * 0.7 + instant * 0.3
                    end
                end
                if velX and velZ then
                    state.horizontalSpeed = math.sqrt(velX * velX + velZ * velZ)
                end
                if velY then
                    state.verticalSpeed = velY
                    state.landingSpeed = -velY
                else
                    state.landingSpeed = math.max(0, -state.verticalSpeed)
                end
                state.lastNavPoint = {x = curX, z = curZ, t = now}
                state.x = curX
                state.z = curZ
                state.yaw = yaw
                if state.auto and state.targetX and state.targetZ then
                    local activeTargetX, activeTargetZ, navStage, approachDistance = getActiveNavTarget()
                    local dx = activeTargetX - curX
                    local dz = activeTargetZ - curZ
                    local distance = math.sqrt(dx * dx + dz * dz)
                    local finalDx = state.targetX - curX
                    local finalDz = state.targetZ - curZ
                    local finalDistance = math.sqrt(finalDx * finalDx + finalDz * finalDz)
                    state.distance = finalDistance
                    state.approachDistance = navStage == "approach" and approachDistance or 0
                    if state.horizontalSpeed > 0.15 and distance > 0 then
                        state.eta = finalDistance / state.horizontalSpeed
                    else
                        state.eta = nil
                    end
                    if state.takeoff then
                        TARGET_ANGLE = 0
                        if state.height >= state.cruiseHeight - 5 then
                            state.takeoff = false
                            setClutch(false)
                            TARGET_HEIGHT = state.cruiseHeight
                            state.targetHeight = TARGET_HEIGHT
                            setLog("Cruise translation")
                        else
                            sleep(0.05)
                            goto continue_navigation
                        end
                    end
                    if not state.landing and navStage == "target" and finalDistance <= state.landingRadius then
                        beginLanding()
                    end
                    if state.landing then
                        local speed = math.max(state.horizontalSpeed, LANDING_MIN_SPEED)
                        local predictX = curX
                        local predictZ = curZ
                        if velX and velZ then
                            predictX = curX + velX * LANDING_PREDICTION_TIME
                            predictZ = curZ + velZ * LANDING_PREDICTION_TIME
                        end
                        local pDx = state.targetX - predictX
                        local pDz = state.targetZ - predictZ
                        local predictedDistance = math.sqrt(pDx * pDx + pDz * pDz)
                        local desiredAngle = 0
                        local normalizedX = pDx / math.max(predictedDistance, 0.001)
                        local normalizedZ = pDz / math.max(predictedDistance, 0.001)
                        local bearing = math.deg(math.atan2(normalizedX, normalizedZ)) * (180 / math.pi)
                        local moveCorrection = 0
                        if predictedDistance > 5 then
                            local strength = clamp(predictedDistance / LANDING_BRAKE_DISTANCE, 0.08, 1)
                            moveCorrection = turnPid:compute(0, normalizeAngle(bearing - yaw), 1) * strength
                        end
                        local normalizedYaw = normalizeAngle(yaw)
                        local yawError = normalizeAngle(FINAL_YAW - normalizedYaw)
                        if predictedDistance <= 5 and math.abs(yawError) > 3 and state.yawAlignSide == 0 then
                            state.yawAlignSide = yawError > 0 and 1 or -1
                        elseif predictedDistance > 8 then
                            state.yawAlignSide = 0
                        end
                        local yawStrength
                        if predictedDistance > 25 then
                            yawStrength = 0.08
                        elseif predictedDistance > 10 then
                            yawStrength = 0.22
                        elseif predictedDistance > 4 then
                            yawStrength = 0.45
                        else
                            yawStrength = 0.9
                        end
                        local yawCorrection = clamp(yawError * 0.9 * yawStrength, -14, 14)
                        state.yawAlignCommand = 0
                        desiredAngle =
                            clamp(
                            yawCorrection + moveCorrection * math.max(0.15, 1 - yawStrength * 0.6),
                            -LANDING_MAX_ANGLE,
                            LANDING_MAX_ANGLE
                        )
                        TARGET_ANGLE = desiredAngle
                        local descentRate
                        if predictedDistance > 50 then
                            state.landingStage = "translate"
                            descentRate = 0.15
                        elseif predictedDistance > 20 then
                            state.landingStage = "approach"
                            descentRate = 0.35
                        elseif predictedDistance > 8 then
                            state.landingStage = "precision"
                            descentRate = 0.22
                        else
                            state.landingStage = "final"
                            descentRate = 0.18
                        end
                        local landingTarget = math.max(0, state.height - descentRate)
                        if velY and velY < -0.8 then
                            landingTarget = landingTarget + math.abs(velY) * 0.25
                        end
                        TARGET_HEIGHT = landingTarget
                        state.targetHeight = landingTarget
                        if predictedDistance <= 5 and math.abs(yawError) > 6 then
                            setClutch(false)
                            if state.yawAlignSide == 0 then
                                state.yawAlignSide = yawError > 0 and 1 or -1
                            end
                        end
                        if math.abs(velY or 0) <= 0.12 and state.horizontalSpeed <= 0.25 then
                            state.landingStage = "yaw_align"
                            local yawError = normalizeAngle(FINAL_YAW - normalizedYaw)
                            TARGET_HEIGHT = 0
                            state.targetHeight = 0
                            if math.abs(yawError) > 1.5 then
                                setClutch(false)
                                state.yawAlignCommand = yawError
                                if state.yawAlignSide == 0 then
                                    state.yawAlignSide = yawError > 0 and 1 or -1
                                end
                                TARGET_ANGLE = state.yawAlignSide > 0 and 90 or -90
                            else
                                state.yawAlignCommand = 0
                                state.yawAlignSide = 0
                                TARGET_ANGLE = 0
                                setClutch(true)
                                stopFlightAndShutdown()
                            end
                        end
                        if math.abs(velY or 0) <= 0.12 and state.horizontalSpeed <= 0.25 then
                            state.yawAlignCommand = 0
                            state.yawAlignSide = 0
                            TARGET_ANGLE = 0
                            setClutch(true)
                            stopFlightAndShutdown()
                        end
                    elseif distance > 0.001 then
                        state.yawAlignSide = 0
                        local stoppingDistance = state.horizontalSpeed * 3.2
                        local targetX = activeTargetX
                        local targetZ = activeTargetZ
                        if velX and velZ then
                            targetX = targetX - velX * 1.4
                            targetZ = targetZ - velZ * 1.4
                        end
                        local correctedDx = targetX - curX
                        local correctedDz = targetZ - curZ
                        local correctedDistance = math.sqrt(correctedDx * correctedDx + correctedDz * correctedDz)
                        if correctedDistance > 0 then
                            local normalizedX = correctedDx / correctedDistance
                            local normalizedZ = correctedDz / correctedDistance
                            local bearing = math.atan(normalizedX, normalizedZ) * (180 / math.pi)
                            local error = turnPid:compute(0, normalizeAngle(bearing - yaw), 1)
                            local brakeFactor = 1
                            if distance < stoppingDistance then
                                brakeFactor = clamp(distance / stoppingDistance, 0.18, 1)
                            end
                            TARGET_ANGLE = error * brakeFactor
                            if navStage == "target" and math.abs(correctedDx) < 3 and correctedDz > 0 then
                                TARGET_ANGLE = clamp(TARGET_ANGLE, -LANDING_MAX_ANGLE, LANDING_MAX_ANGLE)
                            end
                        else
                            TARGET_ANGLE = 0
                        end
                else
                    state.approachDistance = nil
                    state.yawAlignCommand = 0
                    state.yawAlignSide = 0
                    TARGET_ANGLE = 0
                end
                else
                    state.yawAlignCommand = 0
                    state.yawAlignSide = 0
                    state.distance = previewDistance()
                    state.eta = previewEta(state.distance)
                end
            else
                state.yawAlignCommand = 0
                state.yawAlignSide = 0
                state.poseOk = false
            end
            if (ui.screen == "preview" or ui.screen == "journey") and os.clock() - lastUiPush > 0.25 then
                lastUiPush = os.clock()
                markDirty()
            end
            if state.shutdownDeadline and not state.auto and os.clock() >= state.shutdownDeadline then
                os.reboot()
                break
            end
            ::continue_navigation::
            sleep(0.05)
        end
    end,
    function()
        drawUI()
        while state.running do
            local timer = nil
            if isAnimating() then
                timer = os.startTimer(0.05)
            end
            local event = {os.pullEvent()}
            if event[1] == "ui_dirty" then
                drawUI()
            elseif event[1] == "timer" and timer and event[2] == timer then
                drawUI()
                if isAnimating() then
                    markDirty()
                end
            elseif event[1] == "mouse_click" then
                if state.shutdownDeadline then
                    state.shutdownDeadline = nil
                    setLog("Shutdown canceled")
                end
                local mx = event[3]
                local my = event[4]
                for i = #ui.widgets, 1, -1 do
                    local widget = ui.widgets[i]
                    if pointIn(mx, my, widget) then
                        handleWidget(widget.id)
                        break
                    end
                end
            elseif event[1] == "char" then
                if state.shutdownDeadline then
                    state.shutdownDeadline = nil
                    setLog("Shutdown canceled")
                end
                local ch = event[2]
                if ch == "q" then
                    state.running = false
                    break
                end
                setFocusedText(getFocusedText() .. ch)
                startAnimation(0.18)
            elseif event[1] == "paste" then
                if state.shutdownDeadline then
                    state.shutdownDeadline = nil
                    setLog("Shutdown canceled")
                end
                setFocusedText(getFocusedText() .. event[2])
                startAnimation(0.18)
            elseif event[1] == "key" then
                if state.shutdownDeadline then
                    state.shutdownDeadline = nil
                    setLog("Shutdown canceled")
                end
                local keyCode = event[2]
                if keyCode == keys.backspace then
                    local text = getFocusedText()
                    local editingField = (ui.focus == "height") or (ui.focus == "search" and ui.screen == "search")
                    if editingField and #text > 0 then
                        setFocusedText(text:sub(1, #text - 1))
                        startAnimation(0.18)
                    elseif ui.screen == "preview" then
                        switchScreen("search")
                        setLog("Back to search")
                    elseif ui.screen == "journey" then
                        stopFlight()
                    end
                elseif keyCode == keys.tab then
                    focusNext()
                elseif keyCode == keys.up and ui.screen == "search" then
                    ui.selectedSuggestion = math.max(1, ui.selectedSuggestion - 1)
                    markDirty()
                elseif keyCode == keys.down and ui.screen == "search" then
                    ui.selectedSuggestion = ui.selectedSuggestion + 1
                    markDirty()
                elseif (keyCode == keys.left or keyCode == keys.right) and ui.screen == "preview" then
                    ui.selectedAction = ui.selectedAction == "start" and "back" or "start"
                    markDirty()
                elseif keyCode == keys.enter then
                    if ui.screen == "search" then
                        applySelectedSuggestion()
                    elseif ui.screen == "preview" then
                        if ui.selectedAction == "back" then
                            switchScreen("search")
                            setLog("Back to search")
                        else
                            startFlight()
                        end
                    elseif ui.screen == "journey" then
                        stopFlight()
                    end
                elseif keyCode == keys.t and ui.screen == "preview" then
                    state.skipTakeoff = not state.skipTakeoff
                    markDirty()
                elseif keyCode == keys.space then
                    if ui.screen == "preview" then
                        startFlight()
                    elseif ui.screen == "journey" then
                        stopFlight()
                    end
                end
            elseif event[1] == "terminate" then
                state.running = false
                break
            end
        end
    end
)
speedRotation.setTargetSpeed(0)
redstone.setOutput("top", false)
setClutch(true)
term.setBackgroundColor(colors.black)
term.setTextColor(colors.white)
term.clear()
term.setCursorPos(1, 1)
print("Autopilot UI closed")