mecha10_behavior_runtime/

composition.rs

//! Composition primitives for building behavior trees
//!
//! This module provides the fundamental composition nodes: Sequence, Selector, and Parallel.

use crate::{BehaviorNode, BoxedBehavior, NodeStatus};
use async_trait::async_trait;
use mecha10_core::Context;
use tracing::{debug, warn};

// ============================================================================
// Sequence Node
// ============================================================================

/// Executes child behaviors in sequence until one fails or all succeed.
///
/// **Behavior:**
/// - Ticks children in order
/// - Returns `Success` if all children succeed
/// - Returns `Failure` if any child fails
/// - Returns `Running` while executing children
///
/// **Use cases:**
/// - Multi-step tasks that must complete in order
/// - Pipeline processing
/// - Sequential actions (approach → grasp → lift)
///
/// # Example
///
/// ```rust,no_run
/// use mecha10_behavior_runtime::prelude::*;
///
/// // Assuming these behaviors are defined elsewhere
/// # use mecha10_behavior_runtime::BoxedBehavior;
/// # fn get_behaviors() -> Vec<BoxedBehavior> { vec![] }
/// let sequence = SequenceNode::new(get_behaviors());
/// ```
#[derive(Debug)]
pub struct SequenceNode {
    children: Vec<BoxedBehavior>,
    current_index: usize,
}

impl SequenceNode {
    /// Create a new sequence node with the given children.
    pub fn new(children: Vec<BoxedBehavior>) -> Self {
        Self {
            children,
            current_index: 0,
        }
    }

    /// Add a child behavior to the sequence.
    pub fn add_child(&mut self, child: BoxedBehavior) {
        self.children.push(child);
    }

    /// Get the number of children.
    pub fn len(&self) -> usize {
        self.children.len()
    }

    /// Check if the sequence is empty.
    pub fn is_empty(&self) -> bool {
        self.children.is_empty()
    }
}

#[async_trait]
impl BehaviorNode for SequenceNode {
    async fn tick(&mut self, ctx: &Context) -> anyhow::Result<NodeStatus> {
        // Empty sequence succeeds immediately
        if self.children.is_empty() {
            return Ok(NodeStatus::Success);
        }

        // Execute children in sequence
        let total_children = self.children.len();
        while self.current_index < total_children {
            debug!(
                "Sequence: ticking child {} of {}",
                self.current_index + 1,
                total_children
            );

            let child = &mut self.children[self.current_index];
            match child.tick(ctx).await? {
                NodeStatus::Success => {
                    // Move to next child
                    self.current_index += 1;
                }
                NodeStatus::Failure => {
                    // Sequence fails if any child fails
                    debug!("Sequence: child {} failed", self.current_index + 1);
                    return Ok(NodeStatus::Failure);
                }
                NodeStatus::Running => {
                    // Child still running, sequence continues next tick
                    return Ok(NodeStatus::Running);
                }
            }
        }

        // All children succeeded
        debug!("Sequence: all children succeeded");
        Ok(NodeStatus::Success)
    }

    async fn reset(&mut self) -> anyhow::Result<()> {
        self.current_index = 0;
        for child in &mut self.children {
            child.reset().await?;
        }
        Ok(())
    }

    async fn on_init(&mut self, ctx: &Context) -> anyhow::Result<()> {
        for child in &mut self.children {
            child.on_init(ctx).await?;
        }
        Ok(())
    }

    async fn on_terminate(&mut self, ctx: &Context) -> anyhow::Result<()> {
        for child in &mut self.children {
            child.on_terminate(ctx).await?;
        }
        Ok(())
    }

    fn name(&self) -> &str {
        "sequence"
    }
}

// ============================================================================
// Selector Node (Also called Fallback or Priority)
// ============================================================================

