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

import 'dart:typed_data';

import '../../domain/waypoint/flat_waypoint_list.dart';
import '../flight_controller_link.dart';

/// `navWaypointActions_e` (Doku Kommunikationsschicht v2 Abschnitt 6,
/// geprueft gegen den iNAV-9.1.0-Quellcode: `navigation.h`). Nur die vier
/// Werte, auf die unser protokollneutrales [WaypointAction] tatsaechlich
/// abbildet, sind hier benannt - RTH/SET_POI/SET_HEAD kennt unser
/// Domainmodell (noch) nicht.
abstract final class MspNavWpAction {
  static const int waypoint = 0x01;
  static const int holdTime = 0x03;
  static const int jump = 0x06;
  static const int land = 0x08;
}

/// `navWaypointFlags_e` (`navigation.h`). [last] markiert den letzten
/// Wegpunkt einer Mission - erst dieses Flag setzt
/// `posControl.waypointListValid` auf dem FC, siehe encodeMspSetWaypoint-Doku.
abstract final class MspNavWpFlag {
  static const int none = 0;
  static const int last = 0xA5;
}

/// p1 ist `int16_t` auf der Leitung (Doku: `navWaypoint_t`) - groesster
/// darstellbarer positiver Wert, nicht 0xFFFF (das waere als int16 -1 und
/// wuerde NAV_WP_ACTION_HOLD_TIME sofort - statt dauerhaft - abschliessen
/// lassen, siehe navigation.c `navOnEnteringState_NAV_STATE_WAYPOINT_...`:
/// "p1 <= 0" beendet das Halten sofort).
const int _mspInt16Max = 0x7FFF;

/// Kodiert einen einzelnen Wegpunkt als 21-Byte MSP_SET_WP-Payload
/// (`MspCommands.setWp`, Doku Kommunikationsschicht v2 Abschnitt 6).
///
/// [wireIndex] ist die 1-basierte FC-interne Wegpunktnummer (WP#1..#15),
/// NICHT der 0-basierte Index in der App-eigenen Liste - iNAV reserviert
/// WP#0 fuer "Home" und WP#255 fuer einen Direkt-Sprung im Poshold-Modus
/// (`navigation.c` `setWaypoint()`). Der Aufrufer (MspFlightControllerLink)
/// ist dafuer verantwortlich, WP#1 zuerst und danach jede weitere Nummer
/// lueckenlos aufsteigend zu senden - der FC nimmt sonst gar nichts an.
///
/// [isLast] setzt [MspNavWpFlag.last] auf dem letzten Wegpunkt - ohne dieses
/// Flag bleibt `posControl.waypointListValid` auf dem FC false, selbst wenn
/// alle Wegpunkte einzeln angekommen sind (siehe `MspCommands.wpGetInfo`,
/// zur Verifikation nach dem Upload).
///
/// Aktions-P1/P2/P3-Belegung (`navigation.c` `getActiveSpeed()`/
/// `setupJumpCounters()`, Aktion fuer Aktion geprueft statt geraten):
/// - WAYPOINT/LAND: p1 = Sollgeschwindigkeit (cm/s), p2 unbenutzt (0).
/// - Unser generisches `loiter` hat im Domainmodell keine eigene Dauer
///   (HTML-Referenz zeigt es als endloses Kreisen um den Punkt mit dem
///   `loiterRadius` des Drohnenprofils, kein Zeitfeld) - kodiert als
///   NAV_WP_ACTION_HOLD_TIME mit dem groesstmoeglichen p1 (`_mspInt16Max`
///   Sekunden, ueber 9 Stunden - laenger als jeder realistische Flug), p2 =
///   Sollgeschwindigkeit (cm/s, dort das P1-Aequivalent fuer HOLD_TIME).
/// - JUMP: p1 = 1-basierte Ziel-WP# (der FC zieht beim Empfang selbst 1 ab,
///   `setWaypoint()`: "make index (vice WP #)"), p2 = statische
///   Wiederholzahl (-1 = endlos, `jumpRepeatCount == null`). p3 haelt bei
///   JUMP den volatilen Wiederholzaehler - der FC initialisiert ihn beim
///   Missionsstart selbst aus p2 (`setupJumpCounters()`), wir spiegeln das
///   hier nur defensiv.
/// - Bei allen anderen Aktionen ist p3 das Hoehenmodus-Bitfeld
///   (`NAV_WP_ALTMODE`): 0 = relativ zum Startpunkt - passend zu unserem
///   `altitudeM` (siehe TelemetryFrame.altitudeM-Doku: "keine absolute Hoehe
///   ueber Meeresspiegel"), 1 waere absolut (AMSL) und wird hier nie gesetzt.
Uint8List encodeMspSetWaypoint(
  Waypoint waypoint, {
  required int wireIndex,
  required bool isLast,
}) {
  final speedP1 = (waypoint.speedMs * 100).round().clamp(0, _mspInt16Max);

  final int action;
  final int p1;
  final int p2;
  final int p3;
  switch (waypoint.action) {
    case WaypointAction.none:
      action = MspNavWpAction.waypoint;
      p1 = speedP1;
      p2 = 0;
      p3 = 0;
    case WaypointAction.loiter:
      action = MspNavWpAction.holdTime;
      p1 = _mspInt16Max;
      p2 = speedP1;
      p3 = 0;
    case WaypointAction.landing:
      action = MspNavWpAction.land;
      p1 = speedP1;
      p2 = 0;
      p3 = 0;
    case WaypointAction.jump:
      action = MspNavWpAction.jump;
      final target = ((waypoint.jumpTargetIndex ?? 0) + 1)
          .clamp(1, _mspInt16Max);
      final repeat = (waypoint.jumpRepeatCount ?? -1).clamp(-1, _mspInt16Max);
      p1 = target;
      p2 = repeat;
      p3 = repeat;
  }

  final payload = ByteData(21);
  payload.setUint8(0, wireIndex);
  payload.setUint8(1, action);
  payload.setInt32(2, (waypoint.lat * 1e7).round(), Endian.little);
  payload.setInt32(6, (waypoint.lon * 1e7).round(), Endian.little);
  payload.setInt32(10, (waypoint.altitudeM * 100).round(), Endian.little);
  payload.setInt16(14, p1, Endian.little);
  payload.setInt16(16, p2, Endian.little);
  payload.setInt16(18, p3, Endian.little);
  payload.setUint8(20, isLast ? MspNavWpFlag.last : MspNavWpFlag.none);
  return payload.buffer.asUint8List();
}

