import 'dart:math' as math;
import '../models/landmark.dart';

class ResectionCalculator {
  static const double earthRadiusKm = 6371.0;
  static const double magneticDeclinationJordan = 5.5; // +5.5° East avg in Jordan

  /// Calculates true azimuth from magnetic compass heading.
  /// When Magnetic North is East of True North (Jordan ~ +5.5° East),
  /// True Grid Azimuth = (Magnetic Heading - Declination).
  static double getTrueAzimuth(double magneticHeadingDeg, [double customOffset = 5.5]) {
    return (magneticHeadingDeg - customOffset + 360.0) % 360.0;
  }

  /// Calculates observer position from 2 or more landmark observations using triangulation resection.
  static ResectionResult? calculatePosition(List<ResectionObservation> observations) {
    if (observations.length < 2) return null;

    // Use first two observations for base intersection
    final obsA = observations[0];
    final obsB = observations[1];

    final backBearingA = ((obsA.trueAzimuthDeg + 180.0) % 360.0) * (math.pi / 180.0);
    final backBearingB = ((obsB.trueAzimuthDeg + 180.0) % 360.0) * (math.pi / 180.0);

    final latA = obsA.landmark.lat;
    final lngA = obsA.landmark.lng;
    final latB = obsB.landmark.lat;
    final lngB = obsB.landmark.lng;

    final latMidRad = ((latA + latB) / 2.0) * (math.pi / 180.0);
    final cosLat = math.cos(latMidRad);

    final xA = lngA * cosLat;
    final yA = latA;
    final xB = lngB * cosLat;
    final yB = latB;

    final sinA = math.sin(backBearingA);
    final cosA = math.cos(backBearingA);
    final sinB = math.sin(backBearingB);
    final cosB = math.cos(backBearingB);

    final det = sinA * cosB - cosA * sinB;
    if (det.abs() < 0.0001) {
      // Lines are parallel or collinear
      return null;
    }

    final dx = xB - xA;
    final dy = yB - yA;
    final tA = (dx * cosB - dy * sinB) / det;

    final xIntersect = xA + tA * sinA;
    final yIntersect = yA + tA * cosA;

    var calcLng = xIntersect / cosLat;
    var calcLat = yIntersect;

    // Base angular GDOP for 2 landmarks based on cut angle (sin(det)) and landmark distances
    final cutAngleSin = det.abs();
    final distADeg = math.sqrt(math.pow((xIntersect - xA), 2) + math.pow((yIntersect - yA), 2));
    final distBDeg = math.sqrt(math.pow((xIntersect - xB), 2) + math.pow((yIntersect - yB), 2));
    final approxDistMeters = ((distADeg + distBDeg) / 2.0) * 111320.0;
    
    // Compass measurement uncertainty (~1.5 deg = ~0.026 rad) scaled by GDOP (1 / sin(cutAngle))
    double estimatedAccuracy = math.max(10.0, (approxDistMeters * 0.026) / math.max(0.2, cutAngleSin));

    if (observations.length >= 3) {
      final obsC = observations[2];
      final backBearingC = ((obsC.trueAzimuthDeg + 180.0) % 360.0) * (math.pi / 180.0);
      final latC = obsC.landmark.lat;
      final lngC = obsC.landmark.lng;
      final xC = lngC * cosLat;
      final yC = latC;
      final sinC = math.sin(backBearingC);
      final cosC = math.cos(backBearingC);

      // Solve intersection of B and C
      final detBC = sinB * cosC - cosB * sinC;
      // Solve intersection of A and C
      final detAC = sinA * cosC - cosA * sinC;

      if (detBC.abs() >= 0.0001 && detAC.abs() >= 0.0001) {
        final dxBC = xC - xB;
        final dyBC = yC - yB;
        final tB = (dxBC * cosC - dyBC * sinC) / detBC;
        final xBC = xB + tB * sinB;
        final yBC = yB + tB * cosB;

        final dxAC = xC - xA;
        final dyAC = yC - yA;
        final tAc = (dxAC * cosC - dyAC * sinC) / detAC;
        final xAC = xA + tAc * sinA;
        final yAC = yA + tAc * cosA;

        // Centroid of the classic military "Triangle of Error" (مثلث الخطأ)
        final xCentroid = (xIntersect + xBC + xAC) / 3.0;
        final yCentroid = (yIntersect + yBC + yAC) / 3.0;

        calcLng = xCentroid / cosLat;
        calcLat = yCentroid;

        // Compute actual radius of Triangle of Error in meters
        final d1 = math.sqrt(math.pow(xIntersect - xCentroid, 2) + math.pow(yIntersect - yCentroid, 2));
        final d2 = math.sqrt(math.pow(xBC - xCentroid, 2) + math.pow(yBC - yCentroid, 2));
        final d3 = math.sqrt(math.pow(xAC - xCentroid, 2) + math.pow(yAC - yCentroid, 2));
        final triangleRadiusMeters = ((d1 + d2 + d3) / 3.0) * 111320.0;

        // Estimated accuracy is bound by triangle size + residual optical error
        estimatedAccuracy = math.max(5.0, double.parse(triangleRadiusMeters.toStringAsFixed(1)));
      }
    }

    final distances = <String, double>{};
    for (final obs in observations) {
      final d = haversineDistanceKm(calcLat, calcLng, obs.landmark.lat, obs.landmark.lng);
      distances[obs.landmark.id] = d;
    }

    return ResectionResult(
      lat: double.parse(calcLat.toStringAsFixed(6)),
      lng: double.parse(calcLng.toStringAsFixed(6)),
      estimatedAccuracyMeters: estimatedAccuracy,
      observations: observations,
      distanceToLandmarksKm: distances,
    );
  }

