A* in computercraft - Attempt 2

local function log(msg) -- adapted from: https://github.com/rxi/log.lua
  if not os.loadAPI("rwt") then
    if not shell.run("pastebin", "get", "TNLjCCKq", "rwt") then
      print("Could not load Time API")
    else
      os.loadAPI("rwt")
    end
  end
  --logTime = rwt.getTime("utc",-4,dateTime24_mmddyyyy,10)
  logTime = "TIMEOFF"
  local fp = io.open("seek.log", "a")
  --local str = string.format("[%s]%s\n",logTime, msg)
  --local str = string.format("[%s]%s\n",logTime, msg)
  local str = string.format("%s\n",msg)
      fp:write(str)
      fp:close()
end

local function saveTable(table,fileName)
  local hWrite = fs.open(fileName, "w")
  hWrite.write(textutils.serialize(table))
  hWrite.close()
end

local function getFacingDirection()
    log("Get Facing Direction Called")
    local function findClearBlock()
    local turnsToClear = 0
    while turtle.inspect() and turnsToClear <=4 do
      turtle.turnRight()
      turnsToClear = turnsToClear + 1
    end
    if turnsToClear >= 4 then
      return nil
    else
    end
    if turnsToClear ~=4 and turnsToClear ~=0 then
      for faceHome = 1, turnsToClear do
        turtle.turnLeft()
      end
    end
    return turnsToClear
    end
    local startLoc = vector.new(gps.locate())
    local dirLoc
    local facingDir = 0
    local facingVector
    if turtle.forward() then
    dirLoc = vector.new(gps.locate())
    turtle.back()
    facingVector = dirLoc - startLoc
    facingDir = ((facingVector.x + math.abs(facingVector.x) * 2) + (facingVector.z + math.abs(facingVector.z) * 3))
    else
    local tCount = findClearBlock()
    if tCount == nil then
      return nil
    elseif tCount == 2 then
      turtle.back()
      dirLoc = vector.new(gps.locate())
      turtle.forward()
      facingVector = startLoc - dirLoc
      facingDir = (((facingVector.x + math.abs(facingVector.x) * 2) + (facingVector.z + math.abs(facingVector.z) * 3)))
    elseif tCount == 1 then
      turtle.turnRight()
      turtle.forward()
      turtle.turnLeft()
      dirLoc = vector.new(gps.locate())
      turtle.turnLeft()
      turtle.forward()
      turtle.turnRight()
      facingVector = dirLoc - startLoc
      facingDir = (((facingVector.x + math.abs(facingVector.x) * 2) + (facingVector.z + math.abs(facingVector.z) * 3))) - 1
    elseif
      tCount == 3 then
      turtle.turnLeft()
      turtle.forward()
      turtle.turnRight()
      dirLoc = vector.new(gps.locate())
      turtle.turnRight()
      turtle.forward()
      turtle.turnLeft()
      facingVector = dirLoc - startLoc
      facingDir = ((facingVector.x + math.abs(facingVector.x) * 2) + (facingVector.z + math.abs(facingVector.z) * 3)) + 1
      if facingDir > 4 then
      facingDir = 1
      end
    end
    end
    log(string.format("Facing:%s", tostring(facingDir)))
    return facingDir
end

