mecha10-behavior-runtime 0.1.23

//! Built-in action nodes for behavior trees
//!
//! This module provides common action nodes that can be used in behavior tree compositions.
//! All nodes implement the `BehaviorNode` trait and can be registered in the NodeRegistry.

use crate::{BehaviorNode, BoxedBehavior, Context, NodeRegistry, NodeStatus};
use async_trait::async_trait;
use mecha10_core::actuator::Twist;
use mecha10_core::topics::Topic;
use serde::{Deserialize, Serialize};
use serde_json::Value;
use std::time::{Duration, Instant};

/// Register all built-in action nodes with a registry
///
/// This function registers the following nodes:
/// - `wander`: WanderNode
/// - `move`: MoveNode
/// - `sensor_check`: SensorCheckNode
/// - `timer`: TimerNode
/// - `idle`: IdleNode
/// - `pause`: PauseNode
/// - `stop`: StopNode
/// - `plan_path`: PlanPathNode
/// - `follow_path`: FollowPathNode
/// - `check_arrival`: CheckArrivalNode
///
/// # Example
///
/// ```rust
/// use mecha10_behavior_runtime::{NodeRegistry, actions};
///
/// let mut registry = NodeRegistry::new();
/// actions::register_builtin_actions(&mut registry);
/// ```
pub fn register_builtin_actions(registry: &mut NodeRegistry) {
    registry.register("wander", |config: Value| -> anyhow::Result<BoxedBehavior> {
        Ok(Box::new(WanderNode::from_json(config)?))
    });

    registry.register("move", |config: Value| -> anyhow::Result<BoxedBehavior> {
        Ok(Box::new(MoveNode::from_json(config)?))
    });

    registry.register("sensor_check", |config: Value| -> anyhow::Result<BoxedBehavior> {
        Ok(Box::new(SensorCheckNode::from_json(config)?))
    });

    registry.register("timer", |config: Value| -> anyhow::Result<BoxedBehavior> {
        Ok(Box::new(TimerNode::from_json(config)?))
    });

    registry.register("idle", |config: Value| -> anyhow::Result<BoxedBehavior> {
        Ok(Box::new(IdleNode::from_json(config)?))
    });

    registry.register("pause", |config: Value| -> anyhow::Result<BoxedBehavior> {
        Ok(Box::new(PauseNode::from_json(config)?))
    });

    registry.register("stop", |config: Value| -> anyhow::Result<BoxedBehavior> {
        Ok(Box::new(StopNode::from_json(config)?))
    });

    registry.register("plan_path", |config: Value| -> anyhow::Result<BoxedBehavior> {
        Ok(Box::new(PlanPathNode::from_json(config)?))
    });

    registry.register("follow_path", |config: Value| -> anyhow::Result<BoxedBehavior> {
        Ok(Box::new(FollowPathNode::from_json(config)?))
    });

    registry.register("check_arrival", |config: Value| -> anyhow::Result<BoxedBehavior> {
        Ok(Box::new(CheckArrivalNode::from_json(config)?))
    });
}

/// WanderNode - Generates random movement commands
///
/// This node publishes random velocity commands to make a robot wander around.
/// It periodically changes direction to create natural wandering behavior.
///
/// # Configuration
///
/// ```json
/// {
///   "type": "wander",
///   "config": {
///     "topic": "/motor/cmd_vel",
///     "linear_speed": 0.3,
///     "angular_speed": 0.5,
///     "change_interval_secs": 3.0
///   }
/// }
/// ```
#[derive(Debug)]
pub struct WanderNode {
    topic: String,
    linear_speed: f64,
    angular_speed: f64,
    change_interval: Duration,
    last_change: Option<Instant>,
    current_angular: f64,
}

#[derive(Debug, Serialize, Deserialize)]
struct WanderConfig {
    topic: String,
    #[serde(default = "default_linear_speed")]
    linear_speed: f64,
    #[serde(default = "default_angular_speed")]
    angular_speed: f64,
    #[serde(default = "default_change_interval")]
    change_interval_secs: f64,
}

fn default_linear_speed() -> f64 {
    0.3
}

