mecha10-behavior-patterns 0.6.2

//! 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,
        }
    }

    mecha10_core::builder_setter!(
        /// Set the name of this layer.
        into_some, with_name, name, impl Into<String>
    );
}

/// 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_key(|b| std::cmp::Reverse(b.priority));
        self
    }
}

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)
    }

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

    fn children_mut(&mut self) -> Vec<&mut dyn BehaviorNode> {
        self.layers
            .iter_mut()
            .map(|layer| layer.behavior.as_mut() as &mut dyn BehaviorNode)
            .collect()
    }
}

/// 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,
}