BehaviorTree.cpp

Go to the documentation of this file.

#include "Core/BehaviorTree.h"
#include "Utils/Logger.h"
#include <algorithm>
#include <queue>
#include <sstream>
#include <stack>
 
namespace EmberCore {
 
BehaviorTree::BehaviorTree(const EmberCore::String &name)
    : name_(name), description_(""), execution_state_(ExecutionState::Idle) {
    Logger::GetInstance().Info("BehaviorTree", "Created behavior tree: " + name);
}
 
BehaviorTree::~BehaviorTree() { Logger::GetInstance().Info("BehaviorTree", "Destroying behavior tree: " + name_); }
 
void BehaviorTree::SetRootNode(std::unique_ptr<Node> root) {
    root_node_ = std::move(root);
    root_node_shared_.reset(); // Clear shared version
    UpdateNodeIndex();
    NotifyTreeChange("root_changed");
}
 
void BehaviorTree::SetRootNode(std::shared_ptr<Node> root) {
    root_node_shared_ = root;
    root_node_.reset(); // Clear unique version
    UpdateNodeIndex();
    NotifyTreeChange("root_changed");
}
 
size_t BehaviorTree::GetNodeCount() const {
    Node *root = GetRootNode();
    if (!root)
        return 0;
    return root->GetSubtreeNodeCount();
}
 
size_t BehaviorTree::GetMaxDepth() const {
    Node *root = GetRootNode();
    if (!root)
        return 0;
    return root->GetDepth();
}
 
std::vector<Node *> BehaviorTree::GetAllNodes() {
    std::vector<Node *> nodes;
    Node *root = GetRootNode();
    if (root) {
        CollectNodesRecursive(root, nodes);
    }
    return nodes;
}
 
std::vector<const Node *> BehaviorTree::GetAllNodes() const {
    std::vector<const Node *> nodes;
    const Node *root = GetRootNode();
    if (root) {
        CollectNodesRecursive(root, nodes);
    }
    return nodes;
}
 
Node *BehaviorTree::FindNodeById(size_t id) const {
    auto it = node_index_.find(id);
    return (it != node_index_.end()) ? it->second : nullptr;
}
 
Node *BehaviorTree::FindNodeByName(const EmberCore::String &name) const {
    return FindNodeRecursive(GetRootNode(), [&name](const Node *node) { return node->GetName() == name; });
}
 
std::vector<Node *> BehaviorTree::FindNodesByType(Node::Type type) const {
    std::vector<Node *> result;
    // Use const version to avoid const-correctness issues
    TraverseNodes([&result, type](const Node *node) {
        if (node->GetType() == type) {
            result.push_back(const_cast<Node *>(node));
        }
    });
    return result;
}
 
bool BehaviorTree::AddNode(std::unique_ptr<Node> node, Node *parent) {
    if (!node) {
        Logger::GetInstance().Error("BehaviorTree", "Cannot add null node to behavior tree");
        return false;
    }
 
    if (!parent) {
        if (root_node_) {
            Logger::GetInstance().Error("BehaviorTree", "Cannot add root node - tree already has a root");
            return false;
        }
        SetRootNode(std::move(node));
        return true;
    }
 
    // Find parent in our tree
    Node *found_parent = FindNodeById(parent->GetId());
    if (!found_parent) {
        Logger::GetInstance().Error("BehaviorTree", "Parent node not found in tree");
        return false;
    }
 
    Node *node_ptr = node.get();
    found_parent->AddChild(std::move(node));
    UpdateNodeIndex();
    NotifyNodeChange(node_ptr, "added");
    return true;
}
 
bool BehaviorTree::RemoveNode(size_t node_id) {
    Node *node = FindNodeById(node_id);
    if (!node) {
        Logger::GetInstance().Error("BehaviorTree", "Node not found for removal");
        return false;
    }
 
    if (node == root_node_.get()) {
        root_node_.reset();
        node_index_.clear();
        NotifyTreeChange("root_removed");
        return true;
    }
 
    // Find parent and remove child
    auto all_nodes = GetAllNodes();
    for (Node *potential_parent : all_nodes) {
        for (size_t i = 0; i < potential_parent->GetChildCount(); ++i) {
            if (potential_parent->GetChild(i)->GetId() == node_id) {
                potential_parent->RemoveChild(i);
                UpdateNodeIndex();
                NotifyNodeChange(node, "removed");
                return true;
            }
        }
    }
 
    Logger::GetInstance().Error("BehaviorTree", "Could not find parent of node to remove");
    return false;
}
 
bool BehaviorTree::MoveNode(size_t node_id, Node *new_parent) {
    // Move node to a new parent
    // This is a placeholder implementation for step 2
    return false;
}
 
BehaviorTree::ValidationResult BehaviorTree::Validate() const {
    ValidationResult result;
 
    // Check for root node (use both unique_ptr and shared_ptr)
    Node *root = GetRootNode();
    if (!root) {
        result.AddError("Tree has no root node");
        return result;
    }
 
    // Check for cycles
    if (HasCycles()) {
        result.AddError("Tree contains cycles");
    }
 
    // Validate individual nodes recursively
    std::vector<const Node *> visited;
    auto node_result = ValidateNode(root, visited);
 
    // Merge results from node validation
    for (const auto &error : node_result.errors) {
        result.errors.push_back(error);
        result.is_valid = false;
    }
    for (const auto &warning : node_result.warnings) {
        result.warnings.push_back(warning);
    }
 
    return result;
}
 
bool BehaviorTree::HasCycles() const {
    if (!root_node_)
        return false;
    return root_node_->HasCycles();
}
 
void BehaviorTree::Clear() {
    root_node_.reset();
    node_index_.clear();
    execution_state_ = ExecutionState::Idle;
    NotifyTreeChange("cleared");
}
 
std::unique_ptr<BehaviorTree> BehaviorTree::Clone() const {
    auto cloned_tree = std::make_unique<BehaviorTree>(name_ + " (Copy)");
    cloned_tree->description_ = description_;
    cloned_tree->metadata_ = metadata_;
 
    if (root_node_) {
        // Simple clone for now - just create a basic action node with same name
        auto cloned_root = NodeFactory::CreateActionNode(root_node_->GetName());
        cloned_tree->SetRootNode(std::move(cloned_root));
    }
 
    return cloned_tree;
}
 
void BehaviorTree::Reset() {
    TraverseNodes([](Node *node) {
        node->SetStatus(Node::Status::Idle);
        node->SetVisualState(Node::VisualState::Normal);
    });
    execution_state_ = ExecutionState::Idle;
    NotifyTreeChange("reset");
}
 
void BehaviorTree::TraverseNodes(std::function<void(Node *)> visitor) {
    if (root_node_) {
        root_node_->Accept(visitor);
    }
}
 
void BehaviorTree::TraverseNodes(std::function<void(const Node *)> visitor) const {
    if (root_node_) {
        // Use explicit cast to resolve ambiguity
        const Node *const_root = root_node_.get();
        const_root->Accept(visitor);
    }
}
 
void BehaviorTree::TraversePreOrder(std::function<void(Node *)> visitor) {
    if (!root_node_)
        return;
 
    std::stack<Node *> stack;
    stack.push(root_node_.get());
 
    while (!stack.empty()) {
        Node *current = stack.top();
        stack.pop();
 
        visitor(current);
 
        // Add children in reverse order for proper pre-order traversal
        for (int i = current->GetChildCount() - 1; i >= 0; --i) {
            stack.push(current->GetChild(i));
        }
    }
}
 
void BehaviorTree::TraversePostOrder(std::function<void(Node *)> visitor) {
    if (!root_node_)
        return;
 
    std::stack<Node *> stack1, stack2;
    stack1.push(root_node_.get());
 
    while (!stack1.empty()) {
        Node *current = stack1.top();
        stack1.pop();
        stack2.push(current);
 
        for (size_t i = 0; i < current->GetChildCount(); ++i) {
            stack1.push(current->GetChild(i));
        }
    }
 
    while (!stack2.empty()) {
        visitor(stack2.top());
        stack2.pop();
    }
}
 
void BehaviorTree::PrintTree() const {
    if (root_node_) {
        Logger::GetInstance().Info("BehaviorTree", "=== Behavior Tree: " + name_ + " ===");
        root_node_->PrintTree();
    } else {
        Logger::GetInstance().Info("BehaviorTree", "Behavior Tree '" + name_ + "' is empty");
    }
}
 
EmberCore::String BehaviorTree::GetTreeStructure() const {
    if (!root_node_)
        return "Empty tree";
 
    std::ostringstream oss;
    oss << "Tree: " << name_ << "\n";
    oss << "Nodes: " << GetNodeCount() << "\n";
    oss << "Depth: " << GetMaxDepth() << "\n";
    oss << "Structure:\n" << root_node_->ToString();
 
    return EmberCore::String(oss.str());
}
 
std::unordered_map<Node::Type, size_t> BehaviorTree::GetNodeTypeStatistics() const {
    std::unordered_map<Node::Type, size_t> stats;
 
    // Use const version to avoid const-correctness issues
    TraverseNodes([&stats](const Node *node) { stats[node->GetType()]++; });
 
    return stats;
}
 
void BehaviorTree::SetMetadata(const EmberCore::String &key, const EmberCore::String &value) {
    metadata_[key] = value;
    NotifyTreeChange("metadata_changed");
}
 
EmberCore::String BehaviorTree::GetMetadata(const EmberCore::String &key) const {
    auto it = metadata_.find(key);
    return (it != metadata_.end()) ? it->second : EmberCore::String("");
}
 
void BehaviorTree::RemoveMetadata(const EmberCore::String &key) {
    metadata_.erase(key);
    NotifyTreeChange("metadata_changed");
}
 
void BehaviorTree::NotifyNodeChange(Node *node, const EmberCore::String &change_type) {
    if (node_change_callback_) {
        node_change_callback_(node, change_type);
    }
}
 
void BehaviorTree::NotifyTreeChange(const EmberCore::String &change_type) {
    if (tree_change_callback_) {
        tree_change_callback_(change_type);
    }
}
 
void BehaviorTree::UpdateNodeIndex() {
    node_index_.clear();
    if (root_node_) {
        BuildNodeIndex(root_node_.get());
    }
}
 
Node *BehaviorTree::FindNodeRecursive(Node *current, std::function<bool(const Node *)> predicate) const {
    if (!current)
        return nullptr;
 
    if (predicate(current)) {
        return current;
    }
 
    for (size_t i = 0; i < current->GetChildCount(); ++i) {
        Node *result = FindNodeRecursive(current->GetChild(i), predicate);
        if (result)
            return result;
    }
 
    return nullptr;
}
 
void BehaviorTree::CollectNodesRecursive(Node *current, std::vector<Node *> &nodes) {
    if (!current)
        return;
 
    nodes.push_back(current);
    for (size_t i = 0; i < current->GetChildCount(); ++i) {
        CollectNodesRecursive(current->GetChild(i), nodes);
    }
}
 
void BehaviorTree::CollectNodesRecursive(const Node *current, std::vector<const Node *> &nodes) const {
    if (!current)
        return;
 
    nodes.push_back(current);
    for (size_t i = 0; i < current->GetChildCount(); ++i) {
        CollectNodesRecursive(current->GetChild(i), nodes);
    }
}
 
BehaviorTree::ValidationResult BehaviorTree::ValidateNode(const Node *node, std::vector<const Node *> &visited) const {
    ValidationResult result;
 
    if (!node) {
        result.AddError("Null node found in tree");
        return result;
    }
 
    // Check for circular references
    if (std::find(visited.begin(), visited.end(), node) != visited.end()) {
        result.AddError("Circular reference detected at node: " + node->GetName());
        return result;
    }
    visited.push_back(node);
 
    // Basic validity check
    if (!node->IsValid()) {
        result.AddError("Invalid node: " + node->GetName());
    }
 
    // Validate based on node type
    size_t child_count = node->GetChildCount();
 
    switch (node->GetType()) {
    case Node::Type::Decorator:
        // Decorators must have exactly 1 child
        if (child_count == 0) {
            result.AddError("Decorator node '" + node->GetName() + "' has no children (must have exactly 1)");
        } else if (child_count > 1) {
            result.AddWarning("Decorator node '" + node->GetName() + "' has " + std::to_string(child_count) +
                              " children (typically should have exactly 1)");
        }
        break;
 
    case Node::Type::Control:
        // Control nodes typically should have at least 1 child
        if (child_count == 0) {
            result.AddWarning("Control node '" + node->GetName() + "' has no children");
        }
        break;
 
    case Node::Type::Action:
    case Node::Type::Condition:
        // Leaf nodes typically should not have children
        if (child_count > 0) {
            result.AddWarning("Leaf node '" + node->GetName() + "' has " + std::to_string(child_count) +
                              " children (leaf nodes typically have no children)");
        }
        break;
 
    default:
        break;
    }
 
    // Check for required attributes (name should not be empty)
    if (node->GetName().empty()) {
        result.AddError("Node has empty name");
    }
 
    // Recursively validate children
    for (size_t i = 0; i < child_count; ++i) {
        const Node *child = node->GetChild(i);
        if (child) {
            auto child_result = ValidateNode(child, visited);
 
            // Merge child results
            for (const auto &error : child_result.errors) {
                result.errors.push_back(error);
                result.is_valid = false;
            }
            for (const auto &warning : child_result.warnings) {
                result.warnings.push_back(warning);
            }
        } else {
            result.AddError("Node '" + node->GetName() + "' has null child at index " + std::to_string(i));
        }
    }
 
    return result;
}
 
void BehaviorTree::BuildNodeIndex(Node *node) {
    if (!node)
        return;
 
    node_index_[node->GetId()] = node;
    for (size_t i = 0; i < node->GetChildCount(); ++i) {
        BuildNodeIndex(node->GetChild(i));
    }
}
 
// Utility functions
namespace BehaviorTreeUtils {
 
std::unique_ptr<BehaviorTree> CreateSampleTree() {
    auto tree = std::make_unique<BehaviorTree>("Sample Tree");
    tree->SetDescription("A sample behavior tree for demonstration");
 
    // Create a simple tree structure using existing factory methods
    auto root = NodeFactory::CreateControlNode("Root Sequence");
    auto condition = NodeFactory::CreateConditionNode("Check Condition");
    auto action1 = NodeFactory::CreateActionNode("Action 1");
    auto action2 = NodeFactory::CreateActionNode("Action 2");
 
    root->AddChild(std::move(condition));
    root->AddChild(std::move(action1));
    root->AddChild(std::move(action2));
 
    tree->SetRootNode(std::move(root));
    return tree;
}
 
std::unique_ptr<BehaviorTree> CreateEmptyTree(const EmberCore::String &name) {
    auto tree = std::make_unique<BehaviorTree>(name);
    tree->SetDescription("Empty behavior tree");
    return tree;
}
 
} // namespace BehaviorTreeUtils
 
// Blackboard management implementation
void BehaviorTree::AddBlackboard(std::unique_ptr<Blackboard> blackboard) {
    if (!blackboard) {
        LOG_ERROR("BehaviorTree", "Cannot add null blackboard to behavior tree");
        return;
    }
 
    EmberCore::String id = blackboard->GetId();
    if (id.empty()) {
        LOG_ERROR("BehaviorTree", "Cannot add blackboard with empty ID to behavior tree");
        return;
    }
 
    blackboards_[id] = std::move(blackboard);
    LOG_INFO("BehaviorTree", "Added blackboard '" + id + "' to behavior tree '" + name_ + "'");
}
 
Blackboard *BehaviorTree::GetBlackboard(const EmberCore::String &id) {
    auto it = blackboards_.find(id);
    return (it != blackboards_.end()) ? it->second.get() : nullptr;
}
 
const Blackboard *BehaviorTree::GetBlackboard(const EmberCore::String &id) const {
    auto it = blackboards_.find(id);
    return (it != blackboards_.end()) ? it->second.get() : nullptr;
}
 
bool BehaviorTree::HasBlackboard(const EmberCore::String &id) const {
    return blackboards_.find(id) != blackboards_.end();
}
 
void BehaviorTree::RemoveBlackboard(const EmberCore::String &id) {
    auto it = blackboards_.find(id);
    if (it != blackboards_.end()) {
        blackboards_.erase(it);
        LOG_INFO("BehaviorTree", "Removed blackboard '" + id + "' from behavior tree '" + name_ + "'");
    }
}
 
void BehaviorTree::ClearBlackboards() {
    size_t count = blackboards_.size();
    blackboards_.clear();
    LOG_INFO("BehaviorTree", "Cleared " + std::to_string(count) + " blackboards from behavior tree '" + name_ + "'");
}
 
} // namespace EmberCore