local Core = {}
local world = require("world")
local player = require("player")
local threads = require("threads")


local getblock = world.getBlock
local getCollisionBoxes = world.getCollisionBoxes

local floor = math.floor
local abs = math.abs
local max = math.max
local min = math.min
local sqrt = math.sqrt
local ceil = math.ceil
local huge = math.huge
local insert = table.insert
local find = string.find

Core.maxNodes = 50000

Core.jumpHeight = 1
Core.smoothPath = true
Core.fallDepth = 1

-- Weight applied to the heuristic. 0.8 matches original behavior (slightly
-- conservative / more thorough search). Raise toward 1.0-1.3 for faster,
-- greedier (less optimal) paths if you need more speed.
Core.heuristicWeight = 0.8

Core.queuedPaths = {}

Core.debugCapture = true
Core._debugExpanded = {}
Core._debugOpen = {}
local _lock = false

local function key(x, y, z)
    -- Bit-packed integer key: avoids string allocation/GC pressure
    return (x + 30000000) * 0x1000000 + (y + 1024) * 0x1000 + (z + 30000000)
end

-- Caches: block data, ladder checks, collision heights, and full walkability
-- results. These assume the world is static for the duration of a search.
-- They are cleared at the start of every astarSearch via clearCaches().
local _blockCache = {}
local _ladderCache = {}
local _collisionCache = {}
local _walkableCache = {}

local function clearCaches()
    _blockCache = {}
    _ladderCache = {}
    _collisionCache = {}
    _walkableCache = {}
end
Core.clearCaches = clearCaches

local function getBlockCached(bx, by, bz)
    local k = key(bx, by, bz)
    local b = _blockCache[k]
    if b == nil then
        b = getblock(bx, by, bz)
        _blockCache[k] = b or false
    end
    return b ~= false and b or nil
end

local function isSolid(bx, by, bz)
    local block = getBlockCached(bx, by, bz)
    return block and block.is_solid or false
end

local function isLadder(x, y, z)
    local k = key(x, y, z)
    local cached = _ladderCache[k]
    if cached ~= nil then return cached end

    local block = getBlockCached(x, y, z)
    local result = block and find(block.identifier or "", "ladder", 1, true) ~= nil
    _ladderCache[k] = result
    return result
end

local function heuristic(ax, ay, az, bx, by, bz)
    local dx = abs(ax - bx)
    local dy = abs(ay - by)
    local dz = abs(az - bz)
    local hi = max(dx, dz)
    local lo = min(dx, dz)
    return ((hi - lo) + lo * 1.41421356 + dy) * Core.heuristicWeight
end

function Core.getMaxYCollision(x, y, z)
    local k = key(x, y, z)
    local cached = _collisionCache[k]
    if cached ~= nil then return cached end

    local blockState = getBlockCached(x, y, z)
    local maxY = 0
    if blockState then
        local collisions = getCollisionBoxes(x, y, z, blockState)
        if collisions then
            for i = 1, #collisions do
                local c = collisions[i]
                if maxY < c.maxY then maxY = c.maxY end
            end
        end
    end

    _collisionCache[k] = maxY
    return maxY
end

-- isWalkable is a pure function of static world state, so its full result is
-- cached per-coordinate. This is the single biggest win: resolveY previously
-- called isWalkable up to ~16x per neighbor, each doing up to 4 fresh
-- getCollisionBoxes world queries with zero reuse.
local function isWalkable(bx, by, bz)
    local k = key(bx, by, bz)
    local cached = _walkableCache[k]
    if cached ~= nil then return cached end

    local result = false
    repeat
        local gnd = getBlockCached(bx, by - 1, bz)
        if not gnd or not gnd.is_solid then break end
        if gnd.is_liquid then break end

        local gndCollision = Core.getMaxYCollision(bx, by - 1, bz)
        if gndCollision > 1.0 then break end

        local foot = getBlockCached(bx, by, bz)

        -- ladders always walkable
        if foot and isLadder(bx, by, bz) then
            result = true
            break
        end

        local footCollision = Core.getMaxYCollision(bx, by, bz)
        if footCollision > 0.6 then break end

        local head = getBlockCached(bx, by + 1, bz)
        if head and head.is_solid then break end

        if gndCollision + footCollision > 1 then break end
        if Core.getMaxYCollision(bx, by + 1, bz) >= 0.5 then break end
        if Core.getMaxYCollision(bx, by + 2, bz) > 0.5 then break end

        -- lava check
        if foot and foot.identifier and find(foot.identifier, "lava", 1, true) then break end

        result = true
    until true

    _walkableCache[k] = result
    return result
