[package]
name = "sensor-fusion"
version = "0.1.0"
description = "IMU + odometry complementary filter"

use horus::prelude::*;

/// Wheel odometry: position from encoder counts
#[derive(Debug, Clone, Copy, Default, Serialize, Deserialize, LogSummary)]
#[repr(C)]
struct WheelOdom {
    x: f32,
    y: f32,
    theta: f32,
    speed: f32,
}

/// IMU-derived heading
#[derive(Debug, Clone, Copy, Default, Serialize, Deserialize, LogSummary)]
#[repr(C)]
struct ImuHeading {
    yaw: f32,
    yaw_rate: f32,
}

/// Fused state estimate
#[derive(Debug, Clone, Copy, Default, Serialize, Deserialize, LogSummary)]
#[repr(C)]
struct FusedPose {
    x: f32,
    y: f32,
    theta: f32,
    speed: f32,
    confidence: f32,
}

// ── Fusion Node ─────────────────────────────────────────────

struct FusionNode {
    odom_sub: Topic<WheelOdom>,
    imu_sub: Topic<ImuHeading>,
    pose_pub: Topic<FusedPose>,
    last_odom: Option<WheelOdom>,
    last_imu: Option<ImuHeading>,
    alpha: f32,
}

impl FusionNode {
    fn new() -> Result<Self> {
        Ok(Self {
            odom_sub: Topic::new("odom.wheels")?,
            imu_sub: Topic::new("imu.heading")?,
            pose_pub: Topic::new("pose.fused")?,
            last_odom: None,
            last_imu: None,
            alpha: 0.7, // favor IMU for heading (less drift than wheels on turns)
        })
    }
}

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

    fn tick(&mut self) {
        // IMPORTANT: always recv() ALL topics every tick to drain buffers
        if let Some(odom) = self.odom_sub.recv() {
            self.last_odom = Some(odom);
        }
        if let Some(imu) = self.imu_sub.recv() {
            self.last_imu = Some(imu);
        }

        // Fuse only when both sources are available
        let (odom, imu) = match (&self.last_odom, &self.last_imu) {
            (Some(o), Some(i)) => (o, i),
            _ => return,
        };

        // Complementary filter: blend odom heading with IMU heading
        let fused_theta = (1.0 - self.alpha) * odom.theta + self.alpha * imu.yaw;

        let confidence = if imu.yaw_rate.abs() > 0.5 { 0.6 } else { 0.9 };

        self.pose_pub.send(FusedPose {
            x: odom.x,
            y: odom.y,
            theta: fused_theta,
            speed: odom.speed,
            confidence,
        });
    }
}

fn main() -> Result<()> {
    let mut scheduler = Scheduler::new();

    // Execution order: fusion reads both sensors and publishes fused pose
    scheduler.add(FusionNode::new()?)
        .order(0)
        .rate(50_u64.hz())
        .build()?;

    scheduler.run()
}

[HORUS] Scheduler running — tick_rate: 50 Hz
[HORUS] Node "Fusion" started (Rt, 50 Hz, budget: 16.0ms, deadline: 19.0ms)
^C
[HORUS] Shutting down...
[HORUS] Node "Fusion" shutdown complete