rtool

local component = require("component")
local robot = component.robot
local nav = component.navigation
local sides = require("sides")

local RTool = {}

function RTool.getWaypoints(dist)
  points = nav.findWaypoints(dist)
  return points
end

function RTool.dispWaypoint(t)
  print("label : "..t.label)
  print("redstone : "..tostring(t.redstone))
  print("position")
  print(tostring(t.position[1])..","
        ..tostring(t.position[2])..","
        ..tostring(t.position[3]))
end

function RTool.getShortestPoint(points)
  local index = 1
  local lengs = {}
  for i=1, #points do
    x = points[i].position[1]
    y = points[i].position[2]
    z = points[i].position[3]
    lengs[i] = x*x + y*y + z*z
  end

  for j=1, #lengs do
    if(lengs[index] > lengs[j]) then
      index = j
    end
  end

  return index
end

function RTool.turnToSide(side)
  while true do
    if nav.getFacing() == side then
      break;
    else
      robot.turn(true)
    end
  end
end

function RTool.moveToX(X)
  local isPos = true

  if X<0 then
    isPos = false
    X = X*(-1)
  end

  if isPos == true then
    rtool.turnToSide(sides.posx)
  else
    rtool.turnToSide(sides.negx)
  end

  for i=1,X do
    robot.move(sides.front)
  end

end

function RTool.moveToZ(Z)
  local isPos = true

  if Z<0 then
    isPos = false
    Z = Z*(-1)
  end

  if isPos == true then
    rtool.turnToSide(sides.posz)
  else
    rtool.turnToSide(sides.negz)
  end

  for i=1,Z do
    robot.move(sides.front)
  end

end

return RTool