term.clear()
term.setCursorPos(1,1)
rednet.open("right")
if turtle.getFuelLevel() == 0 then
  turtle.refuel()
end

direction = nil

function rec()
  local id, name, msg, content
  id,name = rednet.receive()
  f = fs.open(name, "w")
  id,content = rednet.receive()
  f.write(content)
  f.close()
  print("File "..name.." received!")
  rednet.send(id, name)
end

function getDirection()
  local loc1 = vector.new(gps.locate(2,false))
  if not turtle.forward() then
    if not turtle.back() then
      turtle.turnRight()
      return getDirection()
    end
    loc1 = vector.new(gps.locate(2,false))
    turtle.forward()
  end
  local loc2 = vector.new(gps.locate(2,false)) 
  local heading = loc2-loc1
  if heading.x == 1 then return 4
  elseif heading.x == -1 then return 2
  elseif heading.y == 1 then return 1
  elseif heading.y == -1 then return 3
  else print("ERR Direction") return -1 end  
  --return ((heading.x + math.abs(heading.x)*2)+(heading.y+math.abs(heading.y)*3))
end

function turnTo(dir)
  while direction ~= dir do
    local val = 1
    if math.abs(dir-direction) == 3 then
      val = -1
    end
    if direction < dir then turn(1*val)
    elseif direction > dir then turn(-1*val) end
  end
end

function turn(dir)
  if dir < 0 then
    turtle.turnLeft()
  elseif dir > 0 then
    turtle.turnRight()
  end
  direction = direction + dir
  if direction > 4 then direction = 1
  elseif direction < 1 then direction = 4 end
end


function err(message)
  if compId == nil then
    shell.run("/connect")
  end
  rednet.send(compId, "ERR")
  rednet.send(compId, message)
end

function moveTo(target)
local loc = vector.new(gps.locate(2,false))

print(target.x)
local xdist = target.x-loc.x
local ydist = target.y-loc.y
local zdist = target.z-loc.z
  while loc~=target do
    sleep(1)
    loc = vector.new(gps.locate(2,false))
    xdist = target.x-loc.x
    ydist = target.y-loc.y
    zdist = target.z-loc.z
    print("--")
    print(xdist)
    print(ydist)
    print(zdist)
                
    if math.abs(xdist)>=math.abs(ydist) and math.abs(xdist)>=math.abs(ydist) and xdist~= 0 then
      turnTo(3+xdist/math.abs(xdist))
      if turtle.forward() == false then turtle.dig() end
    elseif math.abs(ydist)>=math.abs(xdist) and math.abs(ydist)>=math.abs(zdist) and ydist~= 0 then
      turnTo(2-ydist/math.abs(ydist))
      if turtle.forward() == false then turtle.dig() end
    else
      if zdist>0 then if turtle.up() == false then turtle.digUp() end
      elseif zdist<0 then if turtle.down() == false then turtle.digDown() end end
    end  
    
    
     
  end
  
end

--Direction: 1 = x-
--           2 = y-
--           3 = x+
--           4 = y+

function init()
  if compId == nil then
    shell.run("/connect")
  end
  direction = getDirection()
  write("Direction: ")
  print(direction) 
end

init()

while 1 == 1 do
  if compId == nil then
    shell.run("/connect")
  end
  local id, message = rednet.receive()
  if id == compId then
    if message == "RECEIVE" then
      rec()
    elseif message == "RUN" then
      id, message = rednet.receive()
      shell.run(message)
    elseif message == "MOVE" then
      local vec = nil
      id, vec = rednet.receive(5)
      if vec == nil then
        print("Missed move vector")
        err(message)
      else
        moveTo(vec)
      end
    elseif message == "TEST" then
      id,message = rednet.receive()
      sleep(2)
      turnTo(message)
    end   
    rednet.send(compId, "DONE")
  end  
end