A* in computercraft

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)
  local fp = io.open("seek.log", "a")
  local str = string.format("[%s]%s\n",logTime, 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

Node = {  --Thanks to Karmakilledthecat for the function tutorials
  x=0, y=0, z=0, parentx=0,parenty=0,parentz=0,parentg=0,startx=0,starty=0,startz = 0,endz=0,endx=0,endy=0,endz=0,g=0,h=0,f=0, w = 0, state = "",block_type = "", meta = {},

  new = function(nodeVector,startVector,endVector,parentNode)
   if  nodeVector then
      if type(nodeVector) ~= "table" then
        error("vector expected",2)
      elseif not nodeVector.x then
        error("vector expected",2)
      end
    else
      error("vector expected",2)
    end
    if  startVector then
      if type(startVector) ~= "table" then
        error("vector expected",2)
      elseif not startVector.x then
        error("vector expected",2)
      end
    else
      error("vector expected",2)
    end
    if  endVector then
      if type(endVector) ~= "table" then
        error("vector expected",2)
      elseif not endVector.x then
        error("vector expected",2)
      end
    else
      error("vector expected",2)
    end
    if parentNode then
      if type(parentNode) ~= "table" then
        error("node expected",2)
      elseif not parentNode.x then
        error("node expected",2)
      end
    else
     parentNode = { x = 0, y = 0, z = 0, g = 0, w = 0}
     --parentNode.x = startVector.x
     --parentNode.y = startVector.y
     --parentNode.z = startVector.z
     parentNode.g = math.abs(( nodeVector.x - startVector.x) + (nodeVector.y - startVector.y) + (nodeVector.z-startVector.z)) + 10 -- so that parent cost is never 0 on first pass
    end

    local n = {}
    setmetatable(n,Node.meta)
    for k,v in pairs(Node) do
      n[k] = v
    end
    n.x = nodeVector.x
    n.y = nodeVector.y
    n.z = nodeVector.z
    n.parentx = parentNode.x
    n.parenty = parentNode.y
    n.parentz = parentNode.z
    n.parentg = parentNode.g
    n.startx = startVector.x
    n.starty = startVector.y
    n.startz = startVector.z
    n.endx = endVector.x
    n.endy = endVector.y
    n.endz = endVector.z
    n.g = math.abs((n.x-n.parentx) + (n.y-n.parenty) + (n.z-n.parentz)) + parentNode.g
    n.h = math.abs((n.x-n.endx) + (n.y-n.endy) + (n.z-n.endz))
    if n.parentx == 0 and n.parenty == 0 and n.parentz == 0 then
    n.state = "closed"
    elseif n.g == 0 or n.h == 0 then
      n.state = "closed"
    else
      n.state = "open"
    end
    n.f = n.g + n.h + n.w
    return n
  end,

  closeNode = function(n)
    n.state = "closed"
  end,

  openNode = function(n)
  if  n then
      if type(n) ~= "table" then
        error("Node expected",2)
      elseif not n.x then
        error("Node expected",2)
      end
    else
      error("Node expected",2)
    end
    n.state = "open"
  end,

  calc = function(n)
   n.g = math.abs((n.x-n.parentx) + (n.y-n.parenty) + (n.z-n.parentz)) + n.parentg
   n.h = math.abs((n.x-n.endx) + (n.y-n.endy) + (n.z-n.endz))
   n.f = n.g + n.h + n.w
  end,

  raise = function(n)
   n.w = n.w + 1
   n.f = n.g + n.h + n.w
  end,

  decrease = function(n)
    if n.w >=1 then
      n.w = n.w -1
    end
    n.f = n.g + n.h + n.w-1
  end,

  serialize = function(n,filename)
  end,

  unserialize = function(n)

  end,
}

Node.meta.__tostring = function(n)
  return "("..n.x..","..n.y..","..n.z..")".."("..n.parentx..","..n.parenty..","..n.parentz..")".."("..n.g..")".."("..n.h..")".."("..n.f..")".."("..n.state..")"
end

local function getFacingDirection()
  log("Get Facing Direction Called")
  local function findClearBlock() -- returns the number of turns you must turn right to find a lear block at the current level
  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
  return facingDir
end

local function seekDestination(currentVector, targetVector,safeHeight, startVector)
  local nodeList = {}
  local bestMove
  local hDisplacement
  local function getNodeData() --get Neighbor details
    log("GetNodeData Called")
    local pNode = Node.new(currentVector,startVector,targetVector) --
    log(string.format("pNode set to: %s", tostring(pNode)))
    local fNode, bNode, lNode, rNode, uNode, dNode
    fNode = Node.new(currentVector,startVector,targetVector,pNode) --Initialize each neighbor as local
    bNode = Node.new(currentVector,startVector,targetVector,pNode)
    lNode = Node.new(currentVector,startVector,targetVector,pNode)
    rNode = Node.new(currentVector,startVector,targetVector,pNode)
    uNode = Node.new(currentVector,startVector,targetVector,pNode)
    dNode = Node.new(currentVector,startVector,targetVector,pNode)
    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
      bNode.x = currentVector.x + 1 -- the node in behind our current x value is + 1
    else
      fNode.x = currentVector.x + 1 -- when x is a possitive coordinate front is +1
      bNode.x = currentVector.x - 1 -- and back is -1
    end
    if currentVector.z <= 0 then
      lNode.z = currentVector.z + 1  -- when z is negative left is +1
      rNode.z = currentVector.z - 1  -- and right is -1
    else
      lNode.z = currentVector.z - 1  -- and the opposite when positive
      rNode.z = currentVector.z + 1
    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
      bNode.x = currentVector.x - 1
    else
      fNode.x = currentVector.x - 1
      bNode.x = currentVector.x + 1
    end
    if currentVector.z <=0 then
      lNode.z = currentVector.z - 1
      rNode.z = currentVector.z + 1
    else
      lNode.z = currentVector.z + 1
      rNode.z = currentVector.z - 1
    end
    elseif facingDirection == 3 then
    if currentVector.x <= 0 then
      fNode.x = currentVector.x + 1
      bNode.x = currentVector.x - 1
    else
      fNode.x = currentVector.x - 1
      bNode.x = currentVector.x + 1
    end
    if currentVector.z <=0 then
      lNode.z = currentVector.z - 1
      rNode.z = currentVector.z + 1
    else
      lNode.z = currentVector.z + 1
      rNode.z = currentVector.z - 1
    end
    elseif facingDirection == 4 then
    if currentVector.x <= 0 then
      fNode.x = currentVector.x - 1
      bNode.x = currentVector.x + 1
    else
      fNode.x = currentVector.x + 1
      bNode.x = currentVector.x - 1
    end
    if currentVector.z <=0 then
      lNode.z = currentVector.z + 1
      rNode.z = currentVector.z - 1
    else
      lNode.z = currentVector.z - 1
      rNode.z = currentVector.z + 1
    end
    end
    uNode.y = currentVector.y + 1 -- up is up
    dNode.y = currentVector.y - 1 -- down is down no matter which way you are facing.
    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
        fNode:closeNode()
        fNode.block_type = fInspect.name
    end
    if uResult == true then
      uNode:closeNode()
      uNode.block_type = uInspect.name
    end
    if dResult == true then
      dNode:closeNode()
      dNode.block_type = dInspect.name
    end
    if rResult == true then
      rNode:closeNode()
      rNode.block_type = rInspect.name
    end
    if bResult == true then
      bNode:closeNode()
      bNode.block_type = bInspect.name
    end
    if lResult == true then
      lNode:closeNode()
      lNode.block_type = lInspect.name
    end
    fNode:calc()  -- recalculate the scores now that actual coordinates are set
    uNode:raise() -- make up down cost a littke more
    uNode:calc()
    dNode:raise() -- make up down cost a little more
    dNode:calc()
    rNode:calc()
    bNode:calc()
    lNode:calc()
    parentNodeKey = string.format("%d,%d,%d", pNode.x, pNode.y, pNode.z) -- want the coordinates as the primay key
    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 nodeList[parentNodeKey] == nil then -- if the prmary key is not in the table
      nodeList[parentNodeKey] = pNode -- add key and set the node table by reference <- I suspect this is the problem
      nodeList[parentNodeKey].state = "closed" -- close the node now that "state" exists
    elseif nodeList[parentNodeKey].state == "open" then
      nodeList[parentNodeKey].state = "closed" -- if it is already there just close it
    end
    if nodeList[fNodeKey] == nil or nodeList[fNodeKey].state == "open" then -- for the rest just add or update if open
        nodeList[fNodeKey] = fNode
    end
    if nodeList[uNodeKey] == nil or nodeList[uNodeKey].state == "open" then
        nodeList[uNodeKey] = uNode
    end
    if nodeList[dNodeKey] == nil or nodeList[dNodeKey].state == "open" then
        nodeList[dNodeKey] = dNode
    end
    if nodeList[rNodeKey] == nil or nodeList[rNodeKey].state == "open" then
       nodeList[rNodeKey] = rNode
    end
    if nodeList[bNodeKey] == nil or nodeList[bNodeKey].state == "open" then
       nodeList[bNodeKey] = bNode
    end
    if nodeList[lNodeKey] == nil or nodeList[lNodeKey].state == "open" then
        nodeList[lNodeKey] = lNode
    end
    --[[for k, v in pairs(nodeList) do -- just cheking that table loaded as expected
        log(string.format("nodeList Key:%s Value:%s", tostring(k), tostring(v)))
        if type(v) then
            for sk, sv in pairs(v) do
                log(string.format("nodeList Key:%s Value:%s", tostring(sk), tostring(sv)))
            end
        end
    end --]]
    return nodeList
  end
 --*************************************************************************************
 local function getBestNode(nodeData)
    for vNode, nTable in pairs(nodeData) do
        if nTable.state == "open" then -- only consider open nodes
            if bestMove == nil then -- grab first open node to have something to compare to
                log(string.format("Setting bestMove to:%s", tostring(vNode)))
                bestMove = vNode
            end
            if nTable.f < nodeList[bestMove].f then -- if this node is better than the best seen
               --nodeList[bestMove].state = "closed" -- close current best <- Might be part of the problem
               bestMove = vNode -- set this node as the new best choice
            elseif nTable.f == nodeList[bestMove].f then -- if nodes costs are equal
               if nTable.g < nodeList[bestMove].g then -- use distance to destination as a tie breaker
                --nodeList[bestMove].state = "closed" --close the old best
                bestMove = vNode -- set the new best
               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
  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 moveCount = 0
 while currentVector ~= targetVector and moveCount < 20 do -- limit number of moves so we don't have to chase turtle
    moveCount = moveCount + 1
    log(string.format("Starting move #%d", moveCount))
    log(string.format("Current Location is :%s", tostring(currentVector)))
    local iNodes = getNodeData() -- this seems to work. Tested all directions with and without blocks on 3 sides
    log(string.format("getNodeData returned %s", tostring(iNodes)))
    local oVect = getBestNode(iNodes) -- I think the problem is in here. After a couple move returns false.
    log(string.format("getBestNode returned %s", tostring(oVect)))
    if oVect then
        log("Calculating MestMove Offset")
        local bestOffset = oVect - currentVector
        log(string.format("Offset of %s beween bestmove:%s and current%s", tostring(bestOffset),tostring(oVect), tostring(currentVector)))
        local bmResult = gotoNode(bestOffset, currentVector)
        log(string.format("gotoNode returned %s", tostring(bmResult)))
    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*************
cVector = vector.new(gps.locate(5))
sVector = cVector
targetVector = vector.new(-12, 76,-562)
m = seekDestination(cVector,targetVector, 86, sVector )