TL2.lua

if not turtle then
  return
end

Point = { }
function Point.copy(pt)
  return { x = pt.x, y = pt.y, z = pt.z }
end

function Point.subtract(a, b)
  a.x = a.x - b.x
  a.y = a.y - b.y
  a.z = a.z - b.z
end

-- real distance
function Point.pythagoreanDistance(a, b)
  return math.sqrt(
           math.pow(a.x - b.x, 2) +
           math.pow(a.y - b.y, 2) +
           math.pow(a.z - b.z, 2))
end

-- turtle distance
function Point.turtleDistance(a, b)
  if b.z then
    return math.max(a.x, b.x) - math.min(a.x, b.x) +
           math.max(a.y, b.y) - math.min(a.y, b.y) +
           math.max(a.z, b.z) - math.min(a.z, b.z)
  else
    return math.max(a.x, b.x) - math.min(a.x, b.x) +
           math.max(a.y, b.y) - math.min(a.y, b.y)
  end
end

function Point.calculateTurns(ih, oh)
  if ih == oh then
    return 0
  end
  if (ih % 2) == (oh % 2) then
    return 2
  end
  return 1
end

-- Calculate distance to location including turns
-- also returns the resulting heading
function Point.calculateMoves(pta, ptb, distance)
  local heading = pta.heading
  local moves = distance or Point.turtleDistance(pta, ptb)
  if (pta.heading % 2) == 0 and pta.y ~= ptb.y then
    moves = moves + 1
    if ptb.heading and (ptb.heading % 2 == 1) then
      heading = ptb.heading
    elseif ptb.y > pta.y then
      heading = 1
    else
      heading = 3
    end
  elseif (pta.heading % 2) == 1 and pta.x ~= ptb.x then
    moves = moves + 1
    if ptb.heading and (ptb.heading % 2 == 0) then
      heading = ptb.heading
    elseif ptb.x > pta.x then
      heading = 0
    else
      heading = 2
    end
  end

  if ptb.heading then
    if heading ~= ptb.heading then
      moves = moves + Point.calculateTurns(heading, ptb.heading)
      heading = ptb.heading
    end
  end

  return moves, heading
end

function Point.toBox(pt, width, length, height)
  return { ax = pt.x,
           ay = pt.y,
           az = pt.z,
           bx = pt.x + width - 1,
           by = pt.y + length - 1,
           bz = pt.z + height - 1
         }
end

function Point.inBox(pt, box)
  return pt.x >= box.ax and
         pt.y >= box.ay and
         pt.x <= box.bx and
         pt.y <= box.by
end

Box = { }

function Box.contain(boundingBox, containedBox)

  local shiftX = boundingBox.ax - containedBox.ax
  if shiftX > 0 then
    containedBox.ax = containedBox.ax + shiftX
    containedBox.bx = containedBox.bx + shiftX
  end
  local shiftY = boundingBox.ay - containedBox.ay
  if shiftY > 0 then
    containedBox.ay = containedBox.ay + shiftY
    containedBox.by = containedBox.by + shiftY
  end

  shiftX = boundingBox.bx - containedBox.bx
  if shiftX < 0 then
    containedBox.ax = containedBox.ax + shiftX
    containedBox.bx = containedBox.bx + shiftX
  end
  shiftY = boundingBox.by - containedBox.by
  if shiftY < 0 then
    containedBox.ay = containedBox.ay + shiftY
    containedBox.by = containedBox.by + shiftY
  end
end

TL2 = { }

local function noop() end

if not _G.turtleApi then
  _G.turtleApi = Util.shallowCopy(turtle)
end

function TL2.replaceApi()
  for k,m in pairs(TL2) do
    turtle[k] = TL2[k]
  end
end

local state = {
  pt = { x = 0, y = 0, z = 0, heading = 0 },
  slot = 1,
  moveAttack = noop,
  moveDig = noop,
  moveCallback = noop,
  locations = {},
}

function TL2.getState()
  return state
end

function TL2.setPoint(pt)
  state.pt.x = pt.x
  state.pt.y = pt.y
  state.pt.z = pt.z
  if pt.heading then
    state.pt.heading = pt.heading
  end