local function getNodeData(currentVector, targetVector , startVector, parentVector)
  log("GetNodeData Called")
  local cNode, cNodeKey = nil
  local fNode, fNodeKey, fInspect, fResult = nil
  local uNode, uNodeKey, uInspect,uResult = nil
  local dNode, dNodeKey, dInspect, dResult = nil
  local rNode, rNodeKey, rInspect, rResult = nil
  local bNode, bNodeKey, bInspect, bResult = nil
  local lNode, lNodeKey, lInspect, lResult = nil
  cNode = {x = 0, y = 0, z = 0}
  fNode = {x = 0, y = 0, z = 0}
  uNode = {x = 0, y = 0, z = 0}
  dNode = {x = 0, y = 0, z = 0}
  rNode = {x = 0, y = 0, z = 0}
  bNode = {x = 0, y = 0, z = 0}
  lNode = {x = 0, y = 0, z = 0}
  facingDirection = getFacingDirection() -- need this to calculate neighbor offsets and move commands
  if facingDirection == 1 then -- when facing west
    if currentVector.x <= 0 then -- and we are in negative X coordinates
      fNode.x = currentVector.x - 1 -- the node in front of us is our current x value - 1
      fNode.z = currentVector.z
      fNode.y = currentVector.y
      bNode.x = currentVector.x + 1 -- the node in behind our current x value is + 1
      bNode.z = currentVector.z
      bNode.y = currentVector.y
    else
      fNode.x = currentVector.x + 1 -- when x is a possitive coordinate front is +1
      fNode.z = currentVector.z
      fNode.y = currentVector.y
      bNode.x = currentVector.x - 1 -- and back is -1
      bNode.z = currentVector.z
      bNode.y = currentVector.y
    end
    if currentVector.z <= 0 then
      lNode.z = currentVector.z + 1  -- when z is negative left is +1
      lNode.x = currentVector.x
      lNode.y = currentVector.y
      rNode.z = currentVector.z - 1  -- and right is -1
      rNode.x = currentVector.x
      rNode.y = currentVector.y
    else
      lNode.z = currentVector.z - 1  -- and the opposite when positive
      lNode.x = currentVector.x
      lNode.y = currentVector.y
      rNode.z = currentVector.z + 1
      rNode.x = currentVector.x
      rNode.y = currentVector.y
    end
  elseif facingDirection == 2 then -- and so on for each direction. 0 and 3 are mirrors as are 2 and 4
    if currentVector.x <= 0 then
      fNode.x = currentVector.x + 1
      fNode.z = currentVector.z
      fNode.y = currentVector.y
      bNode.x = currentVector.x - 1
      bNode.z = currentVector.z
      bNode.y = currentVector.y
    else
      fNode.x = currentVector.x - 1
      fNode.z = currentVector.z
      fNode.y = currentVector.y
      bNode.x = currentVector.x + 1
      bNode.z = currentVector.z
      bNode.y = currentVector.y
    end
    if currentVector.z <=0 then
      lNode.z = currentVector.z - 1
      lNode.x = currentVector.x
      lNode.y = currentVector.y
      rNode.z = currentVector.z + 1
      rNode.x = currentVector.x
      rNode.y = currentVector.y
    else
      lNode.z = currentVector.z + 1
      lNode.x = currentVector.x
      lNode.y = currentVector.y
      rNode.z = currentVector.z - 1
      rNode.x = currentVector.x
      rNode.y = currentVector.y
    end
  elseif facingDirection == 3 then
    if currentVector.x <= 0 then
      fNode.x = currentVector.x + 1
      fNode.z = currentVector.z
      fNode.y = currentVector.y
      bNode.x = currentVector.x - 1
      bNode.z = currentVector.z
      bNode.y = currentVector.y
    else
      fNode.x = currentVector.x - 1
      fNode.z = currentVector.z
      fNode.y = currentVector.y
      bNode.x = currentVector.x + 1
      bNode.z = currentVector.z
      bNode.y = currentVector.y
    end
    if currentVector.z <=0 then
      lNode.z = currentVector.z - 1
      lNode.x = currentVector.x
      lNode.y = currentVector.y
      rNode.z = currentVector.z + 1
      rNode.x = currentVector.x
      rNode.y = currentVector.y
    else
      lNode.z = currentVector.z + 1
      lNode.x = currentVector.x
      lNode.y = currentVector.y
      rNode.z = currentVector.z - 1
      rNode.x = currentVector.x
      rNode.y = currentVector.y
    end
  elseif facingDirection == 4 then
    if currentVector.x <= 0 then
      fNode.x = currentVector.x - 1
      fNode.z = currentVector.z
      fNode.y = currentVector.y
      bNode.x = currentVector.x + 1
      bNode.z = currentVector.z
      bNode.y = currentVector.y
    else
      fNode.x = currentVector.x + 1
      fNode.z = currentVector.z
      fNode.y = currentVector.y
      bNode.x = currentVector.x - 1
      bNode.z = currentVector.z
      bNode.y = currentVector.y
    end
    if currentVector.z <=0 then
      lNode.z = currentVector.z + 1
      lNode.x = currentVector.x
      lNode.y = currentVector.y
      rNode.z = currentVector.z - 1
      rNode.x = currentVector.x
      rNode.y = currentVector.y
    else
      lNode.z = currentVector.z - 1
      lNode.x = currentVector.x
      lNode.y = currentVector.y
      rNode.z = currentVector.z + 1
      rNode.x = currentVector.x
      rNode.y = currentVector.y
    end
  end
  uNode.x = currentVector.x
  uNode.z = currentVector.z
  uNode.y = currentVector.y + 1
  dNode.x = currentVector.x
  dNode.z = currentVector.z
  dNode.y = currentVector.y - 1
  cNodeKey = string.format("%d,%d,%d", currentVector.x, currentVector.y, currentVector.z)
  fNodeKey = string.format("%d,%d,%d", fNode.x, fNode.y, fNode.z)
  uNodeKey = string.format("%d,%d,%d", uNode.x, uNode.y, uNode.z)
  dNodeKey = string.format("%d,%d,%d", dNode.x, dNode.y, dNode.z)
  rNodeKey = string.format("%d,%d,%d", rNode.x, rNode.y, rNode.z)
  bNodeKey = string.format("%d,%d,%d", bNode.x, bNode.y, bNode.z)
  lNodeKey = string.format("%d,%d,%d", lNode.x, lNode.y, lNode.z)
  if not parentVector then
    pNodeKey = string.format("%d,%d,%d", currentVector.x, currentVector.y, currentVector.z)
  else
    pNodeKey = string.format("%d,%d,%d", parentVector.x, parentVector.y, parentVector.z)
  end
  if not nodeList[pNodeKey] then -- this should only happen on the first pass when seekDestination is called. Our last pass currentVector is passed as our parent thereafter.
    log(string.format("Adding pNode:%s to nodeList",pNodeKey))
    nodeList[pNodeKey] = {}
    nodeList[pNodeKey].x = currentVector.x
    nodeList[pNodeKey].y = currentVector.y
    nodeList[pNodeKey].z = currentVector.z
    nodeList[pNodeKey].g = 10
    nodeList[pNodeKey].startx = startVector.x
    nodeList[pNodeKey].starty = startVector.y
    nodeList[pNodeKey].startz = startVector.z
    nodeList[pNodeKey].endx = targetVector.x
    nodeList[pNodeKey].endy = targetVector.y
    nodeList[pNodeKey].endz = targetVector.z
    nodeList[pNodeKey].h = math.abs(nodeList[pNodeKey].x - nodeList[pNodeKey].endx) +  math.abs(nodeList[pNodeKey].y-nodeList[pNodeKey].endy) + math.abs(nodeList[pNodeKey].z-nodeList[pNodeKey].endz)
    nodeList[pNodeKey].f = nodeList[pNodeKey].g + nodeList[pNodeKey].h
    nodeList[pNodeKey].state = "closed"
  elseif nodeList[pNodeKey].state == "open" then
    log("OPEN pNode exists in nodeList. Marking as closed")
    nodeList[pNodeKey].state = "closed"
  end
  if not nodeList[cNodeKey] then -- this should only happen on the first pass when seekDestination is called. Parent and Current are equal on this pass. One of our last pass nodes should become our current node thereafter.
    log(string.format("Adding cNode:%s to nodeList",cNodeKey))
    nodeList[cNodeKey] = {}
    nodeList[cNodeKey].x = currentVector.x
    nodeList[cNodeKey].y = currentVector.y
    nodeList[cNodeKey].z = currentVector.z
    nodeList[cNodeKey].g = 10
    nodeList[cNodeKey].startx = startVector.x
    nodeList[cNodeKey].starty = startVector.y
    nodeList[cNodeKey].startz = startVector.z
    nodeList[cNodeKey].endx = targetVector.x
    nodeList[cNodeKey].endy = targetVector.y
    nodeList[cNodeKey].endz = targetVector.z
    nodeList[cNodeKey].h = math.abs(nodeList[pNodeKey].x - nodeList[cNodeKey].endx) +  math.abs(nodeList[pNodeKey].y-nodeList[cNodeKey].endy) + math.abs(nodeList[pNodeKey].z-nodeList[cNodeKey].endz)
    nodeList[cNodeKey].f = nodeList[cNodeKey].g + nodeList[cNodeKey].h
    nodeList[cNodeKey].state = "closed"
  elseif nodeList[cNodeKey].state == "open" then
    log("cNode exists in nodeList. Marking as closed")
    nodeList[cNodeKey].state = "closed"
  end
  if not nodeList[fNodeKey] then
    log(string.format("Adding fNode:%s to nodeList",fNodeKey))
    nodeList[fNodeKey] = {}
    nodeList[fNodeKey].x = fNode.x
    nodeList[fNodeKey].y = fNode.y
    nodeList[fNodeKey].z = fNode.z
    nodeList[fNodeKey].startx = startVector.x
    nodeList[fNodeKey].starty = startVector.y
    nodeList[fNodeKey].startz = startVector.z
    nodeList[fNodeKey].endx = targetVector.x
    nodeList[fNodeKey].endy = targetVector.y
    nodeList[fNodeKey].endz = targetVector.z
    nodeList[fNodeKey].g = math.abs((nodeList[cNodeKey].x-nodeList[fNodeKey].x) + (nodeList[cNodeKey].y-nodeList[fNodeKey].y) + (nodeList[cNodeKey].z-nodeList[fNodeKey].z)) + nodeList[pNodeKey].g
    nodeList[fNodeKey].h = math.abs(nodeList[fNodeKey].x - nodeList[fNodeKey].endx) +  math.abs(nodeList[fNodeKey].y-nodeList[fNodeKey].endy) + math.abs(nodeList[fNodeKey].z-nodeList[fNodeKey].endz)
    nodeList[fNodeKey].f = nodeList[fNodeKey].g + nodeList[fNodeKey].h
    nodeList[fNodeKey].state = "open"
  elseif nodeList[fNodeKey].state == "open" then
    log("fNode exists. Updating Parent node.")
    --put logic here to recalc g if needed.
  end
  if not nodeList[uNodeKey] then
    log(string.format("Adding uNode:%s to nodeList",uNodeKey))
    nodeList[uNodeKey] = {}
    nodeList[uNodeKey].x = uNode.x
    nodeList[uNodeKey].y = uNode.y
    nodeList[uNodeKey].z = uNode.z
    nodeList[uNodeKey].startx = startVector.x
    nodeList[uNodeKey].starty = startVector.y
    nodeList[uNodeKey].startz = startVector.z
    nodeList[uNodeKey].endx = targetVector.x
    nodeList[uNodeKey].endy = targetVector.y
    nodeList[uNodeKey].endz = targetVector.z
    nodeList[uNodeKey].g = math.abs((nodeList[pNodeKey].x-nodeList[uNodeKey].x) + (nodeList[pNodeKey].y-nodeList[uNodeKey].y) + (nodeList[pNodeKey].z-nodeList[uNodeKey].z)) + nodeList[pNodeKey].g
    nodeList[uNodeKey].h = math.abs(nodeList[uNodeKey].x - nodeList[uNodeKey].endx) +  math.abs(nodeList[uNodeKey].y-nodeList[uNodeKey].endy) + math.abs(nodeList[uNodeKey].z-nodeList[uNodeKey].endz)
    nodeList[uNodeKey].f = nodeList[uNodeKey].g + nodeList[uNodeKey].h
    nodeList[uNodeKey].state = "open"
  elseif nodeList[uNodeKey].state == "open" then
    log("uNode exists. Updating Parent node.")
    --put logic here to recalc g if needed.
  end
  if not nodeList[dNodeKey] then
    log(string.format("Adding dNode:%s to nodeList",dNodeKey))
    nodeList[dNodeKey] = {}
    nodeList[dNodeKey].x = dNode.x
    nodeList[dNodeKey].y = dNode.y
    nodeList[dNodeKey].z = dNode.z
    nodeList[dNodeKey].startx = startVector.x
    nodeList[dNodeKey].starty = startVector.y
    nodeList[dNodeKey].startz = startVector.z
    nodeList[dNodeKey].endx = targetVector.x
    nodeList[dNodeKey].endy = targetVector.y
    nodeList[dNodeKey].endz = targetVector.z
    nodeList[dNodeKey].g = math.abs((nodeList[pNodeKey].x-nodeList[dNodeKey].x) + (nodeList[pNodeKey].y-nodeList[dNodeKey].y) + (nodeList[pNodeKey].z-nodeList[dNodeKey].z)) + nodeList[pNodeKey].g
    nodeList[dNodeKey].h = math.abs(nodeList[dNodeKey].x - nodeList[dNodeKey].endx) +  math.abs(nodeList[dNodeKey].y-nodeList[dNodeKey].endy) + math.abs(nodeList[dNodeKey].z-nodeList[dNodeKey].endz)
    nodeList[dNodeKey].f = nodeList[dNodeKey].g + nodeList[dNodeKey].h
    nodeList[dNodeKey].state = "open"
  elseif nodeList[dNodeKey].state == "open" then
    log("dNode exists. Updating Parent node.")
    --put logic here to recalc g if needed.
  end
  if not nodeList[rNodeKey] then
    log(string.format("Adding rNode:%s to nodeList",rNodeKey))
    nodeList[rNodeKey] = {}
    nodeList[rNodeKey].x = rNode.x
    nodeList[rNodeKey].y = rNode.y
    nodeList[rNodeKey].z = rNode.z
    nodeList[rNodeKey].startx = startVector.x
    nodeList[rNodeKey].starty = startVector.y
    nodeList[rNodeKey].startz = startVector.z
    nodeList[rNodeKey].endx = targetVector.x
    nodeList[rNodeKey].endy = targetVector.y
    nodeList[rNodeKey].endz = targetVector.z
    nodeList[rNodeKey].g = math.abs((nodeList[pNodeKey].x-nodeList[rNodeKey].x) + (nodeList[pNodeKey].y-nodeList[rNodeKey].y) + (nodeList[pNodeKey].z-nodeList[rNodeKey].z)) + nodeList[pNodeKey].g
    nodeList[rNodeKey].h = math.abs(nodeList[rNodeKey].x - nodeList[rNodeKey].endx) +  math.abs(nodeList[rNodeKey].y-nodeList[rNodeKey].endy) + math.abs(nodeList[rNodeKey].z-nodeList[rNodeKey].endz)
    nodeList[rNodeKey].f = nodeList[rNodeKey].g + nodeList[rNodeKey].h
    nodeList[rNodeKey].state = "open"
  elseif nodeList[rNodeKey].state == "open" then
    log("rNode exists. Updating Parent node.")
    --put logic here to recalc g if needed.
  end
  if not nodeList[bNodeKey] then
    log(string.format("Adding bNode:%s to nodeList",bNodeKey))
    nodeList[bNodeKey] = {}
    nodeList[bNodeKey].x = bNode.x
    nodeList[bNodeKey].y = bNode.y
    nodeList[bNodeKey].z = bNode.z
    nodeList[bNodeKey].startx = startVector.x
    nodeList[bNodeKey].starty = startVector.y
    nodeList[bNodeKey].startz = startVector.z
    nodeList[bNodeKey].endx = targetVector.x
    nodeList[bNodeKey].endy = targetVector.y
    nodeList[bNodeKey].endz = targetVector.z
    nodeList[bNodeKey].g = math.abs((nodeList[pNodeKey].x-nodeList[bNodeKey].x) + (nodeList[pNodeKey].y-nodeList[bNodeKey].y) + (nodeList[pNodeKey].z-nodeList[bNodeKey].z)) + nodeList[pNodeKey].g
    nodeList[bNodeKey].h = math.abs(nodeList[bNodeKey].x - nodeList[bNodeKey].endx) +  math.abs(nodeList[bNodeKey].y-nodeList[bNodeKey].endy) + math.abs(nodeList[bNodeKey].z-nodeList[bNodeKey].endz)
    nodeList[bNodeKey].f = nodeList[bNodeKey].g + nodeList[bNodeKey].h
    nodeList[bNodeKey].state = "open"
  elseif nodeList[bNodeKey].state == "open" then
    log("bNode exists. Updating Parent node.")
    --put logic here to recalc g if needed.
  end
  if not nodeList[lNodeKey] then
    log(string.format("Adding lNode:%s to nodeList",lNodeKey))
    nodeList[lNodeKey] = {}
    nodeList[lNodeKey].x = lNode.x
    nodeList[lNodeKey].y = lNode.y
    nodeList[lNodeKey].z = lNode.z
    nodeList[lNodeKey].startx = startVector.x
    nodeList[lNodeKey].starty = startVector.y
    nodeList[lNodeKey].startz = startVector.z
    nodeList[lNodeKey].endx = targetVector.x
    nodeList[lNodeKey].endy = targetVector.y
    nodeList[lNodeKey].endz = targetVector.z
    nodeList[lNodeKey].g = math.abs((nodeList[pNodeKey].x-nodeList[lNodeKey].x) + (nodeList[pNodeKey].y-nodeList[lNodeKey].y) + (nodeList[pNodeKey].z-nodeList[lNodeKey].z)) + nodeList[pNodeKey].g
    nodeList[lNodeKey].h = math.abs(nodeList[lNodeKey].x - nodeList[lNodeKey].endx) +  math.abs(nodeList[lNodeKey].y-nodeList[lNodeKey].endy) + math.abs(nodeList[lNodeKey].z-nodeList[lNodeKey].endz)
    nodeList[lNodeKey].f = nodeList[lNodeKey].g + nodeList[lNodeKey].h
    nodeList[lNodeKey].state = "open"
  elseif nodeList[lNodeKey].state == "open" then
    log("lNode exists. Updating Parent node.")
    --put logic here to recalc g if needed.
  end
  local fResult,fInspect = turtle.inspect() -- inspect Left, up and down at no cost.
  local uResult,uInspect = turtle.inspectUp() -- using inspect instead of detect to capture block type
  local dResult,dInspect = turtle.inspectDown() -- might need it later
  turtle.turnRight()
  local rResult,rInspect = turtle.inspect()  -- time/move cost but no fuel cost so get them all
  turtle.turnRight()
  local bResult,bInspect = turtle.inspect()
  turtle.turnRight()
  local lResult,lInspect = turtle.inspect()
  turtle.turnRight()
  if fResult == true then -- if any block is found close the node and keep the block type
    nodeList[fNodeKey].state = "closed"
    nodeList[fNodeKey].block_type = fInspect.name
  end
  if uResult == true then
    nodeList[uNodeKey].state = "closed"
    nodeList[uNodeKey].block_type = uInspect.name
  end
  if dResult == true then
    nodeList[dNodeKey].state = "closed"
    nodeList[dNodeKey].block_type = dInspect.name
  end
  if rResult == true then
    nodeList[rNodeKey].state = "closed"
    nodeList[rNodeKey].block_type = rInspect.name
  end
  if bResult == true then
    nodeList[bNodeKey].state = "closed"
    nodeList[bNodeKey].block_type = bInspect.name
  end
  if lResult == true then
    nodeList[lNodeKey].state = "closed"
    nodeList[lNodeKey].block_type = lInspect.name
  end
  log(string.format("lResult= %s ", tostring(lResult)))
  for k, v in pairs(nodeList) do -- just cheking that table loaded as expected
    --log(string.format("Node:%s End X:%d End Y:%d End Z:%d", tostring(k), nodeList[k].endx, nodeList[k].endy,nodeList[k].endz))
    log(string.format("%s ; %s ; %d ; %s ; %d", tostring(k), tostring(nodeList[k].state),nodeList[k].f,nodeList[k].g, nodeList[k].h))
  end
  --return nodeList
  return nodeList[parentNodeKey], true
