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;
});

final sensorDiagnosticsStreamProvider = StreamProvider<SensorFusionDiagnostics>((ref) {
  return ref.watch(sensorFusionServiceProvider).diagnosticsStream;
});

enum MountOrientation {
  portraitUpright,
  landscapeLeftUpright,
  landscapeRightUpright,
  flat,
}

class SensorFusionDiagnostics {
  final double rawAccelX;
  final double rawAccelY;
  final double rawAccelZ;
  final double rawGyroX;
  final double rawGyroY;
  final double rawGyroZ;
  final double transformedRoll;
  final double transformedPitch;
  final double rollOffset;
  final double pitchOffset;
  final double finalLeanAngle;
  final double dashboardDisplayedAngle;
  final String dashboardDirection;
  final bool isCalibrated;
  final String mountOrientation;

  const SensorFusionDiagnostics({
    required this.rawAccelX,
    required this.rawAccelY,
    required this.rawAccelZ,
    required this.rawGyroX,
    required this.rawGyroY,
    required this.rawGyroZ,
    required this.transformedRoll,
    required this.transformedPitch,
    required this.rollOffset,
    required this.pitchOffset,
    required this.finalLeanAngle,
    required this.dashboardDisplayedAngle,
    required this.dashboardDirection,
    required this.isCalibrated,
    required this.mountOrientation,
  });

  Map<String, dynamic> toMap() => {
    'rawAccel': [rawAccelX, rawAccelY, rawAccelZ],
    'rawGyro': [rawGyroX, rawGyroY, rawGyroZ],
    'transformedRoll': transformedRoll,
    'transformedPitch': transformedPitch,
    'rollOffset': rollOffset,
    'pitchOffset': pitchOffset,
    'finalLeanAngle': finalLeanAngle,
    'dashboardDisplayedAngle': dashboardDisplayedAngle,
    'dashboardDirection': dashboardDirection,
    'isCalibrated': isCalibrated,
    'mountOrientation': mountOrientation,
  };
}

class GForceData {
  final double longitudinal; // Accel/braking (G's)
  final double lateral;      // Left/right (G's)
  final double accelX;       // User accel m/s^2
  final double accelY;       // User accel m/s^2
  final double accelZ;       // User accel m/s^2
  final double? rawAccelX;   // Raw total accel m/s^2
  final double? rawAccelY;   // Raw total accel m/s^2
  final double? rawAccelZ;   // Raw total accel m/s^2
  final double? rawGyroX;    // Raw gyro rad/s
  final double? rawGyroY;    // Raw gyro rad/s
  final double? rawGyroZ;    // Raw gyro rad/s
  
  GForceData({
    required this.longitudinal, 
    required this.lateral,
    required this.accelX,
    required this.accelY,
    required this.accelZ,
    this.rawAccelX,
    this.rawAccelY,
    this.rawAccelZ,
    this.rawGyroX,
    this.rawGyroY,
    this.rawGyroZ,
  });
}

class SensorFusionService with WidgetsBindingObserver {
  final CalibrationService _calibrationService;

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

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

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

  // Calibration offsets & gyro zero-rate bias
  double _rollOffset = 0.0;
  double _pitchOffset = 0.0;
  double _gyroBiasX = 0.0;
  double _gyroBiasY = 0.0;
  double _gyroBiasZ = 0.0;
  bool _isCalibrated = false;
  bool get isCalibrated => _isCalibrated;
  int _remountAnomalyCount = 0;

  // Stored baseline gravity vector at calibration for remount detection
  double _calGravityX = 0.0;
  double _calGravityY = 0.0;
  double _calGravityZ = 0.0;

  // Raw sensor caches (unfiltered for forensics and diagnostics)
  double _latestRawAccelX = 0.0;
  double _latestRawAccelY = 0.0;
  double _latestRawAccelZ = 0.0;
  double _latestRawGyroX = 0.0;
  double _latestRawGyroY = 0.0;
  double _latestRawGyroZ = 0.0;
  double _latestRollRateDegPerSec = 0.0;

  double get latestRawAccelX => _latestRawAccelX;
  double get latestRawAccelY => _latestRawAccelY;
  double get latestRawAccelZ => _latestRawAccelZ;
  double get latestRawGyroX => _latestRawGyroX;
  double get latestRawGyroY => _latestRawGyroY;
  double get latestRawGyroZ => _latestRawGyroZ;
  double get latestRollRateDegPerSec => _latestRollRateDegPerSec;

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