end

Core.debugResolveY = {}

local function resolveY(cx, cy, cz, nx, nz)
    if isWalkable(nx, cy, nz) then return cy end

    if isLadder(nx, cy, nz) or isLadder(nx, cy + 1, nz) or isLadder(nx, cy + 2, nz) then
        local upY = cy
        while isLadder(nx, upY + 1, nz) do
            upY = upY + 1
        end

        if isWalkable(nx, upY + 1, nz) then
            return upY + 1
        elseif isWalkable(nx, upY, nz) then
            return upY + 1
        end
    end

    local jumpHeight = Core.jumpHeight
    for j = 1, jumpHeight do
        if Core.getMaxYCollision(cx, cy + j + 1, cz) > 0.5 then break end
        if isWalkable(nx, cy + j, nz) then
            return cy + j
        end
        if isSolid(nx, cy + j, nz) and not isWalkable(nx, cy + j + 1, nz) then break end
    end

    local fallDepth = Core.fallDepth
    for d = 0, fallDepth do
        local targetY = cy - d
        if Core.debugCapture and Core.debugResolveY then
            Core.debugResolveY[#Core.debugResolveY + 1] = { x = nx, y = targetY, z = nz }
        end

        if isWalkable(nx, targetY, nz) then
            return targetY
        end

        if isLadder(nx, targetY, nz) then
            local slideY = targetY
            while isLadder(nx, slideY - 1, nz) do
                slideY = slideY - 1
            end
            if isWalkable(nx, slideY - 1, nz) then
                return slideY - 1
            end
            return slideY
        end

        if isSolid(nx, targetY, nz) then
            break
        end
    end

    return nil
end

function Core.debugViewYResolve(ctx)
    local list = Core.debugResolveY
    for i = 1, #list do
        local block = list[i]
        local filled = {
            x = block.x,
            y = block.y,
            z = block.z,
            red = 255,
            green = 0,
            blue = 0,
            alpha = 140,
            through_walls = true
        }
        ctx.renderFilled(filled)
    end
end

local function hasGroundLOS(ax, ay, az, bx, by, bz)
    local dx = bx - ax
    local dz = bz - az
    local distance = sqrt(dx * dx + dz * dz)

    if distance < 0.1 then return true end
    local steps = ceil(distance / 0.3)

    for i = 0, steps do
        local t = i / steps
        local curX = floor(ax + (dx * t) + 0.5)
        local curZ = floor(az + (dz * t) + 0.5)

        local ground = getBlockCached(curX, ay - 1, curZ)
        local groundCol = Core.getMaxYCollision(curX, ay - 1, curZ)

        if not ground or not ground.is_solid or groundCol < 0.1 then
            return false
        end

        local footBlock = getBlockCached(curX, ay, curZ)

        if footBlock and footBlock.is_solid and Core.getMaxYCollision(curX, ay, curZ) > 0.5 then
            return false
        end
    end

    return true
end

local Heap = {}
Heap.__index = Heap

function Heap.new()
    return setmetatable({ _d = {}, _n = 0 }, Heap)
end

function Heap:push(item)
    self._n = self._n + 1
    self._d[self._n] = item
    local i, d = self._n, self._d
    while i > 1 do
        local p = floor(i / 2)
        if d[p].f > d[i].f then
            d[p], d[i] = d[i], d[p]; i = p
        else
            break
        end
    end
end

function Heap:pop()
    if self._n == 0 then return nil end
    local top = self._d[1]
    self._d[1] = self._d[self._n]
    self._d[self._n] = nil
    self._n = self._n - 1
    local i, d, n = 1, self._d, self._n
    while true do
        local s, l, r = i, 2 * i, 2 * i + 1
        if l <= n and d[l].f < d[s].f then s = l end
        if r <= n and d[r].f < d[s].f then s = r end
        if s == i then break end
        d[i], d[s] = d[s], d[i]; i = s
    end
    return top
end

function Heap:size() return self._n end

local function smoothPath(rawPath)
    local n = #rawPath
    if n <= 2 then return rawPath end

    local smooth = { rawPath[1] }
    local anchor = 1
    local i      = 2
    local valid  = true

    while i <= n do
        local a = rawPath[anchor]
        local b = rawPath[i]

        if b.y ~= a.y then
            if i - 1 > anchor then
                if b.y - a.y > Core.jumpHeight then
                    valid = false
                    break
                end
                insert(smooth, rawPath[i - 1])
                anchor = i - 1
            else
                insert(smooth, b)
                anchor = i
                i = i + 1
            end
        else
            if not hasGroundLOS(a.x, a.y, a.z, b.x, b.y, b.z) then
                if i - 1 > anchor then
                    insert(smooth, rawPath[i - 1])
                    anchor = i - 1
                else
                    insert(smooth, b)
                    anchor = i
                    i = i + 1
                end
            else
                i = i + 1
            end
        end
    end

    local last  = rawPath[n]
    local slast = valid and smooth[#smooth] or rawPath[n]
    if not (slast.x == last.x and slast.y == last.y and slast.z == last.z) then
        insert(smooth, last)
    end
    return smooth
end

local function reconstructPath(came, node)
    local raw = {}
    local cur = node
    local n = 0
    while cur do
        n = n + 1
        raw[n] = { x = cur.x, y = cur.y, z = cur.z }
        cur = came[key(cur.x, cur.y, cur.z)]
    end
    local half = floor(n / 2)
    for a = 1, half do
        local b = n - a + 1
        raw[a], raw[b] = raw[b], raw[a]
    end
    return raw
end

local VERTICAL_DIRS = { { 0, 0, 1 }, { 0, 0, -1 } }
local DIRS = {
    { 1, 0 }, { -1, 0 }, { 0, 1 }, { 0, -1 },
    { 1, 1 }, { 1, -1 }, { -1, 1 }, { -1, -1 },
}

local function astarSearch(start, goal)
    -- clear all per-search caches at beginning of each search
    -- clearCaches()
    local open          = Heap.new()
    local gScore        = {}
    local came          = {}
    local closed        = {}

    Core._debugExpanded = {}
    Core._debugOpen     = {}

    local doSmooth      = Core.smoothPath
    local maxNodes      = Core.maxNodes
    local debugCapture  = Core.debugCapture

    local sk            = key(start.x, start.y, start.z)
    gScore[sk]          = 0

    local startH        = heuristic(start.x, start.y, start.z, goal.x, goal.y, goal.z)
    open:push({
        x = start.x,
        y = start.y,
        z = start.z,
        f = startH,
        h = startH,
    })

    local bestNode   = { x = start.x, y = start.y, z = start.z }
    local bestH      = startH

    local expansions = 0

    while open:size() > 0 do
        expansions = expansions + 1
        if expansions > maxNodes then
            local partial = reconstructPath(came, bestNode)
            if doSmooth then
                return smoothPath(partial), "node limit (" .. maxNodes .. ")! partial path returned"
            else
                return partial, "node limit (" .. maxNodes .. ")! partial path returned"
            end
        end

        local cur = open:pop()
        local ck  = key(cur.x, cur.y, cur.z)
        if closed[ck] then goto continue end
        closed[ck] = true

        if debugCapture then
            Core._debugExpanded[#Core._debugExpanded + 1] = { x = cur.x, y = cur.y, z = cur.z }
        end

        if cur.h < bestH then
            bestH    = cur.h
            bestNode = cur
        end

        if abs(cur.x - goal.x) <= 1
            and abs(cur.y - goal.y) <= 1
            and abs(cur.z - goal.z) <= 1 then
            local raw = reconstructPath(came, cur)
            local last = raw[#raw]
            if not (last.x == goal.x and last.y == goal.y and last.z == goal.z) then
                raw[#raw + 1] = { x = goal.x, y = goal.y, z = goal.z }
            end
            if doSmooth then
                return smoothPath(raw), nil
            else
                return raw, nil
            end
        end

        -- gScore[ck] is constant for the duration of this node's expansion;
        -- hoist it out instead of re-indexing the hash table up to 10x below.
        local curG = gScore[ck] or huge

        for vi = 1, 2 do
            local vd = VERTICAL_DIRS[vi]
            local nx, ny, nz = cur.x, cur.y + vd[3], cur.z
            if isLadder(cur.x, cur.y, cur.z) or isLadder(nx, ny, nz) then
                local nk = key(nx, ny, nz)
                if not closed[nk] then
                    local mc = 0.5
                    local tg = curG + mc
                    if tg < (gScore[nk] or huge) then
                        gScore[nk] = tg
                        came[nk] = cur
                        local h = heuristic(nx, ny, nz, goal.x, goal.y, goal.z)
                        open:push({ x = nx, y = ny, z = nz, f = tg + h, h = h })
                    end
                end
            end
        end

        for di = 1, 8 do
            local d = DIRS[di]
            local dx, dz = d[1], d[2]
            local nx, nz = cur.x + dx, cur.z + dz

            local isDiagonal = (abs(dx) + abs(dz) == 2)

            if isDiagonal then
                if isSolid(cur.x + dx, cur.y, cur.z) or isSolid(cur.x, cur.y, cur.z + dz) then
                    goto next_dir
                end
                if isSolid(cur.x + dx, cur.y + 1, cur.z) or isSolid(cur.x, cur.y + 1, cur.z + dz) then
                    goto next_dir
                end
            end
            local ny = resolveY(cur.x, cur.y, cur.z, nx, nz)
            if ny then
                local nk = key(nx, ny, nz)
                if not closed[nk] then
                    local verticalDiff = ny - cur.y

                    local onLadder = isLadder(nx, ny, nz) or isLadder(cur.x, cur.y, cur.z)

                    local fallPenalty = 0
                    local verticalSurcharge = abs(verticalDiff) * 0.5

                    if onLadder then
                        verticalSurcharge = abs(verticalDiff) * 0.1
                    elseif verticalDiff < 0 then
                        fallPenalty = abs(verticalDiff) * 2.0
                    end

                    local mc = (isDiagonal and 1.4142 or 1.0)
                        + verticalSurcharge
                        + fallPenalty

                    local tg = curG + mc

                    if tg < (gScore[nk] or huge) then
                        gScore[nk] = tg
                        came[nk]   = cur
                        local h    = heuristic(nx, ny, nz, goal.x, goal.y, goal.z)
                        open:push({ x = nx, y = ny, z = nz, f = tg + h, h = h })
                        if debugCapture then
                            Core._debugOpen[#Core._debugOpen + 1] = { x = nx, y = ny, z = nz }
                        end
                    end
                end
            end
            ::next_dir::
        end

        ::continue::
    end

    if bestNode.x == start.x and bestNode.y == start.y and bestNode.z == start.z then
        return nil, "no path found"
    end
    local partial = reconstructPath(came, bestNode)
    if doSmooth then
        return smoothPath(partial), "no path found! partial path returned"
    else
        return partial, "no path found! partial path returned"
    end
end

function Core.snapPos(pos)
    return {
        x = floor(pos.x),
        y = floor(pos.y),
        z = floor(pos.z),
    }
end

function Core.search(start, goal)
    return astarSearch(Core.snapPos(start), Core.snapPos(goal))
end

function Core.findPath(goal, callback)
    if _lock then
        if callback then callback(nil, "search already running") end
        return
    end
    _lock = true
    threads.startThread(function()
        local s = Core.snapPos(player.getPos())
        local g = Core.snapPos(goal)
        local path, err = astarSearch(s, g)
        _lock = false
        if callback then
            callback(path, err)
            if #Core.queuedPaths >= 1 then
                Core.findPath(Core.queuedPaths[1].goal, Core.queuedPaths[1].callback)
                table.remove(Core.queuedPaths, 1)
            end
        end
    end)
end

function Core.queuePath(goal, callback)
    if not _lock then
        Core.findPath(goal, callback)
    else
        insert(Core.queuedPaths, { goal = goal, callback = callback })
    end
end

function Core.clearQueue()
    Core.queuedPaths = {}
end

function Core.isSearching()
    return _lock
end

return Core