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

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

  /// Calculates true azimuth from magnetic compass heading.
  static double getTrueAzimuth(double magneticHeadingDeg) {
    return (magneticHeadingDeg + magneticDeclinationJordan) % 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;

    // If 3rd landmark observation exists, calculate centroid / weighted least-squares refinement
    double estimatedAccuracy = 50.0; // Base 50m accuracy for 2 landmarks

    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;
      if (detBC.abs() >= 0.0001) {
        final dxBC = xC - xB;
        final dyBC = yC - yB;
        final tB = (dxBC * cosC - dyBC * sinC) / detBC;
        final xIntersectBC = xB + tB * sinB;
        final yIntersectBC = yB + tB * cosB;

        // Weighted centroid average of the triangle of error
        calcLng = ((xIntersect + xIntersectBC) / 2.0) / cosLat;
        calcLat = (yIntersect + yIntersectBC) / 2.0;

        // 3 sightings tighten the error ellipse to ~20-30 meters
        estimatedAccuracy = 25.0;
      }
    }

    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;
  }
}