CC Create Aeronamics Car

-- Robot Controller
-- Peripheral: navigator (left), bundled cable (front)

local nav = peripheral.wrap("left")
local CABLE_SIDE = "front"

-- Bundle color bitmasks
local YELLOW  = colors.yellow
local BLACK   = colors.black
local BLUE    = colors.blue
local ORANGE  = colors.orange
local MAGENTA = colors.magenta
local RED     = colors.red

local doubleSpeed = false
local shooting = false
local running = true

-- ─── Bundle helpers ───────────────────────────────────────────────

local function getBundle()
    return redstone.getBundledOutput(CABLE_SIDE)
end

local function setBundle(mask)
    redstone.setBundledOutput(CABLE_SIDE, mask)
end

local function setBits(mask)
    setBundle(bit.bor(getBundle(), mask))
end

local function clearBits(mask)
    setBundle(bit.band(getBundle(), bit.bnot(mask)))
end

local function setColor(color, state)
    if state then setBits(color) else clearBits(color) end
end

-- ─── Motor control ────────────────────────────────────────────────

local function stop()
    -- preserve red (shooting) and orange (speed), just kill movement
    local preserve = 0
    if shooting    then preserve = bit.bor(preserve, RED)    end
    if doubleSpeed then preserve = bit.bor(preserve, ORANGE) end
    setBundle(bit.bor(MAGENTA, preserve))
end

local function drive(direction)
    local mask = 0
    -- Left/right are swapped per hardware observation, so invert them here
    if direction == "forward" then
        mask = bit.bor(YELLOW, BLACK)
    elseif direction == "backward" then
        mask = bit.bor(YELLOW, BLACK, BLUE)
    elseif direction == "right" then
        mask = BLACK                    -- was YELLOW, swapped
    elseif direction == "left" then
        mask = YELLOW                   -- was BLACK, swapped
    end
    if doubleSpeed then mask = bit.bor(mask, ORANGE) end
    if shooting    then mask = bit.bor(mask, RED)    end
    setBundle(mask)
end

-- ─── Angle helpers ────────────────────────────────────────────────

local function getAngle()
    return nav.getRelativeAngle()
end

-- Returns the shortest signed delta from `from` to `to`
-- positive = clockwise, negative = counter-clockwise
local function shortestDelta(from, to)
    local d = (to - from) % 360
    if d > 180 then d = d - 360 end
    return d
end

local function turn90(direction)
    local startAngle = getAngle()

    drive(direction)

    while true do
        os.sleep(0.02)
        local traveled = math.abs(shortestDelta(startAngle, getAngle()))
        print(traveled)
        if traveled >= 87 then break end
    end

    stop()
end

-- ─── Timed burst ──────────────────────────────────────────────────

local function burst(direction, seconds)
    drive(direction)
    os.sleep(seconds)
    stop()
end

-- ─── Instruction set runner ───────────────────────────────────────

local function runInstructions(str)
    for i = 1, #str do
        local c = str:sub(i, i)
        if c == "f" then
            burst("forward", 1)
        elseif c == "b" then
            burst("backward", 1)
        elseif c == "l" then
            turn90("left")
        elseif c == "r" then
            turn90("right")
        elseif c == "d" then
            doubleSpeed = not doubleSpeed
            print("Double speed: " .. tostring(doubleSpeed))
        end
        os.sleep(0.1)
    end
    print("Instruction set complete.")
end

-- ─── Key state tracking ───────────────────────────────────────────

local heldKeys = {}

local function updateDrive()
    local w = heldKeys[keys.w]
    local s = heldKeys[keys.s]
    local a = heldKeys[keys.a]
    local d = heldKeys[keys.d]

    if w and not s and not a and not d then
        drive("forward")
    elseif s and not w and not a and not d then
        drive("backward")
    elseif a and not d and not w and not s then
        drive("left")
    elseif d and not a and not w and not s then
        drive("right")
    elseif not w and not s and not a and not d then
        stop()
    end
end

-- ─── Input loop ───────────────────────────────────────────────────

local function inputLoop()
    while running do
        local event, key = os.pullEvent()

        if event == "key" then
            if key == keys.w or key == keys.s or
               key == keys.a or key == keys.d then
                heldKeys[key] = true
                updateDrive()

            elseif key == keys.up then
                parallel.waitForAny(
                    function() burst("forward", 1) end,
                    function() os.pullEvent("key") end
                )

            elseif key == keys.down then
                parallel.waitForAny(
                    function() burst("backward", 1) end,
                    function() os.pullEvent("key") end
                )

            elseif key == keys.left then
                parallel.waitForAny(
                    function() turn90("left") end,
                    function() os.pullEvent("key") end
                )

            elseif key == keys.right then
                parallel.waitForAny(
                    function() turn90("right") end,
                    function() os.pullEvent("key") end
                )

            elseif key == keys.x then
                doubleSpeed = not doubleSpeed
                -- update live if motors are running
                local w = heldKeys[keys.w]; local s = heldKeys[keys.s]
                local a = heldKeys[keys.a]; local d = heldKeys[keys.d]
                if w or s or a or d then updateDrive() else stop() end
                print("Double speed: " .. tostring(doubleSpeed))

            elseif key == keys.r then
                shooting = not shooting
                setColor(RED, shooting)
                print("Shooting: " .. tostring(shooting))

            elseif key == keys.i then
                stop()
                io.write("Enter instruction set: ")
                local input = io.read()
                if input and #input > 0 then
                    runInstructions(input:lower())
                end

            elseif key == keys.q then
                running = false
                stop()
                print("Shutting down.")
            end

        elseif event == "key_up" then
            if key == keys.w or key == keys.s or
               key == keys.a or key == keys.d then
                heldKeys[key] = nil
                updateDrive()
            end
        end
    end
end

-- ─── Main ─────────────────────────────────────────────────────────

stop()
print("Robot controller started.")
print("WASD = drive | arrows = burst/turn90 | X = speed | R = shoot | I = instructions | Q = quit")

inputLoop()