  /// Haversine formula to compute great-circle distance between two GPS coordinates in kilometers.
  static double haversineDistanceKm(double lat1, double lon1, double lat2, double lon2) {
    final dLat = (lat2 - lat1) * (math.pi / 180.0);
    final dLon = (lon2 - lon1) * (math.pi / 180.0);
    final a = math.sin(dLat / 2) * math.sin(dLat / 2) +
        math.cos(lat1 * (math.pi / 180.0)) *
            math.cos(lat2 * (math.pi / 180.0)) *
            math.sin(dLon / 2) *
            math.sin(dLon / 2);
    final c = 2 * math.atan2(math.sqrt(a), math.sqrt(1 - a));
    return earthRadiusKm * c;
  }

  /// Great-circle distance between two points in meters.
  static double haversineDistance(double lat1, double lon1, double lat2, double lon2) {
    return haversineDistanceKm(lat1, lon1, lat2, lon2) * 1000.0;
  }

  /// Calculate forward azimuth/bearing in degrees (0-360°) from point 1 to point 2.
  static double calculateBearing(double lat1, double lon1, double lat2, double lon2) {
    final dLon = (lon2 - lon1) * (math.pi / 180.0);
    final lat1Rad = lat1 * (math.pi / 180.0);
    final lat2Rad = lat2 * (math.pi / 180.0);

    final y = math.sin(dLon) * math.cos(lat2Rad);
    final x = math.cos(lat1Rad) * math.sin(lat2Rad) -
        math.sin(lat1Rad) * math.cos(lat2Rad) * math.cos(dLon);
    final bearingRad = math.atan2(y, x);
    return (bearingRad * (180.0 / math.pi) + 360.0) % 360.0;
  }

  /// Single Landmark Rangefinder (Parallax Triangulation from baseline step-off)
  /// حساب المسافة والإحداثيات من معلم جغرافي وحيد بالتحرك على خط أساس
  ///
  /// [landmark]: المعلم الجغرافي المرصود
  /// [azimuth1Deg]: زاوية الرصد الأولى (بالدرجات)
  /// [azimuth2Deg]: زاوية الرصد الثانية بعد التحرك (بالدرجات)
  /// [baselineMeters]: مسافة التحرك العمودي على خط النظر (مثلاً 10م، 11م، 20م، 50م)
  static ResectionResult? calculateSingleLandmarkPolar({
    required TacticalLandmark landmark,
    required double azimuth1Deg,
    required double azimuth2Deg,
    required double baselineMeters,
  }) {
    final trueAzimuth1 = getTrueAzimuth(azimuth1Deg);
    final trueAzimuth2 = getTrueAzimuth(azimuth2Deg);

    // Angular parallax difference in degrees
    var deltaAngleDeg = (trueAzimuth2 - trueAzimuth1).abs();
    if (deltaAngleDeg > 180.0) {
      deltaAngleDeg = 360.0 - deltaAngleDeg;
    }

    if (deltaAngleDeg < 0.05) {
      return null;
    }

    final deltaAngleRad = deltaAngleDeg * (math.pi / 180.0);

    // Exact trigonometric range: D = Baseline / tan(deltaAngle)
    final distanceMeters = baselineMeters / math.tan(deltaAngleRad);
    final distanceKm = distanceMeters / 1000.0;

    // Project observer position from Landmark along Back-Azimuth
    final backBearingDeg = (trueAzimuth1 + 180.0) % 360.0;
    final backBearingRad = backBearingDeg * (math.pi / 180.0);

    final latRad = landmark.lat * (math.pi / 180.0);
    final angularDist = distanceKm / earthRadiusKm;

    final obsLatRad = math.asin(
      math.sin(latRad) * math.cos(angularDist) +
      math.cos(latRad) * math.sin(angularDist) * math.cos(backBearingRad)
    );

    final obsLngRad = (landmark.lng * (math.pi / 180.0)) +
      math.atan2(
        math.sin(backBearingRad) * math.sin(angularDist) * math.cos(latRad),
        math.cos(angularDist) - math.sin(latRad) * math.sin(obsLatRad)
      );

    final calcLat = obsLatRad * (180.0 / math.pi);
    final calcLng = obsLngRad * (180.0 / math.pi);

    final obs = ResectionObservation(
      landmark: landmark,
      observedAzimuthDeg: azimuth1Deg,
      trueAzimuthDeg: trueAzimuth1,
    );

    return ResectionResult(
      lat: double.parse(calcLat.toStringAsFixed(6)),
      lng: double.parse(calcLng.toStringAsFixed(6)),
      estimatedAccuracyMeters: math.max(15.0, distanceMeters * 0.03),
      observations: [obs],
      distanceToLandmarksKm: {landmark.id: distanceKm},
    );
  }
}