local deque = require "lib.utils.deque"
local SQRT2 = math.sqrt(2)

local function isWalkable(point)
    -- 1) В пределах уровня
    if point.x < 0 or point.y < 0 then return false end
    if point.x >= Tree.level.size.x or point.y >= Tree.level.size.y then return false end

    -- 2) Клетка не занята персонажем
    if Tree.level.characterGrid:get(point) then return false end

    -- 3) Клетка проходима по тайлу
    local tile = Tree.level.tileGrid:get(point)
    if not tile or not tile.walkable then return false end

    return true
end

--- @param a Vec3
--- @param b Vec3
--- @return number
local function heuristic(a, b)
    local dx = math.abs(a.x - b.x)
    local dy = math.abs(a.y - b.y)
    return (dx + dy) + (SQRT2 - 2) * math.min(dx, dy)
end

--- @param from Vec3
--- @param to Vec3
--- @return boolean
local function can_step(from, to)
    if not isWalkable(to) then return false end

    local dx = to.x - from.x
    local dy = to.y - from.y
    if dx ~= 0 and dy ~= 0 then
        -- нельзя просачиваться через диагональ
        local viaX = Vec3 { from.x + dx, from.y }
        local viaY = Vec3 { from.x, from.y + dy }
        if not (isWalkable(viaX) or isWalkable(viaY)) then
            return false
        end
    end
    return true
end

--- @param cur Vec3
--- @return Vec3[]
local function neighbors(cur)
    local res = {}
    for dx = -1, 1 do
        for dy = -1, 1 do
            if not (dx == 0 and dy == 0) then
                local nxt = Vec3 { cur.x + dx, cur.y + dy }
                if can_step(cur, nxt) then
                    res[#res + 1] = nxt
                end
            end
        end
    end
    return res
end

--- Восстановление пути (включая начало и цель)
--- @param cameFrom table<string, Vec3|nil>
--- @param goal Vec3
--- @return Deque
local function reconstruct_path(cameFrom, goal)
    local sequence = {}
    local cur = goal
    while cur do
        sequence[#sequence + 1] = cur
        cur = cameFrom[tostring(cur)]
    end
    local acc = deque.new()
    for i = 1, #sequence, 1 do
        acc = acc:push_front(sequence[i])
    end
    return acc
end

--- @param openSet table множество вершин на границе
--- @param closed table уже обработанные
--- @param cameFrom table путь
--- @param gScore table
--- @param fScore table
--- @param goal Vec3
--- @return Deque
local function a_star_step(openSet, closed, cameFrom, gScore, fScore, goal)
    -- пусто: пути нет
    local anyKey = next(openSet)
    if not anyKey then
        return deque.new()
    end

    -- выбрать узел с минимальным fScore
    local currentKey, currentNode = anyKey, openSet[anyKey]
    local bestF = fScore[anyKey] or math.huge
    for k, node in pairs(openSet) do
        local f = fScore[k] or math.huge
        if f < bestF then
            bestF = f
            currentKey, currentNode = k, node
        end
    end

    -- достигли цели
    if currentNode == goal then
        return reconstruct_path(cameFrom, currentNode)
    end

    openSet[currentKey] = nil
    closed[currentKey] = true

    local gCurrent = gScore[currentKey] or math.huge
    for _, nb in ipairs(neighbors(currentNode)) do
        local nk = tostring(nb)
        if not closed[nk] then
            local dx = nb.x - currentNode.x
            local dy = nb.y - currentNode.y
            local step = (dx ~= 0 and dy ~= 0) and SQRT2 or 1 -- по диагонали ходить "дороже"
            local tentativeG = gCurrent + step
            if tentativeG < (gScore[nk] or math.huge) then
                cameFrom[nk] = currentNode
                gScore[nk] = tentativeG
                fScore[nk] = tentativeG + heuristic(nb, goal)
                if not openSet[nk] then
                    openSet[nk] = nb
                end
            end
        end
    end

    return a_star_step(openSet, closed, cameFrom, gScore, fScore, goal)
end

--- @param from Vec3
--- @param to Vec3
--- @return Deque
local function trace(from, to)
    -- не считаем путь до непроходимой точки (ибо нефиг)
    if not isWalkable(to) then return deque.new():push_back(from) end
    -- тривиальный случай
    if from == to then
        return deque.new():push_back(from)
    end

    local openSet  = {}
    local closed   = {}
    local cameFrom = {}
    local gScore   = {}
    local fScore   = {}

    local sk       = tostring(from)
    openSet[sk]    = from
    gScore[sk]     = 0
    fScore[sk]     = heuristic(from, to)

    return a_star_step(openSet, closed, cameFrom, gScore, fScore, to)
end

return trace