CC_Crate_3DMovement

-- Blocks Per Second at 1 RPM
baseBlocksPerSecond = 0.0380

-- Current RPM
rpm = 0

-- Current Axis Positions
currentX = 0
currentY = 0

minerExtended = false

--Possible Axes
axis = {"x", "y"}

-- Linear Interpolation
function lerp(a,b,t) return a * (1-t) + b * t end

function resetRedstoneOutputs()
    redstone.setOutput("back", false)
    redstone.setOutput("left", false)
    redstone.setOutput("right", false)
    redstone.setOutput("bottom", false)
end

-- Move a Specific Axis by an Amount
function moveAxis (selectedAxis, blocks, speed)
    -- Declare Local Variables
    local currentPosition = 0
    local inverted = false

    -- Output Redstone Signal based on Axis
    if selectedAxis == axis[1] then
        redstone.setOutput("left", false)
        currentPosition = currentX
    elseif selectedAxis == axis[2] then
        redstone.setOutput("left", true)
        currentPosition = currentY
    else
        error("Attempted to Move by Non-Existent Axis")
    end

    -- Should Travel be Inverted?
    if blocks < 0 then
        redstone.setOutput("right", true)
        inverted = true
    else
        redstone.setOutput("right", false)
        inverted = false
    end

    -- Calculate Desired Position
    local desiredPosition = currentPosition + blocks

    -- Calculate Travel Time
    local travelTime = math.abs(blocks / speed)

    -- Start Movement
    redstone.setOutput("back", true)

    -- Wait x Seconds, Finishing on arrival.
    os.sleep(travelTime)

    currentPosition = desiredPosition

    -- Stopping Movement and Resetting to Default Redstone Values
    resetRedstoneOutputs()

    -- Returning New Position
    return currentPosition

end

function resetZ()
    resetRedstoneOutputs()

    redstone.setOutput("left", true)
    redstone.setOutput("bottom", true)
    redstone.setOutput("back", true)
    os.sleep(0.5)
    while not redstone.getInput("top") do
        os.sleep(0.5)
    end

    minerExtended = false

    resetRedstoneOutputs()
end

function mine(speed)
    redstone.setOutput("left", true)
    redstone.setOutput("bottom", true)
    redstone.setOutput("right", true)
    redstone.setOutput("back", true)

    os.sleep(0.5)
    while not redstone.getInput("top") do
        os.sleep(speed)
    end

    minerExtended = true

    resetZ()
end

-- Move to Given Position
function moveToPosition(x, y)
    resetZ()
    -- Calculate Difference in Positions
    local moveX = x - currentX
    local moveY = y - currentY

    -- Calculate Speed
    local speed = baseBlocksPerSecond * rpm

    -- Move To Desired Position and Update Current Position
    if (moveX ~= 0) then currentX = moveAxis(axis[1], moveX, speed) end
    if (moveY ~= 0) then currentY = moveAxis(axis[2], moveY, speed) end
end

-- Initialisation
function init()
    resetRedstoneOutputs()
    term.clear()
    io.write("Current RPM: ")
    rpm = tonumber(read())
end

init()

-- Move To Pos Interaction
while true do
    term.clear()
    io.write("X: ")
    xPos = tonumber(read())
    io.write("Y: ")
    yPos = tonumber(read())
    print("Moving to Position...")
    moveToPosition(xPos, yPos)

    io.write("Start Mining? [Y/N]: ")
    if read() == "y" then
        mine()
    end
end