-- mostly playground for my funny pathfinding
-- all this code should eventually be part of movement.lua

local mv = require("movement")

local directions = {
  vector.new(1, 0, 0),
  vector.new(-1, 0, 0),
  vector.new(0, 0, 1),
  vector.new(0, 0, -1),
  vector.new(0, 1, 0),
  vector.new(0, -1, 0),
}
local obstacles = {}

-- get from a vector table
local function G(table, vector)
  return table[tostring(vector)]
end

-- set to a vector table
local function S(table, vector, value)
  table[tostring(vector)] = value
end

local function distance(v1, v2)
  local delta = (v1 - v2)
  return math.abs(delta.x) + math.abs(delta.y) + math.abs(delta.z)
end

local function pathfind(target, max_iter)
  if G(obstacles, target) then
    return false, "target blocked"
  end

  local source, prev_dir = mv.getPosition()
  local current = source
  local prev_dist = distance(source, target)
  local path_depth = 0
  local prev_nodes = {}
  local visited = {}

  if prev_dist == 1 then
    return true, {target}
  end

  if prev_dist == 0 then
    return true, {}
  end

  local queue = {}
  local queue_min = math.huge
  local queue_length = 0
  local function queue_push(key, value)
    queue_length = queue_length + 1
    if key < queue_min then
      queue_min = key
    end
    if queue[key] == nil then
      queue[key] = {}
    end
    table.insert(queue[key], value)
  end

  local function queue_pop()
    assert(queue_length > 0)
    queue_length = queue_length - 1
    while queue[queue_min] == nil or #queue[queue_min] == 0 do
      queue_min = queue_min + 1
    end
    return table.unpack(table.remove(queue[queue_min]))
  end

  local function reverse(tab)
    for i = 1, math.floor(#tab / 2), 1 do
      tab[i], tab[#tab - i + 1] = tab[#tab - i + 1], tab[i]
    end
    return tab
  end

  --- @return table<number, ccTweaked.Vector>
  local function reconstructPath()
    local path = {}
    local node = target
    while true do
      table.insert(path, node)
      node = G(prev_nodes, node)
      if node == source then break end
    end
    return reverse(path)
  end

  local closest_node = nil
  local closest_distance = math.huge

  for i = 1, max_iter do
    assert(not G(visited, current), "this should never happen")
    S(visited, current, true)
    path_depth = path_depth + 1

    local min_dist = math.huge
    local min_next = nil
    local min_dir = nil
    local function try_dir(direction)
      local next = current + direction
      if G(obstacles, next) or G(visited, next) then return end

      local dist = distance(next, target)
      queue_push(dist + path_depth, { next, current, path_depth })
      if min_dist > dist then
        min_dir = direction
        min_dist = dist
        min_next = next
      end
    end

    try_dir(prev_dir)
    for _, direction in pairs(directions) do
      if direction ~= prev_dir then
        try_dir(direction)
      end
    end

    if min_dist < closest_distance then
      closest_distance = min_dist
      closest_node = min_next
    end

    if min_dist > prev_dist then
      local prev
      while G(visited, current) do
        if queue_length == 0 then
          return false, "target unreachable"
        end

        current, prev, path_depth = queue_pop()
      end
      S(prev_nodes, current, prev)
      prev_dist = distance(current, target)
    else
      assert(min_next and min_dir)
      S(prev_nodes, min_next, current)
      current = min_next
      prev_dist = min_dist
      prev_dir = min_dir

      if prev_dist == 0 then
        return true, reconstructPath()
      end
    end
  end
  return false, "ran out of scanning depth", closest_node
end

local function followPath(target, depth, is_obstacle)
  local source,_ = mv.getPosition()
  if depth == nil then
    depth = distance(target, source) + 40
  end

  local not_done_yet = false
  while true do
    local ok, path, optional_node = pathfind(target, depth)
    if not ok then
      print("path error:", path)
      print("target = ", target, "depth = ", depth, "source = ", source)
      return false, optional_node
    end
    assert(type(path) == "table")
    if #path >= 2 then
      print("-->", table.unpack(path))
    end

    not_done_yet = false
    for _, node in pairs(path) do
      if not mv.attemptMoveTo(node) then
        if is_obstacle(node) then
          print("found new obstacle at " .. tostring(node))
          S(obstacles, node, true)
          not_done_yet = true
          break
        else
          mv.goBackTo(node.x, node.y, node.z)
        end
      end
    end

    if not not_done_yet then
      return true
    end
  end
end

local function forcePath(target, is_obstacle)
  local function panic()
    mv.panic("no path to " .. target .. " was found. press any key to plow thru")
    mv.goBackTo(target.x, target.y, target.z)
  end

  local ok, extra_node = followPath(target, nil, is_obstacle)
  if not ok then
    print("trying to do a 2 part pathfind")
    ok, _ = followPath(extra_node, nil, is_obstacle)
    if not ok then panic() return end
    ok, _ = followPath(target, nil, is_obstacle)
    if not ok then panic() return end
  end
  mv.goBackTo(target.x, target.y, target.z)
end

local function reset(is_obstacle)
  forcePath(vector.new(0, 0, 0), is_obstacle)
  mv.face(1,0)
end

local function digNonObstacles(next, is_obstacle)
  local res = followPath(next, nil, is_obstacle)
  if not res then return end
  local up = vector.new(0,1,0)
  local above = next + up
  local bellow = next - up
  if not is_obstacle(above) then turtle.digUp() end
  if not is_obstacle(bellow) then turtle.digDown() end
end

local function cube(offset, n, height, m, is_obstacle, callback)
  local function digLayer(k)
    for i=0,m-1 do
      for j=0,n-1 do
        local next
        if i % 2 == 0 then
          next = offset + vector.new(1 + j, k, i)
        else
          next = offset + vector.new(1 + (n-j-1), k, i)
        end
        digNonObstacles(next, is_obstacle)
        if callback then callback() end
      end
    end
  end

  for k=0,(height/3)-1 do
    digLayer(1+k*3)
  end
  if height%3 > 0 then
    digLayer(height-2)
  end
  reset(is_obstacle)
end

local obstacle = {}
function obstacle.air(node)
  local success, data = mv.inspectAt(node)
  return success
end

function obstacle.leaves(node)
  local success, data = mv.inspectAt(node)
  if not success then return false end
  if data.name:find("leaves") then return false end
  return true
end

local block_type_rom = {
  ["minecraft:andesite"]             = "stone",
  ["minecraft:stone"]                = "stone",
  ["minecraft:deepslate"]            = "stone",
  ["minecraft:diorite"]              = "stone",
  ["minecraft:granite"]              = "stone",
  ["minecraft:tuff"]                 = "stone",
  ["minecraft:coarse_dirt"]          = "stone",
  ["minecraft:dirt"]                 = "stone",
  ["minecraft:grass_block"]          = "stone",
  ["minecraft:moss_block"]           = "stone",
  ["minecraft:mud"]                  = "stone",
  ["minecraft:muddy_mangrove_roots"] = "stone",
  ["minecraft:mycelium"]             = "stone",
  ["minecraft:podzol"]               = "stone",
  ["minecraft:rooted_dirt"]          = "stone",
  ["minecraft:pale_moss_block"]      = "stone",
  ["minecraft:gravel"]               = "stone",
  ["minecraft:sand"]                 = "stone",
}

function obstacle.stone(node)
  local success, data = mv.inspectAt(node)
  if not success then return false end
  if block_type_rom[data.name] == "stone" then return false end
  return true
end

local function addExclusionZone(a1, a2)
  local b1 = vector.new(math.min(a1.x, a2.x), math.min(a1.y, a2.y), math.min(a1.z, a2.z))
  local b2 = vector.new(math.max(a1.x, a2.x), math.max(a1.y, a2.y), math.max(a1.z, a2.z))

  for x=b1.x,b2.x do
    for y=b1.y,b2.y do
      for z=b1.z,b2.z do
        S(obstacles, vector.new(x,y,z), true)
      end
    end
  end
end

return {
  cube = cube,
  followPath = followPath,
  forcePath = forcePath,
  obstacle = obstacle,
  distance = distance,
  addExclusionZone = addExclusionZone,
}