end

TL2.actions = {
  up = {
    detect = turtleApi.detectUp,
    dig = turtleApi.digUp,
    move = turtleApi.up,
    attack = turtleApi.attackUp,
    place = turtleApi.placeUp,
    drop = turtleApi.dropUp,
    suck = turtleApi.suckUp,
    compare = turtleApi.compareUp,
    side = 'top'
  },
  down = {
    detect = turtleApi.detectDown,
    dig = turtleApi.digDown,
    move = turtleApi.down,
    attack = turtleApi.attackDown,
    place = turtleApi.placeDown,
    drop = turtleApi.dropDown,
    suck = turtleApi.suckDown,
    compare = turtleApi.compareDown,
    side = 'bottom'
  },
  forward = {
    detect = turtleApi.detect,
    dig = turtleApi.dig,
    move = turtleApi.forward,
    attack = turtleApi.attack,
    place = turtleApi.place,
    drop = turtleApi.drop,
    suck = turtleApi.suck,
    compare = turtleApi.compare,
    side = 'front'
  },
  back = {
    detect = noop,
    dig = noop,
    move = turtleApi.back,
    attack = noop,
    place = noop,
    suck = noop,
    compare = noop,
    side = 'back'
  },
}

TL2.headings = {
  [ 0 ] = { xd =  1, yd =  0, zd =  0, heading = 0, direction = 'east' },
  [ 1 ] = { xd =  0, yd =  1, zd =  0, heading = 1, direction = 'south' },
  [ 2 ] = { xd = -1, yd =  0, zd =  0, heading = 2, direction = 'west' },
  [ 3 ] = { xd =  0, yd = -1, zd =  0, heading = 3, direction = 'north' },
  [ 4 ] = { xd =  0, yd =  0, zd =  1, heading = 4, direction = 'up' },
  [ 5 ] = { xd =  0, yd =  0, zd = -1, heading = 5, direction = 'down' }
}

TL2.namedHeadings = {
  east  = TL2.headings[0],
  south = TL2.headings[1],
  west  = TL2.headings[2],
  north = TL2.headings[3],
  up    = TL2.headings[4],
  down  = TL2.headings[5]
}

-- [[ Basic turtle actions ]] --
local function _attack(action)
  if action.attack() then
    repeat until not action.attack()
    return true
  end
  return false
end

function TL2.attack()        return _attack(TL2.actions.forward) end
function TL2.attackUp()      return _attack(TL2.actions.up)      end
function TL2.attackDown()    return _attack(TL2.actions.down)    end

local function _place(action, slot)

  if slot and turtleApi.getItemCount(slot) == 0 then
    return false, 'No items to place'
  end

  return Util.tryTimes(3, function()
    if slot then
      TL2.select(slot)
    end
    local result = { action.place() }
    if result[1] then
      return true
    end
    if not state.moveDig(action) then
      state.moveAttack(action)
    end
    return unpack(result)
  end)
end

function TL2.place(slot)     return _place(TL2.actions.forward, slot) end
function TL2.placeUp(slot)   return _place(TL2.actions.up, slot)      end
function TL2.placeDown(slot) return _place(TL2.actions.down, slot)    end

local function _drop(action, count, slot)
  if slot then
    if turtleApi.getItemCount(slot) == 0 then
      return false, 'No items to drop'
    end
    TL2.select(slot)
  end
  return action.drop(count)
end

function TL2.drop(count, slot)     return _drop(actions.forward, count, slot) end
function TL2.dropUp(count, slot)   return _drop(actions.up, count, slot)      end
function TL2.dropDown(count, slot) return _drop(actions.down, count, slot)    end

--[[
function TL2.dig()           return state.dig(TL2.actions.forward) end
function TL2.digUp()         return state.dig(TL2.actions.up)      end
function TL2.digDown()       return state.dig(TL2.actions.down)    end
--]]

function TL2.isTurtleAt(side)
  local sideType = peripheral.getType(side)
  return sideType and sideType == 'turtle'
end

