void THPathfinder::pathingInit(const THMap *pMap, int iStartX, int iStartY, int *piWidth, node_t *ppNode, int iGuessNumber)
{
    *piWidth = pMap->getWidth();
    m_pDestination = NULL;
    _allocNodeCache(*piWidth, pMap->getHeight());
    *ppNode = m_pNodes + iStartY * *piWidth + iStartX;
    pNode->prev = NULL;
    pNode->distance = 0;
    if (iGuessNumber == 1) MakeGuess1(*ppNode);
    if (iGuessNumber == 2) MakeGuess2(*ppNode);
    if (iGuessNumber == 3) MakeGuess3(*ppNode);
    if (iGuessNumber == 4) MakeGuess4(*ppNode);
    m_ppDirtyList[0] = *ppNode;
    m_iDirtyCount = 1;
    m_iOpenCount = 0;
}

/** No need to check for the node being on the map edge, as the N/E/S/W
   flags are set as to prevent travelling off the map (as well as to
   prevent walking through walls). */
THPathfinder::pathingNeighbours(uint32_t iFlags, int iWidth, int iTryNumber)
{
    uint32_t iPassable = iFlags & THMN_Passable;
    if(iFlags & THMN_CanTravelW)
    {
        if (iTryNumber == 1) TryNode1(pNode - 1, 3, iTryNumber);
        if (iTryNumber == 2) TryNode2(pNode - 1, 3, iTryNumber);
        if (iTryNumber == 3) TryNode3(pNode - 1, 3, iTryNumber);
        if (iTryNumber == 4) TryNode4(pNode - 1, 3, iTryNumber);
    }
    if(iFlags & THMN_CanTravelE)
    {
        if (iTryNumber == 1) TryNode1(pNode + 1, 1, iTryNumber);
        if (iTryNumber == 2) TryNode2(pNode + 1, 1, iTryNumber);
        if (iTryNumber == 3) TryNode3(pNode + 1, 1, iTryNumber);
        if (iTryNumber == 4) TryNode4(pNode + 1, 1, iTryNumber);
    }
    if(iFlags & THMN_CanTravelN)
    {
        if (iTryNumber == 1) TryNode1(pNode - iWidth, 0, iTryNumber);
        if (iTryNumber == 2) TryNode2(pNode - iWidth, 0, iTryNumber);
        if (iTryNumber == 3) TryNode3(pNode - iWidth, 0, iTryNumber);
        if (iTryNumber == 4) TryNode4(pNode - iWidth, 0, iTryNumber);
    }
    if(iFlags & THMN_CanTravelS)
    {
        if (iTryNumber == 1) TryNode1(pNode + iWidth, 2, iTryNumber);
        if (iTryNumber == 2) TryNode2(pNode + iWidth, 2, iTryNumber);
        if (iTryNumber == 3) TryNode3(pNode + iWidth, 2, iTryNumber);
        if (iTryNumber == 4) TryNode4(pNode + iWidth, 2, iTryNumber);
    }
}

void THPathfinder::pathingTryNode(uint32_t iNFlags, uint32_t iPassable, node_t *pNeighbour, node_t *pNode, int iGuessNumber)
{
    if(iNFlags & THMN_Passable || iPassable == 0)
    {
        if(pNeighbour->prev == pNeighbour)
        {
            pNeighbour->prev = pNode;
            pNeighbour->distance = pNode->distance + 1;
            if (iGuessNumber == 1) MakeGuess1(pNeighbour);
            if (iGuessNumber == 2) MakeGuess2(pNeighbour);
            if (iGuessNumber == 3) MakeGuess3(pNeighbour);
            if (iGuessNumber == 4) MakeGuess4(pNeighbour);
            m_ppDirtyList[m_iDirtyCount++] = pNeighbour;
            _openHeapPush(pNeighbour);
        }
        else if((pNode->distance + 1) < pNeighbour->distance)
        {
            pNeighbour->prev = pNode;
            pNeighbour->distance = pNode->distance + 1;
            /* Guess doesn't change, and already in the dirty list. */
            _openHeapPromote(pNeighbour);
        }
    }
}

