import 'dart:math';

/// A simple 4D Kalman Filter implementation for GPS smoothing.
/// State vector x = [lat, lng, velocity_lat, velocity_lng]
class NearleKalmanFilter {
  late List<double> x; // State estimate
  late List<List<double>> P; // Covariance matrix
  late List<List<double>> F; // State transition matrix
  late List<List<double>> H; // Measurement matrix
  late List<List<double>> R; // Measurement noise covariance
  late List<List<double>> Q; // Process noise covariance

  NearleKalmanFilter({
    required double lat,
    required double lng,
  }) {
    // Initial state: [lat, lng, 0, 0]
    x = [lat, lng, 0, 0];

    // Initial covariance: High uncertainty for initial velocity
    P = [
      [1.0, 0.0, 0.0, 0.0],
      [0.0, 1.0, 0.0, 0.0],
      [0.0, 0.0, 1000.0, 0.0],
      [0.0, 0.0, 0.0, 1000.0],
    ];

    // State transition matrix (assuming dt = 1 for simplicity, will update in predict)
    F = [
      [1.0, 0.0, 1.0, 0.0],
      [0.0, 1.0, 0.0, 1.0],
      [0.0, 0.0, 1.0, 0.0],
      [0.0, 0.0, 0.0, 1.0],
    ];

    // Measurement matrix: We only measure lat and lng
    H = [
      [1.0, 0.0, 0.0, 0.0],
      [0.0, 1.0, 0.0, 0.0],
    ];

    // Measurement noise: GPS is typically accurate to ~5-10 meters.
    // In degrees, this is roughly 0.0001
    R = [
      [0.00001, 0.0],
      [0.0, 0.00001],
    ];

    // Process noise: How much we trust our prediction vs measurement
    Q = [
      [0.00001, 0.0, 0.0, 0.0],
      [0.0, 0.00001, 0.0, 0.0],
      [0.0001, 0.0, 0.0001, 0.0],
      [0.0, 0.0001, 0.0, 0.0001],
    ];
  }

  /// Predict the next state
  void predict(double dt) {
    // Update F based on dt
    F[0][2] = dt;
    F[1][3] = dt;

    // x = F * x
    final newX = List<double>.filled(4, 0);
    for (int i = 0; i < 4; i++) {
      for (int j = 0; j < 4; j++) {
        newX[i] += F[i][j] * x[j];
      }
    }
    x = newX;

    // P = F * P * F^T + Q
    final FP = _multiply4x4(F, P);
    final F_T = _transpose4x4(F);
    final FPF_T = _multiply4x4(FP, F_T);
    
    for (int i = 0; i < 4; i++) {
      for (int j = 0; j < 4; j++) {
        P[i][j] = FPF_T[i][j] + Q[i][j];
      }
    }
  }

  /// Update the state with a new measurement
  void update(double measuredLat, double measuredLng) {
    final z = [measuredLat, measuredLng];

    // y = z - H * x (Innovation)
    final y = [
      z[0] - (H[0][0] * x[0] + H[0][1] * x[1] + H[0][2] * x[2] + H[0][3] * x[3]),
      z[1] - (H[1][0] * x[0] + H[1][1] * x[1] + H[1][2] * x[2] + H[1][3] * x[3]),
    ];

    // S = H * P * H^T + R (Innovation covariance)
    // H is 2x4, P is 4x4, H^T is 4x2
    final HP = _multiply2x4_4x4(H, P);
    final H_T = _transpose2x4(H);
    final HPH_T = _multiply2x4_4x2(HP, H_T);
    
    final S = [
      [HPH_T[0][0] + R[0][0], HPH_T[0][1] + R[0][1]],
      [HPH_T[1][0] + R[1][0], HPH_T[1][1] + R[1][1]],
    ];

    // K = P * H^T * S^-1 (Kalman gain)
    final Sinv = _inverse2x2(S);
    final PH_T = _multiply4x4_4x2(P, H_T);
    final K = _multiply4x2_2x2(PH_T, Sinv);

    // x = x + K * y
    for (int i = 0; i < 4; i++) {
      x[i] += K[i][0] * y[0] + K[i][1] * y[1];
    }

    // P = (I - K * H) * P
    final KH = _multiply4x2_2x4(K, H);
    final I = [
      [1.0, 0.0, 0.0, 0.0],
      [0.0, 1.0, 0.0, 0.0],
      [0.0, 0.0, 1.0, 0.0],
      [0.0, 0.0, 0.0, 1.0],
    ];
    
    final I_KH = List.generate(4, (i) => List.generate(4, (j) => I[i][j] - KH[i][j]));
    P = _multiply4x4(I_KH, P);
  }