TL2.attackPolicy = {
  none = noop,

  attack = function(action)
    return _attack(action)
  end,
}

TL2.digPolicy = {
  none = noop,

  dig = function(action)
    return action.dig()
  end,

  turtleSafe = function(action)
    if not TL2.isTurtleAt(action.side) then
      return action.dig()
    end
    os.sleep(.5)
    return not action.detect()
  end,

  digAndDrop = function(action)
    if action.detect() then
      local slots = TL2.getSlots()
      if action.dig() then
        TL2.reconcileSlots(slots)
        return true
      end
    end
    return false
  end
}

TL2.policies = {
  none       = { dig = TL2.digPolicy.none, attack = TL2.attackPolicy.none },
  digOnly    = { dig = TL2.digPolicy.dig,  attack = TL2.attackPolicy.none },
  attackOnly = { dig = TL2.digPolicy.none, attack = TL2.attackPolicy.attack },
  digAttack  = { dig = TL2.digPolicy.dig,  attack = TL2.attackPolicy.attack },
}

function TL2.setPolicy(policy)
  state.moveDig = policy.dig
  state.moveAttack = policy.attack
end

function TL2.setDigPolicy(policy)
  state.moveDig = policy
end

function TL2.setAttackPolicy(policy)
  state.moveAttack = policy
end

function TL2.setMoveCallback(cb)
  state.moveCallback = cb
end

function TL2.clearMoveCallback()
  state.moveCallback = noop
end

local function infoMoveCallback()
  local pt = TL2.getState().pt
  print(string.format('x:%d y:%d z:%d heading:%d', pt.x, pt.y, pt.z, pt.heading))
end
-- TESTING
--TL2.setMoveCallback(infoMoveCallback)

-- [[ Locations ]] --
function TL2.getLocation(name)
  return state.locations[name]
end

function TL2.saveLocation(name, pt)
  pt = pt or state.pt
  state.locations[name] = Point.copy(pt)
end

function TL2.gotoLocation(name)
  local pt = TL2.getLocation(name)
  if pt then
    return TL2.goto(pt.x, pt.y, pt.z, pt.heading)
  end
end

function TL2.storeLocation(name, pt)
  pt = pt or state.pt
  Util.writeTable(name .. '.pt', pt)
end

function TL2.loadLocation(name)
  return Util.readTable(name .. '.pt')
end

function TL2.gotoStoredLocation(name)
  local pt = TL2.loadLocation(name)
  if pt then
    return TL2.gotoPoint(pt)
  end
end

-- [[ Heading ]] --
function TL2.getHeading()
  return state.pt.heading
end

function TL2.turnRight()
  TL2.setHeading(state.pt.heading + 1)
  return state.pt
end

function TL2.turnLeft()
  TL2.setHeading(state.pt.heading - 1)
  return state.pt
end

function TL2.turnAround()
  TL2.setHeading(state.pt.heading + 2)
  return state.pt
end

function TL2.getHeadingInfo(heading)
  if heading and type(heading) == 'string' then
    return TL2.namedHeadings[heading]
  end
  heading = heading or state.pt.heading
  return TL2.headings[heading]
end

function TL2.setNamedHeading(headingName)
  local headingInfo = TL2.namedHeadings[headingName]
  if headingInfo then
    TL2.setHeading(headingInfo.heading)
  end
  return false, 'Invalid heading'
end

function TL2.setHeading(heading)
  if not heading then
    return
  end

  heading = heading % 4
  if heading ~= state.pt.heading then
    while heading < state.pt.heading do
      heading = heading + 4
    end
    if heading - state.pt.heading == 3 then
      turtleApi.turnLeft()
      state.pt.heading = state.pt.heading - 1
    else
      turns = heading - state.pt.heading
      while turns > 0 do
        turns = turns - 1
        state.pt.heading = state.pt.heading + 1
        turtleApi.turnRight()
      end
    end

    state.pt.heading = state.pt.heading % 4
    state.moveCallback('turn', state.pt)
  end
end

function TL2.headTowardsX(dx)
  if state.pt.x ~= dx then
    if state.pt.x > dx then
      TL2.setHeading(2)
    else
      TL2.setHeading(0)
    end
  end
