mecha10-core 0.6.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::prelude::*;
//! use mecha10::topics::{Topic, sensor};
//!
//! // Define custom topics
//! mod my_topics {
//!     use super::*;
//!
//!     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;

/// A type-safe topic with an associated message type
///
/// This is the core abstraction that enables compile-time type safety for pub/sub.
/// Topics are defined as constants and carry type information via phantom types.
///
/// # Type Safety Guarantee
///
/// The compiler ensures that:
/// - Subscribe returns the correct message type
/// - Publish accepts only the correct message type
/// - No runtime type confusion is possible
///
/// # Example: Defining Topics
///
/// ```rust
/// use mecha10::topics::Topic;
/// use mecha10::messages::Image;
///
/// // Simple topic definition
/// pub const CAMERA_RGB: Topic<Image> = Topic::new("/sensor/camera/rgb");
///
/// // Custom message type
/// #[derive(Serialize, Deserialize)]
/// struct MyMessage {
///     value: f32,
/// }
///
/// pub const CUSTOM: Topic<MyMessage> = Topic::new("/custom/topic");
/// ```
///
/// # Example: Using Topics
///
/// ```rust
/// # use mecha10::prelude::*;
/// # use mecha10::topics::sensor;
/// # async fn example(ctx: &Context) -> Result<()> {
/// // Type is inferred as Receiver<Image>
/// let mut images = ctx.subscribe(sensor::CAMERA_RGB).await?;
///
/// while let Some(image) = images.recv().await {
///     // image is type Image
///     println!("Image: {}x{}", image.width, image.height);
///
///     // Compiler ensures we publish the right type
///     ctx.publish_to(sensor::CAMERA_RGB, &image).await?;
///
///     // This would be a compile error:
///     // ctx.publish_to(sensor::CAMERA_RGB, &laser_scan).await?;
///     //                                      ^^^^^^^^^^^ expected Image, found LaserScan
/// }
/// # Ok(())
/// # }
/// ```
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)
    }
}

// ============================================================================
// Standard Topic Definitions
// ============================================================================

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

    /// Raw velocity command (bypasses safety checks)
    pub const CMD_VEL_RAW: Topic<Twist> = Topic::new("/actuator/cmd_vel/raw");

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

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

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

    /// Joint trajectory (sequence of joint states)
    pub const JOINT_TRAJECTORY: Topic<Vec<JointState>> = Topic::new("/actuator/joint_trajectory");

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

    /// Individual motor commands (PWM values)
    pub const MOTOR_CMD: Topic<Vec<f32>> = Topic::new("/actuator/motor_cmd");

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

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

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

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

/// Manipulation topics (for robot arms)
pub mod manipulation {
    use super::*;
    use crate::messages::{GripperCommand, JointState, Pose};

    /// End-effector pose target
    pub const EE_POSE_TARGET: Topic<Pose> = Topic::new("/manipulation/ee_pose_target");

    /// End-effector pose current
    pub const EE_POSE: Topic<Pose> = Topic::new("/manipulation/ee_pose");

    /// Cartesian path (list of poses)
    pub const CARTESIAN_PATH: Topic<Vec<Pose>> = Topic::new("/manipulation/cartesian_path");

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

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

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

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

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

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

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

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

    /// Detections by class
    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");

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

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

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

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

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

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

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

    /// Scene understanding (room, corridor, etc.)
    pub const SCENE_TYPE: Topic<String> = Topic::new("/perception/scene_type");
}

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

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

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

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

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

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

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

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

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

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

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

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

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

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

/// Mapping topics (SLAM)
pub mod mapping {
    use super::*;
    use crate::messages::Pose;

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

    /// Map metadata (resolution, origin, etc.)
    pub const MAP_METADATA: Topic<String> = Topic::new("/mapping/metadata");

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

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

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

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

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

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

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

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

    /// System commands (start, stop, restart, etc.)
    pub const COMMAND: Topic<SystemCommand> = Topic::new("/system/command");

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

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

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

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

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

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

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

    /// Network statistics
    pub const NETWORK_STATS: Topic<(f32, f32)> = Topic::new("/system/network_stats"); // (tx, rx in KB/s)

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

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

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

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

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

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

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

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

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

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

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

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

    /// Teleop mode (normal, turbo, slow, etc.)
    pub const MODE: Topic<String> = Topic::new("/teleop/mode");
}

/// AI/ML model topics
pub mod ml {
    use super::*;
    use crate::messages::Detection;

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

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

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

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

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

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

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

// ============================================================================
// Topic Groups
// ============================================================================

/// Macro to define topic groups
///
/// # Example
/// ```rust
/// topic_group! {
///     pub struct AllSensors {
///         camera: sensor::CAMERA_RGB,
///         lidar: sensor::LIDAR_SCAN,
///         imu: sensor::IMU,
///     }
/// }
/// ```
#[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() ),*
                ]
            }
        }
    };
}

// Example topic groups
topic_group! {
    /// All sensor topics
    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! {
    /// All actuator topics
    pub struct AllActuators {
        cmd_vel: actuator::CMD_VEL,
        joint_cmd: actuator::JOINT_CMD,
        gripper: actuator::GRIPPER_CMD,
    }
}

// ============================================================================
// Builder Pattern for Subscribe Options
// ============================================================================

/// Builder for subscription options
///
/// Enables fluent API for configuring subscriptions:
///
/// ```rust
/// let mut images = ctx.subscribe(sensor::CAMERA_RGB)
///     .buffer(10)
///     .filter(|img| img.width >= 640)
///     .throttle(Duration::from_millis(100))
///     .await?;
/// ```
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> {
    pub fn new(topic: Topic<T>) -> Self {
        Self {
            topic,
            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
    }

    /// Throttle messages to max rate
    pub fn throttle(mut self, duration: std::time::Duration) -> Self {
        self.throttle_ms = Some(duration.as_millis() as u64);
        self
    }

    // Build method would be implemented in Context
}

/// Builder for publish options
///
/// ```rust
/// ctx.publish_to(perception::DETECTIONS, &detections)
///     .when(|d| !d.is_empty())
///     .throttle(Duration::from_secs(1))
///     .await?;
/// ```
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
    }

    /// Throttle publishing to max rate
    pub fn throttle(mut self, duration: std::time::Duration) -> Self {
        self.throttle_ms = Some(duration.as_millis() as u64);
        self
    }

    // Build method would be implemented in Context
}

// ============================================================================
// Tests
// ============================================================================