/// Executes child behaviors until one succeeds or all fail.
///
/// **Behavior:**
/// - Ticks children in order
/// - Returns `Success` if any child succeeds
/// - Returns `Failure` if all children fail
/// - Returns `Running` while executing children
///
/// **Use cases:**
/// - Fallback behaviors (try A, if it fails try B)
/// - Alternative strategies
/// - Error recovery chains
///
/// # Example
///
/// ```rust,no_run
/// use mecha10_behavior_runtime::prelude::*;
///
/// // Assuming these behaviors are defined elsewhere
/// # use mecha10_behavior_runtime::BoxedBehavior;
/// # fn get_behaviors() -> Vec<BoxedBehavior> { vec![] }
/// let selector = SelectOrNode::new(get_behaviors());
/// ```
#[derive(Debug)]
pub struct SelectOrNode {
    children: Vec<BoxedBehavior>,
    current_index: usize,
}

impl SelectOrNode {
    /// Create a new selector node with the given children.
    pub fn new(children: Vec<BoxedBehavior>) -> Self {
        Self {
            children,
            current_index: 0,
        }
    }

    /// Add a child behavior to the selector.
    pub fn add_child(&mut self, child: BoxedBehavior) {
        self.children.push(child);
    }

    /// Get the number of children.
    pub fn len(&self) -> usize {
        self.children.len()
    }

    /// Check if the selector is empty.
    pub fn is_empty(&self) -> bool {
        self.children.is_empty()
    }
}

#[async_trait]
impl BehaviorNode for SelectOrNode {
    async fn tick(&mut self, ctx: &Context) -> anyhow::Result<NodeStatus> {
        // Empty selector fails immediately
        if self.children.is_empty() {
            return Ok(NodeStatus::Failure);
        }

        // Try children in order until one succeeds
        let total_children = self.children.len();
        while self.current_index < total_children {
            debug!(
                "Selector: ticking child {} of {}",
                self.current_index + 1,
                total_children
            );

            let child = &mut self.children[self.current_index];
            match child.tick(ctx).await? {
                NodeStatus::Success => {
                    // Selector succeeds if any child succeeds
                    debug!("Selector: child {} succeeded", self.current_index + 1);
                    return Ok(NodeStatus::Success);
                }
                NodeStatus::Failure => {
                    // Try next child
                    warn!("Selector: child {} failed, trying next", self.current_index + 1);
                    self.current_index += 1;
                }
                NodeStatus::Running => {
                    // Child still running, selector continues next tick
                    return Ok(NodeStatus::Running);
                }
            }
        }

        // All children failed
        debug!("Selector: all children failed");
        Ok(NodeStatus::Failure)
    }

    async fn reset(&mut self) -> anyhow::Result<()> {
        self.current_index = 0;
        for child in &mut self.children {
            child.reset().await?;
        }
        Ok(())
    }

    async fn on_init(&mut self, ctx: &Context) -> anyhow::Result<()> {
        for child in &mut self.children {
            child.on_init(ctx).await?;
        }
        Ok(())
    }

    async fn on_terminate(&mut self, ctx: &Context) -> anyhow::Result<()> {
        for child in &mut self.children {
            child.on_terminate(ctx).await?;
        }
        Ok(())
    }

    fn name(&self) -> &str {
        "selector"
    }
}

// ============================================================================
// Parallel Node
// ============================================================================

/// Policy for determining when a parallel node succeeds or fails.
#[derive(Debug, Clone, Copy, PartialEq, Eq)]
pub enum ParallelPolicy {
    /// Succeed when all children succeed, fail if any child fails.
    RequireAll,

    /// Succeed when any child succeeds, fail when all children fail.
    RequireOne,

    /// Succeed when at least N children succeed.
    RequireN(usize),
}

/// Executes all child behaviors concurrently (in parallel).
///
/// **Behavior:**
/// - Ticks all children every tick
/// - Success/failure depends on the configured policy
/// - Returns `Running` while any child is still running
///
/// **Use cases:**
/// - Concurrent actions (move + look around + monitor obstacles)
/// - Parallel sensor processing
/// - Multi-objective optimization
///
/// # Example
///
/// ```rust,no_run
/// use mecha10_behavior_runtime::prelude::*;
///
/// // Assuming these behaviors are defined elsewhere
/// # use mecha10_behavior_runtime::BoxedBehavior;
/// # fn get_behaviors() -> Vec<BoxedBehavior> { vec![] }
/// // All must succeed
/// let parallel = ParallelNode::new(
///     get_behaviors(),
///     ParallelPolicy::RequireAll,
/// );
/// ```
#[derive(Debug)]
pub struct ParallelNode {
    children: Vec<BoxedBehavior>,
    policy: ParallelPolicy,
    child_statuses: Vec<NodeStatus>,
}