end

function TL2.headTowardsY(dy)
  if state.pt.y ~= dy then
    if state.pt.y > dy then
      TL2.setHeading(3)
    else
      TL2.setHeading(1)
    end
  end
end

function TL2.headTowards(pt)
  if state.pt.x ~= pt.x then
    TL2.headTowardsX(pt.x)
  else
    TL2.headTowardsY(pt.y)
  end
end

-- [[ move ]] --
local function _move(action)
  while not action.move() do
    if not state.moveDig(action) and not state.moveAttack(action) then
      return false
    end
  end
  return true
end

function TL2.up()
  if _move(TL2.actions.up) then
    state.pt.z = state.pt.z + 1
    state.moveCallback('up', state.pt)
    return true, state.pt
  end
end

function TL2.down()
  if _move(TL2.actions.down) then
    state.pt.z = state.pt.z - 1
    state.moveCallback('down', state.pt)
    return true, state.pt
  end
end

function TL2.forward()
  if _move(TL2.actions.forward) then
    state.pt.x = state.pt.x + TL2.headings[state.pt.heading].xd
    state.pt.y = state.pt.y + TL2.headings[state.pt.heading].yd
    state.moveCallback('forward', state.pt)

    return true, state.pt
  end
end

function TL2.back()
  if _move(TL2.actions.back) then
    state.pt.x = state.pt.x - TL2.headings[state.pt.heading].xd
    state.pt.y = state.pt.y - TL2.headings[state.pt.heading].yd
    state.moveCallback('back', state.pt)
    return true, state.pt
  end
end

function TL2.moveTowardsX(dx)

  local direction = dx - state.pt.x
  local move
  if direction > 0 and state.pt.heading == 0 or
     direction < 0 and state.pt.heading == 2 then
    move = TL2.forward
  else
    move = TL2.back
  end

  repeat
    if not move() then
      return false
    end
  until state.pt.x == dx
  return true
end

function TL2.moveTowardsY(dy)

  local direction = dy - state.pt.y
  local move
  if direction > 0 and state.pt.heading == 1 or
     direction < 0 and state.pt.heading == 3 then
    move = TL2.forward
  else
    move = TL2.back
  end

  repeat
    if not move() then
      return false
    end
  until state.pt.y == dy
  return true
end

-- [[ go ]] --
function TL2.gotoPoint(pt)
  return TL2.goto(pt.x, pt.y, pt.z, pt.heading)
end

-- go backwards - turning around if necessary to fight mobs / break blocks
function TL2.goback()
  local hi = TL2.headings[state.pt.heading]
  return TL2.goto(state.pt.x - hi.xd, state.pt.y - hi.yd, state.pt.z, state.pt.heading)
end

function TL2.gotoZlast(pt)
  if TL2.goto(pt.x, pt.y, nil, pt.heading) then
    if TL2.gotoZ(pt.z) then
      TL2.setHeading(pt.heading)
    end
  end
end

function TL2.goto(dx, dy, dz, dh)
  if not TL2.gotoSingleTurn(dx, dy, dz, dh) then
    if not TL2.gotoMultiTurn(dx, dy, dz) then
      return false
    end
  end
  TL2.setHeading(dh)
  return true
end

-- 1 turn goto (going backwards if possible)
function TL2.gotoSingleTurn(dx, dy, dz, dh)

  dz = dz or state.pt.z

  local function gx()
    if state.pt.x ~= dx then
      TL2.moveTowardsX(dx)
    end
    if state.pt.y ~= dy then
      if dh and dh % 2 == 1 then
        TL2.setHeading(dh)
      else
        TL2.headTowardsY(dy)
      end
    end
  end

  local function gy()
    if state.pt.y ~= dy then
      TL2.moveTowardsY(dy)
    end
    if state.pt.x ~= dx then
      if dh and dh % 2 == 0 then
        TL2.setHeading(dh)
      else
        TL2.headTowardsX(dx)
      end
    end
  end

  repeat
    local x, y
    local z = state.pt.z

    repeat
      x, y = state.pt.x, state.pt.y

      if state.pt.heading % 2 == 0 then
        gx()
        gy()
      else
        gy()
        gx()
      end
    until x == state.pt.x and y == state.pt.y

    if state.pt.z ~= dz then
      TL2.gotoZ(dz)
    end

    if state.pt.x == dx and state.pt.y == dy and state.pt.z == dz then
      return true
    end

  until x == state.pt.x and y == state.pt.y and z == state.pt.z

  return false
