MicroPythonOS/internal_filesystem/lib/mpos/sensor_manager.py

"""Android-inspired SensorManager for MicroPythonOS.

Provides unified access to IMU sensors (QMI8658, WSEN_ISDS) and other sensors.
Follows singleton pattern with class method delegation.

Example usage:
    from mpos import SensorManager

    # In board init file:
    SensorManager.init(i2c_bus, address=0x6B)

    # In app:
    if SensorManager.is_available():
        accel = SensorManager.get_default_sensor(SensorManager.TYPE_ACCELEROMETER)
        ax, ay, az = SensorManager.read_sensor(accel)  # Returns m/s²

MIT License
Copyright (c) 2024 MicroPythonOS contributors
"""

import time
try:
    import _thread
    _lock = _thread.allocate_lock()
except ImportError:
    _lock = None


# Sensor type constants (matching Android SensorManager)
TYPE_ACCELEROMETER = 1      # Units: m/s² (meters per second squared)
TYPE_GYROSCOPE = 4          # Units: deg/s (degrees per second)
TYPE_TEMPERATURE = 13       # Units: °C (generic, returns first available - deprecated)
TYPE_IMU_TEMPERATURE = 14   # Units: °C (IMU chip temperature)
TYPE_SOC_TEMPERATURE = 15   # Units: °C (MCU/SoC internal temperature)

# mounted_position:
FACING_EARTH = 20 # underside of PCB, like fri3d_2024
FACING_SKY = 21 # top of PCB, like waveshare_esp32_s3_lcd_touch_2 (default)

# Gravity constant for unit conversions
_GRAVITY = 9.80665  # m/s²

IMU_CALIBRATION_FILENAME = "imu_calibration.json"


class Sensor:
    """Sensor metadata (lightweight data class, Android-inspired)."""

    def __init__(self, name, sensor_type, vendor, version, max_range, resolution, power_ma):
        """Initialize sensor metadata.

        Args:
            name: Human-readable sensor name
            sensor_type: Sensor type constant (TYPE_ACCELEROMETER, etc.)
            vendor: Sensor vendor/manufacturer
            version: Driver version
            max_range: Maximum measurement range (with units)
            resolution: Measurement resolution (with units)
            power_ma: Power consumption in mA (or 0 if unknown)
        """
        self.name = name
        self.type = sensor_type
        self.vendor = vendor
        self.version = version
        self.max_range = max_range
        self.resolution = resolution
        self.power = power_ma

    def __repr__(self):
        return f"Sensor({self.name}, type={self.type})"