fn default_angular_speed() -> f64 {
    0.5
}

fn default_change_interval() -> f64 {
    3.0
}

impl WanderNode {
    /// Create a new WanderNode from JSON configuration
    pub fn from_json(config: Value) -> anyhow::Result<Self> {
        let cfg: WanderConfig = serde_json::from_value(config)?;
        Ok(Self {
            topic: cfg.topic,
            linear_speed: cfg.linear_speed,
            angular_speed: cfg.angular_speed,
            change_interval: Duration::from_secs_f64(cfg.change_interval_secs),
            last_change: None,
            current_angular: 0.0,
        })
    }
}

#[async_trait]
impl BehaviorNode for WanderNode {
    async fn tick(&mut self, ctx: &Context) -> anyhow::Result<NodeStatus> {
        let now = Instant::now();

        // Change direction periodically
        if self.last_change.is_none() || now.duration_since(self.last_change.unwrap()) >= self.change_interval {
            // Generate random angular velocity between -angular_speed and +angular_speed
            self.current_angular = (rand::random::<f64>() * 2.0 - 1.0) * self.angular_speed;
            self.last_change = Some(now);
        }

        // Publish velocity command
        let twist = Twist {
            linear: self.linear_speed as f32,
            angular: self.current_angular as f32,
        };

        // Leak topic path for 'static lifetime
        let topic_static: &'static str = Box::leak(self.topic.clone().into_boxed_str());
        let topic = Topic::<Twist>::new(topic_static);

        ctx.publish_to(topic, &twist).await?;
        Ok(NodeStatus::Running)
    }

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

/// MoveNode - Executes a velocity command
///
/// Publishes a fixed velocity command to control robot movement.
///
/// # Configuration
///
/// ```json
/// {
///   "type": "move",
///   "config": {
///     "topic": "/motor/cmd_vel",
///     "linear": 0.5,
///     "angular": 0.0,
///     "duration_secs": 2.0
///   }
/// }
/// ```
#[derive(Debug)]
pub struct MoveNode {
    topic: String,
    linear: f64,
    angular: f64,
    duration: Option<Duration>,
    start_time: Option<Instant>,
}

#[derive(Debug, Serialize, Deserialize)]
struct MoveConfig {
    topic: String,
    linear: f64,
    angular: f64,
    duration_secs: Option<f64>,
}

impl MoveNode {
    /// Create a new MoveNode from JSON configuration
    pub fn from_json(config: Value) -> anyhow::Result<Self> {
        let cfg: MoveConfig = serde_json::from_value(config)?;
        Ok(Self {
            topic: cfg.topic,
            linear: cfg.linear,
            angular: cfg.angular,
            duration: cfg.duration_secs.map(Duration::from_secs_f64),
            start_time: None,
        })
    }
}

#[async_trait]
impl BehaviorNode for MoveNode {
    async fn tick(&mut self, ctx: &Context) -> anyhow::Result<NodeStatus> {
        let now = Instant::now();

        // Initialize start time on first tick
        if self.start_time.is_none() {
            self.start_time = Some(now);
        }

        // Check if duration has elapsed
        if let Some(duration) = self.duration {
            if now.duration_since(self.start_time.unwrap()) >= duration {
                return Ok(NodeStatus::Success);
            }
        }

        // Publish velocity command
        let twist = Twist {
            linear: self.linear as f32,
            angular: self.angular as f32,
        };

        // Leak topic path for 'static lifetime
        let topic_static: &'static str = Box::leak(self.topic.clone().into_boxed_str());
        let topic = Topic::<Twist>::new(topic_static);

        ctx.publish_to(topic, &twist).await?;

        Ok(NodeStatus::Running)
    }

