/*
 * ESP32 + MPU6050 Earthquake Detector
 * 
 * Fixed for real-world calibration data where idle PGA ≈ 1.0g
 * Uses correct gravity removal via full 3-axis gravity vector subtraction.
 * 
 * Your calibration results showed:
 *   Idle avg PGA : ~1.00g  (should be ~0.0g — gravity was not removed correctly)
 *   Quake avg PGA: ~1.00g  (same as idle — sensor couldn't tell difference)
 * 
 * Root cause: gravity vector projected onto all 3 axes when board is tilted.
 *   Old fix: just subtract 1.0 from Z — only works if board is perfectly flat.
 *   New fix: measure full gravity vector during calibration, subtract all 3 axes.
 * 
 * Wiring:
 *   MPU6050 SDA -> GPIO 21
 *   MPU6050 SCL -> GPIO 22
 *   LCD I2C SDA -> GPIO 21
 *   LCD I2C SCL -> GPIO 22
 *   Buzzer      -> GPIO 16
 *   Red LED     -> GPIO 17
 *   Green LED   -> GPIO 14
 * 
 * Libraries: Adafruit MPU6050, Adafruit Unified Sensor, LiquidCrystal_I2C
 */

#include <Wire.h>
#include <Adafruit_MPU6050.h>
#include <Adafruit_Sensor.h>
#include <LiquidCrystal_I2C.h>
#include <math.h>

// ── Pins ──────────────────────────────────────────────────────────────────────
#define PIN_BUZZER    16
#define PIN_RED_LED   17
#define PIN_GREEN_LED 4

// ── Config ───────────────────────────────────────────────────────────────────
// How many consecutive samples must exceed threshold before alarm triggers.
// Prevents single noise spikes from false-triggering.
#define CONFIRM_COUNT      4

// How many samples to collect after trigger to find peak PGA.
#define NUM_SAMPLES        10
#define SAMPLE_DELAY_MS    50

// Cooldown after a quake event — ignores re-triggers during this window.
#define COOLDOWN_MS        6000

// Calibration
#define CAL_SAMPLES        300    // 300 × 10ms = 3 seconds of calibration

// ── Threshold strategy ────────────────────────────────────────────────────────
// From your calibration data:
//   Idle noise (after proper gravity removal) should be 0.00–0.08g
//   Any shaking above 0.20g is a real event.
//
// Since your old idle avg was 1.00g (gravity leak), we cannot use those
// numbers directly. After gravity is properly removed, idle will be ~0.02–0.05g.
// Threshold is set conservatively and auto-adjusted after calibration below.
#define FALLBACK_THRESHOLD 0.20   // used only if calibration fails

// ── Objects ──────────────────────────────────────────────────────────────────
Adafruit_MPU6050 mpu;
LiquidCrystal_I2C lcd(0x27, 16, 2);  // change to 0x3F if LCD blank

// ── Gravity vector (measured during calibration, in m/s²) ────────────────────
// Unlike the old code that only subtracted 1g from Z, we measure the full
// gravity vector across all 3 axes. This works even if board is tilted.
float gravX = 0, gravY = 0, gravZ = 9.81;

// ── Runtime threshold (set after calibration) ─────────────────────────────────
float triggerThreshold = FALLBACK_THRESHOLD;

// ── Cooldown tracker ─────────────────────────────────────────────────────────
unsigned long lastQuakeTime = 0;

// ── Compute PGA ───────────────────────────────────────────────────────────────
// Subtracts full measured gravity vector from raw reading.
// Result is pure dynamic acceleration = vibration.
float computePGA(float ax_ms2, float ay_ms2, float az_ms2) {
  float dx = ax_ms2 - gravX;
  float dy = ay_ms2 - gravY;
  float dz = az_ms2 - gravZ;
  // Convert from m/s² to g (divide by 9.81)
  float gx = dx / 9.81;
  float gy = dy / 9.81;
  float gz = dz / 9.81;
  return sqrt(gx*gx + gy*gy + gz*gz);
}

// ── Read one PGA sample ───────────────────────────────────────────────────────
float readPGA() {
  sensors_event_t a, g, t;
  mpu.getEvent(&a, &g, &t);
  return computePGA(a.acceleration.x, a.acceleration.y, a.acceleration.z);
}