bool THPathfinder::findPath(const THMap *pMap, int iStartX, int iStartY, int iEndX, int iEndY)
{
    if(pMap == NULL)
        pMap = m_pDefaultMap;
    if(pMap == NULL || pMap->getNode(iEndX, iEndY) == NULL
        || (pMap->getNodeUnchecked(iEndX, iEndY)->iFlags & THMN_Passable) == 0)
    {
        m_pDestination = NULL;
        return false;
    }

    // As diagonal movement is not allowed, the minimum distance between two
    // points is the sum of the distance in X and the distance in Y. Provided
    // that the compiler generates clever assembly for abs(), this means that
    // guesses can be calculated without branching and without sqrt().
#define MakeGuess1(pNode) \
    pNode->guess = abs(pNode->x - iEndX) + abs(pNode->y - iEndY)

    pathingInit(pMap, iStartX, iStartY, &iWidth, &pNode, 1);
    node_t *pTarget = m_pNodes + iEndY * iWidth + iEndX;

    while(true)
    {
        if(pNode == pTarget)
        {
            m_pDestination = pTarget;
            return true;
        }

        uint32_t iFlags = pMap->getNodeUnchecked(pNode->x, pNode->y)->iFlags;

#define TryNode1(n, d, iGuessNumber) \
        node_t *pNeighbour = n; \
        uint32_t iNFlags = pMap->getNodeUnchecked(pNeighbour->x, pNeighbour->y)->iFlags; \
        pathingTryNode(iNFlags, iPassable, pNeighbour, pNode, iGuessNumber)

        pathingNeighbours(iFlags, iWidth, 1);
        if(m_iOpenCount == 0)
        {
            m_pDestination = NULL;
            break;
        }
        else
            pNode = _openHeapPop();
    }
    return false;

#undef MakeGuess1
#undef TryNode
}

bool THPathfinder::findPathToHospital(const THMap *pMap, int iStartX, int iStartY)
{
    if(pMap == NULL)
        pMap = m_pDefaultMap;
    if(pMap == NULL || pMap->getNode(iStartX, iStartY) == NULL
        || (pMap->getNodeUnchecked(iStartX, iStartY)->iFlags & THMN_Passable) == 0)
    {
        m_pDestination = NULL;
        return false;
    }

#define MakeGuess2(pNode) pNode->guess = 0

    pathingInit(pMap, iStartX, iStartY, &iWidth, &pNode, 2);

    while(true)
    {
        uint32_t iFlags = pMap->getNodeUnchecked(pNode->x, pNode->y)->iFlags;

        if(iFlags & THMN_Hospital)
        {
            m_pDestination = pNode;
            return true;
        }

#define TryNode2(n, d, iGuessNumber) \
        node_t *pNeighbour = n; \
        uint32_t iNFlags = pMap->getNodeUnchecked(pNeighbour->x, pNeighbour->y)->iFlags; \
        pathingTryNode(iNFlags, iPassable, pNeighbour, pNode, iGuessNumber)

        pathingNeighbours(iFlags, iWidth, 2);
        if(m_iOpenCount == 0)
        {
            m_pDestination = NULL;
            break;
        }
        else
            pNode = _openHeapPop();
    }
    return false;

#undef MakeGuess
#undef TryNode
}

bool THPathfinder::findIdleTile(const THMap *pMap, int iStartX, int iStartY, int iN)
{
    if(pMap == NULL)
        pMap = m_pDefaultMap;
    if(pMap == NULL)
    {
        m_pDestination = NULL;
        return false;
    }

#define MakeGuess3(pNode) \
    pNode->guess = 0

    pathingInit(pMap, iStartX, iStartY, &iWidth, &pNode, 3);
    node_t *pPossibleResult = NULL;

    while(true)
    {
        pNode->open_idx = -1;
        uint32_t iFlags = pMap->getNodeUnchecked(pNode->x, pNode->y)->iFlags;

        if((iFlags & THMN_DoNotIdle) == 0 && (iFlags & THMN_Passable) && (iFlags & THMN_Hospital))
        {
            if(iN == 0)
            {
                m_pDestination = pNode;
                return true;
            }
            else
            {
                pPossibleResult = pNode;
                --iN;
            }
        }

        node_t* pBestNext = NULL;
        double fBestDistance = 0.0;

#define TryNode3(n, d, iGuessNumber) \
        node_t *pNeighbour = n; \
        uint32_t iNFlags = pMap->getNodeUnchecked(pNeighbour->x, pNeighbour->y)->iFlags; \
        /* When finding an idle tile, do not navigate through doors */ \
        switch(d) \
        { \
        case 0: \
            if((iFlags & THMN_DoorNorth) == 0) { pathingTryNode(iNFlags, iPassable, pNeighbour, pNode, iGuessNumber); } \
            break; \
        case 1: \
            if((iNFlags & THMN_DoorWest) == 0) { pathingTryNode(iNFlags, iPassable, pNeighbour, pNode, iGuessNumber); } \
            break; \
        case 2: \
            if((iNFlags & THMN_DoorNorth) == 0) { pathingTryNode(iNFlags, iPassable, pNeighbour, pNode, iGuessNumber); } \
            break; \
        case 3: \
            if((iFlags & THMN_DoorWest) == 0)  { pathingTryNode(iNFlags, iPassable, pNeighbour, pNode, iGuessNumber); } \
            break; \
        } \
        /* Identify the neighbour in the open list nearest to the start */ \
        if(pNeighbour->prev != pNeighbour && pNeighbour->open_idx != -1) \
        { \
            int iDX = pNeighbour->x - iStartX; \
            int iDY = pNeighbour->y - iStartY; \
            double fDistance = sqrt((double)(iDX * iDX + iDY * iDY)); \
            if(pBestNext == NULL || fDistance < fBestDistance) \
                pBestNext = pNeighbour, fBestDistance = fDistance; \
        }

        pathingNeighbours(iFlags, iWidth, 3);

        if(m_iOpenCount == 0)
        {
            m_pDestination = NULL;
            break;
        }
        if(pBestNext)
        {
            // Promote the best neighbour to the front of the open list
            // This causes sequential iN to give neighbouring results for most iN
            pBestNext->guess = -pBestNext->distance;
            _openHeapPromote(pBestNext);
        }
        pNode = _openHeapPop();
    }
    if(pPossibleResult)
    {
        m_pDestination = pPossibleResult;
        return true;
    }
    return false;

#undef MakeGuess
#undef TryNode
}

