mecha10-core 0.8.2

//! Type-Safe Topics System
//!
//! This module provides compile-time type-safe topic definitions,
//! eliminating string-based topic errors and enabling better IDE support.
//!
//! # Quick Example
//!
//! ```rust
//! use mecha10_core::prelude::*;
//! use mecha10_core::topics::sensor;
//!
//! // Define custom topics
//! mod my_topics {
//!     use mecha10_core::topics::Topic;
//!     use mecha10_core::messages::Message;
//!
//!     // Custom message type
//!     #[derive(Debug, Clone, serde::Serialize, serde::Deserialize)]
//!     pub struct MyMessage {
//!         pub value: f32,
//!     }
//!
//!     impl Message for MyMessage {}
//!
//!     pub const CUSTOM: Topic<MyMessage> = Topic::new("/my/topic");
//! }
//!
//! # async fn example(ctx: &Context) -> Result<()> {
//! // Use built-in topics
//! let mut images = ctx.subscribe(sensor::CAMERA_RGB).await?;
//!
//! // Use custom topics
//! let mut custom = ctx.subscribe(my_topics::CUSTOM).await?;
//! # Ok(())
//! # }
//! ```
//!
//! # Benefits
//!
//! - **Compile-time validation**: No typos, no runtime errors
//! - **IDE autocomplete**: Full IntelliSense support
//! - **Refactoring-safe**: Rename topics safely
//! - **Type inference**: Return types inferred automatically

use crate::messages::Message;
use std::marker::PhantomData;

pub struct Topic<T: Message> {
    path: &'static str,
    _phantom: PhantomData<T>,
}

impl<T: Message> Topic<T> {
    /// Create a new typed topic
    ///
    /// This is a const function, so topics can be defined as constants.
    pub const fn new(path: &'static str) -> Self {
        Self {
            path,
            _phantom: PhantomData,
        }
    }

    /// Get the topic path
    pub const fn path(&self) -> &'static str {
        self.path
    }

    /// Get the topic as a string (for compatibility)
    pub fn as_str(&self) -> &'static str {
        self.path
    }
}

impl<T: Message> Clone for Topic<T> {
    fn clone(&self) -> Self {
        *self
    }
}

impl<T: Message> Copy for Topic<T> {}

impl<T: Message> std::fmt::Debug for Topic<T> {
    fn fmt(&self, f: &mut std::fmt::Formatter<'_>) -> std::fmt::Result {
        write!(f, "Topic<{}>({:?})", std::any::type_name::<T>(), self.path)
    }
}

impl<T: Message> std::fmt::Display for Topic<T> {
    fn fmt(&self, f: &mut std::fmt::Formatter<'_>) -> std::fmt::Result {
        write!(f, "{}", self.path)
    }
}

pub mod sensor {
    use super::*;
    use crate::messages::{GpsData, Image, Imu, JointState, LaserScan, Odometry};

    pub const CAMERA_RGB: Topic<Image> = Topic::new("/sensor/camera/rgb");

    pub const CAMERA_DEPTH: Topic<Image> = Topic::new("/sensor/camera/depth");

    pub const CAMERA_LEFT: Topic<Image> = Topic::new("/sensor/camera/left");

    pub const CAMERA_RIGHT: Topic<Image> = Topic::new("/sensor/camera/right");

    pub const CAMERA_IR: Topic<Image> = Topic::new("/sensor/camera/ir");

    pub const CAMERA_THERMAL: Topic<Image> = Topic::new("/sensor/camera/thermal");

    pub const LIDAR_SCAN: Topic<LaserScan> = Topic::new("/sensor/lidar/scan");

    pub const LIDAR_CLOUD: Topic<Vec<[f32; 3]>> = Topic::new("/sensor/lidar/cloud");

    pub const ODOMETRY: Topic<Odometry> = Topic::new("/sensor/odometry");

    pub const WHEEL_ODOM: Topic<Odometry> = Topic::new("/sensor/wheel_odom");

    pub const VISUAL_ODOM: Topic<Odometry> = Topic::new("/sensor/visual_odom");

    pub const IMU: Topic<Imu> = Topic::new("/sensor/imu");

    pub const IMU_RAW: Topic<Imu> = Topic::new("/sensor/imu/raw");

    pub const GPS: Topic<GpsData> = Topic::new("/sensor/gps");