  // 3D Orthonormal Basis for Vehicle Frame:
  // _rightVector: points to motorcycle right (+X_v)
  // _forwardVector: points to motorcycle forward (+Y_v)
  // _upVector: points to motorcycle up (+Z_v)
  List<double> _rightVector = [1.0, 0.0, 0.0];
  List<double> _forwardVector = [0.0, 0.0, -1.0];
  List<double> _upVector = [0.0, 1.0, 0.0];

  // Low-pass filtered accelerometer values in vehicle frame to reject engine vibration
  double _filteredVx = 0.0;
  double _filteredVy = 0.0;
  double _filteredVz = 0.0;
  bool _hasInitialAccel = false;

  bool _isLandscape = false;
  bool _isLandscapeLeft = true;
  MountOrientation _mountOrientation = MountOrientation.portraitUpright;
  MountOrientation get mountOrientation => _mountOrientation;

  static const double _gravityConst = 9.80665;

  // Low-pass filter for G-Force
  double _filteredLongitudinalG = 0.0;
  double _filteredLateralG = 0.0;
  static const double _gAlpha = 0.40;
  bool _hasReceivedUserAccel = false;

  // Diagnostic throttling
  DateTime? _lastDiagnosticLogTime;
  SensorFusionDiagnostics? _latestDiagnostics;
  SensorFusionDiagnostics? get latestDiagnostics => _latestDiagnostics;

  int _calibrationVersion = 0;
  Future<void>? _initializationFuture;
  Future<void> get initializationDone => _initializationFuture ?? Future.value();

  SensorFusionService(
    this._calibrationService, {
    Stream<AccelerometerEvent>? accelerometerStream,
    Stream<GyroscopeEvent>? gyroscopeStream,
    Stream<UserAccelerometerEvent>? userAccelerometerStream,
    MountOrientation defaultOrientation = MountOrientation.portraitUpright,
    bool autoStartSensors = true,
  }) {
    _mountOrientation = defaultOrientation;
    _recomputeBasisVectors();
    WidgetsBinding.instance.addObserver(this);
    _initializationFuture = _loadInitialCalibration();
    if (autoStartSensors || accelerometerStream != null || gyroscopeStream != null) {
      _startSensors(
        accelStream: accelerometerStream,
        gyroStream: gyroscopeStream,
        userAccelStream: userAccelerometerStream,
      );
    }
  }

  void _recomputeBasisVectors() {
    if (_isCalibrated &&
        (_calGravityX != 0.0 || _calGravityY != 0.0 || _calGravityZ != 0.0)) {
      final calNorm = math.sqrt(_calGravityX * _calGravityX +
          _calGravityY * _calGravityY +
          _calGravityZ * _calGravityZ);
      if (calNorm > 3.0) {
        _upVector = [
          _calGravityX / calNorm,
          _calGravityY / calNorm,
          _calGravityZ / calNorm,
        ];

        // Determine nominal right vector based on mount orientation
        List<double> nominalRight;
        switch (_mountOrientation) {
          case MountOrientation.portraitUpright:
            nominalRight = [1.0, 0.0, 0.0];
            break;
          case MountOrientation.landscapeLeftUpright:
            nominalRight = [0.0, -1.0, 0.0];
            break;
          case MountOrientation.landscapeRightUpright:
            nominalRight = [0.0, 1.0, 0.0];
            break;
          case MountOrientation.flat:
            nominalRight = [1.0, 0.0, 0.0];
            break;
        }

        // Project nominalRight to be strictly orthogonal to _upVector
        final dot = nominalRight[0] * _upVector[0] +
            nominalRight[1] * _upVector[1] +
            nominalRight[2] * _upVector[2];
        final rx = nominalRight[0] - dot * _upVector[0];
        final ry = nominalRight[1] - dot * _upVector[1];
        final rz = nominalRight[2] - dot * _upVector[2];
        final rNorm = math.sqrt(rx * rx + ry * ry + rz * rz);
        if (rNorm > 0.1) {
          _rightVector = [rx / rNorm, ry / rNorm, rz / rNorm];
        } else {
          _rightVector = nominalRight;
        }

        // Forward vector = Up x Right
        _forwardVector = [
          _upVector[1] * _rightVector[2] - _upVector[2] * _rightVector[1],
          _upVector[2] * _rightVector[0] - _upVector[0] * _rightVector[2],
          _upVector[0] * _rightVector[1] - _upVector[1] * _rightVector[0],
        ];
        return;
      }
    }

    // Nominal orthogonal basis for uncalibrated state
    switch (_mountOrientation) {
      case MountOrientation.portraitUpright:
        _rightVector = [1.0, 0.0, 0.0];
        _forwardVector = [0.0, 0.0, -1.0];
        _upVector = [0.0, 1.0, 0.0];
        break;
      case MountOrientation.landscapeLeftUpright:
        _rightVector = [0.0, -1.0, 0.0];
        _forwardVector = [0.0, 0.0, -1.0];
        _upVector = [1.0, 0.0, 0.0];
        break;
      case MountOrientation.landscapeRightUpright:
        _rightVector = [0.0, 1.0, 0.0];
        _forwardVector = [0.0, 0.0, -1.0];
        _upVector = [-1.0, 0.0, 0.0];
        break;
      case MountOrientation.flat:
        _rightVector = [1.0, 0.0, 0.0];
        _forwardVector = [0.0, 1.0, 0.0];
        _upVector = [0.0, 0.0, 1.0];
        break;
    }
  }

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

