PilotVault/Fly App/lib/fly/virtual_sticks.dart

import 'dart:async';
import 'dart:ui' show Offset;

import 'package:flutter/foundation.dart';

/// Which stick does what, as on the remote (RC `AircraftMappingStyle`).
enum StickMode {
  /// Left: forward/back + yaw; right: throttle + sideways.
  mode1('Mode 1'),

  /// Left: throttle + yaw; right: forward/back + sideways. DJI's default.
  mode2('Mode 2'),

  /// Left: forward/back + sideways; right: throttle + yaw.
  mode3('Mode 3');

  const StickMode(this.label);
  final String label;
}

/// What a full stick deflection means.
@immutable
class StickLimits {
  const StickLimits({this.horizontal = 5, this.vertical = 2, this.yaw = 60});

  /// GO 4's Wi-Fi joystick default is gentle; these are a step below P mode.
  static const StickLimits normal = StickLimits();
  static const StickLimits gentle = StickLimits(horizontal: 2, vertical: 1, yaw: 30);

  final double horizontal; // m/s
  final double vertical; // m/s
  final double yaw; // °/s
}

/// One packet for the native stick loop: body-frame velocities and a yaw rate.
@immutable
class StickCommand {
  const StickCommand({this.forward = 0, this.right = 0, this.yaw = 0, this.up = 0});

  static const StickCommand zero = StickCommand();

  final double forward; // m/s
  final double right; // m/s
  final double yaw; // °/s, clockwise
  final double up; // m/s

  bool get isZero => forward == 0 && right == 0 && yaw == 0 && up == 0;

  @override
  bool operator ==(Object other) =>
      other is StickCommand && other.forward == forward && other.right == right && other.yaw == yaw && other.up == up;

  @override
  int get hashCode => Object.hash(forward, right, yaw, up);

  @override
  String toString() => 'StickCommand(f ${forward.toStringAsFixed(2)}, r ${right.toStringAsFixed(2)}, '
      'yaw ${yaw.toStringAsFixed(1)}, up ${up.toStringAsFixed(2)})';
}

/// Stick positions → [StickCommand] (plan P8). Pure, so the mapping is unit-tested.
///
/// Positions are −1…1 with x to the right and **y up** (push = +1).
abstract final class StickMapper {
  /// Below this deflection the stick counts as centred (thumbs never rest at 0).
  static const double deadzone = 0.08;

  /// Share of the cubic term: small deflections are finer than a linear map.
  static const double expo = 0.4;

  /// The curve one axis goes through: deadzone, rescale, expo.
  static double shape(double v) {
    final double a = v.abs().clamp(0.0, 1.0);
    if (a <= deadzone) return 0;
    final double x = (a - deadzone) / (1 - deadzone);
    return (expo * x * x * x + (1 - expo) * x) * v.sign;
  }

  static StickCommand command(Offset left, Offset right, {StickMode mode = StickMode.mode2, StickLimits limits = StickLimits.normal}) {
    final (double throttle, double yaw, double pitch, double roll) = switch (mode) {
      StickMode.mode1 => (right.dy, left.dx, left.dy, right.dx),
      StickMode.mode2 => (left.dy, left.dx, right.dy, right.dx),
      StickMode.mode3 => (right.dy, right.dx, left.dy, left.dx),
    };
    double r(double v) => v == 0 ? 0 : v; // no −0.0 in packets or tests
    return StickCommand(
      forward: r(shape(pitch) * limits.horizontal),
      right: r(shape(roll) * limits.horizontal),
      yaw: r(shape(yaw) * limits.yaw),
      up: r(shape(throttle) * limits.vertical),
    );
  }

  /// Clamps a finger's offset from the pad centre into the unit circle, y up.
  static Offset normalise(Offset fromCentre, double radius) {
    if (radius <= 0) return Offset.zero;
    Offset v = Offset(fromCentre.dx / radius, -fromCentre.dy / radius);
    final double len = v.distance;
    if (len > 1) v = v / len;
    return v;
  }
}

/// The on-screen sticks' state while they fly the aircraft.
///
/// [setLeft] / [setRight] send at once when the command changes, and a
/// [heartbeat] timer repeats it while a stick is off centre — the native loop
/// hovers once a command is 300 ms old, so a held stick must keep saying so.
/// Centred sticks send one zero and go quiet.
///
/// [open] is what the pilot asked for; the aircraft's own answer
/// (`virtualStickEnabled` in the telemetry) closes it through [onAircraft] when
/// the remote's mode switch or RTH takes over.
class VirtualStickController extends ChangeNotifier {
  VirtualStickController({required this.send, this.heartbeat = const Duration(milliseconds: 100)});

  /// Delivers one command to the bridge.
  final Future<void> Function(StickCommand c) send;
  final Duration heartbeat;

  StickMode mode = StickMode.mode2;
  StickLimits limits = StickLimits.normal;

  Offset left = Offset.zero;
  Offset right = Offset.zero;

  /// The sticks are on and flying the aircraft.
  bool open = false;

  /// Why the sticks last closed without the pilot asking, for one snackbar.
  String? closedReason;

  /// The aircraft confirmed virtual-stick mode since [open] turned on.
  bool _confirmed = false;

  StickCommand _last = StickCommand.zero;
  Timer? _beat;

  StickCommand get command => StickMapper.command(left, right, mode: mode, limits: limits);

  void setLeft(Offset v) {
    left = v;
    _changed();
  }

  void setRight(Offset v) {
    right = v;
    _changed();
  }

  /// Both sticks back to centre (a finger lifted off a pad does this too).
  void centre() {
    left = right = Offset.zero;
    _changed();
  }

  void _changed() {
    notifyListeners();
    if (!open) return;
    final StickCommand c = command;
    if (c != _last) _emit(c);
    if (c.isZero) {
      _beat?.cancel();
      _beat = null;
    } else {
      _beat ??= Timer.periodic(heartbeat, (_) => _emit(command));
    }
  }

  void _emit(StickCommand c) {
    _last = c;
    unawaited(send(c).catchError((Object _) {}));
  }

  /// The aircraft entered virtual-stick mode.
  void opened() {
    open = true;
    _confirmed = false;
    closedReason = null;
    left = right = Offset.zero;
    _last = StickCommand.zero;
    notifyListeners();
  }

  /// Stops sending; [reason] is shown when the aircraft, not the pilot, ended it.
  void close([String? reason]) {
    _beat?.cancel();
    _beat = null;
    final bool wasOpen = open;
    open = false;
    left = right = Offset.zero;
    _last = StickCommand.zero;
    if (wasOpen) closedReason = reason;
    notifyListeners();
  }

  /// Follows the flight controller: once it has confirmed the mode, a report
  /// that it left (remote mode switch), or an RTH / landing, closes the sticks.
  void onAircraft({required bool? enabled, required bool goingHome, required bool landing}) {
    if (!open) return;
    if (enabled == true) _confirmed = true;
    if (_confirmed && enabled == false) {
      close('The remote took back control');
    } else if (goingHome) {
      close('Return to Home took over');
    } else if (landing) {
      close('The aircraft is landing');
    }
  }

  @override
  void dispose() {
    _beat?.cancel();
    super.dispose();
  }
}