    pub const JOINT_STATES: Topic<JointState> = Topic::new("/sensor/joint_states");

    pub const BATTERY_VOLTAGE: Topic<f32> = Topic::new("/sensor/battery/voltage");

    pub const BATTERY_PERCENT: Topic<f32> = Topic::new("/sensor/battery/percent");

    pub const TEMPERATURE: Topic<f32> = Topic::new("/sensor/temperature");

    pub const ULTRASONIC: Topic<Vec<f32>> = Topic::new("/sensor/ultrasonic");

    pub const CONTACT: Topic<Vec<bool>> = Topic::new("/sensor/contact");

    pub const FORCE_TORQUE: Topic<[f32; 6]> = Topic::new("/sensor/force_torque");
}

pub mod actuator {
    use super::*;
    use crate::messages::{GripperCommand, JointState, Twist};

    pub const CMD_VEL: Topic<Twist> = Topic::new("/actuator/cmd_vel");

    pub const CMD_VEL_RAW: Topic<Twist> = Topic::new("/actuator/cmd_vel/raw");

    pub const CMD_VEL_TELEOP: Topic<Twist> = Topic::new("/actuator/cmd_vel/teleop");

    pub const CMD_VEL_NAV: Topic<Twist> = Topic::new("/actuator/cmd_vel/nav");

    pub const JOINT_CMD: Topic<JointState> = Topic::new("/actuator/joint_cmd");

    pub const JOINT_TRAJECTORY: Topic<Vec<JointState>> = Topic::new("/actuator/joint_trajectory");

    pub const GRIPPER_CMD: Topic<GripperCommand> = Topic::new("/actuator/gripper_cmd");

    pub const MOTOR_CMD: Topic<Vec<f32>> = Topic::new("/actuator/motor_cmd");

    pub const LIGHTS: Topic<Vec<u8>> = Topic::new("/actuator/lights");

    pub const SOUND: Topic<String> = Topic::new("/actuator/sound");

    pub const EMERGENCY_STOP: Topic<bool> = Topic::new("/actuator/emergency_stop");

    pub const ENABLE: Topic<bool> = Topic::new("/actuator/enable");
}

pub mod manipulation {
    use super::*;
    use crate::messages::{GripperCommand, JointState, Pose};

    pub const EE_POSE_TARGET: Topic<Pose> = Topic::new("/manipulation/ee_pose_target");

    pub const EE_POSE: Topic<Pose> = Topic::new("/manipulation/ee_pose");

    pub const CARTESIAN_PATH: Topic<Vec<Pose>> = Topic::new("/manipulation/cartesian_path");

    pub const TRAJECTORY: Topic<Vec<JointState>> = Topic::new("/manipulation/trajectory");

    pub const GRIPPER: Topic<GripperCommand> = Topic::new("/manipulation/gripper");

    pub const GRASP_POSES: Topic<Vec<Pose>> = Topic::new("/manipulation/grasp_poses");

    pub const SELECTED_GRASP: Topic<Pose> = Topic::new("/manipulation/selected_grasp");

    pub const STATUS: Topic<String> = Topic::new("/manipulation/status");
}

pub mod perception {
    use super::*;
    use crate::messages::{Detection, Image, Pose};

    pub const DETECTIONS: Topic<Vec<Detection>> = Topic::new("/perception/detections");

    pub const HIGH_CONFIDENCE: Topic<Vec<Detection>> = Topic::new("/perception/detections/high_confidence");

    pub const PEOPLE: Topic<Vec<Detection>> = Topic::new("/perception/detections/people");
    pub const OBJECTS: Topic<Vec<Detection>> = Topic::new("/perception/detections/objects");
    pub const OBSTACLES: Topic<Vec<Detection>> = Topic::new("/perception/detections/obstacles");

    pub const TRACKING: Topic<Vec<Detection>> = Topic::new("/perception/tracking");

    pub const POSE: Topic<Pose> = Topic::new("/perception/pose");

    pub const LOCALIZATION_QUALITY: Topic<f32> = Topic::new("/perception/localization/quality");

    pub const SEGMENTATION: Topic<Image> = Topic::new("/perception/segmentation");

    pub const DEPTH_ESTIMATION: Topic<Image> = Topic::new("/perception/depth");

    pub const OPTICAL_FLOW: Topic<Vec<[f32; 2]>> = Topic::new("/perception/optical_flow");