  void _updateDisplayOrientation() {
    try {
      final view = WidgetsBinding.instance.platformDispatcher.views.firstOrNull;
      if (view != null && view.physicalSize.height > 0) {
        final size = view.physicalSize;
        // Only adapt automatically if display dimensions are realistic mobile dimensions
        if (size.width > size.height && size.width < 1500) {
          _isLandscape = true;
          _updateMountOrientation();
          _recomputeBasisVectors();
        } else if (size.height >= size.width) {
          _isLandscape = false;
          _updateMountOrientation();
          _recomputeBasisVectors();
        }
      }
    } catch (_) {}
  }

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

  void _updateMountOrientation() {
    if (!_isLandscape) {
      _mountOrientation = MountOrientation.portraitUpright;
    } else if (_isLandscapeLeft) {
      _mountOrientation = MountOrientation.landscapeLeftUpright;
    } else {
      _mountOrientation = MountOrientation.landscapeRightUpright;
    }
  }

  Future<void> _loadInitialCalibration() async {
    await loadCalibrationForMotorcycle(null);
  }

  void _checkRemountValidity(double ax, double ay, double az) {
    if (!_isCalibrated) return;
    final calNorm = math.sqrt(_calGravityX * _calGravityX + _calGravityY * _calGravityY + _calGravityZ * _calGravityZ);
    final curNorm = math.sqrt(ax * ax + ay * ay + az * az);
    if (calNorm < 3.0 || curNorm < 3.0) return;

    // Normalize current acceleration vector
    final nx = ax / curNorm;
    final ny = ay / curNorm;
    final nz = az / curNorm;

    // Project current vector onto 3D Vehicle Coordinate Frame:
    // vUp: component along calibrated upright axis
    // vForward: component along motorcycle longitudinal/forward axis
    final vUp = (nx * _upVector[0] + ny * _upVector[1] + nz * _upVector[2]).clamp(-1.0, 1.0);
    final vForward = (nx * _forwardVector[0] + ny * _forwardVector[1] + nz * _forwardVector[2]).clamp(-1.0, 1.0);

    // Total 3D angular deviation from calibrated upright gravity
    final totalAngleDiffDeg = math.acos(vUp) * (180.0 / math.pi);

    // Pitch tilt (tilt out of motorcycle roll plane)
    final pitchTiltDeg = (math.asin(vForward)).abs() * (180.0 / math.pi);

    // Motorcycle Lean & Remount Physics:
    // 1. Roll-axis rotation (up to 65°) is normal motorcycle riding, cornering, and kickstand parking (20°-35°).
    //    Deviations within the roll plane with pitchTiltDeg <= 35° and totalAngleDiffDeg <= 68°
    //    must NEVER invalidate calibration!
    // 2. True remount / phone removal:
    //    - Placed flat on desk (screen up): totalAngleDiffDeg ~ 90°, pitchTiltDeg ~ 90°
    //    - Placed in pocket/bag: totalAngleDiffDeg >= 90°
    //    - Mount rotated or twisted: totalAngleDiffDeg > 68° or pitchTiltDeg > 38°
    final isImmediateRemount = totalAngleDiffDeg >= 75.0 || pitchTiltDeg >= 42.0;
    final isSuspectedRemount = totalAngleDiffDeg > 68.0 || pitchTiltDeg > 35.0;

    if (isImmediateRemount) {
      _invalidateRemount(totalAngleDiffDeg);
    } else if (isSuspectedRemount) {
      _remountAnomalyCount++;
      // Require 5 consecutive stationary samples (100ms) to filter out transient bumps or noise
      if (_remountAnomalyCount >= 5) {
        _invalidateRemount(totalAngleDiffDeg);
      }
    } else {
      _remountAnomalyCount = 0;
    }
  }