    async fn reset(&mut self) -> anyhow::Result<()> {
        self.start_time = None;
        Ok(())
    }

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

/// SensorCheckNode - Checks sensor state
///
/// Subscribes to a sensor topic and checks if a condition is met.
///
/// # Configuration
///
/// ```json
/// {
///   "type": "sensor_check",
///   "config": {
///     "topic": "/sensors/obstacle",
///     "field": "distance",
///     "operator": "less_than",
///     "threshold": 0.5
///   }
/// }
/// ```
#[derive(Debug)]
pub struct SensorCheckNode {
    #[allow(dead_code)] // TODO: Implement sensor subscription
    topic: String,
    field: String,
    operator: ComparisonOperator,
    threshold: f64,
}

#[derive(Debug, Serialize, Deserialize)]
#[serde(rename_all = "snake_case")]
enum ComparisonOperator {
    LessThan,
    GreaterThan,
    Equal,
}

#[derive(Debug, Serialize, Deserialize)]
struct SensorCheckConfig {
    topic: String,
    field: String,
    operator: ComparisonOperator,
    threshold: f64,
}

impl SensorCheckNode {
    /// Create a new SensorCheckNode from JSON configuration
    pub fn from_json(config: Value) -> anyhow::Result<Self> {
        let cfg: SensorCheckConfig = serde_json::from_value(config)?;
        Ok(Self {
            topic: cfg.topic,
            field: cfg.field,
            operator: cfg.operator,
            threshold: cfg.threshold,
        })
    }
}

#[async_trait]
impl BehaviorNode for SensorCheckNode {
    async fn tick(&mut self, _ctx: &Context) -> anyhow::Result<NodeStatus> {
        // TODO: Implement sensor checking once we have a get_latest_from_path method
        // For now, return Success to allow testing of behavior tree composition
        tracing::debug!(
            "SensorCheckNode: checking {} {} {}",
            self.field,
            match self.operator {
                ComparisonOperator::LessThan => "<",
                ComparisonOperator::GreaterThan => ">",
                ComparisonOperator::Equal => "==",
            },
            self.threshold
        );
        Ok(NodeStatus::Success)
    }

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

/// TimerNode - Waits for a specified duration
///
/// Returns Running until the duration elapses, then returns Success.
///
/// # Configuration
///
/// ```json
/// {
///   "type": "timer",
///   "config": {
///     "duration_secs": 5.0
///   }
/// }
/// ```
#[derive(Debug)]
pub struct TimerNode {
    duration: Duration,
    start_time: Option<Instant>,
}

#[derive(Debug, Serialize, Deserialize)]
struct TimerConfig {
    duration_secs: f64,
}

impl TimerNode {
    /// Create a new TimerNode from JSON configuration
    pub fn from_json(config: Value) -> anyhow::Result<Self> {
        let cfg: TimerConfig = serde_json::from_value(config)?;
        Ok(Self {
            duration: Duration::from_secs_f64(cfg.duration_secs),
            start_time: None,
        })
    }
}

#[async_trait]
impl BehaviorNode for TimerNode {
    async fn tick(&mut self, _ctx: &Context) -> anyhow::Result<NodeStatus> {
        let now = Instant::now();

        // Initialize start time on first tick
        if self.start_time.is_none() {
            self.start_time = Some(now);
        }

        // Check if duration has elapsed
        if now.duration_since(self.start_time.unwrap()) >= self.duration {
            Ok(NodeStatus::Success)
        } else {
            Ok(NodeStatus::Running)
        }
    }