    pub const FEATURES: Topic<Vec<[f32; 2]>> = Topic::new("/perception/features");

    pub const SCENE_TYPE: Topic<String> = Topic::new("/perception/scene_type");
}

pub mod navigation {
    use super::*;
    use crate::messages::{NavigationGoal, NavigationStatus, Path, Pose, Twist};

    pub const GOAL: Topic<NavigationGoal> = Topic::new("/navigation/goal");

    pub const CANCEL: Topic<bool> = Topic::new("/navigation/cancel");

    pub const STATUS: Topic<NavigationStatus> = Topic::new("/navigation/status");

    pub const GLOBAL_PATH: Topic<Path> = Topic::new("/navigation/global_path");

    pub const LOCAL_PATH: Topic<Path> = Topic::new("/navigation/local_path");

    pub const CURRENT_WAYPOINT: Topic<crate::messages::Waypoint> = Topic::new("/navigation/waypoint");

    pub const NEXT_WAYPOINT: Topic<crate::messages::Waypoint> = Topic::new("/navigation/next_waypoint");

    pub const GLOBAL_COSTMAP: Topic<Vec<Vec<u8>>> = Topic::new("/navigation/costmap/global");

    pub const LOCAL_COSTMAP: Topic<Vec<Vec<u8>>> = Topic::new("/navigation/costmap/local");

    pub const RECOVERY: Topic<String> = Topic::new("/navigation/recovery");

    pub const DYNAMIC_OBSTACLES: Topic<Vec<Pose>> = Topic::new("/navigation/obstacles/dynamic");

    pub const STATIC_OBSTACLES: Topic<Vec<Pose>> = Topic::new("/navigation/obstacles/static");

    pub const VELOCITY_LIMITS: Topic<Twist> = Topic::new("/navigation/velocity_limits");
}

pub mod mapping {
    use super::*;
    use crate::messages::Pose;

    pub const OCCUPANCY_GRID: Topic<Vec<Vec<u8>>> = Topic::new("/mapping/occupancy_grid");

    pub const MAP_METADATA: Topic<String> = Topic::new("/mapping/metadata");

    pub const LOOP_CLOSURES: Topic<Vec<(Pose, Pose)>> = Topic::new("/mapping/loop_closures");

    pub const MAP_UPDATE: Topic<Vec<(usize, usize, u8)>> = Topic::new("/mapping/update");

    pub const SAVE_MAP: Topic<String> = Topic::new("/mapping/save");

    pub const LOAD_MAP: Topic<String> = Topic::new("/mapping/load");

    pub const STATUS: Topic<String> = Topic::new("/mapping/status");
}

pub mod system {
    use super::*;
    use crate::messages::{HealthStatus, LogRecord, NodeStatus, SystemCommand};

    pub const HEALTH: Topic<HealthStatus> = Topic::new("/system/health");

    pub const NODE_STATUS: Topic<NodeStatus> = Topic::new("/system/node_status");

    pub const COMMAND: Topic<SystemCommand> = Topic::new("/system/command");

    pub const HEARTBEAT: Topic<u64> = Topic::new("/system/heartbeat");

    pub const DIAGNOSTICS: Topic<String> = Topic::new("/system/diagnostics");

    pub const WARNINGS: Topic<Vec<String>> = Topic::new("/system/warnings");

    pub const ERRORS: Topic<Vec<String>> = Topic::new("/system/errors");

    pub const CPU_USAGE: Topic<f32> = Topic::new("/system/cpu_usage");

    pub const MEMORY_USAGE: Topic<f32> = Topic::new("/system/memory_usage");

    pub const DISK_USAGE: Topic<f32> = Topic::new("/system/disk_usage");

    pub const NETWORK_STATS: Topic<(f32, f32)> = Topic::new("/system/network_stats");

    pub const SYSTEM_TEMP: Topic<f32> = Topic::new("/system/temperature");

    pub const POWER: Topic<f32> = Topic::new("/system/power");

    pub const UPTIME: Topic<u64> = Topic::new("/system/uptime");

    pub const LOG: Topic<LogRecord> = Topic::new("/system/log");

    pub const CONFIG_UPDATE: Topic<String> = Topic::new("/system/config_update");

    pub const SHUTDOWN: Topic<bool> = Topic::new("/system/shutdown");

    pub const REBOOT: Topic<bool> = Topic::new("/system/reboot");
}

