Topic API

use horus::prelude::*;

// Default capacity and slot size
let topic = Topic::<Twist>::new("cmd_vel");

// Custom capacity (number of slots) and slot size (bytes per slot)
let topic = Topic::<LaserScan>::with_capacity("scan", 8, 4096);

let topic = Topic::<CmdVel>::new("cmd_vel");

// Fire-and-forget (overwrites oldest if full)
topic.send(CmdVel { linear: 1.0, angular: 0.0 });

// Try without blocking
match topic.try_send(CmdVel { linear: 0.5, angular: 0.1 }) {
    Ok(()) => { /* sent */ }
    Err(msg) => { /* buffer full, msg returned */ }
}

// Block up to 10ms
use horus::prelude::*;
match topic.send_blocking(CmdVel { linear: 1.0, angular: 0.0 }, 10_u64.ms()) {
    Ok(()) => { /* sent */ }
    Err(SendBlockingError::Timeout) => { /* timed out */ }
}

let topic = Topic::<Imu>::new("imu");

// IMPORTANT: call recv() every tick to drain the buffer and avoid stale data accumulation
// Process all pending messages
while let Some(msg) = topic.recv() {
    println!("Accel: ({}, {}, {})", msg.linear_accel.x, msg.linear_accel.y, msg.linear_accel.z);
}

// NOTE: read_latest() requires T: Copy — skips older messages and returns only the newest
if let Some(latest) = topic.read_latest() {
    println!("Latest orientation: {:?}", latest.orientation);
}

let topic = Topic::<Twist>::new("cmd_vel");

// Check state
if topic.has_message() {
    println!("{} messages pending", topic.pending_count());
}

// Check for dropped messages
if topic.dropped_count() > 0 {
    hlog!(warn, "Dropped {} messages", topic.dropped_count());
}

// Check metrics
let m = topic.metrics();
println!("Sent: {}, Received: {}", m.messages_sent(), m.messages_received());

let topic = Topic::<Image>::new("camera/rgb");

// Send (moves the Image into the pool slot)
topic.send(image);

// Try send (returns the Image on failure)
match topic.try_send(image) {
    Ok(()) => {}
    Err(img) => { /* buffer full */ }
}

// IMPORTANT: call recv() every tick to drain the buffer — images are large and stale frames waste pool slots
if let Some(img) = topic.recv() {
    println!("{}x{} image received", img.width(), img.height());
}

let topic = Topic::<PointCloud>::new("lidar/points");

topic.send(cloud);

// IMPORTANT: call recv() every tick to avoid stale point clouds filling the pool
if let Some(cloud) = topic.recv() {
    println!("{} points received", cloud.len());
}

let topic = Topic::<DepthImage>::new("camera/depth");

topic.send(depth);

// IMPORTANT: call recv() every tick to drain depth frames
if let Some(depth) = topic.recv() {
    println!("{}x{} depth image", depth.width(), depth.height());
}

let topic = Topic::<Tensor>::new("model/output");

// Send and receive work the same as any other topic type
topic.send(tensor);

// IMPORTANT: call recv() every tick to avoid stale tensor data in the pool
if let Some(t) = topic.recv() {
    println!("Tensor shape: {:?}", t.shape());
}

pub fn new(name: &str) -> Topic<T>

let pub_topic = Topic::<CmdVel>::new("cmd_vel");
let sub_topic = Topic::<CmdVel>::new("cmd_vel"); // connects to same channel

pub fn send(&self, msg: T)

topic.send(CmdVel::new(1.0, 0.0));

// Check if messages were dropped
if topic.dropped_count() > 0 {
    hlog!(warn, "{} messages dropped — subscriber too slow", topic.dropped_count());
}

pub fn recv(&self) -> Option<T>

// Process ALL pending messages each tick
while let Some(scan) = self.scan_topic.recv() {
    self.process(scan);
}

pub fn read_latest(&self) -> Option<T>
where T: Copy

// Good: pose is state — only the latest matters
if let Some(pose) = self.odom_topic.read_latest() {
    self.current_position = pose;
}

// Bad: cmd_vel is a command — you'd miss intermediate commands
// Use recv() instead for command streams

pub fn try_send(&self, msg: T) -> Result<(), T>

match topic.try_send(cmd) {
    Ok(()) => {}
    Err(_returned) => {
        self.drop_count += 1;
        hlog!(warn, "Buffer full — {} drops total", self.drop_count);
    }
}

pub fn try_recv(&self) -> Option<T>

if let Some(msg) = topic.try_recv() {
    self.process(msg);
}

pub fn send_blocking(&self, msg: T, timeout: Duration) -> Result<(), SendBlockingError>

use horus::prelude::*;

// Critical: e-stop must be delivered
match topic.send_blocking(stop_cmd, 1.ms()) {
    Ok(()) => hlog!(info, "E-stop delivered"),
    Err(_) => hlog!(error, "E-stop delivery FAILED — subscriber unresponsive"),
}

