MotorSignal-App/lib/services/sensor_fusion_service.dart

import 'dart:async';
import 'dart:math' as math;
import 'package:flutter_riverpod/flutter_riverpod.dart';
import 'package:sensors_plus/sensors_plus.dart';
import 'calibration_service.dart';

final sensorFusionServiceProvider = Provider<SensorFusionService>((ref) {
  final calibrationService = ref.watch(calibrationServiceProvider);
  final service = SensorFusionService(calibrationService);
  ref.onDispose(() => service.dispose());
  return service;
});

final leanAngleStreamProvider = StreamProvider<double>((ref) {
  return ref.watch(sensorFusionServiceProvider).leanAngleStream;
});

final gForceStreamProvider = StreamProvider<GForceData>((ref) {
  return ref.watch(sensorFusionServiceProvider).gForceStream;
});

class GForceData {
  final double longitudinal; // Accel/braking (G's)
  final double lateral;      // Left/right (G's)
  final double accelX;       // Raw m/s^2
  final double accelY;       // Raw m/s^2
  final double accelZ;       // Raw m/s^2
  
  GForceData({
    required this.longitudinal, 
    required this.lateral,
    required this.accelX,
    required this.accelY,
    required this.accelZ,
  });
}

class SensorFusionService {
  final CalibrationService _calibrationService;

  final _leanAngleController = StreamController<double>.broadcast();
  final _pitchAngleController = StreamController<double>.broadcast();
  final _gForceController = StreamController<GForceData>.broadcast();

  Stream<double> get leanAngleStream => _leanAngleController.stream;
  Stream<double> get pitchAngleStream => _pitchAngleController.stream;
  Stream<GForceData> get gForceStream => _gForceController.stream;

  StreamSubscription? _accelSub;
  StreamSubscription? _gyroSub;
  StreamSubscription? _userAccelSub;

  // Calibration offsets
  double _rollOffset = 0.0;
  double _pitchOffset = 0.0;

  // Sensor fusion state
  double _currentRoll = 0.0; // The lean angle
  double _currentPitch = 0.0;
  DateTime? _lastGyroTime;

  static const double _baseAlpha = 0.94; // Base complementary filter gain
  static const double _gravityConst = 9.80665;

  // Low-pass filter for G-Force to suppress motorcycle vibration spikes
  double _filteredLongitudinalG = 0.0;
  double _filteredLateralG = 0.0;
  static const double _gAlpha = 0.40; // Fast responsive dynamic smoothing

  SensorFusionService(this._calibrationService) {
    _loadInitialCalibration();
    _startSensors();
  }

  Future<void> _loadInitialCalibration() async {
    try {
      final offsets = await _calibrationService.loadCalibrationOffsets();
      _pitchOffset = offsets['pitch'] ?? 0.0;
      _rollOffset = offsets['roll'] ?? 0.0;
    } catch (_) {}
  }

