//! IMU Node
//!
//! This node manages IMU sensors and publishes orientation, angular velocity,
//! and linear acceleration data.
//!
//! # Controller Selection
//!
//! The node uses a factory pattern to create the appropriate controller based
//! on configuration:
//!
//! - `mock`: Simulated IMU for testing/development (default)
//! - `mpu6050`: MPU6050 6-DOF IMU (requires `mpu6050` feature)
//!
//! # Example Configuration
//!
//! ```json
//! {
//!   "controller": {
//!     "type": "mock",
//!     "has_magnetometer": false
//!   },
//!   "update_rate_hz": 100.0
//! }
//! ```
//!
//! # Topics
//!
//! - Publishes: `/imu/data` (ImuData) - IMU sensor data

mod config;
mod factory;

pub use config::{ControllerConfig, ImuConfig, MockControllerConfig, Mpu6050ControllerConfig};
use factory::{create_imu_controller, BoxedImuController};
use mecha10_core::prelude::*;
use mecha10_core::sensor::ImuData;
use mecha10_core::topics::Topic;
use std::time::Duration;

/// IMU node topics
pub mod topics {
    use super::*;

    pub const IMU_DATA: Topic<ImuData> = Topic::new("/imu/data");
}

/// IMU node
///
/// Manages an IMU controller and publishes sensor data at a configured rate.
pub struct ImuNode {
    config: ImuConfig,
    controller: BoxedImuController,
    update_counter: u64,
}

impl std::fmt::Debug for ImuNode {
    fn fmt(&self, f: &mut std::fmt::Formatter<'_>) -> std::fmt::Result {
        f.debug_struct("ImuNode")
            .field("config", &self.config)
            .field("controller", &self.controller.name())
            .field("update_counter", &self.update_counter)
            .finish()
    }
}

#[async_trait]
impl NodeImpl for ImuNode {
    type Config = ImuConfig;

    async fn init(config: Self::Config) -> Result<Self> {
        info!("Initializing IMU node");
        info!("Update rate: {} Hz", config.update_rate_hz);

        // Create controller using factory
        let controller = create_imu_controller(config.controller.clone())
            .await
            .map_err(|e| anyhow::anyhow!("Failed to create IMU controller: {}", e))?;

        info!("IMU controller created: {}", controller.name());

        Ok(Self {
            config,
            controller,
            update_counter: 0,
        })
    }

    async fn run(&mut self, ctx: &Context) -> Result<()> {
        let update_interval = Duration::from_secs_f32(1.0 / self.config.update_rate_hz);
        let mut interval = tokio::time::interval(update_interval);

        // Start controller
        self.controller
            .start()
            .await
            .map_err(|e| anyhow::anyhow!("Failed to start IMU controller: {}", e))?;

        info!("IMU node running at {} Hz", self.config.update_rate_hz);
        info!("Controller: {}", self.controller.name());
        info!("Publishing to: /imu/data");

        loop {
            interval.tick().await;

            // Read IMU data from controller
            let imu_data = self
                .controller
                .read_imu()
                .await
                .map_err(|e| anyhow::anyhow!("Failed to read IMU: {}", e))?;

            // Publish IMU data
            ctx.publish_to(topics::IMU_DATA, &imu_data).await?;

            self.update_counter += 1;

            if self.update_counter % 1000 == 0 {
                debug!("Published {} IMU updates", self.update_counter);
            }
        }
    }
}

/// Run the IMU node
pub async fn run() -> Result<()> {
    // Create context first (needed for config loading)
    let ctx = Context::new("imu").await?;

    // Load config using V2 system with environment-aware file-level fallback
    let config: ImuConfig = ctx.load_node_config("imu").await?;

    // Run the node with Normal priority (sensor data)
    run_node::<ImuNode>(config, ctx, HealthReportingConfig::normal()).await
}