pub mod teleop {
    use super::*;
    use crate::messages::Twist;

    pub const JOYSTICK: Topic<Vec<f32>> = Topic::new("/teleop/joystick");

    pub const BUTTONS: Topic<Vec<bool>> = Topic::new("/teleop/buttons");

    pub const CMD_VEL: Topic<Twist> = Topic::new("/teleop/cmd_vel");

    pub const ENABLE: Topic<bool> = Topic::new("/teleop/enable");

    pub const MODE: Topic<String> = Topic::new("/teleop/mode");
}

pub mod ml {
    use super::*;
    use crate::messages::Detection;

    pub const INFERENCE_REQUEST: Topic<Vec<u8>> = Topic::new("/ml/inference/request");

    pub const INFERENCE_RESULT: Topic<Vec<f32>> = Topic::new("/ml/inference/result");

    pub const PREDICTIONS: Topic<Vec<Detection>> = Topic::new("/ml/predictions");

    pub const METRICS: Topic<String> = Topic::new("/ml/metrics");

    pub const LOAD_MODEL: Topic<String> = Topic::new("/ml/load_model");

    pub const MODEL_INFO: Topic<String> = Topic::new("/ml/model_info");

    pub const TRAINING_DATA: Topic<Vec<u8>> = Topic::new("/ml/training_data");
}

#[macro_export]
macro_rules! topic_group {
    (
        $(#[$meta:meta])*
        $vis:vis struct $name:ident {
            $(
                $field:ident: $topic:expr
            ),* $(,)?
        }
    ) => {
        $(#[$meta])*
        $vis struct $name;

        impl $name {
            /// Get all topics in this group
            pub fn topics() -> Vec<&'static str> {
                vec![
                    $( $topic.path() ),*
                ]
            }
        }
    };
}

topic_group! {
    pub struct AllSensors {
        camera_rgb: sensor::CAMERA_RGB,
        camera_depth: sensor::CAMERA_DEPTH,
        lidar: sensor::LIDAR_SCAN,
        odometry: sensor::ODOMETRY,
        imu: sensor::IMU,
        gps: sensor::GPS,
    }
}

topic_group! {
    pub struct AllActuators {
        cmd_vel: actuator::CMD_VEL,
        joint_cmd: actuator::JOINT_CMD,
        gripper: actuator::GRIPPER_CMD,
    }
}

pub struct SubscribeBuilder<T: Message> {
    #[allow(dead_code)]
    topic: Topic<T>,
    #[allow(dead_code)]
    buffer_size: Option<usize>,
    #[allow(dead_code)]
    latest_only: bool,
    #[allow(dead_code)]
    throttle_ms: Option<u64>,
}

impl<T: Message> SubscribeBuilder<T> {
    crate::new_with_defaults!(
        pub fn new(topic: Topic<T>) -> Self {
            buffer_size: None,
            latest_only: false,
            throttle_ms: None
        }
    );

    crate::builder_setter!(
        /// Set buffer size (default: unbounded)
        into_some, buffer, buffer_size, usize
    );

    /// Only keep latest message, drop old ones
    pub fn latest(mut self) -> Self {
        self.latest_only = true;
        self.buffer_size = Some(1);
        self
    }

    crate::builder_setter!(
        /// Throttle messages to max rate
        duration_millis, throttle, throttle_ms
    );
}

pub struct PublishBuilder<'a, T: Message> {
    #[allow(dead_code)]
    topic: Topic<T>,
    #[allow(dead_code)]
    message: &'a T,
    #[allow(dead_code)]
    #[allow(clippy::type_complexity)]
    condition: Option<Box<dyn Fn(&T) -> bool>>,
    #[allow(dead_code)]
    throttle_ms: Option<u64>,
}

impl<'a, T: Message> PublishBuilder<'a, T> {
    pub fn new(topic: Topic<T>, message: &'a T) -> Self {
        Self {
            topic,
            message,
            condition: None,
            throttle_ms: None,
        }
    }

    /// Only publish if condition is true
    pub fn when<F>(mut self, condition: F) -> Self
    where
        F: Fn(&T) -> bool + 'static,
    {
        self.condition = Some(Box::new(condition));
        self
    }

    crate::builder_setter!(
        /// Throttle publishing to max rate
        duration_millis, throttle, throttle_ms
    );
}