3dPathfindingAlgoritm

monitor = nil
drieD = true

function displayArea()
  maze.setBackgroundColor(colors.white)
  maze.setTextColor(colors.black)
  maze.clear()
  maze.setCursorPos(1,1)
  y = 1
  for x = 319, 296, -1 do
    for z = -258, -230 do
      if not checkObstacle(x,65,z) then
        maze.setBackgroundColor(colors.red)
        maze.write("[]")
      else
        maze.setBackgroundColor(colors.white)
        maze.write("  ")
      end
    end
    y = y + 1
    maze.setCursorPos(1,y)
  end
end

function addBlock(x,y,z,g,h,parent,path,facing)
  object = { x = x, y = y, z = z, g = g, h = h, parent = parent,path = path, facing = facing, list = "open"}
  table.insert(list,object)
end

function save(x,y,z)
  x = tonumber(x)
  y = tonumber(y)
  z = tonumber(z)
  if obstacleList[x] == nil then
    obstacleList[x] = {}
  end
  if obstacleList[x][y] == nil then
    obstacleList[x][y] = {}
  end
  obstacleList[x][y][z] = "iets"
  f = fs.open("obstacleList", "w")
  f.write(textutils.serialize(obstacleList))
  f.close()
  print("added: "..x..","..y..","..z)
end

function calculateH(x,y,z,eindx,eindy,eindz)
  return (10*(math.abs(eindx-x) + math.abs(eindy-y) + math.abs(eindz-z)))
end

function getCoords(x,y,z,dir)
  if dir == 0 then
    return x, y, (z+1)
  elseif dir == 1 then
    return (x-1), y, z
  elseif dir == 2 then
    return x, y, (z-1)
  elseif dir == 3 then
    return (x+1), y, z
  elseif dir == "u" then
    return x, (y+1), z
  else
    return x, (y-1), z
  end
end

function getOpposide(dir)
  if dir == "u" then
    return "d"
  elseif dir == "d" then
    return "u"
  else
    return ((dir+2)%4)
  end
end

function checkObstacle(x,y,z)
  free = true
  if obstacleList[x] ~= nil then
    if obstacleList[x][y] ~= nil then
      if obstacleList[x][y][z] ~= nil then
        free = false
      end
    end
  end
  return free
end

function checkFree(x,y,z)
  free = checkObstacle(x,y,z)
  for k, v in pairs(list) do
    if v.x == x and v.y == y and v.z == z then
      free = false
    end
  end
  return free
end

