mecha10_controllers/mock/

imu.rs

//! Mock IMU controller for testing

use crate::imu::*;
use crate::{Controller, ControllerCapabilities, ControllerError, ControllerHealth, ControllerState};
use async_trait::async_trait;
use mecha10_core::sensor::ImuData;
use std::time::{SystemTime, UNIX_EPOCH};

/// Mock IMU controller configuration
#[derive(Debug, Clone)]
pub struct MockImuConfig {
    pub update_rate_hz: f32,
    pub has_magnetometer: bool,
    pub auto_calibrate: bool,
}

impl Default for MockImuConfig {
    fn default() -> Self {
        Self {
            update_rate_hz: 100.0,
            has_magnetometer: true,
            auto_calibrate: true,
        }
    }
}

/// Mock IMU controller for testing
///
/// Generates synthetic IMU data with simulated motion.
pub struct MockImuController {
    config: MockImuConfig,
    state: ControllerState,
    calibration: CalibrationStatus,
    sample_count: u64,
    orientation: (f32, f32, f32), // (roll, pitch, yaw)
}

#[async_trait]
impl Controller for MockImuController {
    type Config = MockImuConfig;
    type Error = ControllerError;

    async fn init(config: Self::Config) -> Result<Self, Self::Error> {
        let calibration = if config.auto_calibrate {
            CalibrationStatus {
                system: CalibrationLevel::FullyCalibrated,
                accelerometer: CalibrationLevel::FullyCalibrated,
                gyroscope: CalibrationLevel::FullyCalibrated,
                magnetometer: if config.has_magnetometer {
                    Some(CalibrationLevel::FullyCalibrated)
                } else {
                    None
                },
            }
        } else {
            CalibrationStatus::default()
        };

        Ok(Self {
            config,
            state: ControllerState::Initialized,
            calibration,
            sample_count: 0,
            orientation: (0.0, 0.0, 0.0),
        })
    }

    async fn start(&mut self) -> Result<(), Self::Error> {
        self.state = ControllerState::Running;
        Ok(())
    }

    async fn stop(&mut self) -> Result<(), Self::Error> {
        self.state = ControllerState::Stopped;
        Ok(())
    }

    async fn health_check(&self) -> ControllerHealth {
        match self.state {
            ControllerState::Running => {
                if self.calibration.is_fully_calibrated() {
                    ControllerHealth::Healthy
                } else {
                    ControllerHealth::Degraded {
                        reason: "Not fully calibrated".to_string(),
                    }
                }
            }
            ControllerState::Error => ControllerHealth::Unhealthy {
                reason: "Mock error".to_string(),
            },
            _ => ControllerHealth::Unknown,
        }
    }

    fn capabilities(&self) -> ControllerCapabilities {
        ControllerCapabilities::new("imu", "mock")
            .with_vendor("Mecha10")
            .with_model("Mock IMU")
            .with_feature("magnetometer", self.config.has_magnetometer)
            .with_feature("absolute_orientation", self.config.has_magnetometer)
            .with_feature("temperature", true)
    }
}

#[async_trait]
impl ImuController for MockImuController {
    async fn read_imu(&mut self) -> Result<ImuData, Self::Error> {
        if self.state != ControllerState::Running {
            return Err(ControllerError::InvalidState("IMU not running".to_string()));
        }

        self.sample_count += 1;

        let timestamp = SystemTime::now().duration_since(UNIX_EPOCH).unwrap().as_micros() as u64;

        // Simulate slow rotation over time
        let time_sec = (timestamp as f64) / 1_000_000.0;
        self.orientation.2 = (time_sec * 0.1) as f32; // Slow yaw rotation

        Ok(ImuData {
            timestamp,
            roll: self.orientation.0,
            pitch: self.orientation.1,
            yaw: self.orientation.2,
            angular_velocity_x: 0.0,
            angular_velocity_y: 0.0,
            angular_velocity_z: 0.1, // Constant slow rotation
            linear_acceleration_x: 0.0,
            linear_acceleration_y: 0.0,
            linear_acceleration_z: 9.81, // Gravity
        })
    }

    fn calibration_status(&self) -> CalibrationStatus {
        self.calibration.clone()
    }

    async fn calibrate(&mut self) -> Result<CalibrationData, Self::Error> {
        // Simulate calibration
        self.calibration = CalibrationStatus {
            system: CalibrationLevel::FullyCalibrated,
            accelerometer: CalibrationLevel::FullyCalibrated,
            gyroscope: CalibrationLevel::FullyCalibrated,
            magnetometer: if self.config.has_magnetometer {
                Some(CalibrationLevel::FullyCalibrated)
            } else {
                None
            },
        };

        Ok(CalibrationData {
            timestamp: SystemTime::now().duration_since(UNIX_EPOCH).unwrap().as_micros() as u64,
            model: "Mock IMU".to_string(),
            accel_offset: [0.01, -0.02, 0.03],
            accel_radius: Some(1000.0),
            gyro_offset: [0.001, -0.001, 0.002],
            mag_offset: if self.config.has_magnetometer {
                Some([10.0, -5.0, 15.0])
            } else {
                None
            },
            mag_radius: if self.config.has_magnetometer {
                Some(500.0)
            } else {
                None
            },
            raw_data: None,
        })
    }

    async fn save_calibration(&self, _path: &str) -> Result<(), Self::Error> {
        // Mock save - do nothing
        Ok(())
    }

    async fn load_calibration(&mut self, _path: &str) -> Result<(), Self::Error> {
        // Mock load - just mark as calibrated
        self.calibration = CalibrationStatus {
            system: CalibrationLevel::FullyCalibrated,
            accelerometer: CalibrationLevel::FullyCalibrated,
            gyroscope: CalibrationLevel::FullyCalibrated,
            magnetometer: if self.config.has_magnetometer {
                Some(CalibrationLevel::FullyCalibrated)
            } else {
                None
            },
        };
        Ok(())
    }

    async fn reset_fusion(&mut self) -> Result<(), Self::Error> {
        self.orientation = (0.0, 0.0, 0.0);
        Ok(())
    }

    async fn temperature(&self) -> Option<f32> {
        Some(25.0 + (self.sample_count % 10) as f32 * 0.1)
    }
}

impl MockImuController {
    /// Set orientation manually for testing
    pub fn set_orientation(&mut self, roll: f32, pitch: f32, yaw: f32) {
        self.orientation = (roll, pitch, yaw);
    }

    /// Get sample count
    pub fn sample_count(&self) -> u64 {
        self.sample_count
    }

    /// Reset sample count
    pub fn reset_sample_count(&mut self) {
        self.sample_count = 0;
    }
}