mecha10-behavior-runtime 0.6.2

//! Action nodes that publish velocity commands to move the robot.

use crate::{BehaviorNode, Context, 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};

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

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