impl ParallelNode {
    /// Create a new parallel node with the given children and policy.
    pub fn new(children: Vec<BoxedBehavior>, policy: ParallelPolicy) -> Self {
        let child_statuses = vec![NodeStatus::Running; children.len()];
        Self {
            children,
            policy,
            child_statuses,
        }
    }

    /// Add a child behavior to the parallel node.
    pub fn add_child(&mut self, child: BoxedBehavior) {
        self.children.push(child);
        self.child_statuses.push(NodeStatus::Running);
    }

    /// Get the number of children.
    pub fn len(&self) -> usize {
        self.children.len()
    }

    /// Check if the parallel node is empty.
    pub fn is_empty(&self) -> bool {
        self.children.is_empty()
    }
}

#[async_trait]
impl BehaviorNode for ParallelNode {
    async fn tick(&mut self, ctx: &Context) -> anyhow::Result<NodeStatus> {
        // Empty parallel succeeds immediately
        if self.children.is_empty() {
            return Ok(NodeStatus::Success);
        }

        // Tick all children that are still running
        for (i, child) in self.children.iter_mut().enumerate() {
            if self.child_statuses[i].is_running() {
                let status = child.tick(ctx).await?;
                self.child_statuses[i] = status;
                debug!("Parallel: child {} status: {}", i + 1, status);
            }
        }

        // Check policy to determine overall status
        let success_count = self.child_statuses.iter().filter(|s| s.is_success()).count();
        let failure_count = self.child_statuses.iter().filter(|s| s.is_failure()).count();
        let _running_count = self.child_statuses.iter().filter(|s| s.is_running()).count();

        match self.policy {
            ParallelPolicy::RequireAll => {
                if failure_count > 0 {
                    debug!("Parallel (RequireAll): at least one child failed");
                    Ok(NodeStatus::Failure)
                } else if success_count == self.children.len() {
                    debug!("Parallel (RequireAll): all children succeeded");
                    Ok(NodeStatus::Success)
                } else {
                    Ok(NodeStatus::Running)
                }
            }
            ParallelPolicy::RequireOne => {
                if success_count > 0 {
                    debug!("Parallel (RequireOne): at least one child succeeded");
                    Ok(NodeStatus::Success)
                } else if failure_count == self.children.len() {
                    debug!("Parallel (RequireOne): all children failed");
                    Ok(NodeStatus::Failure)
                } else {
                    Ok(NodeStatus::Running)
                }
            }
            ParallelPolicy::RequireN(n) => {
                if success_count >= n {
                    debug!("Parallel (RequireN({})): {} children succeeded", n, success_count);
                    Ok(NodeStatus::Success)
                } else if (self.children.len() - failure_count) < n {
                    // Not enough children can possibly succeed
                    debug!("Parallel (RequireN({})): not enough children can succeed", n);
                    Ok(NodeStatus::Failure)
                } else {
                    Ok(NodeStatus::Running)
                }
            }
        }
    }

    async fn reset(&mut self) -> anyhow::Result<()> {
        for i in 0..self.children.len() {
            self.child_statuses[i] = NodeStatus::Running;
            self.children[i].reset().await?;
        }
        Ok(())
    }

    async fn on_init(&mut self, ctx: &Context) -> anyhow::Result<()> {
        for child in &mut self.children {
            child.on_init(ctx).await?;
        }
        Ok(())
    }

    async fn on_terminate(&mut self, ctx: &Context) -> anyhow::Result<()> {
        for child in &mut self.children {
            child.on_terminate(ctx).await?;
        }
        Ok(())
    }

    fn name(&self) -> &str {
        "parallel"
    }
}