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;
  final double eventRateHz;
  final double longitudinalG;
  final double lateralG;

  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,
    this.eventRateHz = 50.0,
    this.longitudinalG = 0.0,
    this.lateralG = 0.0,
  });

  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,
    'eventRateHz': eventRateHz,
    'longitudinalG': longitudinalG,
    'lateralG': lateralG,
  };
}

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;
  int _remountReturnCount = 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 _eventRateHz = 50.0;
  int _sensorEventCounter = 0;
  DateTime? _lastRateWindow;

  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;
  double get eventRateHz => _eventRateHz;

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

  double _speedMph = 0.0;
  double get speedMph => _speedMph;
  void updateVehicleSpeed(double speedMph) {
    _speedMph = speedMph;
  }

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

  @override
  void didChangeAppLifecycleState(AppLifecycleState state) {
    if (state == AppLifecycleState.resumed) {
      _updateDisplayOrientation();
      _ensureSensorsListening();
      if (!_isCalibrated) {
        loadCalibrationForMotorcycle(null);
      }
    }
  }

  void _ensureSensorsListening() {
    if (_accelSub == null || _gyroSub == null) {
      _startSensors();
    }
  }

  void _updateDisplayOrientation() {
    try {
      final view = WidgetsBinding.instance.platformDispatcher.views.firstOrNull;
      if (view != null && view.physicalSize.height > 0) {
        final size = view.physicalSize;
        if (size.width > size.height) {
          _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) {
    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 calibrated upright gravity axis
    final vUp = (nx * (_calGravityX / calNorm) + ny * (_calGravityY / calNorm) + nz * (_calGravityZ / calNorm)).clamp(-1.0, 1.0);
    final totalAngleDiffDeg = math.acos(vUp) * (180.0 / math.pi);

    if (!_isCalibrated) {
      // Automatic re-lock: if the device returns to the motorcycle mount (within 25° of calibrated gravity)
      if (totalAngleDiffDeg < 25.0) {
        _remountReturnCount++;
        if (_remountReturnCount >= 15) {
          _isCalibrated = true;
          _remountAnomalyCount = 0;
          _remountReturnCount = 0;
          _recomputeBasisVectors();
          _emitDiagnostics(0.0);
        }
      } else {
        _remountReturnCount = 0;
      }
      return;
    }

    _remountReturnCount = 0;

    // Do not invalidate calibration while the vehicle is actively in motion or experiencing dynamic rotation
    if (_speedMph > 2.0 || _latestRollRateDegPerSec.abs() > 3.0) {
      _remountAnomalyCount = 0;
      return;
    }

    // Project current vector onto 3D Vehicle Coordinate Frame:
    final vForward = (nx * _forwardVector[0] + ny * _forwardVector[1] + nz * _forwardVector[2]).clamp(-1.0, 1.0);
    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 must NEVER invalidate calibration.
    // 2. Cockpit mounts: Handlebar mounts (QuadLock, RAM) are commonly pitched backwards 35°-55° facing the rider.
    // 3. True remount / phone removal:
    //    - Placed flat on desk (screen up): totalAngleDiffDeg ~ 90°, pitchTiltDeg ~ 90°
    //    - Placed in pocket/bag: totalAngleDiffDeg >= 80°
    //    - Mount rotated or twisted when stationary: totalAngleDiffDeg >= 80° or pitchTiltDeg >= 65°
    final isImmediateRemount = totalAngleDiffDeg >= 80.0 || pitchTiltDeg >= 65.0;
    final isSuspectedRemount = totalAngleDiffDeg > 70.0 || pitchTiltDeg > 55.0;

    if (isImmediateRemount) {
      _invalidateRemount(totalAngleDiffDeg);
    } else if (isSuspectedRemount) {
      _remountAnomalyCount++;
      // Require 50 consecutive stationary samples (1000ms) to filter out transient bumps, fork dive, or vibrations
      if (_remountAnomalyCount >= 50) {
        _invalidateRemount(totalAngleDiffDeg);
      }
    } else {
      _remountAnomalyCount = 0;
    }
  }

  void _invalidateRemount(double angleDiffDeg) {
    debugPrint('[SensorFusion] Remount detected: device orientation shifted ${angleDiffDeg.toStringAsFixed(1)}° from saved calibration. Invalidating runtime calibration.');
    _calibrationVersion++;
    _isCalibrated = false;
    _remountAnomalyCount = 0;
    _remountReturnCount = 0;
    _rollOffset = 0.0;
    _pitchOffset = 0.0;
    _recomputeBasisVectors();
    _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) {
    _sensorEventCounter++;
    final now = DateTime.now();
    if (_lastRateWindow == null) {
      _lastRateWindow = now;
    } else {
      final elapsedMs = now.difference(_lastRateWindow!).inMilliseconds;
      if (elapsedMs >= 1000) {
        _eventRateHz = (_sensorEventCounter * 1000.0) / elapsedMs;
        _sensorEventCounter = 0;
        _lastRateWindow = now;
      }
    }

    _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,
      eventRateHz: _eventRateHz,
      longitudinalG: _filteredLongitudinalG,
      lateralG: _filteredLateralG,
    );

    _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;
      _remountReturnCount = 0;
      _recomputeBasisVectors();
      if (_isCalibrated) {
        _leanAngleController.add(0.0);
        _pitchAngleController.add(0.0);
        _emitDiagnostics(0.0);
      }
    } 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;
    _remountReturnCount = 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;
    _remountReturnCount = 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();
  }
}