pub fn with_capacity(name: &str, capacity: u32, slot_size: Option<usize>) -> HorusResult<Self>

// 256-slot buffer for a high-frequency sensor
let topic = Topic::<Imu>::with_capacity("imu", 256, None)?;

// Small buffer to save memory on a constrained device
let topic = Topic::<Pose2D>::with_capacity("pose", 4, None)?;

pub fn name(&self) -> &str

let topic = Topic::<f32>::new("sensor.temperature")?;
assert_eq!(topic.name(), "sensor.temperature");

pub fn has_message(&self) -> bool

if topic.has_message() {
    let msg = topic.recv().unwrap();  // guaranteed Some
}

pub fn pending_count(&self) -> u64

if topic.pending_count() > 10 {
    hlog!(warn, "Topic '{}' has {} pending — subscriber may be too slow",
        topic.name(), topic.pending_count());
}

pub fn dropped_count(&self) -> u64

if topic.dropped_count() > 0 {
    hlog!(warn, "{} messages dropped on '{}' — consider increasing capacity or subscriber rate",
        topic.dropped_count(), topic.name());
}

pub fn metrics(&self) -> TopicMetrics

let m = topic.metrics();
println!("Sent: {}, Received: {}, Failures: send={} recv={}",
    m.messages_sent(), m.messages_received(),
    m.send_failures(), m.recv_failures());

use horus::prelude::*;

struct LidarProcessor {
    scan_in: Topic<LaserScan>,
    cmd_out: Topic<CmdVel>,
}

impl LidarProcessor {
    fn new() -> Self {
        Self {
            scan_in: Topic::new("scan"),
            cmd_out: Topic::new("cmd_vel"),
        }
    }
}

impl Node for LidarProcessor {
    fn name(&self) -> &str { "LidarProcessor" }

    fn tick(&mut self) {
        // IMPORTANT: call recv() every tick to drain the scan buffer
        if let Some(scan) = self.scan_in.recv() {
            // Find closest obstacle
            let min_range = scan.ranges.iter()
                .copied()
                .filter(|r| *r > scan.range_min && *r < scan.range_max)
                .fold(f32::MAX, f32::min);

            // SAFETY: send zero velocity when obstacle is too close
            let speed = if min_range < 0.5 { 0.0 } else { 1.0 };
            self.cmd_out.send(CmdVel { linear: speed, angular: 0.0 });
        }
    }

    // SAFETY: stop sending velocity commands on shutdown
    fn shutdown(&mut self) -> Result<()> {
        self.cmd_out.send(CmdVel { linear: 0.0, angular: 0.0 });
        Ok(())
    }
}

fn main() -> Result<()> {
    let mut scheduler = Scheduler::new();
    scheduler.add(LidarProcessor::new())
        // NOTE: .rate() auto-enables RT scheduling with 80% budget and 95% deadline
        .rate(50.hz())
        .build()?;
    scheduler.run()
}

// Monitor topic health
let m = topic.metrics();
if topic.dropped_count() > 0 {
    hlog!(warn, "Topic '{}': {} messages dropped, {} pending",
        topic.name(), topic.dropped_count(), topic.pending_count());
}

struct SafetyMonitor {
    estop_pub: Topic<EmergencyStop>,
}

impl Node for SafetyMonitor {
    fn name(&self) -> &str { "SafetyMonitor" }

    fn tick(&mut self) {
        if self.detect_collision() {
            let stop = EmergencyStop {
                engaged: 1,
                reason: *b"Collision detected\0\0\0\0\0\0\0\0\0\0\0\0\0\0\0\0\0\0\0\0\0\0\0\0\0\0\0\0\0\0\0\0\0\0\0\0\0\0\0\0\0\0\0\0\0\0",
                ..Default::default()
            };
            // Block up to 1ms — guaranteed delivery for safety-critical commands
            let _ = self.estop_pub.send_blocking(stop, 1.ms());
        }
    }
}

struct FusionNode {
    imu_sub: Topic<Imu>,
    odom_sub: Topic<Odometry>,
    pose_pub: Topic<Pose3D>,
    last_imu: Option<Imu>,
    last_odom: Option<Odometry>,
}

impl Node for FusionNode {
    fn name(&self) -> &str { "Fusion" }

    fn tick(&mut self) {
        // Always drain both topics every tick
        if let Some(imu) = self.imu_sub.recv() { self.last_imu = Some(imu); }
        if let Some(odom) = self.odom_sub.recv() { self.last_odom = Some(odom); }

        if let (Some(imu), Some(odom)) = (&self.last_imu, &self.last_odom) {
            let fused = self.fuse(imu, odom);
            self.pose_pub.send(fused);
        }
    }
}