function findRoute(beginx,beginy,beginz,eindx,eindy,eindz,facing)
  beginx = tonumber(beginx)
  beginy = tonumber(beginy)
  beginz = tonumber(beginz)
  eindx = tonumber(eindx)
  eindy = tonumber(eindy)
  eindz = tonumber(eindz)
  facing = tonumber(facing)
  list = {}
  h = calculateH(beginx,beginy,beginz,eindx,eindy,eindz)
  --print("beginh: "..h)
  addBlock(beginx,beginy,beginz,0,h,nil,nil,facing)
  total = 0
  temp = 0
  found = false
  finalIndex = -1
  while not found do
    while not found and temp < 100 do
      selectedIndex = -1
      selectedF = 1000000
      for k, v in pairs(list) do
        if v.list == "open" then
          f = tonumber(v.g) + tonumber(v.h)
          if f < selectedF then
            selectedIndex = k
            selectedF = f
            --print("f: "..f)
          end
        end
      end
      --print("lengte: "..#list)
      --print("x: "..list[selectedIndex].x)
      --print("y: "..list[selectedIndex].y)
      --print("z: "..list[selectedIndex].z)
      list[selectedIndex].list = "closed"
      if list[selectedIndex].x == eindx and list[selectedIndex].y == eindy and list[selectedIndex].z == eindz then
        found = true
        finalIndex = selectedIndex
      else
        for i = 0, 3 do
          dir = (tonumber(list[selectedIndex].facing) + i)%4
          x, y, z = getCoords(list[selectedIndex].x,list[selectedIndex].y,list[selectedIndex].z,dir)
          --print(x..","..y..","..z..","..dir)
          if checkFree(x,y,z) then
            g = list[selectedIndex].g + 10
            if i == 1 or i == 3 then
              g = g + 4
            end
            h = calculateH(x,y,z,eindx,eindy,eindz)
            addBlock(x,y,z,g,h,selectedIndex,dir,dir)
          end
        end
        if drieD then
          if checkFree(list[selectedIndex].x,(list[selectedIndex].y+1),list[selectedIndex].z) then
            g = list[selectedIndex].g + 9
            h = calculateH(list[selectedIndex].x,(list[selectedIndex].y+1),list[selectedIndex].z,eindx,eindy,eindz)
            addBlock(list[selectedIndex].x,(list[selectedIndex].y+1),list[selectedIndex].z,g,h,selectedIndex,"u",list[selectedIndex].facing)
          end
          if checkFree(list[selectedIndex].x,(list[selectedIndex].y-1),list[selectedIndex].z) then
            g = list[selectedIndex].g + 9
            h = calculateH(list[selectedIndex].x,(list[selectedIndex].y-1),list[selectedIndex].z,eindx,eindy,eindz)
            addBlock(list[selectedIndex].x,(list[selectedIndex].y-1),list[selectedIndex].z,g,h,selectedIndex,"d",list[selectedIndex].facing)
          end
        end
        --io.read()
      end
      temp = temp + 1
    end
    total = total + temp
    temp = 0
    term.clear()
    term.setCursorPos(1,1)
    print("total loops: "..total)
    print("size list: "..#list)
    os.sleep(0.1)
  end
  print("total loops: "..total)
  print("size list: "..#list)
  temp = {}
  ready = false
  selectedIndex = finalIndex
  while not ready do
    table.insert(temp,list[selectedIndex].path)
    selectedIndex = list[selectedIndex].parent
    --print("lengte: "..#list)
    --print("index: "..selectedIndex)
    if list[selectedIndex].x == beginx and list[selectedIndex].y == beginy and list[selectedIndex].z == beginz then
      ready = true
    end
  end
  route = {}
  for i = #temp, 1, -1 do
    table.insert(route,temp[i])
  end
  return route
end

-- initialize main
modem = peripheral.wrap("back")
modem.open(7000)
obstacleList = {}
if fs.exists("obstacleList") then
  f = fs.open("obstacleList", "r")
  obstacleList = textutils.unserialize(f.readAll())
  f.close()
end
if monitor ~= nil then
  m = peripheral.wrap(monitor)
  m.setBackgroundColor(colors.black)
  m.clear()
  maze = window.create(m,1,1,61,26)
end

while true do
  event = { os.pullEvent("modem_message") }
  data = textutils.unserialize(event[5])
  if data.type == "routeRequest" then
    request = textutils.unserialize(data.request)
    route = findRoute(request.beginx,request.beginy,request.beginz,request.eindx,request.eindy,request.eindz,request.facing)
    print("route found")
    tabel = { type = "routeAnswer", id = data.id, route = route}
    modem.transmit(7001,7000,textutils.serialize(tabel))
  elseif data.type == "addObstacle" then
    info = data.info
    save(info.x,info.y,info.z)
    if monitor ~= nil then
      displayArea()
    end
  elseif data.type == "addObstacleList" then
    info = data.info
    x1 = tonumber(info.x)
    y1 = tonumber(info.y)
    z1 = tonumber(info.z)
    x2 = tonumber(info.x2)
    y2 = tonumber(info.y2)
    z2 = tonumber(info.z2)
    for x = x1, x2 do
      for y = y1, y2 do
        for z = z1, z2 do
          save(x,y,z)
        end
      end
    end
    print("obstacle list added")
    os.sleep(2)
  end
end