/// Antwort auf `MspCommands.wpGetInfo` (4 Byte): ob der FC die aktuell
/// hochgeladene Mission als vollstaendig/gueltig fuehrt, und wie viele
/// Wegpunkte er dafuer zaehlt - Grundlage der Upload-Verifikation (Doku
/// 2.2/4.5).
class MspWpGetInfo {
  const MspWpGetInfo({
    required this.maxWaypoints,
    required this.isValid,
    required this.waypointCount,
  });

  final int maxWaypoints;
  final bool isValid;
  final int waypointCount;
}

MspWpGetInfo parseMspWpGetInfo(Uint8List payload) {
  return MspWpGetInfo(
    maxWaypoints: payload[1],
    isValid: payload[2] != 0,
    waypointCount: payload[3],
  );
}

/// Sonderwert fuer [encodeMspGetWaypointRequest]/`MspCommands.getWp`: WP#0
/// ist laut `navigation.c` `getWaypoint()` der Homepoint, kein echter
/// Missions-Wegpunkt.
const int mspWpNumberHome = 0;

/// Kodiert den 1-Byte-Request-Payload fuer `MspCommands.getWp` (`MSP_WP`).
Uint8List encodeMspGetWaypointRequest(int wpNumber) =>
    Uint8List.fromList([wpNumber]);

/// Parst die 21-Byte-Antwort auf `MspCommands.getWp` fuer `wp_no ==
/// [mspWpNumberHome]` (siehe dortige Doku zu `getWaypoint()`/`GPS_home`).
///
/// Liefert `null`, wenn der FC noch keinen Homepoint gesetzt hat: dafuer
/// gibt es kein eigenes Flag in der Antwort selbst, `getWaypoint()`
/// initialisiert `lat`/`lon`/`alt` aber generell auf 0 und ueberschreibt
/// das fuer WP#0 nur, wenn `STATE(GPS_FIX_HOME)` gesetzt ist - (0,0) ist
/// also der FC-eigene "kein Homepoint"-Wert, nicht eine echte Koordinate
/// am Nullmeridian/Aequator.
HomePoint? parseMspHomePoint(Uint8List payload) {
  final d = ByteData.sublistView(payload);
  final lat = d.getInt32(2, Endian.little) / 1e7;
  final lon = d.getInt32(6, Endian.little) / 1e7;
  if (lat == 0 && lon == 0) return null;
  final altCm = d.getInt32(10, Endian.little);
  return HomePoint(lat: lat, lon: lon, altitudeM: altCm / 100);
}