class SensorManager:
    """
    Centralized sensor management service.
    Implements singleton pattern for unified sensor access.
    
    Usage:
        from mpos import SensorManager
        
        # Initialize
        SensorManager.init(i2c_bus, address=0x6B)
        
        # Get sensor
        accel = SensorManager.get_default_sensor(SensorManager.TYPE_ACCELEROMETER)
        
        # Read sensor
        ax, ay, az = SensorManager.read_sensor(accel)
    """
    
    _instance = None
    
    # Class-level state variables (for testing and singleton pattern)
    _initialized = False
    _imu_driver = None
    _sensor_list = []
    _i2c_bus = None
    _i2c_address = None
    _mounted_position = FACING_SKY
    _has_mcu_temperature = False
    
    # Class-level constants
    TYPE_ACCELEROMETER = TYPE_ACCELEROMETER
    TYPE_GYROSCOPE = TYPE_GYROSCOPE
    TYPE_TEMPERATURE = TYPE_TEMPERATURE
    TYPE_IMU_TEMPERATURE = TYPE_IMU_TEMPERATURE
    TYPE_SOC_TEMPERATURE = TYPE_SOC_TEMPERATURE
    FACING_EARTH = FACING_EARTH
    FACING_SKY = FACING_SKY
    
    def __init__(self):
        """Initialize SensorManager singleton instance."""
        if SensorManager._instance:
            return
        SensorManager._instance = self
    
    @classmethod
    def get(cls):
        """Get or create the singleton instance."""
        if cls._instance is None:
            cls._instance = cls()
        return cls._instance
    
    def init(self, i2c_bus, address=0x6B, mounted_position=FACING_SKY):
        """Initialize SensorManager. MCU temperature initializes immediately, IMU initializes on first use.

        Args:
            i2c_bus: machine.I2C instance (can be None if only MCU temperature needed)
            address: I2C address (default 0x6B for both QMI8658 and WSEN_ISDS)

        Returns:
            bool: True if initialized successfully
        """
        self._i2c_bus = i2c_bus
        self._i2c_address = address
        self._mounted_position = mounted_position

        # Initialize MCU temperature sensor immediately (fast, no I2C needed)
        try:
            import esp32
            _ = esp32.mcu_temperature()
            self._has_mcu_temperature = True
            self._register_mcu_temperature_sensor()
        except:
            pass

        self._initialized = True
        return True
    
    def _ensure_imu_initialized(self):
        """Perform IMU initialization on first use (lazy initialization).

        Tries to detect QMI8658 (chip ID 0x05) or WSEN_ISDS (WHO_AM_I 0x6A).
        Loads calibration from SharedPreferences if available.

        Returns:
            bool: True if IMU detected and initialized successfully
        """
        if not self._initialized or self._imu_driver is not None:
            return self._imu_driver is not None

        # Try QMI8658 first (Waveshare board)
        if self._i2c_bus:
            try:
                from drivers.imu_sensor.qmi8658 import QMI8658
                chip_id = self._i2c_bus.readfrom_mem(self._i2c_address, 0x00, 1)[0]  # PARTID register
                if chip_id == 0x05:  # QMI8685_PARTID
                    self._imu_driver = _QMI8658Driver(self._i2c_bus, self._i2c_address)
                    self._register_qmi8658_sensors()
                    self._load_calibration()
                    return True
            except:
                pass

            # Try WSEN_ISDS (fri3d_2024) or LSM6DSO (fri3d_2026)
            try:
                from drivers.imu_sensor.wsen_isds import Wsen_Isds
                chip_id = self._i2c_bus.readfrom_mem(self._i2c_address, 0x0F, 1)[0]  # WHO_AM_I register - could also use Wsen_Isds.get_chip_id()
                if chip_id == 0x6A or chip_id == 0x6C:  # WSEN_ISDS WHO_AM_I 0x6A (Fri3d 2024) or 0x6C (Fri3d 2026)
                    self._imu_driver = _WsenISDSDriver(self._i2c_bus, self._i2c_address)
                    self._register_wsen_isds_sensors()
                    self._load_calibration()
                    return True
            except:
                pass

        return False
    
    def is_available(self):
        """Check if sensors are available.

        Does NOT trigger IMU initialization (to avoid boot-time initialization).
        Use get_default_sensor() or read_sensor() to lazily initialize IMU.

        Returns:
            bool: True if SensorManager is initialized (may only have MCU temp, not IMU)
        """
        return self._initialized
    
    def get_sensor_list(self):
        """Get list of all available sensors.

        Performs lazy IMU initialization on first call.

        Returns:
            list: List of Sensor objects
        """
        self._ensure_imu_initialized()
        return self._sensor_list.copy() if self._sensor_list else []
    
    def get_default_sensor(self, sensor_type):
        """Get default sensor of given type.

        Performs lazy IMU initialization on first call.

        Args:
            sensor_type: Sensor type constant (TYPE_ACCELEROMETER, etc.)

        Returns:
            Sensor object or None if not available
        """
        # Only initialize IMU if SensorManager has been initialized and requesting IMU sensor types
        if self._initialized and sensor_type in (TYPE_ACCELEROMETER, TYPE_GYROSCOPE):
            self._ensure_imu_initialized()

        for sensor in self._sensor_list:
            if sensor.type == sensor_type:
                return sensor
        return None
    
    def read_sensor(self, sensor):
        """Read sensor data synchronously.

        Performs lazy IMU initialization on first call for IMU sensors.

        Args:
            sensor: Sensor object from get_default_sensor()

        Returns:
            For motion sensors: tuple (x, y, z) in appropriate units
            For scalar sensors: single value
            None if sensor not available or error
        """
        if sensor is None:
            return None

        # Only initialize IMU if reading IMU sensor
        if sensor.type in (TYPE_ACCELEROMETER, TYPE_GYROSCOPE):
            self._ensure_imu_initialized()

        if _lock:
            _lock.acquire()

        try:
            # Retry logic for "sensor data not ready" (WSEN_ISDS needs time after init)
            max_retries = 3
            retry_delay_ms = 20  # Wait 20ms between retries

            for attempt in range(max_retries):
                try:
                    if sensor.type == TYPE_ACCELEROMETER:
                        if self._imu_driver:
                            ax, ay, az = self._imu_driver.read_acceleration()
                            if self._mounted_position == FACING_EARTH:
                                az *= -1
                            return (ax, ay, az)
                    elif sensor.type == TYPE_GYROSCOPE:
                        if self._imu_driver:
                            return self._imu_driver.read_gyroscope()
                    elif sensor.type == TYPE_IMU_TEMPERATURE:
                        if self._imu_driver:
                            return self._imu_driver.read_temperature()
                    elif sensor.type == TYPE_SOC_TEMPERATURE:
                        if self._has_mcu_temperature:
                            import esp32
                            return esp32.mcu_temperature()
                    elif sensor.type == TYPE_TEMPERATURE:
                        # Generic temperature - return first available (backward compatibility)
                        if self._imu_driver:
                            temp = self._imu_driver.read_temperature()
                            if temp is not None:
                                return temp
                        if self._has_mcu_temperature:
                            import esp32
                            return esp32.mcu_temperature()
                    return None
                except Exception as e:
                    error_msg = str(e)
                    # Retry if sensor data not ready, otherwise fail immediately
                    if "data not ready" in error_msg and attempt < max_retries - 1:
                        import time
                        time.sleep_ms(retry_delay_ms)
                        continue
                    else:
                        return None

            return None
        finally:
            if _lock:
                _lock.release()
    
    def calibrate_sensor(self, sensor, samples=100):
        """Calibrate sensor and save to SharedPreferences.

        Performs lazy IMU initialization on first call.
        Device must be stationary for accelerometer/gyroscope calibration.

        Args:
            sensor: Sensor object to calibrate
            samples: Number of samples to average (default 100)

        Returns:
            tuple: Calibration offsets (x, y, z) or None if failed
        """
        self._ensure_imu_initialized()
        if not self.is_available() or sensor is None:
            return None

        if _lock:
            _lock.acquire()

        try:
            if sensor.type == TYPE_ACCELEROMETER:
                offsets = self._imu_driver.calibrate_accelerometer(samples)
            elif sensor.type == TYPE_GYROSCOPE:
                offsets = self._imu_driver.calibrate_gyroscope(samples)
            else:
                return None

            if offsets:
                self._save_calibration()

            return offsets
        except Exception as e:
            print(f"[SensorManager] Calibration error: {e}")
            return None
        finally:
            if _lock:
                _lock.release()
    
    def check_calibration_quality(self, samples=50):
        """Check quality of current calibration.

        Performs lazy IMU initialization on first call.

        Args:
            samples: Number of samples to collect (default 50)

        Returns:
            dict with:
                - accel_mean: (x, y, z) mean values in m/s²
                - accel_variance: (x, y, z) variance values
                - gyro_mean: (x, y, z) mean values in deg/s
                - gyro_variance: (x, y, z) variance values
                - quality_score: float 0.0-1.0 (1.0 = perfect)
                - quality_rating: string ("Good", "Fair", "Poor")
                - issues: list of strings describing problems
            None if IMU not available
        """
        self._ensure_imu_initialized()
        if not self.is_available():
            return None

        # Don't acquire lock here - let read_sensor() handle it per-read
        # (avoids deadlock since read_sensor also acquires the lock)
        try:
            accel = self.get_default_sensor(TYPE_ACCELEROMETER)
            gyro = self.get_default_sensor(TYPE_GYROSCOPE)

            # Collect samples
            accel_samples = [[], [], []]  # x, y, z lists
            gyro_samples = [[], [], []]

            for _ in range(samples):
                if accel:
                    data = self.read_sensor(accel)
                    if data:
                        ax, ay, az = data
                        accel_samples[0].append(ax)
                        accel_samples[1].append(ay)
                        accel_samples[2].append(az)
                if gyro:
                    data = self.read_sensor(gyro)
                    if data:
                        gx, gy, gz = data
                        gyro_samples[0].append(gx)
                        gyro_samples[1].append(gy)
                        gyro_samples[2].append(gz)
                time.sleep_ms(10)

            # Calculate statistics using helper
            accel_stats = [_calc_mean_variance(s) for s in accel_samples]
            gyro_stats = [_calc_mean_variance(s) for s in gyro_samples]

            accel_mean = tuple(s[0] for s in accel_stats)
            accel_variance = tuple(s[1] for s in accel_stats)
            gyro_mean = tuple(s[0] for s in gyro_stats)
            gyro_variance = tuple(s[1] for s in gyro_stats)

            # Calculate quality score (0.0 - 1.0)
            issues = []
            scores = []

            # Check accelerometer
            if accel:
                # Variance check (lower is better)
                accel_max_variance = max(accel_variance)
                variance_score = max(0.0, 1.0 - (accel_max_variance / 1.0))  # 1.0 m/s² variance threshold
                scores.append(variance_score)
                if accel_max_variance > 0.5:
                    issues.append(f"High accelerometer variance: {accel_max_variance:.3f} m/s²")

                # Expected values check (X≈0, Y≈0, Z≈9.8)
                ax, ay, az = accel_mean
                xy_error = (abs(ax) + abs(ay)) / 2.0
                z_error = abs(az - _GRAVITY)
                expected_score = max(0.0, 1.0 - ((xy_error + z_error) / 5.0))  # 5.0 m/s² error threshold
                scores.append(expected_score)
                if xy_error > 1.0:
                    issues.append(f"Accel X/Y not near zero: X={ax:.2f}, Y={ay:.2f} m/s²")
                if z_error > 1.0:
                    issues.append(f"Accel Z not near 9.8: Z={az:.2f} m/s²")

            # Check gyroscope
            if gyro:
                # Variance check
                gyro_max_variance = max(gyro_variance)
                variance_score = max(0.0, 1.0 - (gyro_max_variance / 10.0))  # 10 deg/s variance threshold
                scores.append(variance_score)
                if gyro_max_variance > 5.0:
                    issues.append(f"High gyroscope variance: {gyro_max_variance:.3f} deg/s")

                # Expected values check (all ≈0)
                gx, gy, gz = gyro_mean
                error = (abs(gx) + abs(gy) + abs(gz)) / 3.0
                expected_score = max(0.0, 1.0 - (error / 10.0))  # 10 deg/s error threshold
                scores.append(expected_score)
                if error > 2.0:
                    issues.append(f"Gyro not near zero: X={gx:.2f}, Y={gy:.2f}, Z={gz:.2f} deg/s")

            # Overall quality score
            quality_score = sum(scores) / len(scores) if scores else 0.0

            # Rating
            if quality_score >= 0.8:
                quality_rating = "Good"
            elif quality_score >= 0.5:
                quality_rating = "Fair"
            else:
                quality_rating = "Poor"

            return {
                'accel_mean': accel_mean,
                'accel_variance': accel_variance,
                'gyro_mean': gyro_mean,
                'gyro_variance': gyro_variance,
                'quality_score': quality_score,
                'quality_rating': quality_rating,
                'issues': issues
            }

        except Exception as e:
            print(f"[SensorManager] Error checking calibration quality: {e}")
            return None
    
    def check_stationarity(self, samples=30, variance_threshold_accel=0.5, variance_threshold_gyro=5.0):
        """Check if device is stationary (required for calibration).

        Args:
            samples: Number of samples to collect (default 30)
            variance_threshold_accel: Max acceptable accel variance in m/s² (default 0.5)
            variance_threshold_gyro: Max acceptable gyro variance in deg/s (default 5.0)

        Returns:
            dict with:
                - is_stationary: bool
                - accel_variance: max variance across axes
                - gyro_variance: max variance across axes
                - message: string describing result
            None if IMU not available
        """
        self._ensure_imu_initialized()
        if not self.is_available():
            return None

        # Don't acquire lock here - let read_sensor() handle it per-read
        # (avoids deadlock since read_sensor also acquires the lock)
        try:
            accel = self.get_default_sensor(TYPE_ACCELEROMETER)
            gyro = self.get_default_sensor(TYPE_GYROSCOPE)

            # Collect samples
            accel_samples = [[], [], []]
            gyro_samples = [[], [], []]

            for _ in range(samples):
                if accel:
                    data = self.read_sensor(accel)
                    if data:
                        ax, ay, az = data
                        accel_samples[0].append(ax)
                        accel_samples[1].append(ay)
                        accel_samples[2].append(az)
                if gyro:
                    data = self.read_sensor(gyro)
                    if data:
                        gx, gy, gz = data
                        gyro_samples[0].append(gx)
                        gyro_samples[1].append(gy)
                        gyro_samples[2].append(gz)
                time.sleep_ms(10)

            # Calculate variance using helper
            accel_var = [_calc_variance(s) for s in accel_samples]
            gyro_var = [_calc_variance(s) for s in gyro_samples]

            max_accel_var = max(accel_var) if accel_var else 0.0
            max_gyro_var = max(gyro_var) if gyro_var else 0.0

            # Check thresholds
            accel_stationary = max_accel_var < variance_threshold_accel
            gyro_stationary = max_gyro_var < variance_threshold_gyro
            is_stationary = accel_stationary and gyro_stationary

            # Generate message
            if is_stationary:
                message = "Device is stationary - ready to calibrate"
            else:
                problems = []
                if not accel_stationary:
                    problems.append(f"movement detected (accel variance: {max_accel_var:.3f})")
                if not gyro_stationary:
                    problems.append(f"rotation detected (gyro variance: {max_gyro_var:.3f})")
                message = f"Device NOT stationary: {', '.join(problems)}"

            return {
                'is_stationary': is_stationary,
                'accel_variance': max_accel_var,
                'gyro_variance': max_gyro_var,
                'message': message
            }

        except Exception as e:
            print(f"[SensorManager] Error checking stationarity: {e}")
            return None
    
    def _register_qmi8658_sensors(self):
        """Register QMI8658 sensors in sensor list."""
        self._sensor_list = [
            Sensor(
                name="QMI8658 Accelerometer",
                sensor_type=TYPE_ACCELEROMETER,
                vendor="QST Corporation",
                version=1,
                max_range="±8G (78.4 m/s²)",
                resolution="0.0024 m/s²",
                power_ma=0.2
            ),
            Sensor(
                name="QMI8658 Gyroscope",
                sensor_type=TYPE_GYROSCOPE,
                vendor="QST Corporation",
                version=1,
                max_range="±256 deg/s",
                resolution="0.002 deg/s",
                power_ma=0.7
            ),
            Sensor(
                name="QMI8658 Temperature",
                sensor_type=TYPE_IMU_TEMPERATURE,
                vendor="QST Corporation",
                version=1,
                max_range="-40°C to +85°C",
                resolution="0.004°C",
                power_ma=0
            )
        ]
    
    def _register_wsen_isds_sensors(self):
        """Register WSEN_ISDS sensors in sensor list."""
        self._sensor_list = [
            Sensor(
                name="WSEN_ISDS Accelerometer",
                sensor_type=TYPE_ACCELEROMETER,
                vendor="Würth Elektronik",
                version=1,
                max_range="±8G (78.4 m/s²)",
                resolution="0.0024 m/s²",
                power_ma=0.2
            ),
            Sensor(
                name="WSEN_ISDS Gyroscope",
                sensor_type=TYPE_GYROSCOPE,
                vendor="Würth Elektronik",
                version=1,
                max_range="±500 deg/s",
                resolution="0.0175 deg/s",
                power_ma=0.65
            ),
            Sensor(
                name="WSEN_ISDS Temperature",
                sensor_type=TYPE_IMU_TEMPERATURE,
                vendor="Würth Elektronik",
                version=1,
                max_range="-40°C to +85°C",
                resolution="0.004°C",
                power_ma=0
            )
        ]
    
    def _register_mcu_temperature_sensor(self):
        """Register MCU internal temperature sensor in sensor list."""
        self._sensor_list.append(
            Sensor(
                name="ESP32 MCU Temperature",
                sensor_type=TYPE_SOC_TEMPERATURE,
                vendor="Espressif",
                version=1,
                max_range="-40°C to +125°C",
                resolution="0.5°C",
                power_ma=0
            )
        )
    
    def _load_calibration(self):
        """Load calibration from SharedPreferences (with migration support)."""
        if not self._imu_driver:
            return

        try:
            from mpos.config import SharedPreferences

            # Try NEW location first
            prefs_new = SharedPreferences("com.micropythonos.settings", filename=IMU_CALIBRATION_FILENAME)
            accel_offsets = prefs_new.get_list("accel_offsets")
            gyro_offsets = prefs_new.get_list("gyro_offsets")

            if accel_offsets or gyro_offsets:
                self._imu_driver.set_calibration(accel_offsets, gyro_offsets)
        except:
            pass
    
    def _save_calibration(self):
        """Save calibration to SharedPreferences."""
        if not self._imu_driver:
            return

        try:
            from mpos.config import SharedPreferences
            prefs = SharedPreferences("com.micropythonos.settings", filename=IMU_CALIBRATION_FILENAME)
            editor = prefs.edit()

            cal = self._imu_driver.get_calibration()
            editor.put_list("accel_offsets", list(cal['accel_offsets']))
            editor.put_list("gyro_offsets", list(cal['gyro_offsets']))
            editor.commit()
        except:
            pass


