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

final sensorFusionServiceProvider = Provider<SensorFusionService>((ref) {
  return SensorFusionService();
});

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)
  
  GForceData({required this.longitudinal, required this.lateral});
}

class SensorFusionService {
  final _leanAngleController = StreamController<double>.broadcast();
  final _gForceController = StreamController<GForceData>.broadcast();

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

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

  // Calibration offsets
  double _rollOffset = 0.0;

  // Sensor fusion state
  double _currentRoll = 0.0; // The lean angle
  double _currentPitch = 0.0;
  DateTime? _lastUpdate;
  
  static const double _alpha = 0.96; // Complementary filter constant

  SensorFusionService() {
    _startSensors();
  }

  void _startSensors() {
    _gyroSub = gyroscopeEventStream(samplingPeriod: const Duration(milliseconds: 20)).listen((GyroscopeEvent event) {
      final now = DateTime.now();
      if (_lastUpdate == null) {
        _lastUpdate = now;
        return;
      }
      
      final dt = now.difference(_lastUpdate!).inMicroseconds / 1000000.0;
      _lastUpdate = now;

      // Integrate gyro (rad/s to degrees)
      final gx = event.x * (180.0 / math.pi);
      final gy = event.y * (180.0 / math.pi);
      
      _currentPitch += gx * dt;
      _currentRoll += gy * dt;
    });

    _accelSub = accelerometerEventStream(samplingPeriod: const Duration(milliseconds: 20)).listen((AccelerometerEvent event) {
      // Calculate roll/pitch from accelerometer (accounting for gravity)
      // Assuming phone is mounted roughly vertical or horizontal, we calculate basic roll
      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);

      // Complementary filter: Combine integrated gyro with absolute accel
      if (_lastUpdate != null) {
        _currentRoll = _alpha * _currentRoll + (1.0 - _alpha) * accelRoll;
        _currentPitch = _alpha * _currentPitch + (1.0 - _alpha) * accelPitch;
        
        double calibratedLean = _currentRoll - _rollOffset;
        _leanAngleController.add(calibratedLean);
      }
    });

    _userAccelSub = userAccelerometerEventStream(samplingPeriod: const Duration(milliseconds: 20)).listen((UserAccelerometerEvent event) {
      // User accelerometer removes gravity.
      // Convert m/s^2 to G's
      const gravity = 9.80665;
      
      // We need to rotate these vectors by the current pitch/roll to get true longitudinal/lateral G's 
      // relative to the motorcycle, but for a mounted phone, Y is typically longitudinal if vertical, 
      // or Z if horizontal. This requires mounting orientation knowledge.
      // Assuming a standard gauge-cluster mount (portrait or landscape).
      // For now, we provide the raw components mapped to standard device axes.
      // E.g., device Y is longitudinal (forward/back), device X is lateral.
      
      double longitudinalG = event.y / gravity; 
      double lateralG = event.x / gravity;
      
      // In a real scenario, these would be transformed by the calibration matrix.
      _gForceController.add(GForceData(
        longitudinal: longitudinalG,
        lateral: lateralG,
      ));
    });
  }

  void calibrateUpright() {
    // Zero out the current lean
    _rollOffset = _currentRoll;
  }

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