MotorSignal-App/lib/services/telemetry_service.dart

import 'dart:async';
import 'package:flutter_riverpod/flutter_riverpod.dart';
import 'package:geolocator/geolocator.dart';
import '../models/telemetry_sample.dart';
import 'location_service.dart';
import 'sensor_fusion_service.dart';
import 'compass_service.dart';
import 'field_test/field_test_logger.dart';

final telemetryServiceProvider = Provider<TelemetryService>((ref) {
  final locationService = ref.watch(locationServiceProvider);
  final sensorFusionService = ref.watch(sensorFusionServiceProvider);
  final compassService = ref.watch(compassServiceProvider);
  final fieldTestLogger = ref.watch(fieldTestLoggerProvider);
  
  final service = TelemetryService(locationService, sensorFusionService, compassService, fieldTestLogger);
  ref.onDispose(() => service.dispose());
  return service;
});

final telemetryStreamProvider = StreamProvider<TelemetrySample>((ref) {
  return ref.watch(telemetryServiceProvider).telemetryStream;
});

class TelemetryService {
  final LocationService _locationService;
  final SensorFusionService _sensorFusionService;
  final CompassService _compassService;
  final FieldTestLogger _fieldTestLogger;

  final _telemetryController = StreamController<TelemetrySample>.broadcast();
  Stream<TelemetrySample> get telemetryStream => _telemetryController.stream;

  StreamSubscription? _locationSub;
  StreamSubscription? _leanSub;
  StreamSubscription? _pitchSub;
  StreamSubscription? _gForceSub;
  Timer? _samplingTimer;

  StreamSubscription? _compassSub;

  // Latest states
  Position? _latestPosition;
  double? _latestLean;
  double? _latestPitch;
  GForceData? _latestGForce;
  double _latestCompassHeading = 0.0;

  TelemetryService(
    this._locationService,
    this._sensorFusionService,
    this._compassService,
    this._fieldTestLogger,
  ) {
    _init();
  }

  void _init() {
    _locationService.onGpsDiagnostic = (pos, accepted, reason) {
      if (_fieldTestLogger.isSessionActive) {
        final health = pos.accuracy <= 15.0 ? 'Healthy' : (pos.accuracy <= 35.0 ? 'Degraded' : 'Poor');
        _fieldTestLogger.logGpsEvent(
          latitude: pos.latitude,
          longitude: pos.longitude,
          altitude: pos.altitude,
          rawSpeed: pos.speed,
          filteredSpeed: _latestPosition?.speed ?? pos.speed,
          heading: pos.heading,
          accuracy: pos.accuracy,
          gpsHealth: health,
          accepted: accepted,
          rejectionReason: reason,
          timestamp: pos.timestamp,
        );
      }
    };

    _locationSub = _locationService.getFilteredPositionStream().listen((pos) {
      _latestPosition = pos;
    });

    _leanSub = _sensorFusionService.leanAngleStream.listen((lean) {
      _latestLean = lean;
    });

    _pitchSub = _sensorFusionService.pitchAngleStream.listen((pitch) {
      _latestPitch = pitch;
    });

    _gForceSub = _sensorFusionService.gForceStream.listen((gForce) {
      _latestGForce = gForce;
      if (_fieldTestLogger.isSessionActive) {
        _fieldTestLogger.logImuDiagnostic(
          accelX: gForce.accelX,
          accelY: gForce.accelY,
          accelZ: gForce.accelZ,
          roll: _latestLean,
          pitch: _latestPitch,
          longitudinalG: gForce.accelY / 9.80665,
          lateralG: gForce.accelX / 9.80665,
          filteredLongitudinalG: gForce.longitudinal,
          filteredLateralG: gForce.lateral,
        );
      }
    });

    _compassSub = _compassService.headingStream.listen((heading) {
      _latestCompassHeading = heading;
    });

    // Sample at 5Hz (200ms)
    _samplingTimer = Timer.periodic(const Duration(milliseconds: 200), (_) {
      _emitSample();
    });
  }

  void _emitSample() {
    final speedMph = _latestPosition != null ? _latestPosition!.speed * 2.23694 : 0.0;
    
    // Use GPS heading if moving, otherwise fallback to compass
    double finalHeading = _latestCompassHeading;
    if (_latestPosition != null && _latestPosition!.heading >= 0 && speedMph > 3.0) {
      finalHeading = _latestPosition!.heading;
    }

    final sample = TelemetrySample(
      timestamp: DateTime.now(),
      latitude: _latestPosition?.latitude,
      longitude: _latestPosition?.longitude,
      altitude: _latestPosition?.altitude,
      speed: _latestPosition != null ? _latestPosition!.speed * 2.23694 : null, // m/s to mph
      heading: finalHeading,
      gpsAccuracy: _latestPosition?.accuracy,
      leanAngle: _latestLean,
      pitchAngle: _latestPitch,
      accelX: _latestGForce?.accelX,
      accelY: _latestGForce?.accelY,
      accelZ: _latestGForce?.accelZ,
      longitudinalG: _latestGForce?.longitudinal,
      lateralG: _latestGForce?.lateral,
    );

    _telemetryController.add(sample);
  }

  void dispose() {
    _locationSub?.cancel();
    _leanSub?.cancel();
    _pitchSub?.cancel();
    _gForceSub?.cancel();
    _compassSub?.cancel();
    _samplingTimer?.cancel();
    _telemetryController.close();
  }
}