// ── Calibration ───────────────────────────────────────────────────────────────
// Measures average gravity vector over CAL_SAMPLES readings.
// Also measures idle noise floor to set threshold automatically.
void calibrate() {
  Serial.println("=== Calibration ===");
  Serial.println("Keep sensor completely still for 3 seconds...");

  lcd.clear();
  lcd.setCursor(0, 0); lcd.print("Calibrating...");
  lcd.setCursor(0, 1); lcd.print("Keep still!");

  double sumX = 0, sumY = 0, sumZ = 0;
  float maxNoise = 0;

  // Pass 1: measure gravity vector (average of all samples = static gravity)
  for (int i = 0; i < CAL_SAMPLES; i++) {
    sensors_event_t a, g, t;
    mpu.getEvent(&a, &g, &t);
    sumX += a.acceleration.x;
    sumY += a.acceleration.y;
    sumZ += a.acceleration.z;
    delay(10);
  }

  gravX = sumX / CAL_SAMPLES;
  gravY = sumY / CAL_SAMPLES;
  gravZ = sumZ / CAL_SAMPLES;

  // Sanity check: gravity magnitude should be ~9.81 m/s²
  float gravMag = sqrt(gravX*gravX + gravY*gravY + gravZ*gravZ);
  Serial.print("Gravity vector: X="); Serial.print(gravX, 3);
  Serial.print(" Y="); Serial.print(gravY, 3);
  Serial.print(" Z="); Serial.println(gravZ, 3);
  Serial.print("Gravity magnitude: "); Serial.print(gravMag, 3);
  Serial.println(" m/s² (should be ~9.81)");

  if (gravMag < 8.0 || gravMag > 12.0) {
    Serial.println("WARNING: Gravity magnitude out of range! Check wiring.");
  }

  // Pass 2: now measure actual noise floor with gravity removed
  for (int i = 0; i < 100; i++) {
    float p = readPGA();
    if (p > maxNoise) maxNoise = p;
    delay(10);
  }

  // Set threshold = 3× noise floor, minimum 0.12g
  triggerThreshold = max(maxNoise * 3.0f, 0.12f);
  // Cap at 0.40g — if noise floor is huge, sensor is still moving
  triggerThreshold = min(triggerThreshold, 0.40f);

  Serial.print("Idle noise floor: "); Serial.print(maxNoise, 4); Serial.println("g");
  Serial.print("Auto threshold  : "); Serial.print(triggerThreshold, 4); Serial.println("g");
  Serial.println("=== Calibration Done ===\n");

  lcd.clear();
  lcd.setCursor(0, 0); lcd.print("Thr:");
  lcd.print(triggerThreshold, 3); lcd.print("g");
  lcd.setCursor(0, 1); lcd.print("Ready!");
  delay(2000);
}

// ── PGA → Richter magnitude ───────────────────────────────────────────────────
// float getMagnitude(float pga) {
//   if (pga < 0.01) return 2.0;
//   if (pga < 0.05) return 3.0;
//   if (pga < 0.10) return 4.0;
//   if (pga < 0.20) return 5.0;
//   if (pga < 0.50) return 6.0;
//   return 7.0;
// }



// getIntensity()
// IO: PGA value
// Returns: realistic intensity + approx magnitude
// Working: uses corrected real-world mapping
float getMagnitude(float pga) {
  if (pga < 0.02) return 1.0;   // noise
  if (pga < 0.05) return 2.5;   // very weak
  if (pga < 0.10) return 3.5;   // weak
  if (pga < 0.20) return 4.5;   // light
  if (pga < 0.40) return 5.0;   // moderate
  if (pga < 0.80) return 5.5;   // strong
  return 6.0;                   // very strong (rare locally)
}




// ── Alert: beep + blink ───────────────────────────────────────────────────────
void triggerAlert() {
  for (int i = 0; i < 5; i++) {
    digitalWrite(PIN_RED_LED, HIGH);
    digitalWrite(PIN_BUZZER,  HIGH);
    delay(150);
    digitalWrite(PIN_RED_LED, LOW);
    digitalWrite(PIN_BUZZER,  LOW);
    delay(150);
  }
}