  void _startSensors() {
    _gyroSub = gyroscopeEventStream(samplingPeriod: const Duration(milliseconds: 20)).listen((GyroscopeEvent event) {
      final now = DateTime.now();
      if (_lastGyroTime == null) {
        _lastGyroTime = now;
        return;
      }

      double dt = now.difference(_lastGyroTime!).inMicroseconds / 1000000.0;
      _lastGyroTime = now;

      // Clamp dt to prevent runaway integration during gaps/app pauses
      if (dt > 0.1) dt = 0.1;
      if (dt <= 0.0) return;

      // Integrate gyro (rad/s to degrees)
      // Phone X is pitch rate, Phone Y is roll rate, Phone Z is yaw rate
      final gx = event.x * (180.0 / math.pi);
      final gy = event.y * (180.0 / math.pi);

      _currentPitch = (_currentPitch + gx * dt).clamp(-90.0, 90.0);
      _currentRoll = (_currentRoll - gy * dt).clamp(-90.0, 90.0);
    });

    _accelSub = accelerometerEventStream(samplingPeriod: const Duration(milliseconds: 20)).listen((AccelerometerEvent event) {
      // Calculate total acceleration magnitude
      final norm = math.sqrt(event.x * event.x + event.y * event.y + event.z * event.z);
      if (norm < 0.1) return;

      // Compute static gravity-based angles
      final accelRoll = -math.atan2(event.x, math.sqrt(event.y * event.y + event.z * event.z)) * (180.0 / math.pi);
      final accelPitch = math.atan2(-event.y, event.z) * (180.0 / math.pi);

      // Dynamic acceleration gating:
      // On a motorcycle, accelerometers measure specific force = gravity + vehicle dynamics.
      // During acceleration, braking, or cornering, the apparent gravity vector tilts significantly.
      // Inhibit accelerometer tilt correction when total force deviates from 1.0g or during motion.
      final gMag = norm / _gravityConst;
      final dynamicDelta = (gMag - 1.0).abs();

      // If dynamic force > 0.08g, trust gyro 99.8% to prevent vehicle acceleration from tilting lean/pitch
      double dynamicAlpha = _baseAlpha;
      if (dynamicDelta > 0.08) {
        dynamicAlpha = 0.998;
      } else if (dynamicDelta > 0.04) {
        dynamicAlpha = 0.985;
      }

      _currentRoll = (dynamicAlpha * _currentRoll + (1.0 - dynamicAlpha) * accelRoll).clamp(-90.0, 90.0);
      _currentPitch = (dynamicAlpha * _currentPitch + (1.0 - dynamicAlpha) * accelPitch).clamp(-90.0, 90.0);

      final calibratedLean = (_currentRoll - _rollOffset).clamp(-65.0, 65.0);
      final calibratedPitch = (_currentPitch - _pitchOffset).clamp(-45.0, 45.0);

      _leanAngleController.add(calibratedLean);
      _pitchAngleController.add(calibratedPitch);
    });

    _userAccelSub = userAccelerometerEventStream(samplingPeriod: const Duration(milliseconds: 20)).listen((UserAccelerometerEvent event) {
      // Project phone linear acceleration vector into motorcycle body frame
      // using calibrated pitch and roll attitude angles
      final pitchRad = (_currentPitch - _pitchOffset) * (math.pi / 180.0);
      final rollRad = (_currentRoll - _rollOffset) * (math.pi / 180.0);

      // Forward/longitudinal acceleration (along motorcycle centerline)
      // Positive = Acceleration, Negative = Braking
      final rawLongitudinal = (event.y * math.cos(pitchRad) - event.z * math.sin(pitchRad));
      // Lateral acceleration (cornering)
      final rawLateral = (event.x * math.cos(rollRad) + event.z * math.sin(rollRad));

      double rawLongitudinalG = rawLongitudinal / _gravityConst;
      double rawLateralG = rawLateral / _gravityConst;

      // Dynamic smoothing: suppress engine high-frequency vibration while preserving vehicle dynamics
      _filteredLongitudinalG = _gAlpha * rawLongitudinalG + (1.0 - _gAlpha) * _filteredLongitudinalG;
      _filteredLateralG = _gAlpha * rawLateralG + (1.0 - _gAlpha) * _filteredLateralG;

      _gForceController.add(GForceData(
        longitudinal: _filteredLongitudinalG,
        lateral: _filteredLateralG,
        accelX: event.x,
        accelY: event.y,
        accelZ: event.z,
      ));
    });
  }

  void calibrateUpright() {
    // Zero out the current lean & pitch and persist to storage
    _rollOffset = _currentRoll;
    _pitchOffset = _currentPitch;
    _calibrationService.saveCalibrationOffsets(_pitchOffset, _rollOffset);
  }

  void dispose() {
    _accelSub?.cancel();
    _gyroSub?.cancel();
    _userAccelSub?.cancel();
    _leanAngleController.close();
    _pitchAngleController.close();
    _gForceController.close();
  }
}