bool THPathfinder::visitObjects(const THMap *pMap, int iStartX, int iStartY,
                                THObjectType eTHOB, int iMaxDistance,
                                lua_State *L, int iVisitFunction, bool anyObjectType)
{
    if(pMap == NULL)
        pMap = m_pDefaultMap;
    if(pMap == NULL)
    {
        m_pDestination = NULL;
        return false;
    }

#define MakeGuess4(pNode) \
    pNode->guess = 0

    pathingInit(pMap, iStartX, iStarty, &iWidth, &pNode, 4);
    uint32_t iTHOB = static_cast<uint32_t>(eTHOB) << 24;

    while(true)
    {
        uint32_t iFlags = pMap->getNodeUnchecked(pNode->x, pNode->y)->iFlags;

#define TryNode4(n, d, iGuessNumber) \
        node_t *pNeighbour = n; \
        int iObjectNumber = 0; \
        const THMapNode *pMapNode = pMap->getNodeUnchecked(pNeighbour->x, pNeighbour->y);\
        uint32_t iNFlags = pMap->getNodeUnchecked(pNeighbour->x, pNeighbour->y)->iFlags; \
        if ((iNFlags & 0xFF000000) == iTHOB) \
            iObjectNumber = 1; \
        if(pMapNode->pExtendedObjectList != NULL)\
        {\
            int count = *pMapNode->pExtendedObjectList & 7;\
            for(int i = 0; i < count; i++) \
            { \
                int thob = (*pMapNode->pExtendedObjectList & (255 << (3  + (i << 3)))) >> (3 + (i << 3)); \
                if(thob == eTHOB)\
                    iObjectNumber++; \
            } \
        } \
        if(anyObjectType) \
            iObjectNumber = 1; \
        bool bSucces = false; \
        for(int i = 0; i < iObjectNumber; i++) \
        { \
            /* call the given Lua function, passing four arguments: */ \
            /* The x and y position of the object (Lua tile co-ords) */ \
            /* The direction which was last travelled in to reach (x,y); */ \
            /*   0 (north), 1 (east), 2 (south), 3 (west) */ \
            /* The distance to the object from the search starting point */ \
            lua_pushvalue(L, iVisitFunction); \
            lua_pushinteger(L, pNeighbour->x + 1); \
            lua_pushinteger(L, pNeighbour->y + 1); \
            lua_pushinteger(L, d); \
            lua_pushinteger(L, pNode->distance); \
            lua_call(L, 4, 1); \
            if(lua_toboolean(L, -1) != 0) \
            { \
                bSucces = true; \
            } \
            lua_pop(L, 1); \
        } \
        if(bSucces) \
            return true; \
        if(pNode->distance < iMaxDistance) \
        { \
            switch(d) \
            { \
            case 0: \
                if((iFlags & THMN_DoorNorth) == 0) { pathingTryNode(iNFlags, iPassable, pNeighbour, pNode, iGuessNumber); } \
                break; \
            case 1: \
                if((iNFlags & THMN_DoorWest) == 0) { pathingTryNode(iNFlags, iPassable, pNeighbour, pNode, iGuessNumber); } \
                break; \
            case 2: \
                if((iNFlags & THMN_DoorNorth) == 0) { pathingTryNode(iNFlags, iPassable, pNeighbour, pNode, iGuessNumber); } \
                break; \
            case 3: \
                if((iFlags & THMN_DoorWest) == 0)  { pathingTryNode(iNFlags, iPassable, pNeighbour, pNode, iGuessNumber); } \
                break; \
            } \
        }

        pathingNeighbours(iFlags, iWidth, 4);
        if(m_iOpenCount == 0)
        {
            m_pDestination = NULL;
            break;
        }
        else
            pNode = _openHeapPop();
    }
    return false;

#undef MakeGuess
#undef TryNode
}