  void _invalidateRemount(double angleDiffDeg) {
    debugPrint('[SensorFusion] Remount detected: device orientation shifted ${angleDiffDeg.toStringAsFixed(1)}° from saved calibration. Invalidating calibration.');
    _calibrationVersion++;
    _isCalibrated = false;
    _remountAnomalyCount = 0;
    _rollOffset = 0.0;
    _pitchOffset = 0.0;
    _calGravityX = 0.0;
    _calGravityY = 0.0;
    _calGravityZ = 0.0;
    _recomputeBasisVectors();
    _calibrationService.clearCalibration();
    _emitDiagnostics(0.0);
  }

  void _startSensors({
    Stream<AccelerometerEvent>? accelStream,
    Stream<GyroscopeEvent>? gyroStream,
    Stream<UserAccelerometerEvent>? userAccelStream,
  }) {
    try {
      final gStream = gyroStream ?? gyroscopeEventStream(samplingPeriod: const Duration(milliseconds: 20));
      _gyroSub = gStream.listen(
        handleGyroscopeEvent,
        onError: (err) {
          debugPrint('[SensorFusion] Gyroscope stream error: $err');
        },
        cancelOnError: false,
      );
    } catch (err) {
      debugPrint('[SensorFusion] Gyroscope initialization skipped (headless test runner): $err');
    }

    try {
      final aStream = accelStream ?? accelerometerEventStream(samplingPeriod: const Duration(milliseconds: 20));
      _accelSub = aStream.listen(
        handleAccelerometerEvent,
        onError: (err) {
          debugPrint('[SensorFusion] Accelerometer stream error: $err');
        },
        cancelOnError: false,
      );
    } catch (err) {
      debugPrint('[SensorFusion] Accelerometer initialization skipped (headless test runner): $err');
    }

    try {
      final uStream = userAccelStream ?? userAccelerometerEventStream(samplingPeriod: const Duration(milliseconds: 20));
      _userAccelSub = uStream.listen(
        handleUserAccelerometerEvent,
        onError: (err) {
          debugPrint('[SensorFusion] UserAccelerometer stream error: $err');
        },
        cancelOnError: false,
      );
    } catch (err) {
      debugPrint('[SensorFusion] UserAccelerometer initialization skipped (headless test runner): $err');
    }
  }

  void handleGyroscopeEvent(GyroscopeEvent event) {
    final now = DateTime.now();
    _latestRawGyroX = event.x;
    _latestRawGyroY = event.y;
    _latestRawGyroZ = event.z;

    if (_lastGyroTime == null) {
      _lastGyroTime = now;
      return;
    }

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

    if (dt > 0.1) dt = 0.1;
    if (dt <= 0.0) return;

    // Compensate for static zero-rate gyro bias
    final unbiasedGx = event.x - _gyroBiasX;
    final unbiasedGy = event.y - _gyroBiasY;
    final unbiasedGz = event.z - _gyroBiasZ;

    // Project device angular rates into 3D Vehicle Coordinate Frame:
    // Roll rate is rotation around the motorcycle forward axis (_forwardVector)
    // Pitch rate is rotation around the motorcycle transverse right axis (_rightVector)
    final vGyroRollRate = (unbiasedGx * _forwardVector[0] +
            unbiasedGy * _forwardVector[1] +
            unbiasedGz * _forwardVector[2]) *
        (180.0 / math.pi);

    final vGyroPitchRate = (unbiasedGx * _rightVector[0] +
            unbiasedGy * _rightVector[1] +
            unbiasedGz * _rightVector[2]) *
        (180.0 / math.pi);

    _latestRollRateDegPerSec = vGyroRollRate;

    // Numerical integration of gyro rate
    _currentPitch = (_currentPitch + vGyroPitchRate * dt).clamp(-90.0, 90.0);
    _currentRoll = (_currentRoll + vGyroRollRate * dt).clamp(-90.0, 90.0);
  }

