void OnlinePlanExpansionExecution::GetReachableReadyNodes(_Out_ IOlcbpPlan::NodeQueue& actionQ, _Out_ IOlcbpPlan::NodeQueue& goalQ)
{
    IOlcbpPlan::NodeQueue Q;
    IOlcbpPlan::NodeID currNodeId;
    IOlcbpPlan::NodeSerializedSet planRoots;
    IOlcbpPlan::NodeSet visitedNodes;

    _ASSERTE(m_planRootNodeId != IOlcbpPlan::NullNodeID);

    Q.push(m_planRootNodeId);
    visitedNodes.insert(m_planRootNodeId);
    goalQ.push(m_planRootNodeId);

    // Do a BFS on the plan an collect only ready nodes (i.e nodes with WaitOnParentsCount = 0)
    // Add ready action nodes to the action Q
    // Add ready goal nodes to the goal Q
    while(!Q.empty())
    {
        currNodeId = Q.front();
        Q.pop();

        if (!m_pOlcbpPlan->Contains(currNodeId))
        {
            LogWarning("A non existing node was there in the update queue, skipping it");
            continue;
        }

        for (auto childNodeId : m_pOlcbpPlan->GetAdjacentNodes(currNodeId))
        {
            if (visitedNodes.count(childNodeId) == 0)
            {
                if (IsNodeReady(childNodeId))
                {
                    if (IsGoalNode(childNodeId))
                    {
                        goalQ.push(childNodeId);
                    }
                    else if (IsActionNode(childNodeId))
                    {
                        actionQ.push(childNodeId);
                    }

                    Q.push(childNodeId);
                }

                visitedNodes.insert(childNodeId);
            }
        }
    }
}
int OnlinePlanExpansionExecution::AdaptSnippet(_In_ IOlcbpPlan::NodeID snippetRootGoalId)
{
    int nodeAddRemoveDelta = 0;
    IOlcbpPlan::NodeQueue Q;
    IOlcbpPlan::NodeSerializedSet planRoots;
    IOlcbpPlan::NodeSet visitedNodes;
    IOlcbpPlan::NodeID parentNodeId = snippetRootGoalId;

    Q.push(parentNodeId);
    visitedNodes.insert(parentNodeId);

    // Do a BFS on the plan an collect only ready nodes (i.e nodes with WaitOnParentsCount = 0)
    // Add ready action nodes to the action Q
    // Add ready goal nodes to the goal Q
    while(!Q.empty())
    {
        parentNodeId = Q.front();
        Q.pop();

        IOlcbpPlan::NodeSerializedSet originalSnippetNodes = m_pOlcbpPlan->GetAdjacentNodes(parentNodeId);
        for (auto childNodeId : originalSnippetNodes)
        {
            if (visitedNodes.count(childNodeId) != 0)
                continue;

            if (IsActionNode(childNodeId))
            {
                ((Action*)m_pOlcbpPlan->GetNode(childNodeId))->InitializeConditions();
                for (auto pExpression : ((Action*)m_pOlcbpPlan->GetNode(childNodeId))->PreCondition()->Expressions())
                {
                    _ASSERTE(pExpression->ExpressionType() == EXPRESSION_Leaf);
                    if (((ConditionEx*)pExpression)->ContainsParameter(PARAM_ResourceId) &&
                        ((ConditionEx*)pExpression)->Parameter(PARAM_ResourceId) == RESOURCE_Supply)
                    {
                        int requiredSupplyAmmount = ((ConditionEx*)pExpression)->Parameter(PARAM_Amount);

                        if (g_Game->Self()->Resources()->AvailableSupply() >= requiredSupplyAmmount)
                            continue;

                        UnlinkNodes(parentNodeId, childNodeId);

                        PlanStepParameters params;
                        params[PARAM_EntityClassId] = g_Game->Self()->Race()->GetResourceSource(RESOURCE_Supply);
                        params[PARAM_Amount] = 1;

                        auto pBuildInfraGoal = g_GoalFactory.GetGoal(GOALEX_BuildInfrastructure, params, true);
                        IOlcbpPlan::NodeID buildInfroNodeId = m_pOlcbpPlan->AddNode(pBuildInfraGoal, pBuildInfraGoal->Id());
                        m_nodeData[pBuildInfraGoal->Id()] = OlcbpPlanNodeData();
                        m_nodeData[pBuildInfraGoal->Id()].ID = pBuildInfraGoal->Id();

                        // Add Data record for the new node
                        SetNodeSatisfyingGoal(pBuildInfraGoal->Id(), snippetRootGoalId);

                        LinkNodes(parentNodeId, buildInfroNodeId);
                        LinkNodes(buildInfroNodeId, childNodeId);
                        ++nodeAddRemoveDelta;
                    }
                }
            }

            Q.push(childNodeId);
            visitedNodes.insert(childNodeId);
        }
    }

    return nodeAddRemoveDelta;
}
void OnlinePlanExpansionExecution::CoverFailedGoals()
{
    IOlcbpPlan::NodeQueue Q;
    IOlcbpPlan::NodeID currNodeId;
    IOlcbpPlan::NodeSerializedSet planRoots;
    IOlcbpPlan::NodeSet visitedNodes;
    IOlcbpPlan::NodeSet toCover;

    _ASSERTE(m_planRootNodeId != IOlcbpPlan::NullNodeID);

    Q.push(m_planRootNodeId);
    visitedNodes.insert(m_planRootNodeId);

    while (!Q.empty())
    {
        currNodeId = Q.front();
        Q.pop();

        if (!m_pOlcbpPlan->Contains(currNodeId))
        {
            LogWarning("A non existing node was there in the update queue, skipping it");
            continue;
        }

        for (auto childNodeId : m_pOlcbpPlan->GetAdjacentNodes(currNodeId))
        {
            if (visitedNodes.count(childNodeId) == 0)
            {
                if (IsGoalNode(childNodeId) && IsNodeDone(childNodeId))
                {
                    auto goalNode = m_pOlcbpPlan->GetNode(childNodeId);
                    if (m_coverGoals.count(childNodeId) == 0 && 
                        !goalNode->SuccessConditionsSatisfied(*g_Game) &&
                        m_backupNodes.count(childNodeId) == 0)
                    {
                        m_coverGoals.insert(childNodeId);
                        toCover.insert(childNodeId);
                    }
                }

                visitedNodes.insert(childNodeId);
                Q.push(childNodeId);
            }
        }
    }

    for (auto nodeId : toCover)
    {
        auto goalNode = m_pOlcbpPlan->GetNode(nodeId);
        GoalEx* backupNode = (GoalEx*)goalNode->Clone();
        m_backupNodes.insert(MakePair(backupNode->Id(), nodeId));
        backupNode->SetState(ESTATE_NotPrepared);
        m_pOlcbpPlan->AddNode(backupNode, backupNode->Id());
        m_nodeData[backupNode->Id()] = OlcbpPlanNodeData();
        m_nodeData[backupNode->Id()].ID = backupNode->Id();
        SetNodeSatisfyingGoal(backupNode->Id(), m_planRootNodeId);
        LinkNodes(m_planRootNodeId, backupNode->Id());
        auto& rootNodeData = GetNodeData(m_planRootNodeId);
        rootNodeData.SetWaitOnChildrenCount(rootNodeData.WaitOnChildrenCount + 1);
    }
}
void OnlinePlanExpansionExecution::Update(_In_ RtsGame& game)
{
    m_pOlcbpPlan->Lock();

    // We are in maintenance mode and its window kicked in
    // Then check if the root goal is not satisfied and try to re expand it
    if (m_pOlcbpPlan->Size() > 0 &&
        m_inMaintenanceMode &&
        game.Clock().ElapsedGameCycles() % PlanMaintenanceWindow == 0)
    {
        auto pPlanGoal = m_pOlcbpPlan->GetNode(m_planRootNodeId);

        // In case the plan goal is not satisfied and the plan
        // has no execution ongoing in any action node, then 
        // its time to replan for the same plan goal
        if (!pPlanGoal->SuccessConditionsSatisfied(*g_Game) &&
            IsPlanDone())
        {
            LogInfo("PLAN MAINTENANCE: Plan %s goal is not succeeded, replanning", pPlanGoal->ToString().c_str());
            // Replan and escape the maintenance mode
            StartNewPlan((GoalEx*)m_pPlanGoalPrototype->Clone());
        }
    }
    else if (m_pOlcbpPlan->Size() > 0 &&
        game.Clock().ElapsedGameCycles() % PlanExecuteWindow == 0)
    {
        IOlcbpPlan::NodeDQueue actionsToUpdate;
        IOlcbpPlan::NodeSet& goalsToUpdate = m_activeGoalSet;
        IOlcbpPlan::NodeSet snippetsToDestroy;
        bool expansionHappend = false;
        unordered_set<TID> visitedGoals;

        do
        {
            expansionHappend = false;
            m_activeGoalSet.clear();
            actionsToUpdate.clear();
            snippetsToDestroy.clear();

            m_pNodeSelector->Select(goalsToUpdate, actionsToUpdate, snippetsToDestroy);

            for (auto goalNodeId : goalsToUpdate)
            {
                if (visitedGoals.count(goalNodeId) > 0)
                    continue;

                // Only update a goal node if it still exist
                // It is normal that a previous updated goal node during this pass 
                // failed and its snippet was destroyed, and as a result a node that
                // was considered for update does not exist anymore
                if (m_pOlcbpPlan->Contains(goalNodeId))
                {
                    // It is illogical to update already succeeding goals, there is
                    // a problem in the node selection strategy
                    _ASSERTE(m_pOlcbpPlan->GetNode(goalNodeId)->GetState() != ESTATE_Succeeded);
                    expansionHappend |= UpdateGoalNode(goalNodeId, game.Clock());
                }

                visitedGoals.insert(goalNodeId);
            }
        } while (expansionHappend);

        while (!actionsToUpdate.empty())
        {
            auto actionNodeId = actionsToUpdate.front();
            // Only update an action node if it still exist
            // What applies to a goal in the 3rd pass apply here
            _ASSERTE(m_pOlcbpPlan->Contains(actionNodeId));
            // It is illogical to update already succeeding actions, there is
            // a problem in the node selection strategy
            _ASSERTE(m_pOlcbpPlan->GetNode(actionNodeId)->GetState() != ESTATE_Succeeded);
            UpdateActionNode(actionNodeId, game.Clock());
            actionsToUpdate.pop_front();
        }

        for (auto goalNodeId : snippetsToDestroy)
        {
            if (m_pOlcbpPlan->Contains(goalNodeId) && !HasActiveAction(goalNodeId))
            {
                (void)DestroyGoalSnippetIfExist(goalNodeId);

                if (m_pOlcbpPlan->GetNode(goalNodeId)->GetState() != ESTATE_Succeeded)
                {
                    OpenNode(goalNodeId);
                    LogInfo("Destroyed %s snippet, it is obsolete",
                        m_pOlcbpPlan->GetNode(goalNodeId)->ToString().c_str());
                }
                else
                {
                    LogInfo("Destroyed %s snippet, its goal succeeded",
                        m_pOlcbpPlan->GetNode(goalNodeId)->ToString().c_str());
                }
            }
        }
    }

    m_pOlcbpPlan->Unlock();

    if (m_planStructureChangedThisFrame)
    {
        g_MessagePump->Send(new Message(game.Clock().ElapsedGameCycles(), MSG_PlanStructureChange));
        m_planStructureChangedThisFrame = false;
    }
}
void LfhdCbNodeSelector::Select(_Out_ IOlcbpPlan::NodeSet& goalsToUpdate,
    _Out_ IOlcbpPlan::NodeDQueue& actionsToUpdate,
    _Out_ IOlcbpPlan::NodeSet& snippetsToDestroy)
{
    IOlcbpPlan* pPlan = m_pPlanner->Plan();
    IOlcbpPlan::NodeQueue actionQ;
    IOlcbpPlan::NodeQueue goalQ;
    typedef unsigned GoalKey;
    map<GoalKey, stack<IOlcbpPlan::NodeID>> goalTable;

    // 1st pass: get ready nodes only
    m_pPlanner->GetReachableReadyNodes(actionQ, goalQ);

    while (!goalQ.empty())
    {
        IOlcbpPlan::NodeID currentGoalId = goalQ.front();
        goalQ.pop();

        if (!pPlan->Contains(currentGoalId))
            continue;

        GoalEx* pCurrGoal = (GoalEx*)pPlan->GetNode(currentGoalId);
        GoalKey typeKey = pCurrGoal->Key();

        if (!m_pPlanner->IsNodeDone(currentGoalId))
        {
            IOlcbpPlan::NodeID newActiveGoalId = currentGoalId;

            if (goalTable.count(typeKey) == 0)
                goalTable[typeKey].push(newActiveGoalId);
            else
            {
                auto& currentOverrideStack = goalTable[typeKey];
                IOlcbpPlan::NodeID currentActiveGoalId = currentOverrideStack.top();

                IOlcbpPlan::NodeSet ancestors;
                stack<IOlcbpPlan::NodeID> newOverrideStack;

                m_pPlanner->GetAncestorSatisfyingGoals(newActiveGoalId, ancestors);
                newOverrideStack.push(newActiveGoalId);

                while (!currentOverrideStack.empty())
                {
                    currentActiveGoalId = currentOverrideStack.top();

                    // Nodes belonging to my ancestors set should not be destroyed
                    // and should be kept in order
                    if (ancestors.count(currentActiveGoalId) != 0)
                    {
                        newOverrideStack.push(currentActiveGoalId);
                    }
                    else if (pPlan->Contains(currentActiveGoalId) &&
                        !m_pPlanner->IsNodeOpen(currentActiveGoalId) &&
                        !m_pPlanner->HasActiveAction(currentActiveGoalId))
                    {
                        snippetsToDestroy.insert(currentActiveGoalId);
                    }

                    currentOverrideStack.pop();
                }

                // Push back new stack into the current one in a LIFO order to reverse the original one
                while (!newOverrideStack.empty())
                {
                    currentOverrideStack.push(newOverrideStack.top());
                    newOverrideStack.pop();
                }
            }
        }
    }

    for (auto& r : goalTable)
    {
        goalsToUpdate.insert(r.second.top());
    }

    while (!actionQ.empty())
    {
        auto actionNodeId = actionQ.front();

        bool satisfyingGoalActive = goalsToUpdate.count(m_pPlanner->GetNodeData(actionNodeId).SatisfyingGoal) > 0;
        bool satisfyingGoalDone = m_pPlanner->IsNodeDone(m_pPlanner->GetNodeData(actionNodeId).SatisfyingGoal);

        if (satisfyingGoalActive || satisfyingGoalDone)
        {
            actionsToUpdate.push_back(actionNodeId);
        }

        actionQ.pop();
    }
}