end

-- fallback goto - will turn around if was previously moving backwards
function TL2.gotoMultiTurn(dx, dy, dz)

  if TL2.gotoEx(dx, dy, dz) then
    return true
  end

  local moved
  repeat
    local x, y, z = state.pt.x, state.pt.y, state.pt.z

    -- try going the other way
    if (state.pt.heading % 2) == 1 then
      TL2.headTowardsX(dx)
    else
      TL2.headTowardsY(dy)
    end

    if TL2.gotoEx(dx, dy, dz) then
      return true
    end

    if dz then
      TL2.gotoZ(dz)
    end

    moved = x ~= state.pt.x or y ~= state.pt.y or z ~= state.pt.z
  until not moved

  return false
end

function TL2.gotoEx(dx, dy, dz)

  -- determine the heading to ensure the least amount of turns
  -- first check is 1 turn needed - remaining require 2 turns
  if state.pt.heading == 0 and state.pt.x <= dx or
     state.pt.heading == 2 and state.pt.x >= dx or
     state.pt.heading == 1 and state.pt.y <= dy or
     state.pt.heading == 3 and state.pt.y >= dy then
    -- maintain current heading
    -- nop
  elseif dy > state.pt.y and state.pt.heading == 0 or
         dy < state.pt.y and state.pt.heading == 2 or
         dx < state.pt.x and state.pt.heading == 1 or
         dx > state.pt.x and state.pt.heading == 3 then
    TL2.turnRight()
  else
    TL2.turnLeft()
  end

  if (state.pt.heading % 2) == 1 then
    if not TL2.gotoY(dy) then return false end
    if not TL2.gotoX(dx) then return false end
  else
    if not TL2.gotoX(dx) then return false end
    if not TL2.gotoY(dy) then return false end
  end

  if dz then
    if not TL2.gotoZ(dz) then return false end
  end

  return true
end

function TL2.gotoX(dx)
  TL2.headTowardsX(dx)

  while state.pt.x ~= dx do
    if not TL2.forward() then
      return false
    end
  end
  return true
end

function TL2.gotoY(dy)
  TL2.headTowardsY(dy)

  while state.pt.y ~= dy do
    if not TL2.forward() then
      return false
    end
  end
  return true
end

function TL2.gotoZ(dz)
  while state.pt.z > dz do
    if not TL2.down() then
      return false
    end
  end

  while state.pt.z < dz do
    if not TL2.up() then
      return false
    end
  end
  return true
end

-- [[ Slot management ]] --
function TL2.select(slot)
  state.slot = slot
  turtleApi.select(slot)
end

function TL2.getInventory(slots)
  slots = slots or { }
  for i = 1, 16 do
    slots[i] = {
      qty = turtleApi.getItemCount(i),
      index = i
    }
  end
  return slots
end

function TL2.emptyInventory(dropAction)
  dropAction = dropAction or turtleApi.drop
  for i = 1, 16 do
    TL2.emptySlot(i, dropAction)
  end
end

function TL2.emptySlot(slot, dropAction)
  dropAction = dropAction or turtleApi.drop
  local count = turtleApi.getItemCount(slot)
  if count > 0 then
    TL2.select(slot)
    return dropAction(count)
  end
  return false, 'No items to drop'
end

function TL2.getFilledSlots(startSlot)
  startSlot = startSlot or 1

  local slots = { }
  for i = startSlot, 16 do
    local count = turtleApi.getItemCount(i)
    if count > 0 then
      slots[i] = {
        qty    = turtleApi.getItemCount(i),
        index = i
      }
    end
  end
  return slots
end

