//! IMU node configuration

use mecha10_core::prelude::*;

/// IMU controller configuration - tagged union
///
/// Select and configure the IMU controller backend:
/// - `mock`: Simulated IMU for testing/development
/// - `mpu6050`: MPU6050 6-DOF IMU (accelerometer + gyroscope)
///
/// # Example JSON
///
/// ```json
/// {
///   "controller": {
///     "type": "mock",
///     "has_magnetometer": false
///   },
///   "update_rate_hz": 100.0
/// }
/// ```
///
/// ```json
/// {
///   "controller": {
///     "type": "mpu6050",
///     "i2c_bus": "/dev/i2c-1",
///     "i2c_address": 104
///   },
///   "update_rate_hz": 100.0
/// }
/// ```
#[derive(Debug, Clone, Serialize, Deserialize)]
#[serde(tag = "type", rename_all = "lowercase")]
pub enum ControllerConfig {
    /// Mock controller for simulation/testing
    Mock(MockControllerConfig),
    /// MPU6050 6-DOF IMU (accelerometer + gyroscope)
    Mpu6050(Mpu6050ControllerConfig),
    // Future: Bno055(Bno055ControllerConfig),
}

impl Default for ControllerConfig {
    fn default() -> Self {
        Self::Mock(MockControllerConfig::default())
    }
}

/// Mock controller configuration
#[derive(Debug, Clone, Serialize, Deserialize)]
pub struct MockControllerConfig {
    /// Whether to simulate magnetometer data
    #[serde(default)]
    pub has_magnetometer: bool,

    /// Auto-calibrate on startup
    #[serde(default = "default_true")]
    pub auto_calibrate: bool,
}

impl Default for MockControllerConfig {
    fn default() -> Self {
        Self {
            has_magnetometer: false,
            auto_calibrate: true,
        }
    }
}

/// MPU6050 controller configuration
#[derive(Debug, Clone, Serialize, Deserialize)]
pub struct Mpu6050ControllerConfig {
    /// I2C bus device path (e.g., "/dev/i2c-1")
    #[serde(default = "default_i2c_bus")]
    pub i2c_bus: String,

    /// I2C address (0x68 or 0x69)
    #[serde(default = "default_i2c_address")]
    pub i2c_address: u8,

    /// Gyroscope sensitivity (250, 500, 1000, 2000 dps)
    #[serde(default = "default_gyro_scale")]
    pub gyro_scale: u16,

    /// Accelerometer sensitivity (2, 4, 8, 16 g)
    #[serde(default = "default_accel_scale")]
    pub accel_scale: u8,

    /// Complementary filter coefficient (0.0-1.0, typical: 0.98)
    #[serde(default = "default_filter_coefficient")]
    pub filter_coefficient: f32,
}

impl Default for Mpu6050ControllerConfig {
    fn default() -> Self {
        Self {
            i2c_bus: default_i2c_bus(),
            i2c_address: default_i2c_address(),
            gyro_scale: default_gyro_scale(),
            accel_scale: default_accel_scale(),
            filter_coefficient: default_filter_coefficient(),
        }
    }
}

/// IMU node configuration
#[derive(Debug, Clone, Deserialize, Serialize)]
pub struct ImuConfig {
    /// Controller backend selection and configuration
    #[serde(default)]
    pub controller: ControllerConfig,

    /// IMU update rate in Hz
    #[serde(default = "default_update_rate")]
    pub update_rate_hz: f32,
}

impl Default for ImuConfig {
    fn default() -> Self {
        Self {
            controller: ControllerConfig::default(),
            update_rate_hz: default_update_rate(),
        }
    }
}

// Default functions
fn default_i2c_bus() -> String {
    "/dev/i2c-1".to_string()
}

fn default_i2c_address() -> u8 {
    0x68
}

fn default_gyro_scale() -> u16 {
    250
}

fn default_accel_scale() -> u8 {
    2
}

fn default_filter_coefficient() -> f32 {
    0.98
}

fn default_update_rate() -> f32 {
    100.0
}

fn default_true() -> bool {
    true
}

#[cfg(test)]
mod tests {
    use super::*;

    #[test]
    fn test_mock_config_deserialize() {
        let json = r#"{
            "controller": {
                "type": "mock",
                "has_magnetometer": true
            },
            "update_rate_hz": 50.0
        }"#;
        let config: ImuConfig = serde_json::from_str(json).unwrap();
        assert!(matches!(config.controller, ControllerConfig::Mock(_)));
        if let ControllerConfig::Mock(mock) = config.controller {
            assert!(mock.has_magnetometer);
        }
        assert!((config.update_rate_hz - 50.0).abs() < 0.001);
    }

    #[test]
    fn test_mpu6050_config_deserialize() {
        let json = r#"{
            "controller": {
                "type": "mpu6050",
                "i2c_bus": "/dev/i2c-1",
                "i2c_address": 104
            }
        }"#;
        let config: ImuConfig = serde_json::from_str(json).unwrap();
        assert!(matches!(config.controller, ControllerConfig::Mpu6050(_)));
        if let ControllerConfig::Mpu6050(mpu) = config.controller {
            assert_eq!(mpu.i2c_address, 104); // 0x68
        }
    }

    #[test]
    fn test_default_config() {
        let config = ImuConfig::default();
        assert!(matches!(config.controller, ControllerConfig::Mock(_)));
        assert!((config.update_rate_hz - 100.0).abs() < 0.001);
    }

    #[test]
    fn test_mpu6050_defaults() {
        let json = r#"{
            "controller": {
                "type": "mpu6050"
            }
        }"#;
        let config: ImuConfig = serde_json::from_str(json).unwrap();
        if let ControllerConfig::Mpu6050(mpu) = config.controller {
            assert_eq!(mpu.i2c_bus, "/dev/i2c-1");
            assert_eq!(mpu.i2c_address, 0x68);
            assert_eq!(mpu.gyro_scale, 250);
            assert_eq!(mpu.accel_scale, 2);
            assert!((mpu.filter_coefficient - 0.98).abs() < 0.001);
        } else {
            panic!("Expected Mpu6050 config");
        }
    }
}
