DRILL_RELAY_PERIPHERAL = "redstone_relay_5"
CONTROLS_RELAY_PERIPHERAL = "redstone_relay_6"
ENDSTOP_RELAY_PERIPHERAL = "redstone_relay_8"
DRILL_RPM = 32
MOVE_RPM = 120
-- The dimensions of the quarry site { x/width, z/length }
LENGTH = { 6, 6 }

local phDrill = peripheral.wrap(DRILL_RELAY_PERIPHERAL)
local phCtrl = peripheral.wrap(CONTROLS_RELAY_PERIPHERAL)
local phEnd = peripheral.wrap(ENDSTOP_RELAY_PERIPHERAL)

local DRILL_CLUTCH = "right"   -- Activates drill if unpowered
local DRILL_GEARSHIFT = "left" -- Moves drill up if powered

local MOVE_CLUTCH = "left"     -- Activates gantry if unpowered
local MOVE_GEARSHIFT = "right" -- Moves gantry backwards if powered, forward if not powered
local MOVE_INVERT = "front"    -- Switches axis Z to X

local ENDSTOP_X = "left"      -- Detects end of X axis
local ENDSTOP_Z = "right"      -- Detects end of Z axis, home
local HOME_TIMEOUT_ITERATIONS = 30


-- END CONFIG

local function pick(value, a, b)
    if value then
        return a
    else
        return b
    end
end
local function clamp(val, min, max)
    if val < min then
        return min
    elseif val > max then
        return max
    else
        return val
    end
end
BLOCKS_PER_TICK = clamp(MOVE_RPM / 512, -0.49, 0.49)
local function calculateSeconds(numBlocks)
    return ((math.abs(numBlocks)) * BLOCKS_PER_TICK) + BLOCKS_PER_TICK
end

local function moveGantry(speed, invert)
    if invert then
        phCtrl.setOutput(MOVE_GEARSHIFT, true)
    end
    phCtrl.setOutput(MOVE_CLUTCH, false)
    sleep(speed)
    phCtrl.setOutput(MOVE_CLUTCH, true)
    phCtrl.setOutput(MOVE_GEARSHIFT, false)
end

function MoveX(blocks)
    local sec = calculateSeconds(blocks)
    -- Set inverse control to switch to X axis
    phCtrl.setOutput(MOVE_INVERT, true)
    moveGantry(sec, blocks < 0)
    phCtrl.setOutput(MOVE_INVERT, false)
end
function MoveZ(blocks)
    local sec = calculateSeconds(blocks)
    moveGantry(sec, blocks < 0)
end
--- Move the drill up or down N blocks
function MoveDrill(blocks)
    local sec = calculateSeconds(blocks)
    if blocks > 0 then
        phDrill.setOutput(DRILL_GEARSHIFT, true)
    end
    sleep(sec)
    phDrill.setOutput(DRILL_CLUTCH, true)
    phDrill.setOutput(DRILL_GEARSHIFT, false)
end

function Home()
    -- Bring the drill up
    phDrill.setOutput(DRILL_CLUTCH, false)
    phDrill.setOutput(DRILL_GEARSHIFT, true)

    io.write("Homing Z-axis.. ")
    local i = 0
    if phEnd.getInput(ENDSTOP_Z) then
        MoveZ(1)
        i = i + 1
        if i > HOME_TIMEOUT_ITERATIONS then
            error("Home X Timed out")
        end
        -- if ENDSTOP still active: at the end
        -- if ENDSTOP not active: at the start
    end
    -- Move back
    repeat
        MoveZ(-1)
    until phEnd.getInput(ENDSTOP_Z)
    print("done")

    io.write("Homing X-axis.. ")
    i = 0
    MoveX(-1)
    repeat
        MoveX(1)
        i = i + 1
        if i > HOME_TIMEOUT_ITERATIONS then
            error("Home X Timed out")
        end
    until phEnd.getInput(ENDSTOP_X)
    print("done")
    Reset()
end


-- Tries to auto determine size, must be called after Home()
function CalculateSize()
    local x = 2 -- timing issues mean its off by 2
    local z = 2
    io.write("X: ")
    repeat
        MoveX(-1)
        x = x + 1
    until phEnd.getInput(ENDSTOP_X)
    print(x)

    sleep(1)

    io.write("Z: ")
    repeat
        MoveZ(1)
        z = z + 1
    until phEnd.getInput(ENDSTOP_Z)
    print(z)

    return { x, z }
end

function IsAtEndstop(type)
    if type == "X" then
        return phEnd.getInput(ENDSTOP_X)
    elseif type == "Z" then
        return phEnd.getInput(ENDSTOP_Z)
    else
        error("invalid type")
    end
end

-- Mines a layer, assuming at home
function MineLayer(width, length)
    local isRight = -1
    local z = 1
    phCtrl.setOutput(MOVE_CLUTCH, false)
    repeat
        print("Start Line", z)
        -- phCtrl.setOutput(MOVE_CLUTCH, false)
        phCtrl.setOutput(MOVE_INVERT, true) -- use x axis
        -- phCtrl.setOutput(MOVE_GEARSHIFT, true)
        -- phCtrl.setOutput(MOVE_GEARSHIFT, (not isLeft) and true or false)
        io.write("X: moving")
        io.write((width * isRight) .. " blocks..")
        
        MoveX(width * isRight)
        isRight = pick(isRight > 0, -1, 1)
        -- repeat until os.pullEvent("redstone") and phEnd.getInput(ENDSTOP_X)
        print("Done")

        -- io.write("X: moving right.. ")
        -- phCtrl.setOutput(MOVE_GEARSHIFT, false)
        -- repeat until os.pullEvent("redstone") and phEnd.getInput(ENDSTOP_X)
        -- print("Done")
        -- isLeft = not isLeft
        -- repeat
        --     sleep(calculateSeconds(2))
        --     -- phCtrl.setOutput(MOVE_CLUTCH, true)
        --     -- phCtrl.setOutput(MOVE_GEARSHIFT, false)
        --     x = x + 1
        -- until phEnd.getInput(ENDSTOP_X)

        -- -- reverse
        -- phCtrl.setOutput(MOVE_GEARSHIFT, false)
        -- repeat
        --     sleep(0.2)
        --     -- phCtrl.setOutput(MOVE_CLUTCH, true)
        --     -- phCtrl.setOutput(MOVE_GEARSHIFT, false)
        --     x = x + 1
        -- until phEnd.getInput(ENDSTOP_X)

        -- advance Z
        -- z = z + 1
        io.write("Z: advancing 1.. ")
        phCtrl.setOutput(MOVE_CLUTCH, false)
        phCtrl.setOutput(MOVE_INVERT, false) -- use z axis
        phCtrl.setOutput(MOVE_GEARSHIFT, false)
        -- MoveZ(-4)
        sleep(calculateSeconds(1))
        phCtrl.setOutput(MOVE_CLUTCH, true)
        z = z + 1
        print("Done")
        -- sleep(0.4)
    until phEnd.getInput(ENDSTOP_Z)
end

function Reset()
    phDrill.setOutput(DRILL_CLUTCH, true)
    phDrill.setOutput(DRILL_GEARSHIFT, false)

    phCtrl.setOutput(MOVE_CLUTCH, true)
    phCtrl.setOutput(MOVE_GEARSHIFT, false)
    phCtrl.setOutput(MOVE_INVERT, false)
end