    async fn reset(&mut self) -> anyhow::Result<()> {
        self.start_time = None;
        Ok(())
    }

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

/// IdleNode - Returns Success immediately
///
/// This node does nothing and returns Success on every tick.
/// Useful for testing and as a placeholder in behavior trees.
///
/// # Configuration
///
/// ```json
/// {
///   "type": "idle",
///   "config": {}
/// }
/// ```
#[derive(Debug)]
pub struct IdleNode;

impl IdleNode {
    /// Create a new IdleNode from JSON configuration
    pub fn from_json(_config: Value) -> anyhow::Result<Self> {
        Ok(Self)
    }
}

#[async_trait]
impl BehaviorNode for IdleNode {
    async fn tick(&mut self, _ctx: &Context) -> anyhow::Result<NodeStatus> {
        Ok(NodeStatus::Success)
    }

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

/// PauseNode - Pauses execution for a specified duration
///
/// Similar to TimerNode but semantically represents a deliberate pause in behavior.
/// Returns Running until the duration elapses, then returns Success.
///
/// # Configuration
///
/// ```json
/// {
///   "type": "pause",
///   "config": {
///     "duration_secs": 2.0
///   }
/// }
/// ```
#[derive(Debug)]
pub struct PauseNode {
    duration: Duration,
    start_time: Option<Instant>,
}

#[derive(Debug, Serialize, Deserialize)]
struct PauseConfig {
    duration_secs: f64,
}

impl PauseNode {
    /// Create a new PauseNode from JSON configuration
    pub fn from_json(config: Value) -> anyhow::Result<Self> {
        let cfg: PauseConfig = serde_json::from_value(config)?;
        Ok(Self {
            duration: Duration::from_secs_f64(cfg.duration_secs),
            start_time: None,
        })
    }
}

#[async_trait]
impl BehaviorNode for PauseNode {
    async fn tick(&mut self, _ctx: &Context) -> anyhow::Result<NodeStatus> {
        let now = Instant::now();

        // Initialize start time on first tick
        if self.start_time.is_none() {
            self.start_time = Some(now);
            tracing::debug!("PauseNode: starting pause for {:?}", self.duration);
        }

        // Check if duration has elapsed
        if now.duration_since(self.start_time.unwrap()) >= self.duration {
            tracing::debug!("PauseNode: pause complete");
            Ok(NodeStatus::Success)
        } else {
            Ok(NodeStatus::Running)
        }
    }