end

local function getBestNode(currentVector, targetVector , startVector)
  local bestMove = nil
  for vNode, nTable in pairs(nodeList) do
    if nTable.state == "open" then
      if bestMove == nil then
        log(string.format("Setting bestMove to:%s", tostring(vNode)))
        bestMove = vNode
      end
      if nTable.f < nodeList[bestMove].f then
        bestMove = vNode
      elseif nTable.f == nodeList[bestMove].f then
        if nTable.h < nodeList[bestMove].h then
          bestMove = vNode
        elseif nTable.h == nodeList[bestMove].h then
          if nodeList[bestMove].y > currentVector.y then
            if nTable.y == currentVector.y then
              bestMove = vNode
            end
          end
        end
      else
      end
    else
    end
  end
  if nodeList[bestMove].state == "open" then -- make sure the best node is open before finishing
    local bmVector = vector.new(nodeList[bestMove].x,nodeList[bestMove].y,nodeList[bestMove].z)
    return bmVector -- returning node as vector. passing as indect to nodeList did not work
  else
    return false -- if bestMove is not open let caller know.
  end
end

local function gotoNode(bnOffset, currentVector)
  facingDirection = getFacingDirection()  -- using to pick move forward or back
  log(string.format("Facing Direction is :%s", tostring(facingDirection)))
  local moves
  if facingDirection == 1 then
    if bnOffset.x > 0 then
      for moves = 1,math.abs(bnOffset.x) do --offsets should always be 1 but will move more if backtracking
        turtle.forward()
      end
    else
      for moves = 1,math.abs(bnOffset.x) do
        turtle.back()
      end
    end
    if bnOffset.z < 0 then
      turtle.turnLeft()
        for moves = 1,math.abs(bnOffset.z) do
          turtle.forward()
        end
      turtle.turnRight()
    else
      turtle.turnRight()
        for moves = 1,math.abs(bnOffset.z) do
          turtle.forward()
        end
      turtle.turnLeft()
    end
    if bnOffset.y < 0 then
      for moves = 1,math.abs(bnOffset.y) do
        turtle.down()
      end
    elseif bnOffset.y > 0 then
      for moves = 1,math.abs(bnOffset.z) do
        turtle.up()
      end
    end
  elseif facingDirection == 2 then
    if bnOffset.z > 0 then
      for moves = 1,math.abs(bnOffset.z) do
        turtle.back()
      end
    elseif bnOffset.z < 0 then
      for moves = 1,math.abs(bnOffset.z) do
        turtle.forward()
      end
    end
    if bnOffset.x < 0 then
      turtle.turnLeft()
      for moves = 1,math.abs(bnOffset.x) do
        turtle.forward()
      end
      turtle.turnRight()
    elseif bnOffset.x > 0 then
      turtle.turnRight()
      for moves = 1,math.abs(bnOffset.x) do
        turtle.forward()
      end
      turtle.turnLeft()
    end
    if bnOffset.y < 0 then
      for moves = 1,math.abs(bnOffset.y) do
        turtle.down()
      end
    elseif bnOffset.y > 0 then
      for moves = 1,math.abs(bnOffset.z) do
        turtle.up()
      end
    end
  elseif facingDirection == 3 then
    if bnOffset.x > 0 then
      for moves = 1,math.abs(bnOffset.x) do
        turtle.forward()
      end
    else
      for moves = 1,math.abs(bnOffset.x) do
        turtle.back()
      end
    end
    if bnOffset.z < 0 then
      turtle.turnLeft()
      for moves = 1,math.abs(bnOffset.z) do
        turtle.forward()
      end
      turtle.turnRight()
    else
      turtle.turnRight()
      for moves = 1,math.abs(bnOffset.z) do
        turtle.forward()
      end
      turtle.turnLeft()
    end
    if bnOffset.y < 0 then
      for moves = 1,math.abs(bnOffset.y) do
        turtle.down()
      end
    elseif bnOffset.y > 0 then
      for moves = 1,math.abs(bnOffset.z) do
        turtle.up()
      end
    end
  elseif facingDirection == 4 then
    if bnOffset.z < 0 then
      for moves = 1,math.abs(bnOffset.z) do
        turtle.forward()
      end
    elseif bnOffset.z > 0 then
      for moves = 1,math.abs(bnOffset.z) do
        turtle.back()
      end
    end
    if bnOffset.x < 0 then
      turtle.turnLeft()
      for moves = 1,math.abs(bnOffset.x) do
        turtle.forward()
      end
      turtle.turnRight()
    elseif bnOffset.x > 0 then
      turtle.turnRight()
      for moves = 1,math.abs(bnOffset.x) do
        turtle.forward()
      end
      turtle.turnLeft()
    end
    if bnOffset.y < 0 then
      for moves = 1,math.abs(bnOffset.y) do
        turtle.down()
      end
    elseif bnOffset.y > 0 then
      for moves = 1,math.abs(bnOffset.z) do
        turtle.up()
      end
    end
  end

