import 'dart:async';
import 'dart:math' as math;
import 'package:flutter/widgets.dart';
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 with WidgetsBindingObserver {
  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;

  bool _isLandscape = false;
  bool _isLandscapeLeft = true;

  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) {
    WidgetsBinding.instance.addObserver(this);
    _updateDisplayOrientation();
    _loadInitialCalibration();
    _startSensors();
  }

  @override
  void didChangeMetrics() {
    _updateDisplayOrientation();
  }

  void _updateDisplayOrientation() {
    try {
      final view = WidgetsBinding.instance.platformDispatcher.views.firstOrNull;
      if (view != null) {
        final size = view.physicalSize;
        _isLandscape = size.width > size.height;
      }
    } catch (_) {}
  }

  void setOrientation(bool isLandscape, {bool isLandscapeLeft = true}) {
    _isLandscape = isLandscape;
    _isLandscapeLeft = isLandscapeLeft;
  }

  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;

      // Transform gyro rates into Vehicle Coordinate Frame based on screen orientation
      double vGyroRollRate;  // Rotation around vehicle forward axis (lean rate)
      double vGyroPitchRate; // Rotation around vehicle transverse axis (pitch rate)

      if (!_isLandscape) {
        // Portrait
        vGyroPitchRate = event.x * (180.0 / math.pi);
        vGyroRollRate = -event.y * (180.0 / math.pi);
      } else if (_isLandscapeLeft) {
        // Landscape Left
        vGyroPitchRate = event.y * (180.0 / math.pi);
        vGyroRollRate = event.x * (180.0 / math.pi);
      } else {
        // Landscape Right
        vGyroPitchRate = -event.y * (180.0 / math.pi);
        vGyroRollRate = -event.x * (180.0 / math.pi);
      }

      _currentPitch = (_currentPitch + vGyroPitchRate * dt).clamp(-90.0, 90.0);
      _currentRoll = (_currentRoll + vGyroRollRate * dt).clamp(-90.0, 90.0);
    });

    _accelSub = accelerometerEventStream(samplingPeriod: const Duration(milliseconds: 20)).listen((AccelerometerEvent event) {
      // Auto-detect landscape sub-type if in landscape
      if (_isLandscape && event.x.abs() > 3.0) {
        _isLandscapeLeft = event.x > 0;
      }

      // Map raw accelerometer to Vehicle Frame (vx = Right, vy = Forward, vz = Up)
      double vx, vy, vz;
      if (!_isLandscape) {
        vx = event.x;
        vy = event.y;
        vz = event.z;
      } else if (_isLandscapeLeft) {
        vx = -event.y;
        vy = event.x;
        vz = event.z;
      } else {
        vx = event.y;
        vy = -event.x;
        vz = event.z;
      }

      // Calculate total acceleration magnitude
      final norm = math.sqrt(vx * vx + vy * vy + vz * vz);
      if (norm < 0.1) return;

      // Compute static gravity-based angles in vehicle frame
      final accelRoll = -math.atan2(vx, math.sqrt(vy * vy + vz * vz)) * (180.0 / math.pi);
      final accelPitch = math.atan2(-vy, vz) * (180.0 / math.pi);

      final gMag = norm / _gravityConst;
      final dynamicDelta = (gMag - 1.0).abs();

      // If dynamic force > 0.08g, trust gyro to prevent vehicle acceleration from distorting lean
      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) {
      double vxUser, vyUser, vzUser;
      if (!_isLandscape) {
        vxUser = event.x;
        vyUser = event.y;
        vzUser = event.z;
      } else if (_isLandscapeLeft) {
        vxUser = -event.y;
        vyUser = event.x;
        vzUser = event.z;
      } else {
        vxUser = event.y;
        vyUser = -event.x;
        vzUser = event.z;
      }

      // Project phone linear acceleration vector into motorcycle body frame
      final pitchRad = (_currentPitch - _pitchOffset) * (math.pi / 180.0);
      final rollRad = (_currentRoll - _rollOffset) * (math.pi / 180.0);

      // Forward/longitudinal acceleration (along motorcycle centerline)
      final rawLongitudinal = (vyUser * math.cos(pitchRad) - vzUser * math.sin(pitchRad));
      // Lateral acceleration (cornering)
      final rawLateral = (vxUser * math.cos(rollRad) + vzUser * math.sin(rollRad));

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

      // Deadband / noise gate for resting phone on table
      if (rawLongitudinalG.abs() < 0.02) rawLongitudinalG = 0.0;
      if (rawLateralG.abs() < 0.02) rawLateralG = 0.0;

      // 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;

      if (_filteredLongitudinalG.abs() < 0.008) _filteredLongitudinalG = 0.0;
      if (_filteredLateralG.abs() < 0.008) _filteredLateralG = 0.0;

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

  void calibrateUpright() {
    _rollOffset = _currentRoll;
    _pitchOffset = _currentPitch;
    _calibrationService.saveCalibrationOffsets(_pitchOffset, _rollOffset);
  }

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