  void handleAccelerometerEvent(AccelerometerEvent event) {
    _latestRawAccelX = event.x;
    _latestRawAccelY = event.y;
    _latestRawAccelZ = event.z;

    // Check remount validity when device is relatively stationary
    final rawNorm = math.sqrt(event.x * event.x + event.y * event.y + event.z * event.z);
    if ((rawNorm - _gravityConst).abs() < 1.0) {
      _checkRemountValidity(event.x, event.y, event.z);
    } else {
      _remountAnomalyCount = 0;
    }

    // Auto-detect landscape subtype when in landscape
    if (_isLandscape && event.x.abs() > 3.0) {
      final wasLeft = _isLandscapeLeft;
      _isLandscapeLeft = event.x > 0;
      if (wasLeft != _isLandscapeLeft) {
        _updateMountOrientation();
        _recomputeBasisVectors();
      }
    }

    // Map raw accelerometer to Vehicle Frame using the 3D basis:
    // vx = Right, vy = Forward, vz = Up
    final vx = event.x * _rightVector[0] + event.y * _rightVector[1] + event.z * _rightVector[2];
    final vy = event.x * _forwardVector[0] + event.y * _forwardVector[1] + event.z * _forwardVector[2];
    final vz = event.x * _upVector[0] + event.y * _upVector[1] + event.z * _upVector[2];

    // Low-pass filter to reject engine vibration (e.g. Yamaha R6 high-RPM inline-4 vibration)
    if (!_hasInitialAccel) {
      _filteredVx = vx;
      _filteredVy = vy;
      _filteredVz = vz;
      _hasInitialAccel = true;
    } else {
      const accelFilterAlpha = 0.25;
      _filteredVx = accelFilterAlpha * vx + (1.0 - accelFilterAlpha) * _filteredVx;
      _filteredVy = accelFilterAlpha * vy + (1.0 - accelFilterAlpha) * _filteredVy;
      _filteredVz = accelFilterAlpha * vz + (1.0 - accelFilterAlpha) * _filteredVz;
    }

    final norm = math.sqrt(_filteredVx * _filteredVx + _filteredVy * _filteredVy + _filteredVz * _filteredVz);
    if (norm < 0.1) return;

    // Compute static gravity-based angles in vehicle frame
    // In vehicle frame: vx = Right, vy = Forward, vz = Up
    // When leaning right: gravity reaction tilts to vehicle left (vx < 0, vz > 0)
    // -atan2(vx, sqrt(vy^2 + vz^2)) gives positive angle for right lean, negative for left lean
    final accelRoll = -math.atan2(_filteredVx, math.sqrt(_filteredVy * _filteredVy + _filteredVz * _filteredVz)) * (180.0 / math.pi);
    final accelPitch = math.atan2(_filteredVy, _filteredVz) * (180.0 / math.pi);

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

    // Complementary filter:
    // - Static / resting: alpha = 0.88 (converges quickly to gravity reference)
    // - Dynamic cruising: alpha = 0.95 (smooth vibration rejection)
    // - Dynamic cornering: alpha transitions up to 0.995 max (relies on gyro rate during turns)
    // Alpha is strictly capped at 0.995, preventing runaway integration or pinning.
    double dynamicAlpha = 0.88;
    final rollRateAbs = _latestRollRateDegPerSec.abs();
    if (rollRateAbs > 2.0 || dynamicDelta > 0.04) {
      final dynamicFactor = math.max(rollRateAbs / 15.0, dynamicDelta / 0.35).clamp(0.0, 1.0);
      dynamicAlpha = (0.95 + 0.045 * dynamicFactor).clamp(0.95, 0.995);
    }

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

    // Resting convergence: when stationary and upright, gently pull residual drift to 0.0
    if (rollRateAbs < 0.5 && dynamicDelta < 0.03 && accelRoll.abs() < 1.0) {
      _currentRoll = _currentRoll * 0.92;
    }

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

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

    // Diagnostics
    _emitDiagnostics(calibratedLean);

    // Fallback linear acceleration when userAccelerometer is unavailable
    if (!_hasReceivedUserAccel) {
      _deriveFallbackLinearG(vx, vy, vz, norm, accelRoll, accelPitch);
    }
  }