  // --- Helper Math Functions ---

  List<List<double>> _multiply4x4(List<List<double>> A, List<List<double>> B) {
    final C = List.generate(4, (_) => List<double>.filled(4, 0));
    for (int i = 0; i < 4; i++) {
      for (int j = 0; j < 4; j++) {
        for (int k = 0; k < 4; k++) {
          C[i][j] += A[i][k] * B[k][j];
        }
      }
    }
    return C;
  }

  List<List<double>> _transpose4x4(List<List<double>> A) {
    final C = List.generate(4, (_) => List<double>.filled(4, 0));
    for (int i = 0; i < 4; i++) {
      for (int j = 0; j < 4; j++) {
        C[i][j] = A[j][i];
      }
    }
    return C;
  }

  List<List<double>> _multiply2x4_4x4(List<List<double>> A, List<List<double>> B) {
    final C = List.generate(2, (_) => List<double>.filled(4, 0));
    for (int i = 0; i < 2; i++) {
      for (int j = 0; j < 4; j++) {
        for (int k = 0; k < 4; k++) {
          C[i][j] += A[i][k] * B[k][j];
        }
      }
    }
    return C;
  }

  List<List<double>> _transpose2x4(List<List<double>> A) {
    final C = List.generate(4, (_) => List<double>.filled(2, 0));
    for (int i = 0; i < 4; i++) {
      for (int j = 0; j < 2; j++) {
        C[i][j] = A[j][i];
      }
    }
    return C;
  }

  List<List<double>> _multiply2x4_4x2(List<List<double>> A, List<List<double>> B) {
    final C = List.generate(2, (_) => List<double>.filled(2, 0));
    for (int i = 0; i < 2; i++) {
      for (int j = 0; j < 2; j++) {
        for (int k = 0; k < 4; k++) {
          C[i][j] += A[i][k] * B[k][j];
        }
      }
    }
    return C;
  }

  List<List<double>> _inverse2x2(List<List<double>> A) {
    final det = A[0][0] * A[1][1] - A[0][1] * A[1][0];
    if (det == 0) return [[1, 0], [0, 1]]; // Should not happen with noise
    return [
      [A[1][1] / det, -A[0][1] / det],
      [-A[1][0] / det, A[0][0] / det],
    ];
  }

  List<List<double>> _multiply4x4_4x2(List<List<double>> A, List<List<double>> B) {
    final C = List.generate(4, (_) => List<double>.filled(2, 0));
    for (int i = 0; i < 4; i++) {
      for (int j = 0; j < 2; j++) {
        for (int k = 0; k < 4; k++) {
          C[i][j] += A[i][k] * B[k][j];
        }
      }
    }
    return C;
  }

  List<List<double>> _multiply4x2_2x2(List<List<double>> A, List<List<double>> B) {
    final C = List.generate(4, (_) => List<double>.filled(2, 0));
    for (int i = 0; i < 4; i++) {
      for (int j = 0; j < 2; j++) {
        for (int k = 0; k < 2; k++) {
          C[i][j] += A[i][k] * B[k][j];
        }
      }
    }
    return C;
  }

  List<List<double>> _multiply4x2_2x4(List<List<double>> K, List<List<double>> H) {
    final C = List.generate(4, (_) => List<double>.filled(4, 0));
    for (int i = 0; i < 4; i++) {
      for (int j = 0; j < 4; j++) {
        for (int k = 0; k < 2; k++) {
          C[i][j] += K[i][k] * H[k][j];
        }
      }
    }
    return C;
  }
}
