-- ============================================================================
-- DFPWM EGPWS/TAWS Warning System for CC: Sable
-- Featuring Attitude-Aware Logic & External Monitor Auto-Detection
-- ============================================================================

if not sublevel then
    error("Error: 'sublevel' API missing! Must be placed on a Sable Sub-Level.")
end

local dfpwm = require("cc.audio.dfpwm")
local speaker = peripheral.find("speaker")

if not speaker then
    error("Error: No CC Speaker found attached to the computer!")
end

-- ----------------------------------------------------------------------------
-- Monitor Auto-Detection & Redirection
-- ----------------------------------------------------------------------------
local mon = peripheral.find("monitor")
if mon then
    -- Scale text to fit. Adjust this value (0.5 to 5) based on your monitor size.
    mon.setTextScale(1)
    term.redirect(mon)
end

-- ----------------------------------------------------------------------------
-- Audio Management & Non-blocking DFPWM Player
-- ----------------------------------------------------------------------------
local currentAlert = nil
local isPlaying = false

-- Ensure these files exist in your computer's directory
local SOUND_FILES = {
    ["pull_up"]         = "sounds/pullup.dfpwm",
    ["terrain_pull_up"] = "sounds/terrain.dfpwm",
    ["dont_sink"]       = "sounds/dontsink.dfpwm",
    ["bank_angle"]      = "sounds/bankangle.dfpwm"
}

local function playDFPWM(filepath)
    if not fs.exists(filepath) then return end

    local decoder = dfpwm.make_decoder()
    local file = fs.open(filepath, "rb")
    if not file then return end

    isPlaying = true
    local chunkSize = 16 * 1024

    while true do
        local chunk = file.read(chunkSize)
        if not chunk then break end

        local buffer = decoder(chunk)
        -- Wait until speaker has capacity for new audio buffer
        while not speaker.playAudio(buffer) do
            os.pullEvent("speaker_audio_empty")
        end
    end

    file.close()
    isPlaying = false
end

-- Audio Worker Coroutine Loop
local function audioThread()
    while true do
        if currentAlert and not isPlaying then
            local file = SOUND_FILES[currentAlert]
            if file then
                playDFPWM(file)
                sleep(0.15) -- Delay between repeating voice loops
            else
                sleep(0.1)
            end
        else
            sleep(0.1)
        end
    end
end

-- ----------------------------------------------------------------------------
-- Telemetry & Quaternion Math Helpers
-- ----------------------------------------------------------------------------
local function quaternionToPitchRoll(q)
    local x, y, z, w = q.x or q[1] or 0, q.y or q[2] or 0, q.z or q[3] or 0, q.w or q[4] or 1

    local sinp = 2 * (w * x - y * z)
    local pitch = (math.abs(sinp) >= 1) and (math.pi / 2 * (sinp > 0 and 1 or -1)) or math.asin(sinp)

    local sinr_cosp = 2 * (w * z + x * y)
    local cosr_cosp = 1 - 2 * (x * x + z * z)
    local roll = math.atan2(sinr_cosp, cosr_cosp)

    return math.deg(pitch), math.deg(roll)
end

local function getTelemetry()
    local pos = (sublevel.getPosition and sublevel.getPosition()) or {x=0, y=0, z=0}
    local altitude = pos.y or pos[2] or 0

    local getVel = sublevel.getLinearVelocity or sublevel.getVelocity
    local vel = getVel and getVel() or {x=0, y=0, z=0}
    local vx, vy, vz = vel.x or vel[1] or 0, vel.y or vel[2] or 0, vel.z or vel[3] or 0
    local speed = math.sqrt(vx^2 + vy^2 + vz^2)

    local getRot = sublevel.getRotation or sublevel.getOrientation or sublevel.getQuaternion
    local pitch, roll = 0, 0
    if getRot then
        local rot = getRot()
        if rot.w or rot[4] then
            pitch, roll = quaternionToPitchRoll(rot)
        else
            pitch = rot.pitch or rot.x or rot[1] or 0
            roll  = rot.roll  or rot.z or rot[3] or 0
        end
    end

    return {
        alt = altitude,
        speed = speed,
        verticalSpeed = vy, -- Negative = Descending
        pitch = pitch,      -- Positive = Nose Up, Negative = Nose Down
        roll = roll         -- Positive/Negative = Left/Right bank
    }
end

-- ----------------------------------------------------------------------------
-- Main Logic & UI Refresh Loop
-- ----------------------------------------------------------------------------
local function mainThread()
    term.clear()
    term.setCursorPos(1, 1)
    print("========================================")
    print("     EGPWS ACTIVE (ATTITUDE AWARE)      ")
    print("========================================")

    while true do
        local tel = getTelemetry()
        local newAlert = nil

        -- Real-time Flight Data Display
        term.setCursorPos(1, 4)
        term.clearLine()
        print(string.format("ALT: %6.1fm | VS: %6.1fm/s | SPD: %5.1fm/s", tel.alt, tel.verticalSpeed, tel.speed))
        term.clearLine()
        print(string.format("PITCH: %5.1fdeg | ROLL: %5.1fdeg", tel.pitch, tel.roll))

        -- ====================================================================
        -- ALERT EVALUATION (Evaluated by Priority Order)
        -- ====================================================================

        -- 1. PULL UP (Altitude + Sink Rate + Pitch Angle)
        local pullUpThreshold = -12
        if tel.pitch < -15 then
            pullUpThreshold = -8   -- Nose pitched down: trigger earlier
        elseif tel.pitch > 5 then
            pullUpThreshold = -18  -- Nose pitched up (recovering): relax threshold
        end

        if tel.alt < 80 and tel.verticalSpeed < pullUpThreshold then
            newAlert = "pull_up"

        -- 2. TERRAIN PULL UP (Extreme sink rate OR low altitude steep dive)
        elseif tel.alt < 150 and (tel.verticalSpeed < -20 or (tel.alt < 60 and tel.pitch < -25)) then
            newAlert = "terrain_pull_up"

        -- 3. BANK ANGLE (Dynamic bank limit based on pitch)
        -- Default 45 deg limit, tightens to 30 deg if diving
        elseif math.abs(tel.roll) > ((tel.pitch < -10) and 30 or 45) then
            newAlert = "bank_angle"

        -- 4. DON'T SINK (Nose is pitched up > 2 deg, but altitude is dropping)
        -- Detects low altitude stalls or windshear during takeoff/climb
        elseif tel.alt < 100 and tel.pitch > 2 and tel.verticalSpeed < -1.5 and tel.speed > 5 then
            newAlert = "dont_sink"
        end

        -- Update global active alert
        currentAlert = newAlert

        -- UI Alert Bar
        term.setCursorPos(1, 8)
        term.clearLine()
        if currentAlert then
            term.blit(" WARN: " .. string.format("%-25s", currentAlert:upper()) .. " ", "fffffffffffffffffffffffffffffffff", "e44444444444444444444444444444444")
        else
            term.write(" STATUS: NORMAL FLIGHT")
        end

        sleep(0.05) -- 20Hz update tick
    end
end

-- Run UI/Telemetry logic and DFPWM Audio Driver concurrently
parallel.waitForAny(mainThread, audioThread)