Advanced Movement

    -- pastebin get Z5rK8b3r Advanced Movement

    BM = require("BM")
    POS = require("POS")

    AM = {}

    local function isTurtleBlock(data)
        return data and type(data.name) == "string" and data.name:match("computercraft:turtle")
    end

    function AM.joinQueue()
        local initData = SDM.TURTLE.loadInitData()
        local finishCoord = initData.diskFinishCoord
        finishCoord.y = initData.diskFinishCoord.y + 1
        if not CVF.equalCoords(POS.position, finishCoord) then return false end

        BM.turnInDirection("BACKWARD")

        local level = 0
        while true do
            local success, data = turtle.inspect()
            if not (success and isTurtleBlock(data)) then
                while not BM.forward() do sleep(0.5) end
                print("\nEntering queue...")
                if level % 2 == 0 then
                    BM.turnInDirection("RIGHT")
                else
                    BM.turnInDirection("LEFT")
                end
                break
            else
                while not BM.up() do sleep(0.1) end
                level = level + 1
            end
        end

        SDM.TURTLE.setQueueState(true)
    end

    function AM.queueForTask()
        if not SDM.TURTLE.getQueueState() then return false end

        local initData = SDM.TURTLE.loadInitData()
        local taskOutputCoord = {x = initData.diskOutCoord.x, y = initData.diskOutCoord.y + 1, z = initData.diskOutCoord.z}

        while true do
            local level = POS.position.y - initData.diskFinishCoord.y - 1

            local posQueueLeft = CVF.sub(initData.diskFinishCoord, initData.dirSection)
            local posQueueRight = CVF.add(posQueueLeft, CVF.mult(4, initData.dirTopBelt))

            posQueueLeft.y = POS.position.y
            posQueueRight.y = POS.position.y

            if level % 2 == 0 then
                BM.turnInDirection("RIGHT")
                if level == 0 then
                    local bFirst = true
                    while not CVF.equalCoords(taskOutputCoord, POS.position) do
                        if not BM.forward() then
                            sleep(0.5)
                        end
                    end
                    print("Exiting Queue")
                    break
                end

                while not CVF.equalCoords(posQueueRight, POS.position) do
                    if not BM.forward() then sleep(0.5) end
                end
                while not BM.down() do sleep(0.5) end
                level = level - 1
            else
                BM.turnInDirection("LEFT")
                while not CVF.equalCoords(posQueueLeft, POS.position) do
                    if not BM.forward() then sleep(0.5) end
                end
                while not BM.down() do sleep(0.5) end
                level = level - 1
            end
        end

        SDM.TURTLE.setQueueState(false)
        return true
    end

    function AM.reachChestPlacementCoord(targPos)
        local initData = SDM.TURTLE.loadInitData()

        local targSection = POS.getSection(targPos)
        local d1 = (targPos[POS.sectionDir] - POS.position[POS.sectionDir]) * initData.dirSection[POS.sectionDir]

        if POS.position.x ~= targPos.x or POS.position.z ~= targPos.z then
            while POS.position.y % initData.blockHeight ~= (initData.lowBeltY + 6) % initData.blockHeight do
                BM.up()
            end
        end

        local d2 = (targPos[POS.beltDir] - POS.position[POS.beltDir]) * initData.dirTopBelt[POS.beltDir]

        if d2 < 0 then
            BM.turnInDirection("LEFT")
            while targPos[POS.beltDir] ~= POS.position[POS.beltDir] do
                while not BM.forward() do sleep(0.1) end
            end
        elseif d2 > 0 then
            BM.turnInDirection("RIGHT")
            while targPos[POS.beltDir] ~= POS.position[POS.beltDir] do
                while not BM.forward() do sleep(0.1) end
            end
        end
        BM.turnInDirection("FORWARD")

        if d1 > 0 then
            while targPos[POS.sectionDir] ~= POS.position[POS.sectionDir] do
                while not BM.forward() do sleep(0.1) end
            end
        else
            while targPos[POS.sectionDir] ~= POS.position[POS.sectionDir] do
                while not BM.back() do sleep(0.1) end
            end
        end
        while targPos.y > POS.position.y - 1 do
            while not BM.up() do sleep(0.1) end
        end
        while targPos.y < POS.position.y - 1 do
            while not BM.down() do sleep(0.1) end
        end
    end
    -----------------------------------------------------------------------------------
    -- joinBeltFromOutside      - joins lower belt
    -- reachOutsideTarget       - reaches outside target if on route
    -----------------------------------------------------------------------------------
    function AM.joinBeltFromOutside()
        if POS.isInBounds() then return false end

        local initData = SDM.TURTLE.loadInitData()
        local B_Dir = POS.beltDir
        local S_Dir = POS.sectionDir

        if POS.position[B_Dir] % 2 ~= initData.entryModulo then
            BM.turnInDirection("LEFT")
            while not BM.forward() do sleep(0.1) end
        end

        while POS.position.y > initData.lowBeltY do
            while not BM.down() do sleep(0.1) end
        end
        while POS.position.y < initData.lowBeltY do
            while not BM.up() do sleep(0.1) end
        end

        BM.turnInDirection("FORWARD")
        while not POS.isOnBottomBelt() do
            while not BM.forward() do sleep(0.1) end
        end
        BM.turnInDirection("LEFT")
        return true
    end
    function AM.reachOutsideTarget(targPos)
        if POS.isInBounds(targPos) then return false end
        local initData = SDM.TURTLE.loadInitData()
        local B_Dir = POS.beltDir
        local S_Dir = POS.sectionDir

        local d1 = (POS.position[S_Dir] - targPos[S_Dir]) * initData.dirTopBelt[S_Dir]
        if d1 < 0 then return false end

        if targPos[B_Dir] % 2 == initData.exitModulo then
            if POS.position[B_Dir] ~= targPos[B_Dir] then
                return false
            end
            if POS.position[S_Dir] ~= targPos[S_Dir] and POS.position.y ~= initData.lowBeltY then
                return false
            end
            if POS.position.y > initData.lowBeltY and targPos.y < POS.position.y then
                return false
            end
            if POS.position.y < initData.lowBeltY and targPos.y > POS.position.y then
                return false
            end
        elseif targPos[B_Dir] % 2 ~= initData.exitModulo then
            if POS.position[B_Dir] ~= targPos[B_Dir] + initData.dirTopBelt[B_Dir] then
                return false
            end
            if POS.position[S_Dir] ~= targPos[S_Dir] and POS.position.y ~= initData.lowBeltY then
                return false
            end
            if POS.position.y > initData.lowBeltY and targPos.y < POS.position.y then
                return false
            end
            if POS.position.y < initData.lowBeltY and targPos.y > POS.position.y then
                return false
            end
        end

        BM.turnInDirection("BACKWARD")

        while POS.position[S_Dir] ~= targPos[S_Dir] do
            while not BM.forward() do sleep(0.1) end
        end
        while POS.position.y > targPos.y do
            while not BM.down() do sleep(0.1) end
        end
        while POS.position.y < targPos.y do
            while not BM.up() do sleep(0.1) end
        end


        if POS.position[B_Dir] ~= targPos[B_Dir] then
            BM.turnInDirection("LEFT")
            while not BM.forward() do sleep(0.1) end
            BM.turnInDirection("FORWARD")
        end

        if CVF.equalCoords(targPos, POS.position) then
            return true
        end
        return false
    end
    -----------------------------------------------------------------------------------
    -- switchBelts          - switches the belts by using the elevators
    -- useAndExitBelt       - reaches belt exit (elevator or right exitmodulo)
    -----------------------------------------------------------------------------------
    function AM.switchBelts()
        if POS.isOnBottomBelt() then
            if not POS.isOnBeltExitUp() then
                BM.turnInDirection("LEFT")
            end
            while not POS.isOnBeltExitUp() do
                while not (BM.up() or BM.forward()) do sleep(0.1) end
                if POS.isOnTopBelt() then
                    BM.turnInDirection("RIGHT")
                    return true
                end
            end
            BM.turnInDirection("FORWARD")
            while not BM.forward() do sleep(0.1) end
            while not BM.up() do sleep(0.1) end
            while not BM.back() do sleep(0.1) end
            BM.turnInDirection("RIGHT")
            return true
        elseif POS.isOnTopBelt() then
            if not POS.isOnBeltExitDown() then
                BM.turnInDirection("RIGHT")
            end
            while not POS.isOnBeltExitDown() do
                while not (BM.down() or BM.forward()) do sleep(0.1) end
                if POS.isOnBottomBelt() then
                    BM.turnInDirection("LEFT")
                    return true
                end
            end
            BM.turnInDirection("FORWARD")
            while not BM.forward() do sleep(0.1) end
            while not BM.down() do sleep(0.1) end
            while not BM.back() do sleep(0.1) end
            BM.turnInDirection("LEFT")
            return true
        end
        return false
    end
    function AM.useAndExitBelt(targPos)
        local initData = SDM.TURTLE.loadInitData()
        if not POS.isInBounds(targPos) then
            local exitPos = targPos[POS.beltDir]
            if exitPos % 2 ~= initData.exitModulo then
                exitPos = exitPos + initData.dirTopBelt[POS.beltDir]
            end
            local d1 = (exitPos - POS.position[POS.beltDir]) * initData.dirTopBelt[POS.beltDir]
            if d1 > 0 then                      -- exit on right
                if POS.isOnBottomBelt() then
                    AM.switchBelts()
                end
                BM.turnInDirection("RIGHT")
            elseif d1 < 0 then                  -- exit on left
                if POS.isOnTopBelt() then
                    AM.switchBelts()
                end
                BM.turnInDirection("LEFT")
            end

            while not (POS.position[POS.beltDir] == exitPos) do
                while not BM.forward() do sleep(0.1) end
            end

            if POS.isOnTopBelt() then
                AM.switchBelts()
                while not (POS.position[POS.beltDir] == exitPos) do
                    while not BM.forward() do sleep(0.1) end
                end
            end
            BM.turnInDirection("BACKWARD")
            while not BM.forward() do sleep(0.1) end

            return true
        end
        if POS.isOnBottomBelt(targPos) or POS.isOnTopBelt(targPos) then
            local d1 = (targPos[POS.beltDir] - POS.position[POS.beltDir]) * initData.dirTopBelt[POS.beltDir]
            if d1 > 0 and POS.isOnBottomBelt() then
                AM.switchBelts()
            elseif d1 < 0 and POS.isOnTopBelt() then
                AM.switchBelts()
            end
            while targPos[POS.beltDir] ~= POS.position[POS.beltDir] do
                while not BM.forward() do sleep(0.1) end
            end
            if targPos.y ~= POS.position.y then
                AM.switchBelts()
                while targPos[POS.beltDir] ~= POS.position[POS.beltDir] do
                    while not BM.forward() do sleep(0.1) end
                end
            end
            return true
        end
        local turtSection = POS.getSection()
        local targSection = POS.getSection(targPos)

        local targSecHeight = POS.getSectionHeight(targPos)
        local exitPos = targSection
        local bUp = true

        if targSecHeight > initData.lowBeltY then
            exitPos = exitPos + initData.dirTopBelt[POS.beltDir]
        elseif targSecHeight < initData.lowBeltY then
            exitPos = exitPos + initData.dirBottomBelt[POS.beltDir]
            bUp = false
        else
            bUp = false
        end
        local d1 = (exitPos - POS.position[POS.beltDir]) * initData.dirTopBelt[POS.beltDir]

        if d1 > 0 and exitPos == targSection then
            exitPos = exitPos - initData.dirTopBelt[POS.beltDir]
        end

        if d1 > 0 and POS.isOnBottomBelt() then
            AM.switchBelts()
        end
        if d1 < 0 and POS.isOnTopBelt() then
            AM.switchBelts()
        end

        if POS.isOnBottomBelt() then BM.turnInDirection("LEFT") end
        if POS.isOnTopBelt() then BM.turnInDirection("RIGHT") end

        while exitPos ~= POS.position[POS.beltDir] do
            while not BM.forward() do sleep(0.1) end
        end

        if POS.isOnBottomBelt() and not bUp and not (exitPos == targSection) then
            AM.switchBelts()
            BM.turnInDirection("RIGHT")
            while exitPos ~= POS.position[POS.beltDir] do
                while not BM.forward() do sleep(0.1) end
            end
        elseif POS.isOnTopBelt() and bUp then
            AM.switchBelts()
            BM.turnInDirection("LEFT")
            while exitPos ~= POS.position[POS.beltDir] do
                while not BM.forward() do sleep(0.1) end
            end
        end

        BM.turnInDirection("FORWARD")
        while not BM.forward() do sleep(0.1) end

        return true
    end


    function AM.useElevatorUp(targPos)
        if not POS.isOnElevatorUp() then return false end

        local initData = SDM.TURTLE.loadInitData()

        local turtSection = POS.getSection()
        local targSection = POS.getSection(targPos)

        local targSecHeight = POS.getSectionHeight(targPos)

        if type(targSection) == "string" then
            targSection = targPos[POS.beltDir]
            if targSection == turtSection then
                targSection = targSection + initData.dirBottomBelt[POS.beltDir]
            end
            targSecHeight = initData.lowBeltY
        end
        if POS.isOnElevatorDown(targPos) then targSecHeight = targSecHeight + initData.blockHeight end

        local d1 = (targSection - turtSection) * initData.dirTopBelt[POS.beltDir]

        if turtSection ~= targSection then
            if POS.position.y <= initData.lowBeltY + 1 then
                while POS.position.y ~=  initData.lowBeltY + 1 do
                    while not BM.up() do sleep(0.1) end
                end
                BM.turnInDirection("BACKWARD")
                while not BM.forward() do sleep(0.1) end
                return
            end
            while POS.position.y % 8 ~= initData.lowBeltY % 8 do
                while not BM.up() do sleep(0.1) end
            end
            BM.turnInDirection("LEFT")
            while not BM.forward() do sleep(0.1) end
            return
        end

        if POS.isOnElevatorUp(targPos)then
            if targPos.y >= POS.position.y then
                while targPos.y ~= POS.position.y do
                    while not BM.up() do sleep(0.1) end
                end
            else
                while POS.position.y % 8 ~= initData.lowBeltY % 8 do
                    while not BM.up() do sleep(0.1) end
                end
                BM.turnInDirection("LEFT")
                while not BM.forward() do sleep(0.1) end
            end
            return
        end

        while targSecHeight > POS.position.y do
            while not BM.up() do sleep(0.1) end
        end
        while POS.position.y % 8 ~= initData.lowBeltY % 8 do
            while not BM.up() do sleep(0.1) end
        end
        BM.turnInDirection("LEFT")
        while not BM.forward() do sleep(0.1) end
        return
    end

    -- Turtle_ST = require("Turtle_ST")
    function AM.useElevatorDown(targPos)
        if not POS.isOnElevatorDown() then return false end

        local initData = SDM.TURTLE.loadInitData()

        local turtSection = POS.getSection()
        local targSection = POS.getSection(targPos)

        local targSecHeight = POS.getSectionHeight(targPos)

        if type(targSection) == "string" then
            targSection = targPos[POS.beltDir]
            if targSection == turtSection then
                targSection = targSection + initData.dirTopBelt[POS.beltDir]
            end
            targSecHeight = initData.lowBeltY
        end
        if POS.isOnElevatorUp(targPos) then targSecHeight = targSecHeight - initData.blockHeight end

        local d1 = (targSection - turtSection) * initData.dirTopBelt[POS.beltDir]

        if turtSection ~= targSection then
            if POS.position.y >= initData.lowBeltY then
                while POS.position.y ~=  initData.lowBeltY do
                    while not BM.down() do sleep(0.1) end
                end
                BM.turnInDirection("BACKWARD")
                while not BM.forward() do sleep(0.1) end
                return
            end
            while POS.position.y % 8 ~= initData.lowBeltY % 8 do
                while not BM.down() do sleep(0.1) end
            end
            BM.turnInDirection("RIGHT")
            while not BM.forward() do sleep(0.1) end
            return
        end

        if POS.isOnElevatorDown(targPos)then
            if targPos.y <= POS.position.y then
                while targPos.y ~= POS.position.y do
                    while not BM.down() do sleep(0.1) end
                end
            else
                while POS.position.y % 8 ~= initData.lowBeltY % 8 do
                    while not BM.down() do sleep(0.1) end
                end
                BM.turnInDirection("RIGHT")
                while not BM.forward() do sleep(0.1) end
            end
            return
        end

        while targSecHeight < POS.position.y do
            while not BM.down() do sleep(0.1) end
        end
        while POS.position.y % 8 ~= initData.lowBeltY % 8 do
            while not BM.down() do sleep(0.1) end
        end
        BM.turnInDirection("RIGHT")
        while not BM.forward() do sleep(0.1) end
        return
    end

    function AM.useBottomSection(targPos)
        if not POS.isOnBottomSection() then return false end

        local initData = SDM.TURTLE.loadInitData()

        local turtSection = POS.getSection()
        local targSection = POS.getSection(targPos)

        local targSecHeight = POS.getSectionHeight(targPos)
        local turtSecHeight = POS.getSectionHeight()

        local d1 = (targPos[POS.sectionDir] - POS.position[POS.sectionDir]) * initData.dirSection[POS.sectionDir]

        local exception1 = targSection ~= turtSection
        local exception2 = targSecHeight ~= turtSecHeight
        local exception3 = d1 < 0

        if d1 ~= 0 or POS.isOnElevatorUp(targPos) or POS.isOnElevatorDown(targPos) then
            BM.turnInDirection("FORWARD")
        end

        if exception1 or exception2 or exception3 then
            while not BM.forward() do sleep(0.1) end
            while not BM.forward() do sleep(0.1) end
            BM.turnInDirection("LEFT")
            while not BM.forward() do sleep(0.1) end
            return
        end

        while targPos[POS.sectionDir] ~= POS.position[POS.sectionDir] do
            while not BM.forward() do sleep(0.1) end
        end

        local d2 = (targPos[POS.beltDir] - POS.position[POS.beltDir]) * initData.dirTopBelt[POS.beltDir]

        if d2 > 0 then BM.turnInDirection("RIGHT") end
        if d2 < 0 then BM.turnInDirection("LEFT") end
        if d2 == 0 then return end

        while not BM.forward() do sleep(0.1) end
        return
    end

    function AM.useChestRow(targPos)
        if not (POS.isOnLeftChestRow() or POS.isOnRightChestRow()) then return end

        local initData = SDM.TURTLE.loadInitData()

        local targSecHeight = POS.getSectionHeight(targPos)
        local turtSecHeight = POS.getSectionHeight()

        local exception1 = targPos[POS.sectionDir] ~= POS.position[POS.sectionDir]
        local exception2 = targPos[POS.beltDir] ~= POS.position[POS.beltDir]
        local exception3 = targPos.y < POS.position.y
        local exception4 = targSecHeight ~= turtSecHeight

        if exception1 or exception2 or exception3 or exception4 then
            while not POS.isOnHighestChestRow() do
                while not BM.up() do sleep(0.1) end
            end

            if POS.isOnLeftChestRow() then BM.turnInDirection("LEFT") end
            if POS.isOnRightChestRow() then BM.turnInDirection("RIGHT") end

            while not BM.back() do sleep(0.1) end

            BM.turnInDirection("BACKWARD")
            return
        end

        while targPos.y ~= POS.position.y do
            while not BM.up() do sleep(0.1) end
        end
        return
    end

    function AM.useTopSection(targPos)
        if not POS.isOnTopsection() then return end

        local initData = SDM.TURTLE.loadInitData()
        local elevatorPos = initData.initChestCoord[POS.sectionDir] - initData.dirSection[POS.sectionDir]

        local targSecHeight = POS.getSectionHeight(targPos)

        local turtSection = POS.getSection()
        local targSection = POS.getSection(targPos)

        BM.turnInDirection("BACKWARD")
        while POS.position[POS.sectionDir] ~= elevatorPos do
            while not BM.forward() do sleep(0.1) end
        end

        if POS.position.y > initData.lowBeltY then
            if targSecHeight > POS.position.y and targSection == turtSection then
                BM.turnInDirection("RIGHT")
            else
                BM.turnInDirection("LEFT")
            end
        elseif POS.position.y < initData.lowBeltY then
            if targSecHeight < POS.position.y and targSection == turtSection then
                BM.turnInDirection("LEFT")
            else
                BM.turnInDirection("RIGHT")
            end
        end
        while not BM.forward() do sleep(0.1) end

        return
    end


    -- turtle.back() TURT = require("TURT") TURT.init()
    -- t = {x=-50, y=72, z=-28}
    -- t = {x=-45, y=77, z=-18}
    function AM.goToCoord(targPos)
        if CVF.equalCoords(targPos, POS.position) then return true end

        term.clear()
        term.setCursorPos(1, 1)

        local bTurtleInBounds = POS.isInBounds()
        local bTargetInBounds = POS.isInBounds(targPos)

        local startPos = {x=POS.position.x, y=POS.position.y, z=POS.position.z}

        if not POS.isInBounds() then
            print("Out Bounds")
            if not AM.reachOutsideTarget(targPos) then
                AM.joinBeltFromOutside(targPos)              -- Not ready
            else
                return true
            end
        end
        if POS.isOnBottomBelt() or POS.isOnTopBelt() then
            print("On Belt")
            AM.useAndExitBelt(targPos)
        end

        if POS.isOnElevatorUp() then
            print("On Elevator Up")
            AM.useElevatorUp(targPos)
        end
        if POS.isOnElevatorDown() then
            print("On Elevator Down")
            AM.useElevatorDown(targPos)
        end

        if POS.isOnBottomSection() then
            print("On Bottom Section")
            AM.useBottomSection(targPos)
        end

        if POS.isOnLeftChestRow() or POS.isOnRightChestRow() then
            print("On Chest Row")
            AM.useChestRow(targPos)
        end

        if POS.isOnTopsection() then
            print("On Top Section")
            AM.useTopSection(targPos)
        end

        if CVF.equalCoords(startPos, POS.position) then return false end

        return AM.goToCoord(targPos)

    end

    return AM