mecha10_core/

behavior_interrupt.rs

//! Behavior Interrupt Trigger
//!
//! Reusable system for nodes to interrupt and resume behavior tree execution.
//!
//! # Overview
//!
//! This module provides a configurable interrupt trigger that any node can use
//! to pause autonomous behaviors when issuing direct commands (motor, navigation, etc.).
//!
//! # Features
//!
//! - **Interrupt Modes**: interrupt-only, interrupt-with-auto-resume, disabled
//! - **Auto-resume**: Automatically resume after timeout or command completion
//! - **Completion Detection**: Wait for specific topic before resuming
//! - **Configurable Timeouts**: Control how long to wait before auto-resume
//! - **Source Tracking**: Know which node triggered the interrupt
//!
//! # Example Usage
//!
//! ```rust
//! use mecha10_core::behavior_interrupt::{BehaviorInterruptTrigger, BehaviorInterruptConfig};
//! use mecha10_core::prelude::*;
//!
//! // Create trigger from config
//! let config = BehaviorInterruptConfig {
//!     enabled: true,
//!     mode: InterruptMode::InterruptWithAutoResume,
//!     timeout_secs: 30,
//!     await_completion: true,
//!     completion_topic: Some("/motor/status".to_string()),
//!     ..Default::default()
//! };
//!
//! let mut trigger = BehaviorInterruptTrigger::new("llm-command", config);
//!
//! // Interrupt behavior tree before sending motor command
//! trigger.interrupt(&ctx).await?;
//!
//! // ... send motor command ...
//!
//! // Manually resume (or let auto-resume handle it)
//! trigger.resume(&ctx).await?;
//! ```

use crate::context::Context;
use crate::messages::Message;
use crate::topics::Topic;
use anyhow::Result;
use serde::{Deserialize, Serialize};
use std::time::{Duration, SystemTime};
use tracing::{debug, info, warn};

/// Behavior interrupt mode
#[derive(Debug, Clone, Serialize, Deserialize, PartialEq)]
#[serde(rename_all = "snake_case")]
pub enum InterruptMode {
    /// Disable interrupt functionality (no-op)
    Disabled,

    /// Interrupt behavior tree but don't auto-resume
    /// Requires manual resume via dashboard or another command
    InterruptOnly,

    /// Interrupt behavior tree and auto-resume after timeout
    /// or when completion topic receives a message
    InterruptWithAutoResume,
}

impl Default for InterruptMode {
    fn default() -> Self {
        Self::InterruptOnly
    }
}

/// Configuration for behavior interrupt trigger
#[derive(Debug, Clone, Serialize, Deserialize)]
pub struct BehaviorInterruptConfig {
    /// Whether interrupt functionality is enabled
    #[serde(default = "default_enabled")]
    pub enabled: bool,

    /// Interrupt mode
    #[serde(default)]
    pub mode: InterruptMode,

    /// Timeout in seconds before auto-resume (for InterruptWithAutoResume mode)
    #[serde(default = "default_timeout_secs")]
    pub timeout_secs: u64,

    /// Whether to wait for completion topic before resuming
    #[serde(default)]
    pub await_completion: bool,

    /// Optional topic to monitor for command completion
    /// When this topic receives a message, behavior tree will resume
    #[serde(skip_serializing_if = "Option::is_none")]
    pub completion_topic: Option<String>,

    /// Control topic path (where to send interrupt/resume commands)
    #[serde(default = "default_control_topic")]
    pub control_topic: String,
}

impl Default for BehaviorInterruptConfig {
    fn default() -> Self {
        Self {
            enabled: default_enabled(),
            mode: InterruptMode::default(),
            timeout_secs: default_timeout_secs(),
            await_completion: false,
            completion_topic: None,
            control_topic: default_control_topic(),
        }
    }
}

fn default_enabled() -> bool {
    true
}

fn default_timeout_secs() -> u64 {
    30
}

fn default_control_topic() -> String {
    "/behavior/control".to_string()
}

/// Enhanced behavior control message
///
/// This message is sent to the behavior executor to control execution.
/// It supports both simple enable/disable and advanced interrupt/resume with metadata.
#[derive(Debug, Clone, Serialize, Deserialize)]
pub struct BehaviorControl {
    /// Action to perform: "enable", "disable", "interrupt", "resume"
    pub action: String,