function TL2.reconcileInventory(slots, dropAction)
  dropAction = dropAction or turtleApi.drop
  for _,s in pairs(slots) do
    local qty = turtleApi.getItemCount(s.index)
    if qty > s.qty then
      TL2.select(s.index)
      dropAction(qty-s.qty, s)
    end
  end
end

function TL2.selectSlotWithItems(startSlot)
  startSlot = startSlot or 1
  for i = startSlot, 16 do
    if turtleApi.getItemCount(i) > 0 then
      TL2.select(i)
      return i
    end
  end
end

function TL2.selectOpenSlot(startSlot)
  return TL2.selectSlotWithQuantity(0, startSlot)
end

function TL2.selectSlotWithQuantity(qty, startSlot)
  startSlot = startSlot or 1

  for i = startSlot, 16 do
    if turtleApi.getItemCount(i) == qty then
      TL2.select(i)
      return i
    end
  end
end

GPS = { }
function GPS.locate()
  local pt = { }
  pt.x, pt.z, pt.y = gps.locate(10)
  if pt.x then
    return pt
  end
end

function GPS.isAvailable()
  return Util.hasDevice("modem") and GPS.locate()
end

function GPS.getPoint()
  local pt = Point.copy(state.pt)

  local apt = GPS.locate()
  if not apt then
    print("GPS.getPoint: GPS not available")
    return
  end

  while not TL2.forward() do
    TL2.turnRight()
    if TL2.getHeading() == pt.heading then
      print('GPS.getPoint: Unable to move forward')
      return
    end
  end

  local bpt = GPS.locate()
  if not apt then
    print("No GPS")
    return
  end

  if not TL2.back() then
    print("GPS.getPoint: Unable to move back")
--    return
  end

  if apt.x < bpt.x then
    apt.heading = 0
  elseif apt.y < bpt.y then
    apt.heading = 1
  elseif apt.x > bpt.x then
    apt.heading = 2
  else
    apt.heading = 3
  end

  return apt
end

--[[
  All pathfinding related follows

  b = block
  a = adjacent block
  bb = bounding box
  c = coordinates
--]]
local function addAdjacentBlock(blocks, b, dir, bb, a)
  local key = a.x .. ':' .. a.y .. ':' .. a.z

  if b.adj[dir] then
    a = b.adj[dir]
  else
    local _a = blocks[key]
    if _a then
      a = _a
    else
      blocks[key] = a
    end
  end
  local revDir = { 2, 3, 0, 1, 5, 4 }
  b.adj[dir] = a
  a.adj[revDir[dir+1]] = b
--[[
  -- too much time...
  if dir == 4 and turtle.detectUp() then
    a.blocked = true
  elseif dir == 5 and turtle.detectDown() then
    a.blocked = true
  elseif dir == state.pt.heading and turtle.detect() then
    a.blocked = true
--]]
  if a.x < bb.ax or a.x > bb.bx or
         a.y < bb.ay or a.y > bb.by or
         a.z < bb.az or a.z > bb.bz then
    a.blocked = true
  end
end

local function addAdjacentBlocks(blocks, b, bb)
  if not b.setAdj then
    addAdjacentBlock(blocks, b, 0, bb,
      { x = state.pt.x+1, y = state.pt.y  , z = state.pt.z    , adj = {} })
    addAdjacentBlock(blocks, b, 1, bb,
      { x = state.pt.x  , y = state.pt.y+1, z = state.pt.z    , adj = {} })
    addAdjacentBlock(blocks, b, 2, bb,
      { x = state.pt.x-1, y = state.pt.y  , z = state.pt.z    , adj = {} })
    addAdjacentBlock(blocks, b, 3, bb,
      { x = state.pt.x  , y = state.pt.y-1, z = state.pt.z    , adj = {} })
    addAdjacentBlock(blocks, b, 4, bb,
      { x = state.pt.x  , y = state.pt.y  , z = state.pt.z + 1, adj = {} })
    addAdjacentBlock(blocks, b, 5, bb,
      { x = state.pt.x  , y = state.pt.y  , z = state.pt.z - 1, adj = {} })
  end
  b.setAdj = true
