mecha10_behavior_patterns/

subsumption.rs

//! Subsumption architecture for priority-based control
//!
//! This module implements the subsumption architecture pattern where higher priority
//! behaviors can override lower priority ones. This is particularly useful for
//! safety-critical systems where safety behaviors must take precedence over task behaviors.

use async_trait::async_trait;
use mecha10_behavior_runtime::{BehaviorNode, BoxedBehavior, NodeStatus};
use mecha10_core::Context;
use serde::{Deserialize, Serialize};
use tracing::{debug, info};

/// A layer in the subsumption architecture with a priority and behavior.
#[derive(Debug)]
pub struct SubsumptionLayer {
    /// Priority level (higher = more important, executes first)
    pub priority: u8,
    /// The behavior to execute for this layer
    pub behavior: BoxedBehavior,
    /// Optional name for debugging
    pub name: Option<String>,
}

impl SubsumptionLayer {
    /// Create a new subsumption layer.
    pub fn new(priority: u8, behavior: BoxedBehavior) -> Self {
        Self {
            priority,
            behavior,
            name: None,
        }
    }

    /// Set the name of this layer.
    pub fn with_name(mut self, name: impl Into<String>) -> Self {
        self.name = Some(name.into());
        self
    }
}

/// Subsumption node for priority-based layered control.
///
/// This implements the subsumption architecture where behaviors are organized in layers
/// with different priorities. Higher priority layers can override lower priority ones.
///
/// # Behavior
///
/// - Layers are executed in order of priority (highest first)
/// - If a layer returns `Success` or `Running`, execution stops and that status is returned
/// - If a layer returns `Failure`, the next lower priority layer is tried
/// - If all layers fail, the node returns `Failure`
///
/// # Use Cases
///
/// - **Safety-critical systems**: Safety behaviors override task behaviors
/// - **Reactive control**: Obstacle avoidance overrides navigation
/// - **Hierarchical control**: High-level planning with low-level reactive layers
///
/// # Example
///
/// ```rust
/// use mecha10_behavior_patterns::prelude::*;
///
/// # #[derive(Debug)]
/// # struct EmergencyStopBehavior;
/// # #[async_trait]
/// # impl BehaviorNode for EmergencyStopBehavior {
/// #     async fn tick(&mut self, _ctx: &Context) -> anyhow::Result<NodeStatus> {
/// #         Ok(NodeStatus::Failure) // No emergency
/// #     }
/// # }
/// # #[derive(Debug)]
/// # struct AvoidObstacleBehavior;
/// # #[async_trait]
/// # impl BehaviorNode for AvoidObstacleBehavior {
/// #     async fn tick(&mut self, _ctx: &Context) -> anyhow::Result<NodeStatus> {
/// #         Ok(NodeStatus::Failure) // No obstacle
/// #     }
/// # }
/// # #[derive(Debug)]
/// # struct NavigateBehavior;
/// # #[async_trait]
/// # impl BehaviorNode for NavigateBehavior {
/// #     async fn tick(&mut self, _ctx: &Context) -> anyhow::Result<NodeStatus> {
/// #         Ok(NodeStatus::Running)
/// #     }
/// # }
///
/// let subsumption = SubsumptionNode::new()
///     .add_layer(10, Box::new(EmergencyStopBehavior))    // Highest priority
///     .add_layer(5, Box::new(AvoidObstacleBehavior))     // Medium priority
///     .add_layer(1, Box::new(NavigateBehavior));         // Lowest priority
/// ```
#[derive(Debug)]
pub struct SubsumptionNode {
    pub layers: Vec<SubsumptionLayer>,
}

impl SubsumptionNode {
    /// Create a new empty subsumption node.
    pub fn new() -> Self {
        Self { layers: Vec::new() }
    }

    /// Add a layer with the given priority and behavior.
    ///
    /// Layers are automatically sorted by priority (highest first).
    pub fn add_layer(mut self, priority: u8, behavior: BoxedBehavior) -> Self {
        self.layers.push(SubsumptionLayer::new(priority, behavior));
        // Sort by priority descending (highest first)
        self.layers.sort_by(|a, b| b.priority.cmp(&a.priority));
        self
    }

    /// Add a named layer.
    pub fn add_named_layer(mut self, priority: u8, name: impl Into<String>, behavior: BoxedBehavior) -> Self {
        self.layers
            .push(SubsumptionLayer::new(priority, behavior).with_name(name));
        self.layers.sort_by(|a, b| b.priority.cmp(&a.priority));
        self
    }

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

impl Default for SubsumptionNode {
    fn default() -> Self {
        Self::new()
    }
}

#[async_trait]
impl BehaviorNode for SubsumptionNode {
    async fn tick(&mut self, ctx: &Context) -> anyhow::Result<NodeStatus> {
        if self.layers.is_empty() {
            return Ok(NodeStatus::Failure);
        }

        // Try layers in priority order (highest first)
        for layer in &mut self.layers {
            let layer_name = layer.name.as_deref().unwrap_or("unnamed");

            debug!(
                "Subsumption: trying layer '{}' (priority {})",
                layer_name, layer.priority
            );

            let status = layer.behavior.tick(ctx).await?;

            match status {
                NodeStatus::Success | NodeStatus::Running => {
                    // This layer takes control
                    info!(
                        "Subsumption: layer '{}' (priority {}) taking control with status {}",
                        layer_name, layer.priority, status
                    );
                    return Ok(status);
                }
                NodeStatus::Failure => {
                    // Try next lower priority layer
                    debug!("Subsumption: layer '{}' failed, trying next layer", layer_name);
                    continue;
                }
            }
        }

        // All layers failed
        debug!("Subsumption: all layers failed");
        Ok(NodeStatus::Failure)
    }

    async fn reset(&mut self) -> anyhow::Result<()> {
        for layer in &mut self.layers {
            layer.behavior.reset().await?;
        }
        Ok(())
    }

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

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

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

/// Configuration for a subsumption layer (for JSON deserialization).
#[allow(dead_code)] // Will be used when implementing JSON loading
#[derive(Debug, Clone, Serialize, Deserialize)]
pub struct SubsumptionLayerConfig {
    /// Priority level (higher = more important)
    pub priority: u8,
    /// Node type to instantiate
    pub node: String,
    /// Optional name for this layer
    #[serde(skip_serializing_if = "Option::is_none")]
    pub name: Option<String>,
    /// Configuration for the node
    #[serde(default)]
    pub config: serde_json::Value,
}