//! IMU Node
//!
//! This node manages the IMU driver with configurable driver selection.
//! It wraps the IMU driver (fake or real) and publishes orientation,
//! angular velocity, and linear acceleration data.
//!
//! **Driver Selection:**
//! The node selects between fake (simulation) and real (hardware) drivers
//! based on the `driver` configuration option:
//! - "fake": Uses imu_fake driver (for local testing/development)
//! - "real": Uses real IMU driver (for actual hardware - not implemented yet)
//!
//! Topics:
//! - Publishes: `/imu/data` (ImuData) - IMU sensor data

mod config;

pub use config::{DriverMode, ImuConfig};
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
pub struct ImuNode {
    config: ImuConfig,
    update_counter: u64,
    // Simulated orientation
    roll: f32,
    pitch: f32,
    yaw: f32,
}

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("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!("Driver mode: {:?}", config.driver);
        info!("Update rate: {} Hz", config.update_rate_hz);

        // Log which driver will be used
        match config.driver {
            DriverMode::Fake => {
                info!("Using FAKE IMU driver for simulation/testing");
            }
            DriverMode::Real => {
                warn!("Real IMU driver not implemented yet, falling back to fake driver");
                info!("To use simulation, set driver: 'fake' in config");
            }
        }

        Ok(Self {
            config,
            update_counter: 0,
            roll: 0.0,
            pitch: 0.0,
            yaw: 0.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);

        info!("IMU node running at {} Hz", self.config.update_rate_hz);
        info!("Driver mode: {:?}", self.config.driver);
        info!("Publishing to: /imu/data");

        loop {
            interval.tick().await;

            // Update simulated IMU state
            self.update_state();

            // Publish IMU data
            self.publish_imu_data(ctx).await?;

            self.update_counter += 1;

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

    async fn health_check(&self) -> HealthStatus {
        HealthStatus {
            healthy: true,
            last_check: now_micros(),
            failure_count: 0,
            message: Some(format!("IMU node healthy, {} updates", self.update_counter)),
        }
    }
}

impl ImuNode {
    fn update_state(&mut self) {
        // Simulate small drift in yaw (typical for robots)
        self.yaw += 0.0001;

        // Keep roll and pitch stable (robot is on level ground)
        self.roll = 0.0;
        self.pitch = 0.0;

        // Normalize yaw to [-PI, PI]
        while self.yaw > std::f32::consts::PI {
            self.yaw -= 2.0 * std::f32::consts::PI;
        }
        while self.yaw < -std::f32::consts::PI {
            self.yaw += 2.0 * std::f32::consts::PI;
        }
    }

    async fn publish_imu_data(&self, ctx: &Context) -> Result<()> {
        let data = ImuData {
            timestamp: now_micros(),
            // Orientation (stable, level robot)
            roll: self.roll,
            pitch: self.pitch,
            yaw: self.yaw,
            // Angular velocity (minimal drift)
            angular_velocity_x: 0.0,
            angular_velocity_y: 0.0,
            angular_velocity_z: 0.0001, // Small yaw drift
            // Linear acceleration (gravity + small noise)
            linear_acceleration_x: 0.0,
            linear_acceleration_y: 0.0,
            linear_acceleration_z: 9.81, // Gravity
        };

        ctx.publish_to(topics::IMU_DATA, &data).await?;
        Ok(())
    }
}

/// Run the IMU node with V2 configuration system
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
}
