local homeX, homeY, homeZ -- Coordinates of turtle home station
homeX = 420
homeY = 65
homeZ = -104
local direction
local timeout = 5 -- The timeout of the gps 
local safeHeight = 35 --The height that is safe to dig at


function go(dist, dig)
    dig = (dig == true)
    dist = dist or 1
    if(dist <= 0) then return end
    for i = 1, dist do
        while(not turtle.forward()) do
            turtle.attack()
            if(turtle.detect()) then
                if(dig) then
                    turtle.dig()
                else
                    local x,y,z = gps.locate(timeout, true)
                    if(safeHeight<y) then
                        goDown(y-safeHeight)
                        turtle.dig()
                    else
                        turtle.dig()
                    end
                end
            end
        end
    end        
end

function goDown(dist, dig)
    dig = true -- Until I find a way to make sure it does go home if it can't go down
    dist = dist or 1
    if(dist<=0) then return end
    for i = 1, dist do
        while(not turtle.down()) do
            turtle.attackDown()
            if(dig) then
                turtle.digDown()
            else
                return false
            end
        end
    end
end

function goUp(dist)
    dist = dist or 1
    if(dist<=0)then return end
    for i = 1, dist do
        while(not turtle.up()) do
            turtle.up()
            turtle.digUp()
        end
    end
end

function getDirection(dig, times)
    dig = dig ~= false
    times = times or 1
    local loc1 = vector.new(gps.locate(timeout, false))
    if(not turtle.forward()) then
        if(turtle.detect()) then
            local succ, block = turtle.inspect()
            if(block.name == "minecraft:dirt" or block.name == "minecraft:cobblestone" or block.name == "minecraft:stone" or block.name == "minecraft:sand" or block.name == "minecraft:sandstone" or block.name == "minecraft:clay" or block.name == "minecraft:gravel" or block.name == "minecraft:yellow_flower" or block.name == "minecraft:red_flower" or block.name == "minecraft:granite" or block.name == "minecraft:wood" or block.name == "minecraft:double_plant" or block.name == "minecraft:leaves" or block.name == "minecraft:moss_stone" or block.name == "minecraft:grass" or dig) then
                turtle.dig()
                turtle.forward()
            else
                if(times<4) then
                    turnLeft(false)
                    local tmp = getDirection(dig, times+1)
                    turnRight(false)
                    return (tmp+1)%4
                end
                return false
            end
        end
    end
    local loc2 = vector.new(gps.locate(timeout, false))
    heading = loc2-loc1
    if(not turtle.back()) then
        turtle.turnLeft()
        turtle.turnLeft()
        while(not turtle.forward()) do
            turtle.dig()
            turtle.attack()
        end
        turtle.turnRight()
        turtle.turnRight()
    end
    return ((heading.x + math.abs(heading.x) * 2) + (heading.z + math.abs(heading.z) * 3))-1
end

function turnLeft(followDir)
    followDir = followDir ~= false
    turtle.turnLeft()
    if(followDir) then
        direction = (direction-1)%4
    end
end

function turnRight(followDir)
    followDir = followDir ~= false
    turtle.turnRight()
    if(followDir) then
        direction = (direction+1)%4
    end
end

function turnTo(dir)
    if(dir >= 0 and dir <= 3) then
        local turns = (dir-direction)%4
        if    (turns == 1 or turns == -3) then
            turnRight()
        elseif(turns == 2) then
            turnRight()
            turnRight()
        elseif(turns == -1 or turns == 3) then
            turnLeft()
        elseif(turns == -2) then
            turnLeft()
            turnLeft()
        end
    end
    if(dir ~= direction) then 
        print((dir-direction)%4)
        print("Problem in turnTo()") 
    end
end

function goHome()
    local x,y,z = gps.locate(5, false)
    
--    if(y > safeHeight) then
  --      goDown(y-safeHeight)
    --end
    direction = getDirection(y<=safeHeight)
    print("Got direction: "..direction)
    if(x>homeX) then
        turnTo(0)
    elseif(x<homeX) then
        turnTo(2)
    end
    go(math.abs(x-homeX))
    if(z>homeZ) then
        turnTo(1)
    elseif(z<homeZ) then
        turnTo(3)
    end
    go(math.abs(z-homeZ))
    if(y>homeY) then
        goDown(y-homeY)
    elseif(y<homeY) then
        goUp(homeY-y)
    end
    if(vector.new(x,y,z)-vector.new(homeX,homeY,homeZ) == 0) then
        print("Error in going home equation")
        print("Current position: ("..x..","..y..","..z..")")
    end
end

os.sleep(3)
goHome()