// ── Setup ─────────────────────────────────────────────────────────────────────
void setup() {
  Serial.begin(115200);

  pinMode(PIN_BUZZER,    OUTPUT);
  pinMode(PIN_RED_LED,   OUTPUT);
  pinMode(PIN_GREEN_LED, OUTPUT);
  digitalWrite(PIN_GREEN_LED, HIGH);

  Wire.begin();

  lcd.init();
  lcd.backlight();
  lcd.print("EQ Detector");

  if (!mpu.begin()) {
    Serial.println("MPU6050 not found!");
    lcd.clear(); lcd.print("MPU6050 Error!");
    while (true) delay(100);
  }

  mpu.setAccelerometerRange(MPU6050_RANGE_4_G);
  mpu.setFilterBandwidth(MPU6050_BAND_10_HZ);  // 10Hz low-pass — cuts noise

  calibrate();

  lcd.clear();
  lcd.setCursor(0, 0); lcd.print("Monitoring...");
  lcd.setCursor(0, 1);
  lcd.print("T:");
  lcd.print(triggerThreshold, 2);
  lcd.print("g");

  Serial.println("System ready. Live PGA printed every second.");
  Serial.println("FORMAT:  PGA | threshold | status");
}

// ── Main Loop ─────────────────────────────────────────────────────────────────
void loop() {
  static unsigned long lastPrint = 0;
  static int confirmHits = 0;

  float pga = readPGA();
  unsigned long now = millis();

  // ── Print idle PGA every second so you can monitor baseline ──────────────
  if (now - lastPrint >= 1000) {
    lastPrint = now;
    Serial.print("PGA: "); Serial.print(pga, 4);
    Serial.print("g  |  threshold: "); Serial.print(triggerThreshold, 2);
    Serial.print("g  |  ");
    Serial.println(pga > triggerThreshold ? "ABOVE" : "quiet");
  }

  // ── Cooldown guard ────────────────────────────────────────────────────────
  if (now - lastQuakeTime < COOLDOWN_MS) {
    delay(10);
    return;
  }

  // ── Confirmation counter — needs CONFIRM_COUNT consecutive hits ───────────
  if (pga > triggerThreshold) {
    confirmHits++;
  } else {
    confirmHits = 0;
  }

  if (confirmHits >= CONFIRM_COUNT) {
    confirmHits = 0;
    lastQuakeTime = now;

    // ── Alert state ───────────────────────────────────────────────────────
    digitalWrite(PIN_GREEN_LED, LOW);
    digitalWrite(PIN_RED_LED,   HIGH);

    Serial.println("\nQuake detected...");
    lcd.clear();
    lcd.print("!! QUAKE !!");


    // ── Collect NUM_SAMPLES to find peak ──────────────────────────────────
    float peakPGA = pga;
    for (int i = 0; i < NUM_SAMPLES; i++) {
      float s = readPGA();
      if (s > peakPGA) peakPGA = s;
      delay(SAMPLE_DELAY_MS);
    }

    float magnitude = getMagnitude(peakPGA);

    // make api call upload peakPGA, magnitude
    /*
    23:31:19.475 -> Quake detected...
    23:31:20.022 -> 
    23:31:20.022 -> --- EARTHQUAKE REPORT ---
    23:31:20.022 -> Peak PGA:  0.80
    23:31:20.022 -> Magnitude: 6.0
    */


    // ── Serial report ─────────────────────────────────────────────────────
    Serial.println("\n--- EARTHQUAKE REPORT ---");
    Serial.print("Peak PGA:  "); Serial.println(peakPGA, 2);
    Serial.print("Magnitude: "); Serial.println(magnitude, 1);
    Serial.println("-------------------------\n");

    // ── LCD report ────────────────────────────────────────────────────────
    lcd.clear();
    lcd.setCursor(0, 0);
    lcd.print("PGA:"); lcd.print(peakPGA, 2); lcd.print("g");
    lcd.setCursor(0, 1);
    lcd.print("Mag:~"); lcd.print(magnitude, 1);

    triggerAlert();

    // ── Return to idle ────────────────────────────────────────────────────
    digitalWrite(PIN_RED_LED,   LOW);
    digitalWrite(PIN_GREEN_LED, HIGH);

    delay(2000);
    lcd.clear();
    lcd.setCursor(0, 0); lcd.print("Monitoring...");
    lcd.setCursor(0, 1);
    lcd.print("T:"); lcd.print(triggerThreshold, 2); lcd.print("g");
  }

  delay(10);
}