OpenComputers compatable A* Algorythm

--[[
    Lua star example - Run with love (https://love2d.org/)
    Copyright 2018 Wesley Werner <wesley.werner@gmail.com>

    Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions:

    The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software.

    THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.

    References:
    https://en.wikipedia.org/wiki/A*_search_algorithm
    https://www.redblobgames.com/pathfinding/a-star/introduction.html
    https://www.raywenderlich.com/4946/introduction-to-a-pathfinding
]]--

--- Provides easy A* path finding.
-- @module lua-star

local module = {}

--- Clears all cached paths.
function module:clearCached()
    module.cache = nil
end

-- (Internal) Returns a unique key for the start and end points.
local function keyOf(start, goal)
    return string.format("%d,%d,%d>%d,%d,%d", start.x, start.y, start.z, goal.x, goal.y, goal.z)
end

-- (Internal) Returns the cached path for start and end points.
local function getCached(start, goal)
    if module.cache then
        local key = keyOf(start, goal)
        return module.cache[key]
    end
end

-- (Internal) Saves a path to the cache.
local function saveCached(start, goal, path)
    module.cache = module.cache or { }
    local key = keyOf(start, goal)
    module.cache[key] = path
end

-- (Internal) Return the distance between two points.
-- This method doesn't bother getting the square root of s, it is faster
-- and it still works for our use.

local function distance(x1,y1,z1,x2,y2,z2)
  local dx = x1 - x2
  local dy = y1 - y2
  local dz = z1 - z2
  local s = dx * dx + dy * dy + dz * dz
  return s
end

-- (Internal) Clamp a value to a range.
local function clamp(x, min, max)
  return x < min and min or (x > max and max or x)
end

-- (Internal) Return the score of a node.
-- G is the cost from START to this node.
-- H is a heuristic cost, in this case the distance from this node to the goal.
-- Returns F, the sum of G and H.
local function calculateScore(previous, node, goal)

    local G = previous.score + 1 + node.p
    local H = distance(node.x, node.y, node.z, goal.x, goal.y, goal.z)
    return G + H, G, H

end

-- (Internal) Returns true if the given list contains the specified item.
local function listContains(list, item)
    for _, test in ipairs(list) do
        if test.x == item.x and test.y == item.y and test.z == item.z then
            return true
        end
    end
    return false
end

-- (Internal) Returns the item in the given list.
local function listItem(list, item)
    for _, test in ipairs(list) do
        if test.x == item.x and test.y == item.y and test.z == item.z then
            return test
        end
    end
end

local function getPowerTurn(start,point,facing)
  --Facing 2=north, 3 = south, 4 = west, 5 = east
  local function fresh(needed,thisis)
    if needed == thisis then
      return 0, facing
    end
    if needed == 2 then
      if facing == 3 then
        return 1.9
      else
        return 1
      end
    end
    if needed == 3 then
      if facing == 2 then
        return 1.9
      else
        return 1
      end
    end
    if needed == 4 then
      if facing == 5 then
        return 1.9
      else
        return 1
      end
    end
    if needed == 5 then
      if facing == 4 then
        return 1.9
      else
        return 1
      end
    end
  end
  if point.y == -1 then
    return 0.1, facing
  elseif point.y == 1 then
    return 0.1, facing
  elseif point.x == -1 then
    return fresh(4,facing), 4
  elseif point.x == 1 then
    return fresh(5,facing), 5
  elseif point.z == -1 then
    return fresh(2,facing), 2
  elseif point.z == 1 then
    return fresh(3,facing), 3
  end
end

-- (Internal) Requests adjacent map values around the given node.
local function getAdjacent(width, height, length, node, positionIsOpenFunc)
    local result = { }

    local positions = {
        { x = 0, y = 0, z = -1 },  -- north
        { x = -1, y = 0, z = 0 },  -- west
        { x = 0, y = 0, z = 1 },   -- south
        { x = 1, y = 0, z = 0 },   -- east
        { x = 0, y = 1, z = 0 },   -- up
        { x = 0, y = -1, z = 0}    -- down
    }

    for _, point in ipairs(positions) do
        local px = clamp(node.x + point.x, -1 * width, width)
        local py = clamp(node.y + point.y, -1 * height, height)
        local pz = clamp(node.z + point.z, -1 * length, length)
        local value = positionIsOpenFunc( px, py, pz )
        if value then
            local power, facing = getPowerTurn(node,point,node.f)
            table.insert( result, { x = px, y = py, z = pz, f = facing, p = power } )
        end
    end

    return result

end

-- Returns the path from start to goal, or false if no path exists.
function module:find(width, height, length, start, goal, positionIsOpenFunc, useCache, distanceMode)

    if useCache then
        local cachedPath = getCached(start, goal)
        if cachedPath then
            return cachedPath
        end
    end

    local success = false
    local open = { }
    local closed = { }

    start.score = 0
    start.G = 0
    start.H = distance(start.x, start.y, start.z, goal.x, goal.y, goal.z)
    start.parent = { x = 0, y = 0, z = 0 }
    table.insert(open, start)

    while not success and #open > 0 do

        -- sort by score or distance: high to low
        if distanceMode == false then
          table.sort(open, function(a, b) return a.score > b.score end)
        else
          table.sort(open, function(a, b) return a.H > b.H end)
        end

        local current = table.remove(open)
        table.insert(closed, current)

        success = listContains(closed, goal)

        if not success then

            local adjacentList = getAdjacent(width, height, length, current, positionIsOpenFunc, false)

            for _, adjacent in ipairs(adjacentList) do

                if not listContains(closed, adjacent) then

                    if not listContains(open, adjacent) then

                        adjacent.score,_,adjacent.H = calculateScore(current, adjacent, goal)
                        adjacent.parent = current
                        table.insert(open, adjacent)

                    end

                end

            end

        end

    end

    if not success then
        return false
    end

    -- traverse the parents from the last point to get the path
    local node = listItem(closed, closed[#closed])
    local path = { }

    while node do

        table.insert(path, 1, { x = node.x, y = node.y, z = node.z } )
        node = listItem(closed, node.parent)

    end

    saveCached(start, goal, path)

    -- reverse the closed list to get the solution
    return path

end

return module