local dbg
if moddebug then dbg=moddebug.dbg("people") else dbg={v1=function() end,v2=function() end,v3=function() end} end

-- "Area Node To Pos". Footpath area nodes are 1x1 areas ABOVE the path. People
-- positions (to match player positions) are 0.5 above the node they are
-- standing on. Add this to the Y coordinate to convert from a path node
-- position to a people position.
local ANTP = -0.5

-- "Path Node To Pos". But the nodes that form the actual path are below.
-- This is all very messy and needs rationalising!!
local PNTP = 0.5

-- Maximum distance from a destination at which we will attempt to use
-- pathfinding (e.g. after a collision)
local MAX_PATH_DIST = 48

people.actions.go = function(state)

    if state.action.rel then
        state.action.pos = vector.add(vector.round(state.pos), state.action.rel)
        dbg.v2(state.ent.name.." converted relative go to destination "..minetest.pos_to_string(state.action.pos))
        state.action.rel = nil
    end

    if (not state.action.pos) and (not state.action.footpathdest) and state.action.name then

        -- The action says to go to a named place. We're going to convert this
        -- into a position (if we have a way of doing so, e.g. via the footpath
        -- or areas mods), ready for the next call...

        if type(state.action.name) ~= "string" then
            dbg.v1(state.ent.name.." can't go to a named destination that isn't a string")
            return true, false
        end

        -- Look for a footpath node with the right name
        if footpath then
            local pathdestpos = footpath.get_nodepos(state.action.name)
            if pathdestpos then

                dbg.v2(state.ent.name.." selected path destination "..state.action.name)
                -- Find the nearest footpath junction to us. We'll head to that
                -- first (as a pos) and then follow the footpath network from
                -- there. (Note, the nearest junction could be the destination,
                -- and that's ok!)
                local pathstartnode = footpath.nearest_junction(state.pos)
                if pathstartnode then
                    dbg.v2(state.ent.name.." selected path start "..pathstartnode.name.." at "..minetest.pos_to_string(pathstartnode.pos))
                    state.action.pos = vector.new(pathstartnode.pos)
                    state.action.pos.y = state.action.pos.y + ANTP
                    state.action.footpathdest = state.action.name
                    return false
                end
            end
        end

        -- No path node, so look for any area with the actual name and
        -- just go there.
        if areas then
            local nearest = areas:findNearestArea(state.pos, "^"..people.escape_pattern(state.action.name).."$")
            if nearest then
                dbg.v2(state.ent.name.." set destination for centre of area "..nearest.name)
                state.action.pos = vector.interpolate(nearest.pos1, nearest.pos2, 0.5)
                state.action.pos.y = state.action.pos.y - 0.5
                return false
            end
        end

        dbg.v2(state.ent.name.." couldn't find location "..state.action.name)
        return true, false
    end

    if not state.action.pos and state.action.footpathdest then

        -- path handling - only comes here when we've arrived at a new path
        -- square (note, the range passed to nearest_junction determines our
        -- lookahead distance
        dbg.v3(state.ent.name.." at new footpath node at "..minetest.pos_to_string(state.pos))
        local curpathnode, distance = footpath.nearest_junction(state.pos, nil, 10)
        if curpathnode and curpathnode.pos.x == state.pos.x and curpathnode.pos.z == state.pos.z then

            if curpathnode.name == state.action.footpathdest then
                dbg.v2(state.ent.name.." arrived at footpath destination")
                if state.action.endpos then
                    state.action.footpathdest = nil
                    state.action.pos = state.action.endpos
                    return false
                end
                if state.action.faceexit then
                    local jct = footpath.get_junction(curpathnode.name)
                    if not jct then
                        dbg.v1(state.ent.name.." couldn't do faceexit at "..curpathnode.name.." - no such junction")
                        return true, false
                    end
                    local fdir = nil
                    for dir, exit in pairs(jct.exits) do
                        if exit == state.action.faceexit then
                            fdir = dir
                            break
                        end
                    end
                    if not fdir then
                        dbg.v1(state.ent.name.." couldn't do faceexit at "..curpathnode.name.." - no such exit")
                        return truen, false
                    end
                    local fpos
                    if fdir == "n" then
                        fpos = vector.add(state.pos, {x=0, y=0, z=1})
                    elseif fdir == "s" then
                        fpos = vector.add(state.pos, {x=0, y=0, z=-1})
                    elseif fdir == "e" then
                        fpos = vector.add(state.pos, {x=1, y=0, z=0})
                    else
                        fpos = vector.add(state.pos, {x=-1, y=0, z=0})
                    end
                    state.yaw = vector.get_yaw(state.pos, fpos)
                    dbg.v2(state.ent.name.." set faceexit yaw of "..state.yaw)
                end
                return true, true
            end

            if not state.action.name then
                dbg.v1(state.ent.name.." unexpectedly missing destination name - aborting go action - fix this!")
                return true, false
            end

            -- We're at a junction
            dbg.v2(state.ent.name.." is at footpath junction "..curpathnode.name.." - looking for route to "..state.action.name)
            state.action.lastpathpos = vector.new(state.pos.x, state.pos.y - PNTP, state.pos.z)
            local nextpathdest = footpath.get_route(curpathnode.name, state.action.name)
            if not nextpathdest then
                dbg.v1(state.ent.name.." can't find route")
                return true, false
            end
            local exitoff = nil
            for dir, exitto in pairs(curpathnode.neighbours) do
                if exitto.name == nextpathdest[2] then
                    if dir == "n" then
                        exitoff = {x=0, z=1}
                    elseif dir == "s" then
                        exitoff = {x=0, z=-1}
                    elseif dir == "e" then
                        exitoff = {x=1, z=0}
                    elseif dir == "w" then
                        exitoff = {x=-1, z=0}
                    else
                        dbg.v1(state.ent.name.." can't go in direction "..dir)
                        return true, false
                    end
                    break
                end
            end
            if not exitoff then
                dbg.v1(state.ent.name.." can't get exit offset from "..curpathnode.name.." to "..nextpathdest[2])
                return true, false
            end
            dbg.v2(state.ent.name.." leaving junction towards "..nextpathdest[2])
            state.action.pos = vector.add(curpathnode.pos, {x=exitoff.x, y=ANTP, z=exitoff.z})
            return false

        end

        if not state.action.lastpathpos then
            dbg.v1(state.ent.name.." should have lastpathpos")
            return true, false
        end

        -- extra 0.5 allowing for dodgy step-ups, for now...
        local thispathpos = vector.round({x=state.pos.x, y=state.pos.y + 0.5, z=state.pos.z})
        thispathpos.y = thispathpos.y - 1
        local nextpathpos, digit = footpath.findnext(thispathpos, state.action.lastpathpos, false)
        if nextpathpos == "unloaded" then
            state.wait = 1
            dbg.v3(state.ent.name.." waiting for block load")
            return false
        end
        if not nextpathpos then
            dbg.v1(state.ent.name.." can't find next footpath node at "..
                    minetest.pos_to_string(thispathpos).." after "..
                    minetest.pos_to_string(state.action.lastpathpos))
            return true, false
        end

        if not distance or distance > 10 then distance = 10 end
        while distance > 2 and not digit do
            -- Look ahead a bit while going in a straight line, to increase speed
            local aheadpathpos, digit = footpath.findnext(nextpathpos, thispathpos, true)
            if aheadpathpos == "unloaded" then
                state.wait = 1
                dbg.v3(state.ent.name.." waiting for block load for lookahead")
                return false
            end
            if aheadpathpos and not digit then
                thispathpos = nextpathpos
                nextpathpos = aheadpathpos
                distance = distance - 1
            else
                break
            end
        end
        state.action.lastpathpos = thispathpos
        state.action.pos = vector.add(nextpathpos, {x=0, y=0.5, z=0})
        if digit then
            dbg.v3(state.ent.name.." clearing snow from path at "..minetest.pos_to_string(state.action.pos))
            state.action={"go", pos=state.action.pos, prevaction={"dig", pos=vector.add(nextpathpos, {x=0, y=1, z=0}), prevaction=state.action}}
        end
        dbg.v3(state.ent.name.." set next footpath dest "..minetest.pos_to_string(state.action.pos))
        return false

    end

    if not state.action.pos or not state.action.pos.x or not state.action.pos.y or not state.action.pos.z then
        dbg.v1(state.ent.name.." has invalid go action "..dump(state.action))
        return true, false
    end

    -- Get our current destination, which might be either our
    -- actual final one, or an intermediate point
    local curdest
    if state.action.intermediate then
        curdest = state.action.intermediate[1]
    else
        curdest = state.action.pos
    end

    if not (type(curdest) == "table" and curdest.x and curdest.y and curdest.z) then
        dbg.v1(state.ent.name.." has invalid destination "..dump(curdest))
        curdest = pos
    end

    local distance = vector.distance({x=state.pos.x, y=0, z=state.pos.z}, {x=curdest.x, y=0, z=curdest.z})

    -- If it's a long way to the destination position, see if we can find
    -- a route using a footpath instead.
    if footpath and distance > 30 and not state.action.footpathdest then
        local pathstartnode = footpath.nearest_junction(state.pos, nil, distance)
        if pathstartnode then
            local pathdestnode = footpath.nearest_junction(state.action.pos, nil, distance)
            if pathdestarea then
                state.action.endpos = state.action.pos
                state.action.pos = vector.new(pathstartnode.pos)
                state.action.pos.y = state.action.pos.y + ANTP
                state.action.footpathdest = pathdestnode.name
                dbg.v2(state.ent.name.." using footpath from "..pathstartnode.name.." to "..pathdestnode.name.." for intermediate route")
                return false
            end
        end
    end

    if distance < 0.2 then
        if state.action.intermediate then
            dbg.v3(state.ent.name.." arrived intermediate ")
            if #state.action.intermediate == 1 then
                state.action.intermediate = nil
            else
                table.remove(state.action.intermediate, 1)
            end
        end

        dbg.v2(state.ent.name.." arrived near "..minetest.pos_to_string(curdest))
        state.setpos = vector.round(curdest)
        state.setpos.y = state.pos.y
        -- If we arrived at the pos, but we still have a
        -- pathdest or intermediate dest, we carry on to do that.
        if state.action.intermediate then
            state.action.pos = state.action.intermediate[1]
            return false
        elseif state.action.footpathdest then
            state.action.pos = nil
            return false
        end
        -- We've finished!
        dbg.v1(state.ent.name.." completed go action at "..minetest.pos_to_string(curdest))
        return true, true
    end

    -- Ok, we're actually walking towards a point!
    -- See if we've hit something last step, or if we're about to venture
    -- somewhere unwholesome...

    local unwholesome = false
    local colres = state.ent.object:get_last_collision_result()
    if colres.collides_xz then
        dbg.v3(state.ent.name.." collided at "..minetest.pos_to_string(state.pos).." : "..dump(colres))

        -- Maybe we hit a door and can open it?
        -- TODO - we ought to do something here to prevent repeated opening
        --        attempts if this goes wrong!
        if state.ent.can_use_doors then
            for _, col in pairs(colres.collisions) do
                local colpos = col.node_p
                local colnode = minetest.get_node(colpos)
                local door = minetest.registered_nodes[colnode.name].groups.door
                local gate = minetest.registered_nodes[colnode.name].groups.gate
                if (door and door ~= 0) or (gate and gate ~= 0) then
                    state.action = {"rightclicknode", pos=colpos, prevaction=state.action}
                    return false
                end
            end
        end

        unwholesome = true
        -- We've hit something...

        -- Apply some damage for bashing into something. Apart from being
        -- the right thing to do, it will also kill off things that get
        -- stuck in a wall. ;)
        state.damage = 0.4
    end

    if not unwholesome and distance > 1 then
        local erk = nil
        local ahead = state.speed
        if ahead > 1 then ahead = 1 end
        local nd = vector.from_speed_yaw(ahead, state.yaw)
        local nextpos = vector.add(vector.round(state.pos), nd)
        -- Don't look at the ahead position if it's the same node we're
        -- already at...
        if not vector.equals(vector.round(state.pos), nextpos) then
            nextpos.y = nextpos.y + 2
            -- How far down we'll look, which needs to be at least this to
            -- be able to descend stairs.
            local ncount = 6
            local diags = "at:" ..minetest.pos_to_string(state.pos)
            local notwalkable = 0
            local unloaded = false
            diags = diags .. " next+1:" ..minetest.pos_to_string(nextpos)
            while ncount > 0 do
                local nn = minetest.get_node(nextpos).name
                diags = diags .. ":" .. nn
                if nn == "ignore" then
                    unloaded = true
                    break
                end
                nd = minetest.registered_nodes[nn]
                if not nd then
                    erk = "mysterious " .. nn
                    break
                end
                local gg = nd.groups or {}
                if gg.water or gg.lava then
                    erk = nn
                    break
                end

                -- Use 'walkable' to decide if we will bump into, and/or can stand
                -- on the node. But with a bit of variation. We ignore doors so we
                -- bump into them on purpose!?
                local iswalkable = nd.walkable
                if iswalkable and state.ent.can_use_doors and (
                        (nd.groups.door and nd.groups.door ~= 0) or
                        (nd.groups.gate and nd.groups.gate ~= 0)) then
                    iswalkable = false
                end

                if iswalkable and ncount < 6 then
                    break
                end
                if not iswalkable then
                    notwalkable = notwalkable + 1
                end
                nextpos.y = nextpos.y - 1
                ncount = ncount - 1
            end
            if unloaded then
                -- If we hit an unloaded block, just wait until it loads.
                dbg.v3(state.ent.name.." waiting for unloaded to check ahead")
                state.wait = 1
                return false
            end
            if ncount == 0 then
                erk = "fall"
            elseif notwalkable < 2 then
                erk = "bump"
            end
            if erk then
                dbg.v1(state.ent.name.." aborted go due to fear of " ..
                       erk .. " - " .. diags)
                unwholesome = true
            end
        end
    end

    if unwholesome then
        if distance <= MAX_PATH_DIST then

            if state.action.triedpath then
                state.action.triedpath = false
                dbg.v2(state.ent.name.." is waiting before trying a path again ")
                state.action.intermediate = nil
                state.action = {"wait", time=30, prevaction=state.action}
                return false
            end

            local spos = vector.new(state.pos)
            if state.ent.yoffset then
                spos.y = spos.y - state.ent.yoffset
            end
            local path = minetest.find_path(spos, curdest,
                            distance + 16, 1, 3, "A*_noprefetch")
            if not path then

                dbg.v2(state.ent.name.." cannot find path from "..
                        minetest.pos_to_string(state.pos).." to "..
                        minetest.pos_to_string(curdest))
                if state.action.intermediate then
                    state.action.intermediate = nil
                end
                state.wait = 2
                return true, false

            else

                state.action.intermediate = {}

                -- Start following the path from the rounded position, in case
                -- we're off centre and will collide immediately...
                table.insert(state.action.intermediate, vector.round(state.pos))

                for i = 2, #path do
                    table.insert(state.action.intermediate, {x=path[i].x, y=path[i].y -0.5, z=path[i].z})
                end

                dbg.v3(state.ent.name.." found path to "..minetest.pos_to_string(curdest).." : "..dump(path))
                state.action.triedpath = true
                return false
            end
        end

        -- We hit something, and we're too far from our destination to try
        -- pathfinding. Pick a couple of nearby points and walk to them,
        -- then try again
        local randomdir = math.random(math.pi * 2)
        vec = vector.from_speed_yaw(8, randomdir)
        local rpos = {x=math.floor(state.pos.x + vec.x + 0.5), y=state.pos.y, z=math.floor(state.pos.z + vec.z + 0.5)}
        randomdir = math.random(math.pi * 2)
        vec = vector.from_speed_yaw(8, randomdir)
        local rpos2 = {x=math.floor(rpos.x + vec.x + 0.5), y=state.pos.y, z=math.floor(rpos.z + vec.z + 0.5)}
        if not state.action.intermediate or #state.action.intermediate > 10 then
            state.action.intermediate = {}
        end
        table.insert(state.action.intermediate, 1, rpos2)
        table.insert(state.action.intermediate, 1, rpos)

        dbg.v2(state.ent.name.." hit something, far from destination - talking a random detour")
        return false
    end

    if distance < 1.5 then
        state.speed = distance * 1.5
    else
        state.speed = 3
    end

    -- Apply max speed if there is one
    if state.ent.maxspeed and state.speed > state.ent.maxspeed then
        state.speed = state.ent.maxspeed
    end

    state.yaw = vector.get_yaw(state.pos, curdest)

    return false

end