dmc/app/lib/domain/mission/route_geometry.dart

import 'dart:math' as math;

import 'package:latlong2/latlong.dart';

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

/// Ein Streckenabschnitt der Flugpfad-Darstellung: entweder ein
/// Geradenstueck zwischen zwei Tangentenpunkten oder ein abgetasteter
/// Kreisbogen (Fillet) an einer Kursaenderung (Architektur-Doku 3.7/4.6).
class RouteSegment {
  const RouteSegment({required this.points, required this.bad});

  final List<LatLng> points;
  final bool bad;
}

class RouteGeometry {
  const RouteGeometry({required this.segments, required this.vertexBad});

  final List<RouteSegment> segments;

  /// Pro Wegpunkt: true, wenn die Kursaenderung an diesem Punkt mit dem
  /// minimalen Kurvenradius der Drohne nicht fliegbar ist (Tangentenlaenge
  /// ueberschreitet Fangradius oder angrenzendes Geradenstueck, oder der
  /// Kurswinkel ist zu scharf).
  final List<bool> vertexBad;

  static const empty = RouteGeometry(segments: [], vertexBad: []);
}

class _V {
  const _V(this.x, this.y);
  final double x;
  final double y;

  _V operator -(_V other) => _V(x - other.x, y - other.y);
  _V operator +(_V other) => _V(x + other.x, y + other.y);
  _V scaled(double f) => _V(x * f, y * f);
  double get length => math.sqrt(x * x + y * y);
  double dot(_V other) => x * other.x + y * other.y;
  double cross(_V other) => x * other.y - y * other.x;
}

class _MetricPoint {
  const _MetricPoint(this.pos, this.alt, this.speed);
  final _V pos;
  final double alt;
  final double speed;
}

class _VertexArc {
  const _VertexArc({
    required this.t1,
    required this.t2,
    required this.center,
    required this.radius,
    required this.angleStart,
    required this.angleEnd,
  });

  final _V t1;
  final _V t2;
  final _V center;
  final double radius;
  final double angleStart;
  final double angleEnd;
}

const _maxTurnAngle = 160 * math.pi / 180;