# ============================================================================
# Helper functions for calibration quality checking
# ============================================================================

def _calc_mean_variance(samples_list):
    """Calculate mean and variance for a list of samples."""
    if not samples_list:
        return 0.0, 0.0
    n = len(samples_list)
    mean = sum(samples_list) / n
    variance = sum((x - mean) ** 2 for x in samples_list) / n
    return mean, variance


def _calc_variance(samples_list):
    """Calculate variance for a list of samples."""
    if not samples_list:
        return 0.0
    n = len(samples_list)
    mean = sum(samples_list) / n
    return sum((x - mean) ** 2 for x in samples_list) / n


# ============================================================================
# Internal driver abstraction layer
# ============================================================================

class _IMUDriver:
    """Base class for IMU drivers (internal use only)."""

    def read_acceleration(self):
        """Returns (x, y, z) in m/s²"""
        raise NotImplementedError

    def read_gyroscope(self):
        """Returns (x, y, z) in deg/s"""
        raise NotImplementedError

    def read_temperature(self):
        """Returns temperature in °C"""
        raise NotImplementedError

    def calibrate_accelerometer(self, samples):
        """Calibrate accel, return (x, y, z) offsets in m/s²"""
        raise NotImplementedError

    def calibrate_gyroscope(self, samples):
        """Calibrate gyro, return (x, y, z) offsets in deg/s"""
        raise NotImplementedError

    def get_calibration(self):
        """Return dict with 'accel_offsets' and 'gyro_offsets' keys"""
        raise NotImplementedError

    def set_calibration(self, accel_offsets, gyro_offsets):
        """Set calibration offsets from saved values"""
        raise NotImplementedError


