local tilemap

local pathfinding = {}


pathfinding.jumpConsiderSpeeds = {0.5,1,2,3,4,5,6}
pathfinding.jumpConsiderDistanceHorizontal = 10
pathfinding.jumpConsiderDistanceUp = 5
pathfinding.jumpConsiderDistanceDown = 20
pathfinding.jumpConsiderGravity = 0.4
pathfinding.jumpConsiderWidth = 28

pathfinding.jumpConsiderMinSpeedY = -11
pathfinding.jumpConsiderMaxSpeedY = -5


pathfinding.nodeList = {}
pathfinding.nodeGrid = {}
pathfinding.nodeIDMap = {}

pathfinding.edgeIDMap = {}


pathfinding.debug = false


local sectionBounds

local nodeIDCounter = 0
local edgeIDCounter = 0


function pathfinding.getNodeFromGrid(gridX,gridY)
    if gridX < 1 or gridY < 1 or gridX > tilemap.gridWidth or gridY > tilemap.gridHeight then
        return nil
    end

    return pathfinding.nodeGrid[gridX][gridY]
end

function pathfinding.getNearbyNode(x,y)
    local gridX,gridY = tilemap.convertWorldPositionToGrid(x,y)
    local gridCellNode = pathfinding.getNodeFromGrid(gridX,gridY)

    if gridCellNode ~= nil then
        return gridCellNode
    end

    -- holy nested loops
    for searchSize = 1,10 do
        for directionY = -1,1,2 do
            for directionX = -1,1,2 do
                for offsetX = 0,searchSize do
                    local nearbyNode = pathfinding.getNodeFromGrid(gridX + directionX*offsetX,gridY + directionY*searchSize)

                    if nearbyNode ~= nil then
                        return nearbyNode
                    end
                end
            end
        end

        for directionX = -1,1,2 do
            for directionY = -1,1,2 do
                for offsetY = 0,(searchSize - 1) do
                    local nearbyNode = pathfinding.getNodeFromGrid(gridX + directionX*searchSize,gridY + directionY*offsetY)

                    if nearbyNode ~= nil then
                        return nearbyNode
                    end
                end
            end
        end
    end

    return nil
end


local function gridCellCanMakeNode(gridX,gridY)
    if ((gridY - 0.5)*tilemap.gridSize + tilemap.gridOriginY) < sectionBounds.top then
        return false
    end

    if not tilemap.cellIsFull(gridX,gridY + 1) or not tilemap.gridAreaIsEmpty(gridX,gridY - 1,gridX,gridY,false) then
        return false
    end

    return true
end

local function addNode(gridX,gridY)
    local belowBlock = tilemap.getCellBlock(gridX,gridY + 1)

    local x,y = tilemap.convertGridPositionToWorldCentred(gridX,gridY)

    nodeIDCounter = nodeIDCounter + 1

    local node = {
        id = nodeIDCounter,
        gridX = gridX,
        gridY = gridY,
        x = x,
        y = y + tilemap.gridSize*0.5,
        edges = {},
        onSemisolid = (belowBlock ~= nil and Block.SEMISOLID_MAP[belowBlock.id]),
    }

    node.y = tilemap.snapDown(node.x,node.y) or node.y

    pathfinding.nodeIDMap[nodeIDCounter] = node
    pathfinding.nodeGrid[gridX][gridY] = node
    table.insert(pathfinding.nodeList,node)

    return node
end

local function addEdge(originNode,destinationNode)
    edgeIDCounter = edgeIDCounter + 1

    local edge = {
        id = edgeIDCounter,
        originNode = originNode,
        destinationNode = destinationNode,
        weight = 1,
    }

    pathfinding.edgeIDMap[edgeIDCounter] = edge
    table.insert(originNode.edges,edge)

    return edge
end


