lib/erobot.lua

local component = require("component")
local sides = require("sides")

local robot = require("robot")

-------------------------------------------------------------------------------
-- Navigation

-- Minecraft
--
--            z
--           (-)
--          north
-- x (-) west   east (+) x
--          south
--           (+)
--            z
--         forward

-- Virtual
--
--         forward
--            z
--           (+)
--          -----
-- x (-) ---- ^ ---- (+) x
--          -----
--           (-)
--            z

local orientation = sides.east
local x, y, z = 0, 0, 0
local startOrientation = orientation
local startX, startY, startZ = x, y, z

local clockwiseSide = {
    [sides.south] = sides.west,
    [sides.west] = sides.north,
    [sides.north] = sides.east,
    [sides.east] = sides.south,
}

local counterclockwiseSide = {
    [sides.south] = sides.east,
    [sides.east] = sides.north,
    [sides.north] = sides.west,
    [sides.west] = sides.south,
}

local defMove = component.robot.move
local defTurn = component.robot.turn

function navTurn(clockwise)
  local ok, err = defTurn(clockwise)
  if ok then
    if clockwise then
      orientation = clockwiseSide[orientation]
    else
      orientation = counterclockwiseSide[orientation]
    end
  end
  return ok, err
end

local hmover = {
    [sides.south] = function(n) z = z + n end,
    [sides.north] = function(n) z = z - n end,
    [sides.east] = function(n) x = x + n end,
    [sides.west] = function(n) x = x - n end,
}

local mover = {
  [sides.forward] = function() hmover[orientation](1) end,
  [sides.back] = function () hmover[orientation](-1) end,
  [sides.up] = function () y = y + 1 end,
  [sides.down] = function () y = y - 1 end,
}

function navMove(side)
  local ok, err
  while true do
    ok, err =  defMove(side)
    if not ok and err == "already moving" then
      os.sleep(0.5)
    else
      break
    end
  end
  if ok then
    local fn = mover[side]
    assert(fn, "internal error")
    fn()
  end
  return ok, err
end

component.robot.move = navMove
component.robot.turn =navTurn

function robot.navInit(side, start_x, start_y, start_z)
  checkArg(1, side, "nil", "number")
  checkArg(2, start_x, "nil", "number")
  checkArg(3, start_y, "nil", "number")
  checkArg(4, start_z, "nil", "number")
  side = side or sides.east
  start_x = start_x or 0
  start_y = start_y or 0
  start_z = start_z or 0
  assert(side >= sides.north and side <= sides.east, "Bad orientation argument.") -- see sides
  startX, startY, startZ = start_x, start_y, start_z
  startOrientation = side
  x, y, z = startX, startY, startZ
  orientation = startOrientation
end

function robot.getOrientation()
  return orientation
end

function robot.getPos()
  return x, y, z
end

-- условная система координат
--
--
--      (+x)
-- (-z)  ^   (+z)
--      (-x)
-- ^ стартовая ориентация
-- отсчёт идёт от точки старта

local toVirtFn = {
  [sides.east] = function(realX, realY, realZ) return   realX - startX, realY - startY, realZ - startZ end,
  [sides.south] = function(realX, realY, realZ) return  realZ - startZ, realY - startY, startX - realX end,
  [sides.west] = function(realX, realY, realZ) return   startX - realX, realY - startY, startZ - realZ end,
  [sides.north] = function(realX, realY, realZ) return  startZ - realZ, realY - startY, realX - startX end,
}

function robot.toVirtCoord(realX, realY, realZ)
  return toVirtFn[startOrientation](realX, realY, realZ)
end

--            z
--           (-)
--          north
-- x (-) west   east (+) x
--          south
--           (+)
--            z
--         forward

local toRealFn = {
  [sides.east] = function(virtX, virtY, virtZ) return   startX + virtX, startY + virtY, startZ + virtZ end,
  [sides.south] = function(virtX, virtY, virtZ) return  startX - virtZ, startY + virtY, startZ + virtX end,
  [sides.west] = function(virtX, virtY, virtZ) return   startX - virtX, startY + virtY, startZ - virtZ end,
  [sides.north] = function(virtX, virtY, virtZ) return  startX + virtZ, startY + virtY, startZ - virtX end,
}

function robot.toRealCoord(virtX, virtY, virtZ)
  return toRealFn[startOrientation](virtX, virtY, virtZ)
end

function robot.getStartOrientation()
  return startOrientation
end

function robot.getStartPos()
  return startX, startY, startZ
end

function robot.rotateTo(side)
  checkArg(1, side, "number")
  assert(side >= sides.north and side <= sides.east, "Bad side argument.") -- see sides
  if side == orientation then
    return true
  end
  local clockwise = (clockwiseSide[orientation] == side)
  local n = 1
  if not clockwise and (counterclockwiseSide[orientation] ~= side) then
    n = 2
  end
  for i = 1, n do
    local ok, err = navTurn(clockwise)
    if not ok then
      return false, err
    end
  end
  return true
end

function robot.moveTo(targetX, targetY, targetZ, mover)
  checkArg(1, targetX, "number")
  checkArg(2, targetY, "number")
  checkArg(3, targetZ, "number")
  checkArg(4, action, "nil", "function")
  mover = mover or component.robot.move
  if x ~= targetX then
    if targetX < x then
      robot.rotateTo(sides.west)
    else
      robot.rotateTo(sides.east)
    end
    while x ~= targetX do
      local ok, err = mover(sides.forward)
      if not ok then
        return false, err
      end
    end
  end
  if z ~= targetZ then
    if targetZ < z then
      robot.rotateTo(sides.north)
    else
      robot.rotateTo(sides.south)
    end
    while z ~= targetZ do
      local ok, err = mover(sides.forward)
      if not ok then
        return false, err
      end
    end
  end
  if y ~= targetY then
    local dir
    if targetY < y then
      dir = sides.down
    else
      dir = sides.up
    end
    while y ~= targetY do
      local ok, err = mover(dir)
      if not ok then
        return false, err
      end
    end
  end
  return true
end

function robot.moveByPath(path, pos, mover)
  for i, p in ipairs(path) do
    if i >= pos then
      pos = i
      local ok, err = robot.moveTo(p.x, p.y, p.z, mover)
      if not ok then
        return false, pos, err
      end
    end
  end
  return true, pos
end

-------------------------------------------------------------------------------

return robot