dmc/app/lib/transport/msp/msp_telemetry_poller.dart

import 'dart:async';

import '../flight_controller_link.dart';
import 'msp_client.dart';
import 'msp_commands.dart';
import 'msp_telemetry_codec.dart';

/// Eigener Poll-Takt ueber [MspClient] (Doku Kommunikationsschicht Abschnitt
/// 4, "Polling"): MSP liefert nichts von selbst. Rundenbasiert statt
/// paralleler Timer pro Nachrichtengruppe, weil [MspClient] Anfragen
/// ohnehin serialisiert (immer nur eine offene Anfrage) - ein
/// Round-Robin-Zyklus bildet die in der Doku genannten Raten ab, ohne dass
/// sich Anfragen ueberlappen koennten:
///
/// - Position/Lage/Hoehe (5-10 Hz): jeden Zyklus (`MSP_RAW_GPS` +
///   `MSP_ALTITUDE`).
/// - Status/Flugmodus/Akku/Link/Temperaturen (2 Hz): jeden
///   [_statusEveryNCycles]-ten Zyklus (`MSP2_INAV_STATUS`, `MSP_NAV_STATUS`,
///   `MSP2_INAV_ANALOG`, `MSP2_INAV_GET_LINK_STATS`,
///   `MSP2_INAV_TEMPERATURES`) - Flugmodus wird aus derselben
///   `MSP_NAV_STATUS`-Antwort gelesen wie der aktive Wegpunkt, keine
///   zusaetzliche Anfrage noetig. `sensorStatus` ebenso aus derselben
///   `MSP2_INAV_STATUS`-Antwort wie der ARMED-Status.
///
/// Der Strom wird NICHT auf einen gueltigen GPS-Fix gegated - jeder Zyklus
/// emittiert ein [TelemetryFrame], auch ohne Fix (dann mit `hasFix: false`
/// und der vom FC gemeldeten, ggf. bedeutungslosen Position). Konsumenten
/// wie FlyScreen entscheiden selbst anhand von [TelemetryFrame.hasFix], ob
/// sie die Position anzeigen.
class MspTelemetryPoller {
  MspTelemetryPoller(this._client);

  final MspClient _client;
  final _framesController = StreamController<TelemetryFrame>.broadcast();

  static const _cycleInterval = Duration(milliseconds: 150); // ~6-7 Hz
  static const _statusEveryNCycles = 3; // ~2 Hz bei 150-ms-Takt

  bool _running = false;
  int _cycle = 0;
  bool _armed = false;
  int? _activeWaypointIndex;
  int _batteryPercent = 0;
  double _batteryVoltage = 0;
  double _currentA = 0;
  int _linkQuality = 0;
  int _snrDb = 0;
  int _navMode = 0;
  String _flightMode = 'Idle';
  int _sensorStatusBits = 0;
  List<double?> _temperaturesC = const [null, null, null];

  Stream<TelemetryFrame> get frames => _framesController.stream;

  void start() {
    if (_running) return;
    _running = true;
    unawaited(_loop());
  }

  void stop() {
    _running = false;
  }

  Future<void> _loop() async {
    while (_running) {
      try {
        await _runCycle();
      } catch (_) {
        // Eine einzelne fehlgeschlagene Anfrage (Timeout/CRC/Transport
        // getrennt) darf den Takt nicht stoppen - der naechste Zyklus
        // startet nach der Wartezeit regulaer weiter (Doku 4: Zeitgrenze +
        // Wiederholung; Wiederverbinden passiert bereits auf Transport-
        // Ebene, siehe BluetoothClassicTransport).
      }
      await Future<void>.delayed(_cycleInterval);
    }
  }

  Future<void> _runCycle() async {
    final gps = parseMspRawGps(await _client.request(MspCommands.rawGps));
    final altitudePayload = await _client.request(MspCommands.altitude);
    final altitudeM = parseMspAltitudeMeters(altitudePayload);
    final verticalSpeedMs = parseMspAltitudeVerticalSpeedMs(altitudePayload);

    _cycle++;
    if (_cycle % _statusEveryNCycles == 0) {
      final statusPayload = await _client.request(MspCommands.inavStatus);
      _armed = parseMspInavStatusArmed(statusPayload);
      _sensorStatusBits = parseMspInavStatusSensorStatus(statusPayload);
      final navStatusPayload = await _client.request(MspCommands.navStatus);
      _activeWaypointIndex = parseMspNavStatusActiveWaypoint(navStatusPayload);
      final navMode = parseMspNavMode(navStatusPayload);
      _navMode = navMode.mode;
      _flightMode = navMode.formatted;
      final analogPayload = await _client.request(MspCommands.inavAnalog);
      _batteryPercent = parseMspInavAnalogBatteryPercent(analogPayload);
      _batteryVoltage = parseMspInavAnalogVoltage(analogPayload);
      _currentA = parseMspInavAnalogAmperage(analogPayload);
      final linkStatsPayload = await _client.request(MspCommands.inavLinkStats);
      _linkQuality = parseMspLinkQuality(linkStatsPayload);
      _snrDb = parseMspLinkStatsSnr(linkStatsPayload);
      final temperaturesPayload =
          await _client.request(MspCommands.inavTemperatures);
      _temperaturesC = parseMspInavTemperaturesC(temperaturesPayload);
    }

    if (!_framesController.isClosed) {
      _framesController.add(TelemetryFrame(
        lat: gps.lat,
        lon: gps.lon,
        hasFix: gps.hasFix,
        numSat: gps.numSat,
        altitudeM: altitudeM,
        gpsAltitudeM: gps.altitudeM,
        speedMs: gps.speedMs,
        headingDeg: gps.headingDeg,
        armed: _armed,
        batteryPercent: _batteryPercent,
        batteryVoltage: _batteryVoltage,
        currentA: _currentA,
        linkQuality: _linkQuality,
        snrDb: _snrDb,
        hdop: gps.hdop,
        navMode: _navMode,
        flightMode: _flightMode,
        sensorStatusBits: _sensorStatusBits,
        temperaturesC: _temperaturesC,
        verticalSpeedMs: verticalSpeedMs,
        activeWaypointIndex: _activeWaypointIndex,
      ));
    }
  }

  Future<void> dispose() async {
    stop();
    await _framesController.close();
  }
}