local function checkJumpArc(startX,startY,targetX,targetY,speedX,speedY,gravity)
    local diffX = targetX - startX
    local diffY = targetY - startY

    local segmentCount = math.max(0,math.floor(math.sqrt(diffX*diffX + diffY*diffY)/32 + 0.5)) + 3
    local lastX = startX
    local lastY = startY

    local arc = {vector(startX,startY)}

    for segmentIndex = 1,segmentCount do
        local t = segmentIndex/segmentCount
        local checkTime = math.abs(t*diffX/speedX)

        local checkX = startX + diffX*t
        local checkY = startY + speedY*checkTime + gravity*checkTime*checkTime*0.5

        local hit,hitPointX,hitPointY,hitNormalX,hitNormalY = tilemap.lineCast(lastX,lastY,checkX,checkY)

        if hit then
            if math.abs(hitPointX - targetX) > 1 or math.abs(hitPointY - targetY) > 1 then
                return nil
            end

            --if hitNormalY >= 0 then
            --    return nil
            --end
        end

        if checkY > lastY and checkY < targetY then
            local minX = math.min(checkX,lastX) - pathfinding.jumpConsiderWidth*0.5
            local maxX = math.max(checkX,lastX) + pathfinding.jumpConsiderWidth*0.5

            local floorY = tilemap.boxCastDown(minX,maxX - minX, lastY,math.min(checkY,targetY - 1), true)

            if floorY ~= nil then
                return nil
            end
        end

        lastX = checkX
        lastY = checkY

        table.insert(arc,vector(checkX,checkY))
    end

    return arc
end

local function considerJumpConnection(startNode,targetNode)
    local gravity = pathfinding.jumpConsiderGravity

    for _,speedX in ipairs(pathfinding.jumpConsiderSpeeds) do
        local time = math.max(1,math.abs(startNode.x - targetNode.x)/speedX)
        local speedY = (targetNode.y - startNode.y)/time - gravity*time*0.5

        if speedY > pathfinding.jumpConsiderMinSpeedY and speedY < pathfinding.jumpConsiderMaxSpeedY then
            local arc = checkJumpArc(startNode.x,startNode.y,targetNode.x,targetNode.y,speedX,speedY,gravity)

            if arc ~= nil then
                local distX = startNode.x - targetNode.x
                local distY = startNode.y - targetNode.y

                local edge = addEdge(startNode,targetNode)
                
                edge.jumpSpeedX = speedX*math.sign(targetNode.x - startNode.x)
                edge.jumpSpeedY = speedY
                edge.jumpArc = arc

                edge.weight = math.sqrt(distX*distX + distY*distY)*4
            end
        end
    end
end


function pathfinding.refreshGraph()
    sectionBounds = Section(maskCollection.section).origBoundary

    nodeIDCounter = 0
    edgeIDCounter = 0

    pathfinding.nodeIDMap = {}
    pathfinding.edgeIDMap = {}

    for gridX = 1,tilemap.gridWidth do
        pathfinding.nodeGrid[gridX] = {}
    end

    -- Create nodes for any valid positions in the map
    pathfinding.nodeList = {}

    for gridX = 1,tilemap.gridWidth do
        for gridY = 1,(tilemap.gridHeight - 1) do
            if gridCellCanMakeNode(gridX,gridY) then
                addNode(gridX,gridY)
            end
        end
    end

    -- Link neighbouring nodes
    for _,node in ipairs(pathfinding.nodeList) do
        local belowBlock = tilemap.blockGrid[node.gridX][node.gridY + 1]
        local belowBlockConfig = Block.config[belowBlock.id]

        local jumpBlacklistedNodeMap = {[node] = true}

        for offsetX = -1,1,2 do
            -- Straight-on connection
            local neighbourNode = pathfinding.getNodeFromGrid(node.gridX + offsetX,node.gridY)

            if neighbourNode ~= nil then
                addEdge(node,neighbourNode)
                jumpBlacklistedNodeMap[neighbourNode] = true
            end

            if neighbourNode == nil then
                -- Downward slope
                if belowBlockConfig.floorslope == offsetX then
                    local belowNode = pathfinding.getNodeFromGrid(node.gridX + offsetX,node.gridY + 1)

                    if belowNode ~= nil then
                        addEdge(node,belowNode)
                        jumpBlacklistedNodeMap[belowNode] = true
                    end
                end

                -- Upward slope
                local upwardBlock = tilemap.getCellBlock(node.gridX + offsetX,node.gridY)

                if upwardBlock ~= nil and Block.config[upwardBlock.id].floorslope == -offsetX then
                    local upwardNode = pathfinding.getNodeFromGrid(node.gridX + offsetX,node.gridY - 1)

                    if upwardNode ~= nil then
                        addEdge(node,upwardNode)
                        jumpBlacklistedNodeMap[upwardNode] = true
                    end
                end
            end
        end

        -- Consider jumps
        for gridX = math.max(1,node.gridX - pathfinding.jumpConsiderDistanceHorizontal),math.min(tilemap.gridWidth,node.gridX + pathfinding.jumpConsiderDistanceHorizontal) do
            for gridY = math.max(1,node.gridY - pathfinding.jumpConsiderDistanceUp),math.min(tilemap.gridHeight,node.gridY + pathfinding.jumpConsiderDistanceDown) do
                local jumpNode = pathfinding.getNodeFromGrid(gridX,gridY)

                if jumpNode ~= nil and not jumpBlacklistedNodeMap[jumpNode] ~= nil then
                    considerJumpConnection(node,jumpNode)
                end
            end
        end
    end