class _QMI8658Driver(_IMUDriver):
    """Wrapper for QMI8658 IMU (Waveshare board)."""

    def __init__(self, i2c_bus, address):
        from drivers.imu_sensor.qmi8658 import QMI8658
        # QMI8658 scale constants (can't import const() values)
        _ACCELSCALE_RANGE_8G = 0b10
        _GYROSCALE_RANGE_256DPS = 0b100
        self.sensor = QMI8658(
            i2c_bus,
            address=address,
            accel_scale=_ACCELSCALE_RANGE_8G,
            gyro_scale=_GYROSCALE_RANGE_256DPS
        )
        # Software calibration offsets (QMI8658 has no built-in calibration)
        self.accel_offset = [0.0, 0.0, 0.0]
        self.gyro_offset = [0.0, 0.0, 0.0]

    def read_acceleration(self):
        """Read acceleration in m/s² (converts from G)."""
        ax, ay, az = self.sensor.acceleration
        # Convert G to m/s² and apply calibration
        return (
            (ax * _GRAVITY) - self.accel_offset[0],
            (ay * _GRAVITY) - self.accel_offset[1],
            (az * _GRAVITY) - self.accel_offset[2]
        )

    def read_gyroscope(self):
        """Read gyroscope in deg/s (already in correct units)."""
        gx, gy, gz = self.sensor.gyro
        # Apply calibration
        return (
            gx - self.gyro_offset[0],
            gy - self.gyro_offset[1],
            gz - self.gyro_offset[2]
        )

    def read_temperature(self):
        """Read temperature in °C."""
        return self.sensor.temperature

    def calibrate_accelerometer(self, samples):
        """Calibrate accelerometer (device must be stationary)."""
        sum_x, sum_y, sum_z = 0.0, 0.0, 0.0

        for _ in range(samples):
            ax, ay, az = self.sensor.acceleration
            sum_x += ax * _GRAVITY
            sum_y += ay * _GRAVITY
            sum_z += az * _GRAVITY
            time.sleep_ms(10)

        if FACING_EARTH == FACING_EARTH:
            sum_z *= -1

        # Average offsets (assuming Z-axis should read +9.8 m/s²)
        self.accel_offset[0] = sum_x / samples
        self.accel_offset[1] = sum_y / samples
        self.accel_offset[2] = (sum_z / samples) - _GRAVITY

        return tuple(self.accel_offset)

    def calibrate_gyroscope(self, samples):
        """Calibrate gyroscope (device must be stationary)."""
        sum_x, sum_y, sum_z = 0.0, 0.0, 0.0

        for _ in range(samples):
            gx, gy, gz = self.sensor.gyro
            sum_x += gx
            sum_y += gy
            sum_z += gz
            time.sleep_ms(10)

        # Average offsets (should be 0 when stationary)
        self.gyro_offset[0] = sum_x / samples
        self.gyro_offset[1] = sum_y / samples
        self.gyro_offset[2] = sum_z / samples

        return tuple(self.gyro_offset)

    def get_calibration(self):
        """Get current calibration."""
        return {
            'accel_offsets': self.accel_offset,
            'gyro_offsets': self.gyro_offset
        }

    def set_calibration(self, accel_offsets, gyro_offsets):
        """Set calibration from saved values."""
        if accel_offsets:
            self.accel_offset = list(accel_offsets)
        if gyro_offsets:
            self.gyro_offset = list(gyro_offsets)


