MotorSignal-App/lib/services/telemetry_service.dart

import 'dart:async';
import 'dart:math' as math;
import 'package:flutter_riverpod/flutter_riverpod.dart';
import 'package:geolocator/geolocator.dart';
import '../models/telemetry_sample.dart';
import '../models/recording_health_status.dart';
import '../models/field_test_event.dart';
import 'location_service.dart';
import 'sensor_fusion_service.dart';
import 'compass_service.dart';
import 'field_test/field_test_logger.dart';
import 'validation/telemetry_validator.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 validator = ref.watch(telemetryValidatorProvider);
  
  final service = TelemetryService(locationService, sensorFusionService, compassService, fieldTestLogger, validator);
  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 TelemetryValidator _validator;

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

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

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

  // GPS Kinematics state
  double _gpsLongitudinalG = 0.0;
  double _gpsLateralG = 0.0;

  // Monotonic authoritative clock
  final Stopwatch _monotonicStopwatch = Stopwatch();
  int _sequenceCounter = 0;
  int _lastEmitMonotonicMs = 0;
  int get lastEmitMonotonicMs => _lastEmitMonotonicMs;

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

  void _init() {
    _locationService.onGpsDiagnostic = (pos, accepted, reason) {
      if (_fieldTestLogger.isSessionActive && !accepted) {
        _fieldTestLogger.log(
          eventType: FieldTestEventType.gpsJumpRejected,
          severity: EventSeverity.error,
          message: 'GPS fix rejected: $reason',
          latitude: pos.latitude,
          longitude: pos.longitude,
          speed: pos.speed * 2.23694,
          gpsAccuracy: pos.accuracy,
          timestamp: pos.timestamp,
        );
      }
    };

    _locationSub = _locationService.getFilteredPositionStream().listen((pos) {
      final speedMph = pos.speed * 2.23694;
      _sensorFusionService.updateVehicleSpeed(speedMph);

      if (_latestPosition != null) {
        final dt = pos.timestamp.difference(_latestPosition!.timestamp).inMicroseconds.abs() / 1000000.0;
        if (dt >= 0.05 && dt <= 3.0) {
          final currentSpeedMps = pos.speed;
          final prevSpeedMps = _latestPosition!.speed;
          final accelMps2 = (currentSpeedMps - prevSpeedMps) / dt;
          _gpsLongitudinalG = (accelMps2 / 9.80665).clamp(-1.5, 1.5);

          if (speedMph >= 2.5 && pos.heading >= 0 && _latestPosition!.heading >= 0) {
            var dHeading = pos.heading - _latestPosition!.heading;
            while (dHeading > 180) {
              dHeading -= 360;
            }
            while (dHeading < -180) {
              dHeading += 360;
            }
            final omegaRadS = (dHeading * math.pi / 180.0) / dt;
            final centripetalMps2 = currentSpeedMps * omegaRadS;
            final latG = (centripetalMps2 / 9.80665).clamp(-1.5, 1.5);
            _gpsLateralG = latG;
          } else {
            _gpsLateralG = 0.0;
          }
        }
      }
      _latestPosition = pos;
    });

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

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

    _gForceSub = _sensorFusionService.gForceStream.listen((gForce) {
      _latestGForce = gForce;
      _latestImuTimestamp = DateTime.now();
    });

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

    // High-rate refresh at strictly 20Hz (50ms) as the single authoritative telemetry clock
    _samplingTimer = Timer.periodic(const Duration(milliseconds: 50), (_) {
      _emitSample();
    });
  }

  DateTime? _lastSuspectLogTime;

  void _emitSample() {
    final now = DateTime.now();
    final monotonicMs = _monotonicStopwatch.elapsedMilliseconds;
    final sequence = ++_sequenceCounter;
    
    final speedMph = _latestPosition != null ? _latestPosition!.speed * 2.23694 : 0.0;
    final gpsAgeMs = _latestPosition != null ? now.difference(_latestPosition!.timestamp).inMilliseconds : null;
    final isGpsFresh = gpsAgeMs != null &&
        gpsAgeMs >= -2000 &&
        gpsAgeMs <= RecordingHealthContract.evidenceStaleAfterMs;
    
    // Use GPS heading if moving with fresh GPS evidence, otherwise fallback to compass
    double finalHeading = _latestCompassHeading;
    if (isGpsFresh && _latestPosition != null && _latestPosition!.heading >= 0 && speedMph > 3.0) {
      finalHeading = _latestPosition!.heading;
    }

    // Reset GPS kinematics to zero if vehicle is stationary or GPS is stale
    if (!isGpsFresh || speedMph < 2.5) {
      _gpsLongitudinalG = 0.0;
      _gpsLateralG = 0.0;
    }

    final hasActiveImuG = _latestGForce != null &&
        (_latestGForce!.longitudinal.abs() > 0.01 || _latestGForce!.lateral.abs() > 0.01);
    final effectiveLongitudinalG = hasActiveImuG ? _latestGForce!.longitudinal : _gpsLongitudinalG;
    final effectiveLateralG = hasActiveImuG ? _latestGForce!.lateral : _gpsLateralG;

    bool isImuCalibrated = false;
    try {
      isImuCalibrated = _sensorFusionService.isCalibrated;
    } catch (_) {
      isImuCalibrated = false;
    }

    final double? effectiveLean = isImuCalibrated ? _latestLean : null;

    final rawSample = TelemetrySample(
      timestamp: now,
      sequenceNumber: sequence,
      monotonicTimestampMs: monotonicMs,
      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,
      gpsTimestamp: _latestPosition?.timestamp,
      gpsAgeMs: gpsAgeMs,
      imuTimestamp: _latestImuTimestamp,
      leanAngle: effectiveLean,
      pitchAngle: _latestPitch,
      rollRateDegPerSec: _sensorFusionService.latestRollRateDegPerSec,
      accelX: _latestGForce?.accelX,
      accelY: _latestGForce?.accelY,
      accelZ: _latestGForce?.accelZ,
      longitudinalG: effectiveLongitudinalG,
      lateralG: effectiveLateralG,
      rawAccelX: _latestGForce?.rawAccelX ?? _sensorFusionService.latestRawAccelX,
      rawAccelY: _latestGForce?.rawAccelY ?? _sensorFusionService.latestRawAccelY,
      rawAccelZ: _latestGForce?.rawAccelZ ?? _sensorFusionService.latestRawAccelZ,
      rawGyroX: _latestGForce?.rawGyroX ?? _sensorFusionService.latestRawGyroX,
      rawGyroY: _latestGForce?.rawGyroY ?? _sensorFusionService.latestRawGyroY,
      rawGyroZ: _latestGForce?.rawGyroZ ?? _sensorFusionService.latestRawGyroZ,
    );

    _lastEmitMonotonicMs = monotonicMs;

    // Validate sample
    final quality = _validator.validate(rawSample, isMountCalibrated: isImuCalibrated);
    final validatedSample = rawSample.copyWith(quality: quality);

    // Log diagnostic if sample is suspect or rejected during an active field test (throttled for suspect)
    if (_fieldTestLogger.isSessionActive) {
      if (quality.isRejected) {
        _fieldTestLogger.log(
          eventType: FieldTestEventType.sampleRejected,
          message: 'Sample validation rejected: ${quality.reasons.join(", ")}',
          severity: EventSeverity.critical,
          telemetryTimestamp: rawSample.timestamp,
          speed: speedMph,
          overallConfidence: quality.overallConfidence,
        );
      } else if (quality.isSuspect) {
        final shouldLogSuspect = _lastSuspectLogTime == null || now.difference(_lastSuspectLogTime!).inMilliseconds >= 2000;
        if (shouldLogSuspect) {
          _lastSuspectLogTime = now;
          _fieldTestLogger.log(
            eventType: FieldTestEventType.sampleSuspect,
            message: 'Sample validation suspect: ${quality.reasons.join(", ")}',
            severity: EventSeverity.warning,
            telemetryTimestamp: rawSample.timestamp,
            speed: speedMph,
            overallConfidence: quality.overallConfidence,
          );
        }
      }
    }

    _telemetryController.add(validatedSample);
  }

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