mecha10-core 0.6.2

// Navigation Message Types

use super::perception::Pose;
use serde::{Deserialize, Serialize};

/// Navigation goal
///
/// Represents a desired destination for the robot
#[derive(Debug, Clone, Serialize, Deserialize)]
pub struct NavigationGoal {
    /// Timestamp in microseconds since epoch
    pub timestamp: u64,

    /// Unique goal ID (for tracking completion)
    pub goal_id: String,

    /// Target pose
    pub target: Pose,

    /// Maximum linear velocity in m/s (optional)
    #[serde(skip_serializing_if = "Option::is_none")]
    pub max_linear_velocity: Option<f32>,

    /// Maximum angular velocity in rad/s (optional)
    #[serde(skip_serializing_if = "Option::is_none")]
    pub max_angular_velocity: Option<f32>,

    /// Goal tolerance in meters (optional, default: 0.1)
    #[serde(skip_serializing_if = "Option::is_none")]
    pub position_tolerance: Option<f32>,

    /// Orientation tolerance in radians (optional, default: 0.1)
    #[serde(skip_serializing_if = "Option::is_none")]
    pub orientation_tolerance: Option<f32>,
}

impl NavigationGoal {
    /// Create a simple 2D navigation goal
    pub fn new_2d(goal_id: String, x: f32, y: f32, yaw: f32) -> Self {
        Self {
            timestamp: crate::prelude::now_micros(),
            goal_id,
            target: Pose::new_2d(x, y, yaw),
            max_linear_velocity: None,
            max_angular_velocity: None,
            position_tolerance: None,
            orientation_tolerance: None,
        }
    }
}

/// Navigation path
///
/// Represents a planned path as a sequence of waypoints
#[derive(Debug, Clone, Serialize, Deserialize)]
pub struct Path {
    /// Timestamp in microseconds since epoch
    pub timestamp: u64,

    /// Waypoints along the path
    pub waypoints: Vec<Waypoint>,

    /// Total path length in meters
    pub total_length: f32,

    /// Estimated time to complete path in seconds
    #[serde(skip_serializing_if = "Option::is_none")]
    pub estimated_time: Option<f32>,

    /// Frame ID for the path coordinates
    #[serde(skip_serializing_if = "Option::is_none")]
    pub frame_id: Option<String>,
}

impl Path {
    /// Create an empty path
    pub fn empty() -> Self {
        Self {
            timestamp: crate::prelude::now_micros(),
            waypoints: Vec::new(),
            total_length: 0.0,
            estimated_time: None,
            frame_id: None,
        }
    }

    /// Check if path is empty
    pub fn is_empty(&self) -> bool {
        self.waypoints.is_empty()
    }

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

    /// Calculate total path length from waypoints
    pub fn calculate_length(&mut self) {
        let mut length = 0.0;

        for i in 1..self.waypoints.len() {
            let prev = &self.waypoints[i - 1];
            let curr = &self.waypoints[i];

            let dx = curr.pose.position[0] - prev.pose.position[0];
            let dy = curr.pose.position[1] - prev.pose.position[1];
            let dz = curr.pose.position[2] - prev.pose.position[2];

            length += (dx * dx + dy * dy + dz * dz).sqrt();
        }

        self.total_length = length;
    }
}

/// Waypoint in a path
#[derive(Debug, Clone, Serialize, Deserialize)]
pub struct Waypoint {
    /// Pose of this waypoint
    pub pose: Pose,

    /// Desired velocity at this waypoint in m/s (optional)
    #[serde(skip_serializing_if = "Option::is_none")]
    pub velocity: Option<f32>,

    /// Cumulative distance along path to this waypoint in meters
    #[serde(skip_serializing_if = "Option::is_none")]
    pub distance_along_path: Option<f32>,
}

impl Waypoint {
    /// Create a simple waypoint from 2D coordinates
    pub fn new_2d(x: f32, y: f32, yaw: f32) -> Self {
        Self {
            pose: Pose::new_2d(x, y, yaw),
            velocity: None,
            distance_along_path: None,
        }
    }
}

/// Navigation status
///
/// Reports the current state of navigation execution
#[derive(Debug, Clone, Serialize, Deserialize)]
pub struct NavigationStatus {
    /// Timestamp in microseconds since epoch
    pub timestamp: u64,

    /// Goal ID being executed
    pub goal_id: String,

    /// Current navigation state
    pub state: NavigationState,

    /// Distance remaining to goal in meters
    pub distance_remaining: f32,

    /// Estimated time remaining in seconds
    #[serde(skip_serializing_if = "Option::is_none")]
    pub time_remaining: Option<f32>,

    /// Current progress (0.0 to 1.0)
    pub progress: f32,
}

/// Navigation execution state
#[derive(Debug, Clone, Copy, PartialEq, Eq, Serialize, Deserialize)]
pub enum NavigationState {
    /// Planning a path
    Planning,

    /// Executing the path
    Moving,

    /// Goal reached successfully
    Succeeded,

    /// Navigation failed
    Failed,

    /// Waiting for obstacle clearance
    Waiting,

    /// Recovering from collision
    Recovering,
}