/// Baut den physikalisch fundierten Flugpfad: Geradenstuecke + tangentiale
/// Kreisboegen an jeder Kursaenderung (Doku 3.7), abgeleitet aus dem
/// minimalen Kurvenradius der Drohne. Ersetzt einen naiven Spline durch
/// alle Punkte (Doku 4.6) - ein Spline wuerde unrealistische Kurvenradien
/// suggerieren und ein falsches Sicherheitsgefuehl bei engen Kurven geben.
///
/// Windkorrektur des effektiven Kurvenradius (HTML-Demonstrator:
/// vertexGroundSpeed-Anpassung) ist noch nicht portiert, da das Wind-System
/// (Doku 3.8) in der Flutter-App noch nicht existiert.
RouteGeometry buildRouteGeometry(
  List<Waypoint> waypoints, {
  required double minTurnRadius,
  required double maxClimbRate,
  required double maxDescentRate,
}) {
  final n = waypoints.length;
  if (n == 0) return RouteGeometry.empty;

  final ref = LatLng(waypoints.first.lat, waypoints.first.lon);
  final pts = [for (final w in waypoints) _toMeters(w, ref)];

  final vertexBad = List<bool>.filled(n, false);
  final arcs = List<_VertexArc?>.filled(n, null);

  for (var i = 1; i < n - 1; i++) {
    final pPrev = pts[i - 1];
    final pCur = pts[i];
    final pNext = pts[i + 1];

    final inVec = pCur.pos - pPrev.pos;
    final outVec = pNext.pos - pCur.pos;
    final inLen = inVec.length;
    final outLen = outVec.length;
    final vIn = inLen > 1e-6 ? inVec.scaled(1 / inLen) : const _V(1, 0);
    final vOut = outLen > 1e-6 ? outVec.scaled(1 / outLen) : const _V(1, 0);

    final dot = vIn.dot(vOut).clamp(-1.0, 1.0);
    final turnAngle = math.acos(dot);
    if (turnAngle <= 0.02) continue;

    final cappedAngle = math.min(turnAngle, _maxTurnAngle);
    final t = minTurnRadius * math.tan(cappedAngle / 2);

    var bad = false;
    if (turnAngle >= _maxTurnAngle) bad = true;
    if (t > inLen * 0.9 || t > outLen * 0.9) bad = true;
    if (t > waypoints[i].catchRadiusM) bad = true;

    final turnLeft = vIn.cross(vOut) > 0;
    final normalIn =
        turnLeft ? _V(-vIn.y, vIn.x) : _V(vIn.y, -vIn.x);

    final t1 = pCur.pos - vIn.scaled(t);
    final t2 = pCur.pos + vOut.scaled(t);
    final center = t1 + normalIn.scaled(minTurnRadius);
    final angleStart = math.atan2(t1.y - center.y, t1.x - center.x);
    final angleEnd = angleStart + (turnLeft ? cappedAngle : -cappedAngle);

    vertexBad[i] = bad;
    arcs[i] = _VertexArc(
      t1: t1,
      t2: t2,
      center: center,
      radius: minTurnRadius,
      angleStart: angleStart,
      angleEnd: angleEnd,
    );
  }

  final legClimbBad = List<bool>.filled(math.max(n - 1, 0), false);
  for (var i = 0; i < n - 1; i++) {
    final a = pts[i];
    final b = pts[i + 1];
    final horiz = (b.pos - a.pos).length;
    legClimbBad[i] = _isClimbBad(
      a.alt,
      b.alt,
      horiz,
      b.speed,
      maxClimbRate: maxClimbRate,
      maxDescentRate: maxDescentRate,
    );
  }

  final segments = <RouteSegment>[];
  for (var leg = 0; leg < n - 1; leg++) {
    final startArc = arcs[leg];
    final endArc = arcs[leg + 1];
    final startPt = (leg == 0) ? pts[0].pos : (startArc?.t2 ?? pts[leg].pos);
    final endPt =
        (leg == n - 2) ? pts[n - 1].pos : (endArc?.t1 ?? pts[leg + 1].pos);

    segments.add(RouteSegment(
      points: [_fromMeters(startPt, ref), _fromMeters(endPt, ref)],
      bad: legClimbBad[leg],
    ));

    if (leg + 1 <= n - 2 && endArc != null) {
      const steps = 14;
      final arcPoints = [
        for (var k = 0; k <= steps; k++)
          _fromMeters(
            _pointOnCircle(
              endArc.center,
              endArc.radius,
              endArc.angleStart +
                  (endArc.angleEnd - endArc.angleStart) * k / steps,
            ),
            ref,
          ),
      ];
      segments.add(RouteSegment(points: arcPoints, bad: vertexBad[leg + 1]));
    }
  }

  return RouteGeometry(segments: segments, vertexBad: vertexBad);
}

_V _pointOnCircle(_V center, double radius, double angle) =>
    _V(center.x + radius * math.cos(angle), center.y + radius * math.sin(angle));

bool _isClimbBad(
  double altA,
  double altB,
  double horizDist,
  double speed, {
  required double maxClimbRate,
  required double maxDescentRate,
}) {
  final vert = altB - altA;
  if (horizDist < 0.001) return vert.abs() > 0.001;
  final rate = vert / (horizDist / speed);
  if (rate > 0) return rate > maxClimbRate;
  return rate.abs() > maxDescentRate;
}

// Equirektangulare Naeherung relativ zum ersten Wegpunkt - ausreichend fuer
// missionstypische Distanzen (HTML-Demonstrator: toMeters()/fromMeters()).
_MetricPoint _toMeters(Waypoint w, LatLng ref) {
  final dLat = (w.lat - ref.latitude) * 110540;
  final dLon =
      (w.lon - ref.longitude) * 111320 * math.cos(ref.latitude * math.pi / 180);
  return _MetricPoint(_V(dLon, dLat), w.altitudeM, w.speedMs);
}

LatLng _fromMeters(_V m, LatLng ref) {
  return LatLng(
    ref.latitude + m.y / 110540,
    ref.longitude + m.x / (111320 * math.cos(ref.latitude * math.pi / 180)),
  );
}