AR-Autopilot/display/lib/data/autopilot_state.dart

// =============================================================================
//  data/autopilot_state.dart — Live autopilot data model
// =============================================================================
//
//  Dual-mode: demo simulation (Sprint 4) or live USB serial (Sprint 7).
//
//  Default constructor starts in demo mode (animated vessel simulation).
//  Call [connectToSerial] to switch to live data from the AR-Concentrador.
//  If the serial link drops, the state automatically falls back to demo.
//
//  The public API (fields + methods) is identical in both modes — the UI
//  never needs to know which mode is active; it reads [isConnected] for
//  the status indicator only.
// =============================================================================

import 'dart:async';
import 'dart:math' as math;

import 'package:flutter/foundation.dart';

import '../services/concentrador_service.dart';
import '../services/parp_codec.dart';
import '../widgets/themed/mode_selector.dart';

class AutopilotState extends ChangeNotifier {
  // ── Navigation data ──────────────────────────────────────────────────────────
  double headingDeg  = 125.0;
  double setpointDeg = 125.0;
  double rudderDeg   = 0.0;
  double sogKn       = 6.2;
  double cogDeg      = 127.0;
  double rotDpm      = 0.0;
  double depthM      = 42.5;

  // ── Autopilot state ──────────────────────────────────────────────────────────
  AutopilotMode mode = AutopilotMode.standby;

  /// True when the USB serial link to the concentrador is active.
  bool isConnected = false;

  // ── Serial service ───────────────────────────────────────────────────────────
  ConcentradorService? _service;

  // ── Demo simulation ──────────────────────────────────────────────────────────
  Timer? _demoTimer;
  final _rng = math.Random();

  AutopilotState() {
    _startDemo();
  }

  // ---------------------------------------------------------------------------
  //  Serial connection
  // ---------------------------------------------------------------------------

  /// Connect to the AR-Concentrador via USB serial.
  ///
  /// Stops the demo timer and switches to live data.
  /// Falls back to demo automatically if the link drops.
  ///
  /// Throws if the ports cannot be opened (caller should catch and show error).
  Future<void> connectToSerial({
    required String rxPort,
    required String txPort,
    int stationId = 2,
  }) async {
    _demoTimer?.cancel();
    _demoTimer = null;

    _service?.disconnect();
    _service = ConcentradorService(
      rxPort: rxPort,
      txPort: txPort,
      stationId: stationId,
    );
    _service!.onStatus = _onSerialStatus;
    _service!.onConnectionChanged = _onConnectionChanged;

    await _service!.connect(); // may throw — caller handles
  }

  /// Disconnect the serial link and return to demo mode.
  Future<void> disconnectSerial() async {
    await _service?.disconnect();
    _service = null;
    _startDemo();
  }

  void _onConnectionChanged(bool connected) {
    isConnected = connected;
    if (!connected) {
      // Link dropped — fall back to animated demo so the UI stays alive.
      _startDemo();
    }
    notifyListeners();
  }

  void _onSerialStatus(ParpStatus status) {
    isConnected = true;
    headingDeg  = status.headingDeg;
    setpointDeg = status.setpointDeg;
    rudderDeg   = status.rudderDeg;
    mode        = status.mode;
    notifyListeners();
  }

  // ---------------------------------------------------------------------------
  //  Demo simulation
  // ---------------------------------------------------------------------------

  void _startDemo() {
    _demoTimer?.cancel();
    _demoTimer = Timer.periodic(const Duration(milliseconds: 500), (_) => _tick());
  }

  void _tick() {
    switch (mode) {
      case AutopilotMode.standby:
        headingDeg = (headingDeg + (_rng.nextDouble() - 0.5) * 0.5) % 360;
        if (headingDeg < 0) headingDeg += 360;
        rudderDeg  = (rudderDeg + (_rng.nextDouble() - 0.5) * 0.8).clamp(-5.0, 5.0);
        rotDpm     = rudderDeg * 0.3 + (_rng.nextDouble() - 0.5) * 0.2;

      case AutopilotMode.headingHold:
      case AutopilotMode.trackKeep:
        final error = _angleDiff(setpointDeg, headingDeg);
        rudderDeg  = (error * 1.2 + (_rng.nextDouble() - 0.5) * 0.5).clamp(-35.0, 35.0);
        headingDeg = (headingDeg + error * 0.025 + (_rng.nextDouble() - 0.5) * 0.08) % 360;
        if (headingDeg < 0) headingDeg += 360;
        rotDpm = error * 0.4;
    }

    cogDeg = (headingDeg + rudderDeg * 0.15 + (_rng.nextDouble() - 0.5) * 0.3) % 360;
    if (cogDeg < 0) cogDeg += 360;

    notifyListeners();
  }

  double _angleDiff(double target, double current) {
    double d = (target - current) % 360;
    if (d > 180) d -= 360;
    if (d < -180) d += 360;
    return d;
  }

  // ---------------------------------------------------------------------------
  //  Commands — work in both demo and serial modes
  // ---------------------------------------------------------------------------

  void engage() {
    setpointDeg = headingDeg;
    mode        = AutopilotMode.headingHold;
    _service?.sendEngage(headingDeg);
    notifyListeners();
  }

  void disengage() {
    mode = AutopilotMode.standby;
    _service?.sendDisengage();
    notifyListeners();
  }

  void adjustSetpoint(double deltaDeg) {
    if (mode != AutopilotMode.headingHold) return;
    setpointDeg = (setpointDeg + deltaDeg) % 360;
    if (setpointDeg < 0) setpointDeg += 360;

    // Route to the appropriate serial command
    if (_service != null && isConnected) {
      if (deltaDeg == -10) _service!.sendPortTen(setpointDeg);
      else if (deltaDeg == -1) _service!.sendPortOne(setpointDeg);
      else if (deltaDeg == 1) _service!.sendStbdOne(setpointDeg);
      else if (deltaDeg == 10) _service!.sendStbdTen(setpointDeg);
      else _service!.sendSetHeading(setpointDeg);
    }

    notifyListeners();
  }

  void selectMode(AutopilotMode newMode) {
    switch (newMode) {
      case AutopilotMode.standby:
        disengage();
      case AutopilotMode.headingHold:
        engage();
      case AutopilotMode.trackKeep:
        mode        = AutopilotMode.trackKeep;
        setpointDeg = headingDeg;
        notifyListeners();
    }
  }

  // ---------------------------------------------------------------------------
  //  Port discovery (delegate to service layer)
  // ---------------------------------------------------------------------------

  static List<String> availablePorts() => ConcentradorService.availablePorts();

  @override
  void dispose() {
    _demoTimer?.cancel();
    _service?.disconnect();
    super.dispose();
  }
}