end

local function calculateAdjacentBlockDistances(b, dest)
  --local pt = state.pt
  for k,a in pairs(b.adj) do
    if not a.blocked then
      --a.distance, a.heading = Point.calculateMoves(pt, a)
      --a.distance = a.distance + Point.calculateMoves(a, dest)
      a.distance = Point.pythagoreanDistance(a, dest)
    else
      a.distance = 9000
    end
--print(string.format('%d: %f %d,%d,%d, %s', k, a.distance, a.x, a.y, a.z, tostring(a.blocked)))
  end
--read()
end

local function blockedIn(b)
  for _,a in pairs(b.adj) do
    if not a.blocked then
      return false
    end
  end
  return true
end

local function pathfindMove(b)
  for _,a in Util.spairs(b.adj, function(a, b) return a.distance < b.distance end) do

    --print('shortest: ' .. pc(a) .. ' distance: ' .. a.distance)
    if not a.blocked then
      local success = TL2.gotoSingleTurn(a.x, a.y, a.z)
      if success then
        return a
      end
      a.blocked = true
    end
  end
end

local function rpath(blocks, block, dest, boundingBox)

  addAdjacentBlocks(blocks, block, boundingBox)
  calculateAdjacentBlockDistances(block, dest)

  block.blocked = true
  repeat
    local newBlock = pathfindMove(block)
    if newBlock then
      if state.pt.x == dest.x and state.pt.y == dest.y and state.pt.z == dest.z then
        block.blocked = false
        return true
      end
      --[[
      if goto then return true end
      block = getCurrentBlock() (gets or creates block)
      but this might - will - move us into a block we marked as blockedin
      if block.blocked then
        goto newBlock
        block = getCurrentBlock() (gets or creates block)
      end
      maybe check if we have a clear line of sight to the destination
      ie. keep traveling towards destination building up blocked blocks
      as we encounter them (instead of adding all blocks on the path)
      --]]
      if rpath(blocks, newBlock, dest, boundingBox) then
        block.blocked = false
        return true
      end
      if not TL2.gotoSingleTurn(block.x, block.y, block.z) then
        return false
      end
    end
  until blockedIn(block)
  return false
end

--[[
  goto will traverse towards the destination until it is blocked
  by something on the x, y, or z coordinate of the destination

  pathfinding will attempt to find a way around the blockage

  goto example:
  . . >-X B D stuck behind block
  . . | . B .
  . . | . . .
  S >-^B. . .

  pathfind example:
  . . >-v B D when goto fails, pathfinding kicks in
  . . | |-B-|
  . . | >---^
  S >-^B. . .

--]]
function TL2.pathtofreely(dx, dy, dz, dh)
  local boundingBox = {
      ax = -9000,
      ay = -9000,
      az = -9000,
      bx = 9000,
      by = 9000,
      bz = 9000
    }
  return TL2.pathto(dx, dy, dz, dh, boundingBox)
end

local function pathfind(dx, dy, dz, dh, boundingBox)

  local blocks = { } -- memory.blocks
  local dest = { x = dx, y = dy, z = dz }
  local block = { x = state.pt.x, y = state.pt.y, z = state.pt.z, adj = {} }
  local key = block.x .. ':' .. block.y .. ':' .. block.z

  blocks[key] = block

  if rpath(blocks, block, dest, boundingBox) then
    TL2.setHeading(dh)
    return true
  end

  return false
end

function TL2.pathto(dx, dy, dz, dh, boundingBox)
  local start = { x = state.pt.x, y = state.pt.y, z = state.pt.z }
  if TL2.goto(dx, dy, dz, dh) then
    return true
  end

  if not dz then
    dz = state.pt.z
  end

  if not boundingBox then
    boundingBox = {
      ax = math.min(dx, start.x),
      ay = math.min(dy, start.y),
      az = math.min(dz, start.z),
      bx = math.max(dx, start.x),
      by = math.max(dy, start.y),
      bz = math.max(dz, start.z),
    }
  end
  return pathfind(dx, dy, dz, dh, boundingBox)
end