local nav = peripheral.wrap("left")
local vel = peripheral.wrap("right")

-- toggle TOP   = stop LEFT rotor  = ship turns LEFT  (right rotor pushes)
-- toggle FRONT = stop RIGHT rotor = ship turns RIGHT (left rotor pushes)
local LEFT_STOP  = "top"
local RIGHT_STOP = "front"

local TURN_THRESHOLD = 10
local STOP_DIST      = 10

local sx, sz      = 0, 0
local tx, tz      = 0, 0
local heading     = 0
local speed       = 0
local forceMode   = nil
local statusText  = "Flying"
local arrived     = false
local angleLocked = false
local logs        = {}
local fixedBearing = 0
local totalDist    = 0
local elapsed      = 0

local W, H     = term.getSize()
local MAX_LOGS = H - 11

local function turnLeft()   rs.setOutput(LEFT_STOP, true)  rs.setOutput(RIGHT_STOP, false) end
local function turnRight()  rs.setOutput(LEFT_STOP, false) rs.setOutput(RIGHT_STOP, true)  end
local function flyForward() rs.setOutput(LEFT_STOP, false) rs.setOutput(RIGHT_STOP, false) end
local function stopAll()    rs.setOutput(LEFT_STOP, true)  rs.setOutput(RIGHT_STOP, true)  end

local function normAngle(a)
    a = a % 360
    if a > 180 then a = a - 360 end
    return a
end

-- compass: 0=South, 90=West, 180=North, 270=East
local function calcBearing(x1, z1, x2, z2)
    local dx = x2 - x1
    local dz = z2 - z1
    return (math.deg(math.atan2(-dx, dz)) + 360) % 360
end

local function dist(x1, z1, x2, z2)
    return math.sqrt((x2-x1)^2 + (z2-z1)^2)
end

local function addLog(msg)
    table.insert(logs, os.date("%H:%M:%S") .. " " .. msg)
    if #logs > MAX_LOGS then table.remove(logs, 1) end
end

local function drawUI()
    term.clear()
    term.setCursorPos(1, 1)
    term.setBackgroundColor(colors.blue)
    term.setTextColor(colors.white)
    term.clearLine()
    term.write(" Aeronautics Autopilot")
    term.setBackgroundColor(colors.black)

    local progress = totalDist > 0 and math.min(1, elapsed / totalDist) or 0
    local barW     = W - 12
    local filled   = math.floor(progress * barW)
    local distLeft = math.max(0, totalDist - elapsed)
    local err      = normAngle(fixedBearing - heading)

    term.setTextColor(colors.white)
    term.setCursorPos(1, 2)
    term.write(string.format(" Elapsed %-8.1f blk  Hdg: %-6.1f", elapsed, heading))
    term.setCursorPos(1, 3)
    term.write(string.format(" Remain  %-8.1f blk  Brg: %-6.1f", distLeft, fixedBearing))
    term.setCursorPos(1, 4)
    term.write(string.format(" Total   %-8.1f blk  Err: %-+6.1f", totalDist, err))
    term.setCursorPos(1, 5)
    term.write(string.format(" Speed   %-8.2f b/s", speed))

    term.setCursorPos(1, 6)
    term.setTextColor(colors.gray)
    term.write(" Prog [")
    term.setTextColor(colors.lime)
    term.write(string.rep("=", filled))
    term.setTextColor(colors.gray)
    term.write(string.rep("-", barW - filled))
    term.write("] ")
    term.setTextColor(colors.white)
    term.write(string.format("%.0f%%", progress * 100))

    term.setCursorPos(1, 7)
    local modeStr, modeCol
    if forceMode then
        modeStr = "MANUAL: " .. forceMode
        modeCol = colors.orange
    elseif arrived then
        modeStr = "ARRIVED"
        modeCol = colors.lime
    elseif angleLocked then
        modeStr = "LOCKED: Flying straight"
        modeCol = colors.cyan
    else
        modeStr = "AUTO: " .. statusText
        modeCol = colors.green
    end
    term.setTextColor(modeCol)
    term.write(" " .. modeStr)
    term.setTextColor(colors.white)

    term.setCursorPos(1, 8)
    term.setTextColor(colors.gray)
    term.write(string.rep("-", W))

    term.setTextColor(colors.lightGray)
    for i, log in ipairs(logs) do
        term.setCursorPos(1, 8 + i)
        term.write((" " .. log):sub(1, W))
    end

    term.setCursorPos(1, H - 1)
    term.setTextColor(colors.gray)
    term.write(string.rep("-", W))
    term.setCursorPos(1, H)
    term.setTextColor(colors.yellow)
    term.write("[F]wd [L]ft [R]ght [S]top [A]uto [K]lock [Q]uit")
    term.setTextColor(colors.white)
end