end

local function seekDestination(currentVector, targetVector)
  local hDisplacement
  local moveCount = 0
  local startVector = currentVector
  local parentVector = currentVector
  log(string.format("Current X:%s, Current Y:%s, Current Z:%s, Target X:%s,Target Y:%s,Target Z:%s", tostring(currentVector.x), tostring(currentVector.y), tostring(currentVector.z), tostring(targetVector.x), tostring(targetVector.y), tostring(targetVector.z)))
  while currentVector.x ~= targetVector.x and currentVector.z ~= targetVector.z and moveCount < 8 do
    moveCount = moveCount + 1
    log(string.format("Starting move #%d", moveCount))
    log(string.format("Current X:%s, Current Y:%s, Current Z:%s, Target X:%s,Target Y:%s,Target Z:%s", tostring(currentVector.x), tostring(currentVector.y), tostring(currentVector.z), tostring(targetVector.x), tostring(targetVector.y), tostring(targetVector.z)))
    local lastParentNode, nodeDataStatus = getNodeData(currentVector, targetVector, startVector, parentVector)
    log(string.format("getNodeData returned %s", tostring(nodeDataStatus)))
    local bestMoveVector = getBestNode(currentVector, targetVector , startVector)
    log(string.format("getBestNode returned %s", tostring(bestMoveVector)))
    if bestMoveVector then
      log("Calculating BestMove Offset")
      local bestMoveOffset = bestMoveVector - currentVector
      log(string.format("Offset of %s beween bestmove:%s and current%s", tostring(bestMoveOffset),tostring(bestMoveVector), tostring(currentVector)))
      local bmResult = gotoNode(bestMoveOffset, currentVector)
      log(string.format("gotoNode returned %s", tostring(bmResult)))
      parentVector = currentVector
      log(string.format("Parent Vector set to %s", tostring(parentVector)))
    else
      log("No Best Node Returned")
    end
    currentVector = vector.new(gps.locate(5))
    log(string.format("new current Vector set to %s", tostring(currentVector)))
  end
end
--************Starting Seek*************
nodeList = {}
cVector = vector.new(gps.locate(5))
sVector = cVector
targetVector = vector.new(-12, 75,-562)
m = seekDestination(cVector,targetVector,sVector )