//! IMU Controller Factory
//!
//! Creates IMU controllers based on configuration.
//! Uses dynamic dispatch to support multiple controller backends.

#[cfg(feature = "mpu6050")]
use crate::config::Mpu6050ControllerConfig;
use crate::config::{ControllerConfig, MockControllerConfig};
use mecha10_controllers::imu::*;
use mecha10_controllers::mock::imu::{MockImuConfig, MockImuController};
use mecha10_controllers::{Controller, ControllerError};
use mecha10_core::sensor::ImuData;
use tracing::info;

/// Boxed IMU controller trait object
pub type BoxedImuController = Box<dyn DynImuController>;

/// Trait for dynamic dispatch of IMU controllers
///
/// This trait provides a subset of `ImuController` that can be used
/// with trait objects for runtime polymorphism.
#[async_trait::async_trait]
pub trait DynImuController: Send + Sync {
    /// Read current IMU data
    async fn read_imu(&mut self) -> Result<ImuData, ControllerError>;

    /// Start the controller
    async fn start(&mut self) -> Result<(), ControllerError>;

    /// Stop the controller
    async fn stop(&mut self) -> Result<(), ControllerError>;

    /// Get controller name for logging
    fn name(&self) -> &'static str;
}

// ============================================================================
// Mock Controller Wrapper
// ============================================================================

/// Wrapper for MockImuController to implement DynImuController
struct MockControllerWrapper {
    inner: MockImuController,
}

#[async_trait::async_trait]
impl DynImuController for MockControllerWrapper {
    async fn read_imu(&mut self) -> Result<ImuData, ControllerError> {
        ImuController::read_imu(&mut self.inner).await
    }

    async fn start(&mut self) -> Result<(), ControllerError> {
        Controller::start(&mut self.inner).await
    }

    async fn stop(&mut self) -> Result<(), ControllerError> {
        Controller::stop(&mut self.inner).await
    }

    fn name(&self) -> &'static str {
        "mock"
    }
}

// ============================================================================
// MPU6050 Controller Wrapper (feature-gated)
// ============================================================================

#[cfg(feature = "mpu6050")]
struct Mpu6050ControllerWrapper {
    inner: mecha10_controllers::mpu6050::Mpu6050Controller,
}

#[cfg(feature = "mpu6050")]
#[async_trait::async_trait]
impl DynImuController for Mpu6050ControllerWrapper {
    async fn read_imu(&mut self) -> Result<ImuData, ControllerError> {
        ImuController::read_imu(&mut self.inner).await
    }

    async fn start(&mut self) -> Result<(), ControllerError> {
        Controller::start(&mut self.inner).await
    }

    async fn stop(&mut self) -> Result<(), ControllerError> {
        Controller::stop(&mut self.inner).await
    }

    fn name(&self) -> &'static str {
        "mpu6050"
    }
}

// ============================================================================
// Factory Function
// ============================================================================

/// Create an IMU controller based on configuration
///
/// # Arguments
/// * `config` - Controller configuration (tagged union)
///
/// # Returns
/// * `Ok(BoxedImuController)` - Initialized controller ready to use
/// * `Err(ControllerError)` - If initialization fails
///
/// # Example
/// ```ignore
/// use mecha10_nodes_imu::config::ControllerConfig;
/// use mecha10_nodes_imu::factory::create_imu_controller;
///
/// let config = ControllerConfig::Mock(Default::default());
/// let controller = create_imu_controller(config).await?;
/// ```
pub async fn create_imu_controller(config: ControllerConfig) -> Result<BoxedImuController, ControllerError> {
    match config {
        ControllerConfig::Mock(cfg) => {
            info!("Creating Mock IMU controller");
            create_mock_controller(cfg).await
        }
        #[cfg(feature = "mpu6050")]
        ControllerConfig::Mpu6050(cfg) => {
            info!("Creating MPU6050 IMU controller");
            create_mpu6050_controller(cfg).await
        }
        #[cfg(not(feature = "mpu6050"))]
        ControllerConfig::Mpu6050(_) => Err(ControllerError::NotSupported(
            "MPU6050 support not compiled. Enable 'mpu6050' feature.".to_string(),
        )),
    }
}

/// Create a mock controller
async fn create_mock_controller(config: MockControllerConfig) -> Result<BoxedImuController, ControllerError> {
    let driver_config = MockImuConfig {
        update_rate_hz: 100.0,
        has_magnetometer: config.has_magnetometer,
        auto_calibrate: config.auto_calibrate,
    };

    let controller = MockImuController::init(driver_config).await?;
    info!("Mock IMU controller initialized");

    Ok(Box::new(MockControllerWrapper { inner: controller }))
}

/// Create an MPU6050 controller
#[cfg(feature = "mpu6050")]
async fn create_mpu6050_controller(config: Mpu6050ControllerConfig) -> Result<BoxedImuController, ControllerError> {
    use mecha10_controllers::mpu6050::{Mpu6050Controller, Mpu6050ControllerConfig as DriverConfig};

    let driver_config = DriverConfig {
        i2c_bus: config.i2c_bus,
        i2c_address: config.i2c_address,
        gyro_scale: config.gyro_scale,
        accel_scale: config.accel_scale,
        filter_coefficient: config.filter_coefficient,
    };

    let controller = Mpu6050Controller::init(driver_config).await?;
    info!("MPU6050 controller initialized");

    Ok(Box::new(Mpu6050ControllerWrapper { inner: controller }))
}