local function correctHeading()
    local err = normAngle(fixedBearing - heading)
    if math.abs(err) < TURN_THRESHOLD then return end

    local goRight = err > 0
    local dir = goRight and "Right" or "Left"
    addLog(string.format("Turning %s (hdg=%.1f err=%.1f)", dir, heading, err))

    if goRight then turnRight() else turnLeft() end

    while true do
        sleep(0.1)
        heading = nav.getRelativeAngle()
        speed   = vel.getVelocity()
        elapsed = elapsed + speed * 0.1
        statusText = string.format("Turning %s hdg=%.1f", dir, heading)
        drawUI()

        local newErr = normAngle(fixedBearing - heading)

        -- Stop early — ship coasts ~25 degrees after motor cut
        local stopEarly = TURN_THRESHOLD + 25
        if math.abs(newErr) < stopEarly then
            stopAll()
            addLog(string.format("Motor cut at hdg=%.1f err=%.1f (coasting...)", heading, newErr))
            sleep(3.0)
            heading = nav.getRelativeAngle()
            local finalErr = normAngle(fixedBearing - heading)
            addLog(string.format("Coasted to hdg=%.1f err=%.1f", heading, finalErr))
            -- If still off after coasting, correctHeading will be called again by flightLoop
            return
        end

        -- Overshot: sign flipped well past threshold
        local overshot = (goRight and newErr < -stopEarly) or
                         (not goRight and newErr > stopEarly)
        if overshot then
            stopAll()
            addLog(string.format("Overshot! hdg=%.1f err=%.1f — correcting...", heading, newErr))
            sleep(3.0)
            heading = nav.getRelativeAngle()
            correctHeading()
            return
        end
    end
end

local function inputLoop()
    while not arrived do
        local _, key = os.pullEvent("key")
        if key == keys.f then
            forceMode = "FORWARD"
            angleLocked = false
            flyForward()
            addLog("Manual: Forward")
        elseif key == keys.l then
            forceMode = "LEFT"
            angleLocked = false
            turnLeft()
            addLog("Manual: Turn Left")
        elseif key == keys.r then
            forceMode = "RIGHT"
            angleLocked = false
            turnRight()
            addLog("Manual: Turn Right")
        elseif key == keys.s then
            forceMode = "STOP"
            angleLocked = false
            stopAll()
            addLog("Manual: Stop")
        elseif key == keys.a then
            forceMode = nil
            angleLocked = false
            addLog("Resumed auto")
        elseif key == keys.k then
            forceMode = nil
            angleLocked = not angleLocked
            if angleLocked then
                flyForward()
                addLog(string.format("Angle locked at hdg=%.1f", heading))
            else
                addLog("Angle lock released, resuming auto")
            end
        elseif key == keys.q then
            stopAll()
            addLog("Aborted by user")
            arrived = true
        end
        drawUI()
    end
end

local function flightLoop()
    local lastT = os.clock()

    while not arrived do
        local now = os.clock()
        local dt  = now - lastT
        lastT = now

        heading = nav.getRelativeAngle()
        speed   = vel.getVelocity()
        elapsed = elapsed + speed * dt

        if elapsed >= totalDist - STOP_DIST then
            stopAll()
            statusText = "Arrived"
            addLog(string.format("Arrived! Flew %.1f blk", elapsed))
            arrived = true
            drawUI()
            break
        end

        if not forceMode then
            if angleLocked then
                flyForward()
                statusText = "Locked"
            else
                local err = normAngle(fixedBearing - heading)
                if math.abs(err) < TURN_THRESHOLD then
                    flyForward()
                    statusText = "Forward"
                else
                    stopAll()
                    sleep(0.5)
                    heading = nav.getRelativeAngle()
                    correctHeading()
                end
            end
        end

        drawUI()
        sleep(0.1)
    end
end

-- Setup
term.clear()
term.setCursorPos(1, 1)
print("=== Aeronautics Autopilot ===")
print("Compass: 0=South 90=West 180=North 270=East")
print("")
io.write("Start X:  ") sx = tonumber(io.read())
io.write("Start Z:  ") sz = tonumber(io.read())
io.write("Target X: ") tx = tonumber(io.read())
io.write("Target Z: ") tz = tonumber(io.read())

fixedBearing = calcBearing(sx, sz, tx, tz)
totalDist    = dist(sx, sz, tx, tz)

addLog(string.format("Route: (%d,%d) -> (%d,%d)  %.1f blk", sx, sz, tx, tz, totalDist))
addLog(string.format("Bearing: %.1f  Current hdg: %.1f", fixedBearing, nav.getRelativeAngle()))

parallel.waitForAny(flightLoop, inputLoop)

sleep(2)
term.clear()
term.setCursorPos(1, 1)
print("Autopilot ended.")