class _WsenISDSDriver(_IMUDriver):
    """Wrapper for WSEN_ISDS IMU (Fri3d badge)."""

    def __init__(self, i2c_bus, address):
        from drivers.imu_sensor.wsen_isds import Wsen_Isds
        self.sensor = Wsen_Isds(
            i2c_bus,
            address=address,
            acc_range="8g",
            acc_data_rate="104Hz",
            gyro_range="500dps",
            gyro_data_rate="104Hz"
        )
        # Software calibration offsets
        self.accel_offset = [0.0, 0.0, 0.0]
        self.gyro_offset = [0.0, 0.0, 0.0]

    def read_acceleration(self):
        """Read acceleration in m/s² (converts from mg)."""
        ax, ay, az = self.sensor._read_raw_accelerations()

        # Convert G to m/s² and apply calibration
        return (
            ((ax / 1000) * _GRAVITY) - self.accel_offset[0],
            ((ay / 1000) * _GRAVITY) - self.accel_offset[1],
            ((az / 1000) * _GRAVITY) - self.accel_offset[2]
        )

    def read_gyroscope(self):
        """Read gyroscope in deg/s (converts from mdps)."""
        gx, gy, gz = self.sensor.read_angular_velocities()
        # Convert mdps to deg/s and apply calibration
        return (
            gx / 1000.0 - self.gyro_offset[0],
            gy / 1000.0 - self.gyro_offset[1],
            gz / 1000.0 - self.gyro_offset[2]
        )

    def read_temperature(self):
        return self.sensor.temperature

    def calibrate_accelerometer(self, samples):
        """Calibrate accelerometer (device must be stationary)."""
        sum_x, sum_y, sum_z = 0.0, 0.0, 0.0

        for _ in range(samples):
            ax, ay, az = self.sensor._read_raw_accelerations()
            sum_x += (ax / 1000.0) * _GRAVITY
            sum_y += (ay / 1000.0) * _GRAVITY
            sum_z += (az / 1000.0) * _GRAVITY
            time.sleep_ms(10)

        print(f"sumz: {sum_z}")
        z_offset = 0
        if FACING_EARTH == FACING_EARTH:
            sum_z *= -1
        print(f"sumz: {sum_z}")

        # Average offsets (assuming Z-axis should read +9.8 m/s²)
        self.accel_offset[0] = sum_x / samples
        self.accel_offset[1] = sum_y / samples
        self.accel_offset[2] = (sum_z / samples) - _GRAVITY
        print(f"offsets: {self.accel_offset}")

        return tuple(self.accel_offset)

    def calibrate_gyroscope(self, samples):
        """Calibrate gyroscope (device must be stationary)."""
        sum_x, sum_y, sum_z = 0.0, 0.0, 0.0

        for _ in range(samples):
            gx, gy, gz = self.sensor.read_angular_velocities()
            sum_x += gx / 1000.0
            sum_y += gy / 1000.0
            sum_z += gz / 1000.0
            time.sleep_ms(10)

        # Average offsets (should be 0 when stationary)
        self.gyro_offset[0] = sum_x / samples
        self.gyro_offset[1] = sum_y / samples
        self.gyro_offset[2] = sum_z / samples

        return tuple(self.gyro_offset)

    def get_calibration(self):
        """Get current calibration."""
        return {
            'accel_offsets': self.accel_offset,
            'gyro_offsets': self.gyro_offset
        }

    def set_calibration(self, accel_offsets, gyro_offsets):
        """Set calibration from saved values."""
        if accel_offsets:
            self.accel_offset = list(accel_offsets)
        if gyro_offsets:
            self.gyro_offset = list(gyro_offsets)


# ============================================================================
# Class method delegation (at module level)
# ============================================================================

_original_methods = {}
_methods_to_delegate = [
    'init', 'is_available', 'get_sensor_list', 'get_default_sensor',
    'read_sensor', 'calibrate_sensor', 'check_calibration_quality',
    'check_stationarity'
]

for method_name in _methods_to_delegate:
    _original_methods[method_name] = getattr(SensorManager, method_name)

def _make_class_method(method_name):
    """Create a class method that delegates to the singleton instance."""
    original_method = _original_methods[method_name]
    
    @classmethod
    def class_method(cls, *args, **kwargs):
        instance = cls.get()
        return original_method(instance, *args, **kwargs)
    
    return class_method

for method_name in _methods_to_delegate:
    setattr(SensorManager, method_name, _make_class_method(method_name))