WitherBuilder1.1 WIP

--Home Coords
xH = -307
yH = 6
zH = -135

-- Coords when facing the center of the Wither
xW = -302
yW = 8
zW = -135

homeFace = 1 --direction to face when at home coords
witherFace = 2 --direction to face when starts building the wither
recur = false
isStuck = false
count = 0

for k,v in pairs(rs.getSides()) do
  if peripheral.getType(v) == "modem" then
   rednet.open(v)
  end
end


function compass()
        print("Compass")
    compx,compy,compz = gps.locate()
    while turtle.forward() == false do
        turtle.turnRight()
    end
    x1,y1,z1 = gps.locate()
    direction = {compx = compx - x1, compy = compy - y1, compz = compz - z1}
    for k,v in pairs( direction ) do
        if k == "compx" and v == 1 then
            facing = 1 -- west
                        print("Facing West")
        elseif k == "compx" and v == -1 then
            facing = 3 -- east
            print("Facing East")
        elseif k == "compz" and v == 1 then
            facing = 2 -- north
            print("Facing North")
        elseif k == "compz" and v == -1 then
            facing = 0 -- south
            print("Facing South")
        end
    end
    turtle.back()
    return facing
end

function goTo(xP,yP,zP)
print("goto function")
lostCount = 0
while true do
    xL,yL,zL = gps.locate()
    x = xL - xP
    y = yL - yP
    z = zL - zP
        print(x.." "..y.." "..z)
    if y > 0.5 and yStuck ~= true then
                for i = 1,y do
          if turtle.down() == false then
                    yStuck = true
                  end
        end
    elseif y < -0.5 and yStuck ~= true then
                for i = y,0 do
          if turtle.up() == false then
                    yStuck = true
                  end
        end
    elseif z < -0.5 and zStuck ~= true then
        currentDir = compass()
        if currentDir ~= 0 then
            turnTo(0)
        end
                for i = z,0 do
          if turtle.forward() == false then
                    zStuck = true
                  end
        end
        print("z less than 1")
    elseif z > 0.5 and zStuck ~= true then
        currentDir = compass()
        if currentDir ~= 2 then
            turnTo(2)
        end
                for i = 1,z do
          if turtle.forward() == false then
                    zStuck = true
                  end
        end
    elseif x < -0.5 and xStuck ~= true then
        currentDir = compass()
        if currentDir ~= 3 then
            turnTo(3)
        end
                for i = x,0 do
          if turtle.forward() == false then
                    xStuck = true
                  end
        end
    elseif x > 0.5 and xStuck ~= true then
        currentDir = compass()
        if currentDir ~= 1 then
            turnTo(1)
        end
                for i = 1,x do
          if turtle.forward() == false then
                    xStuck = true
                  end
        end
    else
          xL,yL,zL = gps.locate()
            if xL == xP and yL == yP and zL == zP then
              print("At destination")
          break
            else
              lostCount = lostCount + 1
              print("Going to random location for the "..lostCount.." time.")
              goRandom()
            end
    end
end
end

function goRandom()
  compass()
  turnTo(homeFace)
  isStuck = false
  xS,yS,zS = gps.locate()
  xS = math.floor(math.random(xS-5,xS+5))
  yS = math.floor(math.random(yS-5,yS+5))
  zS = math.floor(math.random(zS-5,zS+5))
  while true do
        print("random")
    xRL,yRL,zRL = gps.locate()
    xR = math.floor(xRL - xS)
    yR = math.floor(yRL - yS)
    zR = math.floor(zRL - zS)
        print(xR.." "..yR.." "..zR)
    if yR > 0.5 and yIsStuck ~= true then
                for i = 1,y do
          if turtle.down() == false then
                    yIsStuck = true
                  end
        end
    elseif yR < -0.5 and yIsStuck ~= true then
                for i = y,0 do
          if turtle.up() == false then
                    yIsStuck = true
                  end
        end
    elseif zR < -0.5 and zIsStuck ~= true then
        currentDir = compass()
        if currentDir ~= 0 then
            turnTo(0)
        end
                for i = z,0 do
          if turtle.forward() == false then
                    zIsStuck = true
                  end
        end
        print("z less than 1")
    elseif zR > 0.5 and zIsStuck ~= true then
        currentDir = compass()
        if currentDir ~= 2 then
            turnTo(2)
        end
                for i = 1,z do
          if turtle.forward() == false then
                    zIsStuck = true
                  end
        end
    elseif xR < -0.5 and xIsStuck ~= true then
        currentDir = compass()
        if currentDir ~= 3 then
            turnTo(3)
        end
                for i = x,0 do
          if turtle.forward() == false then
                    xIsStuck = true
                  end
        end
    elseif xR > 0.5 and xIsStuck ~= true then
        currentDir = compass()
        if currentDir ~= 1 then
            turnTo(1)
        end
                for i = 1,x do
          if turtle.forward() == false then
                    xIsStuck = true
                  end
        end
    else
          print("End Random")
          yIsStuck = false
          zIsStuck = false
          xIsStuck = false
          isStuck = false
          break
    end
  end
end

function buildWither()
    turtle.down()
    turtle.select(1)
    turtle.place()
    turtle.up()
    turtle.turnRight()
    turtle.forward()
    turtle.turnLeft()
    turtle.place()
    for i = 1, 2 do
      turtle.turnLeft()
      turtle.forward()
      turtle.turnRight()
      turtle.place()
    end
    turtle.up()
    turtle.select(2)
    turtle.place()
    for i = 1,2 do
      turtle.turnRight()
      turtle.forward()
      turtle.turnLeft()
      turtle.place()
    end
    turtle.down()
    turtle.turnLeft()
    turtle.forward()
    turtle.turnLeft()
end

function turnTo(Int)
    if facing ~= Int then
        local current = facing - Int
        if current < 0 then
            for i = 1, math.abs(current) do
                turtle.turnRight()
            end
        else
            for i = 1,current do
            turtle.turnLeft()
            end
        end
    end
end


if fs.exists("abort") == false then
  print("Installing abort")
  shell.run("pastebin get eM0db9xZ abort")
end
shell.openTab("abort")
sleep(1)
goTo(xH,yH,zH)
compass()
turnTo(homeFace)
print("home")
while true do
  event, p1, p2, p3 = os.pullEvent("rednet_message")
  if tonumber(p3) == 33 and p2 == "BuildWither" then
    print("got message")
    goTo(xW,yW,zW)
    turnTo(witherFace)
    buildWither()
    goTo(xH,yH,zH)
    rednet.broadcast("WitherBuilt", 33)
    turtle.select(3)
    turtle.refuel(1)
    sleep(60)
    compass()
    turnTo(homeFace)
  else
    sleep(0.05)
  end
end