turtlelib

computers = {"computercraft:turtle_normal", "computercraft:turtle_advanced", "computercraft:computer_normal", "computercraft:computer_advanced", "computercraft:wireless_modem_normal", "computercraft:wireless_modem_advanced"}
turtles = {"computercraft:turtle_normal", "computercraft:turtle_advanced"}
dirTable = {{0, 1, 2, -1}, {-1, 0, 1, 2}, {2, -1, 0, 1}, {1, 2, -1, 0}}
-- Rotations to get from a rotation to another rotation
-- 0 is minecraft south (+z)
-- dirtable[2][3] gives signed clockwise rotations to direction 2 to 3

function IsComputer(success, data)
    if success then
        for _, computer in ipairs(computers) do
            if computer == data.name then
                return true
            end
        end
    end
    return false
end

function RequestRescue(pos)
    helpMessage = "Turtle rescue needed X:"..pos["x"].." Y:"..pos["y"].." Z:"..pos["z"]
    print(helpMessage)
    rednet.broadcast(helpMessage)
end

function IsTurtle(success, data)
    if success then
        for _, turtle in ipairs(turtles) do
            if turtle == data.name then
                return true
            end
        end
    end
    return false
end

-- Returns true if theres at least one fuel available
function CheckFuel()
    if turtle.getFuelLevel() > 0 then
        return true
    end

    return false
end

function AvoidUp(pos)
    pos = Forward(pos)
    pos = Up(pos)
    pos = Up(pos)
    pos = Turn(2, pos)
    pos = Forward(pos)
    pos = Turn(2, pos)
end

function AvoidDown(pos)
    pos = Turn(2, pos)
    pos = Forward(pos)
    pos = Down(pos)
    pos = Down(pos)
    pos = Turn(2, pos)
    pos = Forward(pos)
end

function AvoidForward(pos)
    pos = Turn(1, pos)
    pos = Forward(pos)
    pos = Turn(-1, pos)
    pos = Forward(pos)
    pos = Forward(pos)
    pos = Turn(-1, pos)
    pos = Forward(pos)
    pos = Turn(1, pos)
end

function Forward(pos)
    if CheckFuel() == false then
        return pos, false
    end

    if IsComputer(turtle.inspect()) then
        AvoidForward(pos)
    end

    while turtle.forward() == false do
        turtle.dig()
    end
    --Set position
    if pos["rot"] == 0 then
        pos["z"] = pos["z"] + 1
    elseif pos["rot"] == 1 then
        pos["x"] = pos["x"] - 1
    elseif pos["rot"] == 2 then
        pos["z"] = pos["z"] - 1
    elseif pos["rot"] == 3 then
        pos["x"] = pos["x"] + 1
    end

    return pos, true
end

function Up(pos)
    if CheckFuel() == false then
        return pos, false
    end

    if IsComputer(turtle.inspectUp()) then
        AvoidDown(pos)
    end

    while turtle.up() == false do
        turtle.digUp()
    end
    --Set position
    pos["y"] = pos["y"] + 1
    return pos, true
end

function Down(pos)
    if CheckFuel() == false then
        return pos, false
    end

    if IsComputer(turtle.inspectDown()) then
        AvoidDown(pos)
    end

    while turtle.down() == false do
        turtle.digDown()
    end
    --Set position
    pos["y"] = pos["y"] - 1
    return pos, true
end

function Turn(amount, pos)
    for i = 1, math.abs(amount), 1 do
        print(i)
        if amount < 0 then
            turtle.turnLeft()
            pos["rot"] = pos["rot"] - 1
        else
            turtle.turnRight()
            pos["rot"] = pos["rot"] + 1
        end

        if pos["rot"] > 3 then
            pos["rot"] = 0
        end

        if pos["rot"] < 0 then
            pos["rot"] = 3
        end
    end

    return pos
end

function FaceDir(dir, pos)
    print("facing"..dir)
    delta = dirTable[pos["rot"] + 1][dir + 1]
    --delta = dir - pos["rot"]
    print(delta)
    pos = Turn(delta, pos)

    return pos
end

function GetPos(pos)
    xA,yA,zA = gps.locate(2)

    if x == nil then
        if pos == nil then
            pos = {}
            pos["rot"] = 0
            pos["x"] = 0
            pos["y"] = 0
            pos["z"] = 0
        end
        return pos
    end

    xB,yB,zB = gps.locate(2) --Used to compare pos to get rotation
    if xA ~= xB then
        if xA > xB then
            pos["rot"] = 3
        else
            pos["rot"] = 1
        end
    end

    if zA ~= zB then
        if zA > zB then
            pos["rot"] = 0
        else
            pos["rot"] = 2
        end
    end

    pos["x"] = xB
    pos["y"] = yB
    pos["z"] = zB
    return pos
end

function ToWorldHeight(pos)
    while pos["y"] < 319 do
        success, pos = Up(pos)
    end

    return pos
end

function Goto(targetPos, currentPos)
    print("Going to "..targetPos["x"].." "..targetPos["y"].." "..targetPos["z"])
    print("from "..currentPos["x"].." "..currentPos["y"].." "..currentPos["z"])

    --pos = ToWorldHeight(currentPos)
    pos = currentPos --Links these variables

    deltaX = targetPos["x"] - currentPos["x"]
    deltaY = targetPos["y"] - currentPos["y"]
    deltaZ = targetPos["z"] - currentPos["z"]

    print("deltaX"..deltaX)
    print("deltaY"..deltaY)
    print("deltaZ"..deltaZ)

    if deltaY ~= 0 then
        if deltaY > 0 then
            while currentPos["y"] ~= targetPos["y"] do
                pos = Up(pos)

                if targetPos["y"] > currentPos["y"] then
                    RequestRescue(currentPos)
                    error("Overshot")
                end
            end
        else
            while currentPos["y"] ~= targetPos["y"] do
                pos = Down(pos)

                if targetPos["y"] < currentPos["y"] then
                    RequestRescue(currentPos)
                    error("Overshot")
                end
            end
        end
    end

    if deltaX ~= 0 then
        if deltaX > 0 then
            pos = FaceDir(3, pos)
        else
            pos = FaceDir(1, pos)
        end

        while currentPos["x"] ~= targetPos["x"] do
            pos = Forward(pos)

            if deltaX < 0 then
                if targetPos["x"] > currentPos["x"] then
                    RequestRescue(currentPos)
                    error("Overshot")
                end
            else
                if targetPos["x"] < currentPos["x"] then
                    RequestRescue(currentPos)
                    error("Overshot")
                end
            end
        end
    end

    if deltaZ ~= 0 then
        if deltaZ > 0 then
            pos = FaceDir(0, pos)
        else
            pos = FaceDir(2, pos)
        end

        while currentPos["z"] ~= targetPos["z"] do
            pos = Forward(pos)

            if deltaZ < 0 then
                if targetPos["z"] > currentPos["z"] then
                    RequestRescue(currentPos)
                    error("Overshot")
                end
            else
                if targetPos["z"] < currentPos["z"] then
                    RequestRescue(currentPos)
                    error("Overshot")
                end
            end
        end
    end

    return pos
end