[package]
name = "imu-reader"
version = "0.1.0"
description = "100Hz IMU sensor with orientation publishing"

use horus::prelude::*;

/// Simplified orientation from integrated gyro
#[derive(Debug, Clone, Copy, Default, Serialize, Deserialize, LogSummary)]
#[repr(C)]
struct Orientation {
    roll: f32,
    pitch: f32,
    yaw: f32,
    timestamp_ns: u64,
}

// ── IMU Node ────────────────────────────────────────────────

struct ImuNode {
    imu_pub: Topic<Imu>,
    orientation_pub: Topic<Orientation>,
    roll: f32,
    pitch: f32,
    yaw: f32,
    tick_count: u64,
}

impl ImuNode {
    fn new() -> Result<Self> {
        Ok(Self {
            imu_pub: Topic::new("imu.raw")?,
            orientation_pub: Topic::new("imu.orientation")?,
            roll: 0.0,
            pitch: 0.0,
            yaw: 0.0,
            tick_count: 0,
        })
    }

    /// Simulate IMU hardware read (replace with real driver)
    fn read_hardware(&self) -> Imu {
        Imu {
            orientation: [1.0, 0.0, 0.0, 0.0],  // identity quaternion (w,x,y,z)
            angular_velocity: [0.01, 0.0, 0.05],  // rad/s (roll, pitch, yaw)
            linear_acceleration: [0.0, 0.0, 9.81], // m/s² (x, y, z)
        }
    }
}

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

    fn tick(&mut self) {
        let imu = self.read_hardware();

        // Publish raw IMU for any subscriber
        self.imu_pub.send(imu);

        // Simple gyro integration (replace with Madgwick/Mahony in production)
        let dt = 1.0 / 100.0; // 100Hz
        self.roll += imu.angular_velocity[0] as f32 * dt;
        self.pitch += imu.angular_velocity[1] as f32 * dt;
        self.yaw += imu.angular_velocity[2] as f32 * dt;

        self.tick_count += 1;

        self.orientation_pub.send(Orientation {
            roll: self.roll,
            pitch: self.pitch,
            yaw: self.yaw,
            timestamp_ns: self.tick_count * 10_000_000, // 10ms per tick
        });
    }
}

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

    // Execution order: IMU reads hardware and publishes
    scheduler.add(ImuNode::new()?)
        .order(0)
        .rate(100_u64.hz())    // 100Hz sensor rate — auto-enables RT
        .build()?;

    scheduler.run()
}

[HORUS] Scheduler running — tick_rate: 100 Hz
[HORUS] Node "ImuReader" started (Rt, 100 Hz, budget: 8.0ms, deadline: 9.5ms)
^C
[HORUS] Shutting down...
[HORUS] Node "ImuReader" shutdown complete