  void _deriveFallbackLinearG(double vx, double vy, double vz, double norm, double accelRoll, double accelPitch) {
    final gx = -norm * math.sin(accelRoll * math.pi / 180.0);
    final gy = -norm * math.sin(accelPitch * math.pi / 180.0) * math.cos(accelRoll * math.pi / 180.0);
    final gz = norm * math.cos(accelPitch * math.pi / 180.0) * math.cos(accelRoll * math.pi / 180.0);

    final userVx = vx - gx;
    final userVy = vy - gy;
    final userVz = vz - gz;

    final pitchRad = _currentPitch * (math.pi / 180.0);
    final rollRad = _currentRoll * (math.pi / 180.0);

    final rawLongitudinal = (userVy * math.cos(pitchRad) - userVz * math.sin(pitchRad));
    final rawLateral = (userVx * math.cos(rollRad) + userVz * math.sin(rollRad));

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

    if (rawLongitudinalG.abs() < 0.02) rawLongitudinalG = 0.0;
    if (rawLateralG.abs() < 0.02) rawLateralG = 0.0;

    _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: userVx,
      accelY: userVy,
      accelZ: userVz,
      rawAccelX: _latestRawAccelX,
      rawAccelY: _latestRawAccelY,
      rawAccelZ: _latestRawAccelZ,
      rawGyroX: _latestRawGyroX,
      rawGyroY: _latestRawGyroY,
      rawGyroZ: _latestRawGyroZ,
    ));
  }

  void handleUserAccelerometerEvent(UserAccelerometerEvent event) {
    _hasReceivedUserAccel = true;
    final vxUser = event.x * _rightVector[0] + event.y * _rightVector[1] + event.z * _rightVector[2];
    final vyUser = event.x * _forwardVector[0] + event.y * _forwardVector[1] + event.z * _forwardVector[2];
    final vzUser = event.x * _upVector[0] + event.y * _upVector[1] + event.z * _upVector[2];

    final pitchRad = _currentPitch * (math.pi / 180.0);
    final rollRad = _currentRoll * (math.pi / 180.0);

    final rawLongitudinal = (vyUser * math.cos(pitchRad) - vzUser * math.sin(pitchRad));
    final rawLateral = (vxUser * math.cos(rollRad) + vzUser * math.sin(rollRad));

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

    if (rawLongitudinalG.abs() < 0.02) rawLongitudinalG = 0.0;
    if (rawLateralG.abs() < 0.02) rawLateralG = 0.0;

    _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,
      rawAccelX: _latestRawAccelX,
      rawAccelY: _latestRawAccelY,
      rawAccelZ: _latestRawAccelZ,
      rawGyroX: _latestRawGyroX,
      rawGyroY: _latestRawGyroY,
      rawGyroZ: _latestRawGyroZ,
    ));
  }

  void _emitDiagnostics(double finalLean) {
    final now = DateTime.now();
    final direction = finalLean < -0.5 ? 'LEFT' : (finalLean > 0.5 ? 'RIGHT' : 'CENTER');
    final displayedAngle = finalLean.abs();

    final diag = SensorFusionDiagnostics(
      rawAccelX: _latestRawAccelX,
      rawAccelY: _latestRawAccelY,
      rawAccelZ: _latestRawAccelZ,
      rawGyroX: _latestRawGyroX,
      rawGyroY: _latestRawGyroY,
      rawGyroZ: _latestRawGyroZ,
      transformedRoll: _currentRoll,
      transformedPitch: _currentPitch,
      rollOffset: _rollOffset,
      pitchOffset: _pitchOffset,
      finalLeanAngle: finalLean,
      dashboardDisplayedAngle: displayedAngle,
      dashboardDirection: direction,
      isCalibrated: _isCalibrated,
      mountOrientation: _mountOrientation.name,
    );

    _latestDiagnostics = diag;
    _diagnosticsController.add(diag);

    // Throttled debugPrint logging (every 1.5 seconds)
    if (_lastDiagnosticLogTime == null || now.difference(_lastDiagnosticLogTime!).inMilliseconds >= 1500) {
      _lastDiagnosticLogTime = now;
      debugPrint('[SensorFusion Diag] Accel: (${_latestRawAccelX.toStringAsFixed(2)}, ${_latestRawAccelY.toStringAsFixed(2)}, ${_latestRawAccelZ.toStringAsFixed(2)}) '
          'Gyro: (${_latestRawGyroX.toStringAsFixed(3)}, ${_latestRawGyroY.toStringAsFixed(3)}, ${_latestRawGyroZ.toStringAsFixed(3)}) '
          'Roll: ${_currentRoll.toStringAsFixed(1)}° Offset: ${_rollOffset.toStringAsFixed(1)}° Lean: ${finalLean.toStringAsFixed(1)}° '
          'Display: ${displayedAngle.toStringAsFixed(0)}° $direction Calibrated: $_isCalibrated');
    }
  }

  Future<void> loadCalibrationForMotorcycle(int? motorcycleId) async {
    final version = ++_calibrationVersion;
    try {
      final offsets = await _calibrationService.loadCalibrationOffsets(motorcycleId: motorcycleId);
      final mountMode = await _calibrationService.loadMountMode(motorcycleId: motorcycleId);
      if (version != _calibrationVersion) return;

      _pitchOffset = offsets['pitch'] ?? 0.0;
      _rollOffset = offsets['roll'] ?? 0.0;
      _gyroBiasX = offsets['gyroBiasX'] ?? 0.0;
      _gyroBiasY = offsets['gyroBiasY'] ?? 0.0;
      _gyroBiasZ = offsets['gyroBiasZ'] ?? 0.0;
      final isCal = (offsets['isCalibrated'] ?? 0.0) > 0.5;
      _calGravityX = offsets['accelCalX'] ?? 0.0;
      _calGravityY = offsets['accelCalY'] ?? 0.0;
      _calGravityZ = offsets['accelCalZ'] ?? 0.0;

      if (mountMode != null) {
        for (final m in MountOrientation.values) {
          if (m.name == mountMode) {
            _mountOrientation = m;
            break;
          }
        }
      }

      _isCalibrated = isCal;
      _remountAnomalyCount = 0;
      _recomputeBasisVectors();
    } catch (_) {
      if (version != _calibrationVersion) return;
      _isCalibrated = false;
      _recomputeBasisVectors();
    }
  }

  Future<void> calibrateUpright({int? motorcycleId}) async {
    ++_calibrationVersion;
    _gyroBiasX = _latestRawGyroX;
    _gyroBiasY = _latestRawGyroY;
    _gyroBiasZ = _latestRawGyroZ;
    _calGravityX = _latestRawAccelX;
    _calGravityY = _latestRawAccelY;
    _calGravityZ = _latestRawAccelZ;
    _isCalibrated = true;
    _remountAnomalyCount = 0;

    // Auto-detect mount orientation from gravity
    final ax = _calGravityX.abs();
    final ay = _calGravityY.abs();
    final az = _calGravityZ.abs();
    if (ax > ay && ax > az) {
      _mountOrientation = _calGravityX > 0
          ? MountOrientation.landscapeLeftUpright
          : MountOrientation.landscapeRightUpright;
    } else if (_calGravityZ > 7.5 && ay < 4.0) {
      _mountOrientation = MountOrientation.flat;
    } else {
      _mountOrientation = MountOrientation.portraitUpright;
    }

    _recomputeBasisVectors();

    // In newly calibrated frame, current lean and pitch start at zero
    _currentRoll = 0.0;
    _currentPitch = 0.0;
    _rollOffset = 0.0;
    _pitchOffset = 0.0;

    // Immediately re-emit calibrated 0.0° lean
    _leanAngleController.add(0.0);
    _pitchAngleController.add(0.0);
    _emitDiagnostics(0.0);

    await _calibrationService.saveCalibrationOffsets(
      0.0,
      0.0,
      gyroBiasX: _gyroBiasX,
      gyroBiasY: _gyroBiasY,
      gyroBiasZ: _gyroBiasZ,
      accelCalX: _calGravityX,
      accelCalY: _calGravityY,
      accelCalZ: _calGravityZ,
      mountMode: _mountOrientation.name,
      isCalibrated: true,
      motorcycleId: motorcycleId,
    );
  }

  Future<void> resetCalibration({int? motorcycleId}) async {
    _calibrationVersion++;
    _isCalibrated = false;
    _remountAnomalyCount = 0;
    _rollOffset = 0.0;
    _pitchOffset = 0.0;
    _calGravityX = 0.0;
    _calGravityY = 0.0;
    _calGravityZ = 0.0;
    _recomputeBasisVectors();
    await _calibrationService.clearCalibration(motorcycleId: motorcycleId);
    _emitDiagnostics(0.0);
  }

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