end


local function getMinimumDistanceNode(nodeList,nodeDistances)
    local minimumDistance = math.huge
    local minimumDistanceNodeIndex = 0

    for nodeIndex,node in ipairs(nodeList) do
        if nodeDistances[node] ~= nil and nodeDistances[node] < minimumDistance then
            minimumDistance = nodeDistances[node]
            minimumDistanceNodeIndex = nodeIndex
        end
    end

    return minimumDistanceNodeIndex
end

function pathfinding.calculateDistances(startNode,targetNode)
    -- Basic implementation of Dijkstra's Algorithm
    local nodeDistances = {[startNode] = 0}
    local nodeBackhops = {}

    --local unexploredNodes = table.iclone(pathfinding.nodeList)
    local relevantNodes = {startNode}

    while (#relevantNodes > 0) do
        local nodeIndex = getMinimumDistanceNode(relevantNodes,nodeDistances)

        --[[if nodeIndex == 0 then
            break
        end]]

        local node = relevantNodes[nodeIndex]

        if node == targetNode then
            break
        end

        table.remove(relevantNodes,nodeIndex)

        -- Update distance for this node's edges
        for _,edge in ipairs(node.edges) do
            local edgeDistance = nodeDistances[node] + edge.weight

            if nodeDistances[edge.destinationNode] == nil or edgeDistance < nodeDistances[edge.destinationNode] then
                if nodeDistances[edge.destinationNode] == nil then
                    table.insert(relevantNodes,edge.destinationNode)
                end

                nodeDistances[edge.destinationNode] = edgeDistance
                nodeBackhops[edge.destinationNode] = edge
            end
        end
    end

    return nodeDistances,nodeBackhops
end

function pathfinding.findPathFromDistances(targetNode,nodeDistances,nodeBackhops)
    if nodeDistances[targetNode] == nil then
        return nil
    end

    local processingNode = targetNode
    local path = {}

    while (processingNode ~= nil and nodeBackhops[processingNode] ~= nil) do
        local edge = nodeBackhops[processingNode]

        table.insert(path,edge)
        processingNode = edge.originNode
    end

    return path
end

function pathfinding.findPath(startNode,targetNode)
    local nodeDistances,nodeBackhops = pathfinding.calculateDistances(startNode,targetNode)
    local path = pathfinding.findPathFromDistances(targetNode,nodeDistances,nodeBackhops)

    return path
end


function pathfinding.onDraw()
    if not pathfinding.debug then
        return
    end

    for _,node in ipairs(pathfinding.nodeList) do
        Graphics.drawCircle{
            color = Color.blue.. 0.5,
            sceneCoords = true,

            radius = 8,
            x = node.x,
            y = node.y,
        }

        for _,edge in ipairs(node.edges) do
            --[[Graphics.drawCircle{
                color = Color.red.. 0.5,
                sceneCoords = true,

                radius = 4,
                x = edge.destinationNode.x,
                y = edge.destinationNode.y,
            }]]

            if edge.jumpArc ~= nil then
                --[[for i = 1,(#edge.jumpArc - 1) do
                    local pointA = edge.jumpArc[i]
                    local pointB = edge.jumpArc[i + 1]

                    Graphics.drawLine{
                        color = Color.purple.. 0.5,
                        sceneCoords = true,

                        start = pointA,
                        stop = pointB,
                    }
                end]]
            else
                --[[Graphics.drawLine{
                    color = Color.purple.. 0.5,
                    sceneCoords = true,

                    x1 = edge.originNode.x,
                    y1 = edge.originNode.y,
                    x2 = edge.destinationNode.x,
                    y2 = edge.destinationNode.y,
                }]]
            end
        end
    end
end


function pathfinding.onInitAPI()
    registerEvent(pathfinding,"onDraw")

    tilemap = require("maskCollection/scripts/tilemap")
end


return pathfinding