    /// Optional source node that triggered this control command
    #[serde(skip_serializing_if = "Option::is_none")]
    pub source: Option<String>,

    /// Optional duration in seconds for auto-resume
    #[serde(skip_serializing_if = "Option::is_none")]
    pub duration_secs: Option<u64>,

    /// Timestamp of the control command
    #[serde(default)]
    pub timestamp: u64,
}

impl Message for BehaviorControl {}

impl BehaviorControl {
    /// Create a new interrupt command
    pub fn interrupt(source: &str) -> Self {
        Self {
            action: "interrupt".to_string(),
            source: Some(source.to_string()),
            duration_secs: None,
            timestamp: now_micros(),
        }
    }

    /// Create a new interrupt command with auto-resume duration
    pub fn interrupt_with_duration(source: &str, duration_secs: u64) -> Self {
        Self {
            action: "interrupt".to_string(),
            source: Some(source.to_string()),
            duration_secs: Some(duration_secs),
            timestamp: now_micros(),
        }
    }

    /// Create a new resume command
    pub fn resume(source: &str) -> Self {
        Self {
            action: "resume".to_string(),
            source: Some(source.to_string()),
            duration_secs: None,
            timestamp: now_micros(),
        }
    }

    /// Create an enable command (backward compatible)
    pub fn enable() -> Self {
        Self {
            action: "enable".to_string(),
            source: None,
            duration_secs: None,
            timestamp: now_micros(),
        }
    }

    /// Create a disable command (backward compatible)
    pub fn disable() -> Self {
        Self {
            action: "disable".to_string(),
            source: None,
            duration_secs: None,
            timestamp: now_micros(),
        }
    }
}

/// Reusable behavior interrupt trigger
///
/// This trigger can be embedded in any node that needs to interrupt behavior tree execution.
/// It handles the complexity of auto-resume, timeouts, and completion detection.
pub struct BehaviorInterruptTrigger {
    /// Source node identifier
    source: String,

    /// Configuration
    config: BehaviorInterruptConfig,

    /// Time when interrupt was triggered (for auto-resume)
    interrupt_time: Option<SystemTime>,

    /// Auto-resume task handle
    auto_resume_task: Option<tokio::task::JoinHandle<()>>,
}

impl BehaviorInterruptTrigger {
    /// Create a new behavior interrupt trigger
    pub fn new(source: &str, config: BehaviorInterruptConfig) -> Self {
        Self {
            source: source.to_string(),
            config,
            interrupt_time: None,
            auto_resume_task: None,
        }
    }

    /// Interrupt behavior tree execution
    ///
    /// Sends an interrupt command to the behavior executor.
    /// If auto-resume is enabled, schedules automatic resume after timeout.
    pub async fn interrupt(&mut self, ctx: &Context) -> Result<()> {
        if !self.config.enabled || self.config.mode == InterruptMode::Disabled {
            debug!("Behavior interrupt disabled, skipping");
            return Ok(());
        }

        // Cancel any existing auto-resume task
        if let Some(task) = self.auto_resume_task.take() {
            task.abort();
        }

        // Build control message
        let control = match self.config.mode {
            InterruptMode::InterruptOnly => BehaviorControl::interrupt(&self.source),
            InterruptMode::InterruptWithAutoResume => {
                BehaviorControl::interrupt_with_duration(&self.source, self.config.timeout_secs)
            }
            InterruptMode::Disabled => return Ok(()),
        };

        // Publish interrupt command
        let control_topic =
            Topic::<BehaviorControl>::new(Box::leak(self.config.control_topic.clone().into_boxed_str()));
        ctx.publish_to(control_topic, &control).await?;

        info!(
            "⏸️  Interrupted behavior tree (source: {}, mode: {:?})",
            self.source, self.config.mode
        );

        // Record interrupt time
        self.interrupt_time = Some(SystemTime::now());

        // Schedule auto-resume if enabled
        if self.config.mode == InterruptMode::InterruptWithAutoResume {
            self.schedule_auto_resume(ctx).await?;
        }

        Ok(())
    }