    async fn reset(&mut self) -> anyhow::Result<()> {
        self.start_time = None;
        Ok(())
    }

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

/// StopNode - Publishes zero velocity to stop the robot
///
/// This node publishes a zero velocity command to stop the robot immediately.
/// Returns Success after publishing the stop command.
///
/// # Configuration
///
/// ```json
/// {
///   "type": "stop",
///   "config": {
///     "topic": "/motor/cmd_vel"
///   }
/// }
/// ```
#[derive(Debug)]
pub struct StopNode {
    topic: String,
}

#[derive(Debug, Serialize, Deserialize)]
struct StopConfig {
    topic: String,
}

impl StopNode {
    /// Create a new StopNode from JSON configuration
    pub fn from_json(config: Value) -> anyhow::Result<Self> {
        let cfg: StopConfig = serde_json::from_value(config)?;
        Ok(Self { topic: cfg.topic })
    }
}

#[async_trait]
impl BehaviorNode for StopNode {
    async fn tick(&mut self, ctx: &Context) -> anyhow::Result<NodeStatus> {
        // Publish zero velocity
        let twist = Twist {
            linear: 0.0,
            angular: 0.0,
        };

        // Leak topic path for 'static lifetime
        let topic_static: &'static str = Box::leak(self.topic.clone().into_boxed_str());
        let topic = Topic::<Twist>::new(topic_static);

        ctx.publish_to(topic, &twist).await?;
        tracing::debug!("StopNode: published stop command");
        Ok(NodeStatus::Success)
    }

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

/// PlanPathNode - Plans a path to a target position (STUB)
///
/// This is a stub implementation for path planning.
/// In a full implementation, this would calculate a collision-free path.
///
/// # Configuration
///
/// ```json
/// {
///   "type": "plan_path",
///   "config": {
///     "target_position": [5.0, 3.0, 0.0],
///     "path_planning_algorithm": "a_star",
///     "max_speed": 1.0
///   }
/// }
/// ```
#[derive(Debug)]
pub struct PlanPathNode {
    #[allow(dead_code)]
    target_position: Vec<f64>,
    #[allow(dead_code)]
    algorithm: String,
}

#[derive(Debug, Serialize, Deserialize)]
struct PlanPathConfig {
    target_position: Vec<f64>,
    #[serde(default = "default_algorithm")]
    path_planning_algorithm: String,
}

fn default_algorithm() -> String {
    "a_star".to_string()
}

impl PlanPathNode {
    /// Create a new PlanPathNode from JSON configuration
    pub fn from_json(config: Value) -> anyhow::Result<Self> {
        let cfg: PlanPathConfig = serde_json::from_value(config)?;
        Ok(Self {
            target_position: cfg.target_position,
            algorithm: cfg.path_planning_algorithm,
        })
    }
}

#[async_trait]
impl BehaviorNode for PlanPathNode {
    async fn tick(&mut self, _ctx: &Context) -> anyhow::Result<NodeStatus> {
        // TODO: Implement actual path planning
        // For now, return Success to allow behavior tree testing
        tracing::debug!("PlanPathNode: planning path to {:?} (stub)", self.target_position);
        Ok(NodeStatus::Success)
    }

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

/// FollowPathNode - Follows a planned path (STUB)
///
/// This is a stub implementation for path following.
/// In a full implementation, this would execute the planned path.
///
/// # Configuration
///
/// ```json
/// {
///   "type": "follow_path",
///   "config": {
///     "max_speed": 1.0,
///     "turn_rate": 0.5,
///     "topic": "/motor/cmd_vel"
///   }
/// }
/// ```
#[derive(Debug)]
pub struct FollowPathNode {
    #[allow(dead_code)] // TODO: Use this when implementing path following
    topic: String,
    #[allow(dead_code)]
    max_speed: f64,
    #[allow(dead_code)]
    turn_rate: f64,
}

#[derive(Debug, Serialize, Deserialize)]
struct FollowPathConfig {
    #[serde(default = "default_motor_topic")]
    topic: String,
    #[serde(default = "default_max_speed")]
    max_speed: f64,
    #[serde(default = "default_turn_rate")]
    turn_rate: f64,
}

fn default_motor_topic() -> String {
    "/motor/cmd_vel".to_string()
}

fn default_max_speed() -> f64 {
    1.0
}

fn default_turn_rate() -> f64 {
    0.5
}

impl FollowPathNode {
    /// Create a new FollowPathNode from JSON configuration
    pub fn from_json(config: Value) -> anyhow::Result<Self> {
        let cfg: FollowPathConfig = serde_json::from_value(config)?;
        Ok(Self {
            topic: cfg.topic,
            max_speed: cfg.max_speed,
            turn_rate: cfg.turn_rate,
        })
    }
}

#[async_trait]
impl BehaviorNode for FollowPathNode {
    async fn tick(&mut self, _ctx: &Context) -> anyhow::Result<NodeStatus> {
        // TODO: Implement actual path following
        // For now, return Success to allow behavior tree testing
        tracing::debug!("FollowPathNode: following path (stub)");
        Ok(NodeStatus::Success)
    }

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

/// CheckArrivalNode - Checks if the robot has arrived at the target (STUB)
///
/// This is a stub implementation for arrival checking.
/// In a full implementation, this would check the robot's position against the target.
///
/// # Configuration
///
/// ```json
/// {
///   "type": "check_arrival",
///   "config": {
///     "position_tolerance": 0.1,
///     "angular_tolerance": 0.05
///   }
/// }
/// ```
#[derive(Debug)]
pub struct CheckArrivalNode {
    #[allow(dead_code)]
    position_tolerance: f64,
    #[allow(dead_code)]
    angular_tolerance: f64,
}

#[derive(Debug, Serialize, Deserialize)]
struct CheckArrivalConfig {
    position_tolerance: f64,
    angular_tolerance: f64,
}

impl CheckArrivalNode {
    /// Create a new CheckArrivalNode from JSON configuration
    pub fn from_json(config: Value) -> anyhow::Result<Self> {
        let cfg: CheckArrivalConfig = serde_json::from_value(config)?;
        Ok(Self {
            position_tolerance: cfg.position_tolerance,
            angular_tolerance: cfg.angular_tolerance,
        })
    }
}

#[async_trait]
impl BehaviorNode for CheckArrivalNode {
    async fn tick(&mut self, _ctx: &Context) -> anyhow::Result<NodeStatus> {
        // TODO: Implement actual arrival checking
        // For now, return Success to allow behavior tree testing
        tracing::debug!("CheckArrivalNode: checking arrival (stub)");
        Ok(NodeStatus::Success)
    }

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