    /// Resume behavior tree execution
    ///
    /// Sends a resume command to the behavior executor.
    /// Cancels any pending auto-resume task.
    pub async fn resume(&mut self, ctx: &Context) -> Result<()> {
        if !self.config.enabled {
            return Ok(());
        }

        // Cancel auto-resume task if running
        if let Some(task) = self.auto_resume_task.take() {
            task.abort();
        }

        // Build control message
        let control = BehaviorControl::resume(&self.source);

        // Publish resume command
        let control_topic =
            Topic::<BehaviorControl>::new(Box::leak(self.config.control_topic.clone().into_boxed_str()));
        ctx.publish_to(control_topic, &control).await?;

        info!("▶️  Resumed behavior tree (source: {})", self.source);

        // Clear interrupt time
        self.interrupt_time = None;

        Ok(())
    }

    /// Schedule auto-resume task
    async fn schedule_auto_resume(&mut self, ctx: &Context) -> Result<()> {
        let timeout = Duration::from_secs(self.config.timeout_secs);
        let control_topic = self.config.control_topic.clone();
        let source = self.source.clone();
        let ctx_clone = ctx.clone();

        let task = tokio::spawn(async move {
            tokio::time::sleep(timeout).await;

            // Send resume command
            let control = BehaviorControl::resume(&source);
            let topic = Topic::<BehaviorControl>::new(Box::leak(control_topic.into_boxed_str()));

            match ctx_clone.publish_to(topic, &control).await {
                Ok(_) => {
                    info!(
                        "⏰ Auto-resumed behavior tree after {}s timeout (source: {})",
                        timeout.as_secs(),
                        source
                    );
                }
                Err(e) => {
                    warn!("Failed to auto-resume behavior tree: {}", e);
                }
            }
        });

        self.auto_resume_task = Some(task);

        Ok(())
    }

    /// Get interrupt duration (time since interrupt was triggered)
    pub fn interrupt_duration(&self) -> Option<Duration> {
        self.interrupt_time
            .and_then(|t| SystemTime::now().duration_since(t).ok())
    }

    /// Check if currently interrupted
    pub fn is_interrupted(&self) -> bool {
        self.interrupt_time.is_some()
    }
}

impl Drop for BehaviorInterruptTrigger {
    fn drop(&mut self) {
        // Cancel auto-resume task on drop
        if let Some(task) = self.auto_resume_task.take() {
            task.abort();
        }
    }
}

fn now_micros() -> u64 {
    SystemTime::now()
        .duration_since(SystemTime::UNIX_EPOCH)
        .unwrap()
        .as_micros() as u64
}

#[cfg(test)]
mod tests {
    use super::*;

    #[test]
    fn test_interrupt_mode_default() {
        assert_eq!(InterruptMode::default(), InterruptMode::InterruptOnly);
    }

    #[test]
    fn test_config_default() {
        let config = BehaviorInterruptConfig::default();
        assert!(config.enabled);
        assert_eq!(config.mode, InterruptMode::InterruptOnly);
        assert_eq!(config.timeout_secs, 30);
        assert!(!config.await_completion);
        assert!(config.completion_topic.is_none());
        assert_eq!(config.control_topic, "/behavior/control");
    }

    #[test]
    fn test_control_message_creation() {
        let interrupt = BehaviorControl::interrupt("test-node");
        assert_eq!(interrupt.action, "interrupt");
        assert_eq!(interrupt.source, Some("test-node".to_string()));
        assert!(interrupt.duration_secs.is_none());

        let interrupt_with_duration = BehaviorControl::interrupt_with_duration("test-node", 60);
        assert_eq!(interrupt_with_duration.action, "interrupt");
        assert_eq!(interrupt_with_duration.duration_secs, Some(60));

        let resume = BehaviorControl::resume("test-node");
        assert_eq!(resume.action, "resume");

        let enable = BehaviorControl::enable();
        assert_eq!(enable.action, "enable");
        assert!(enable.source.is_none());

        let disable = BehaviorControl::disable();
        assert_eq!(disable.action, "disable");
    }
}