#include <Arduino.h>
#include <Wire.h>
#include <Adafruit_MPU6050.h>
#include <Adafruit_Sensor.h>

// ================================================================
// PIN DEFINITIONS
// ================================================================
#define BATTERY_RELAY_PIN   26
#define ENGINE_RELAY_PIN    27
#define BUZZER_PIN          25
#define LED_BATTERY_PIN     32
#define LED_ENGINE_PIN      33

// ================================================================
// LEDC (PWM) SETUP (ESP32 Core 3.0+ Compatible)
// ================================================================
#define LEDC_RESOLUTION  8       
#define ALARM_FREQ       2500    

// ================================================================
// THRESHOLDS & TIMING
// ================================================================
#define SMALL_MOTION_BASE    4.0f
#define BIG_MOTION_BASE      10.0f
#define SMALL_MOTION_MARGIN  2.0f
#define BIG_MOTION_MARGIN    4.0f
#define DYNAMIC_RECALC_MS    10000
#define NOISE_SAMPLES        50
#define TIMEOUT_MS           180000  // 3 minutes
#define COOLDOWN_MS          5000
#define ALARM_DURATION       60000   // 60 seconds

const String SIM_LOCATION = "Lat: 14.599500, Lon: 120.984200 (Simulated)";

enum State { ARMED, WAITING_RESPONSE, BATTERY_DISCONNECTED, ENGINE_OFF };
State currentState = ARMED;

// ================================================================
// GLOBALS
// ================================================================
Adafruit_MPU6050 mpu;
unsigned long waitStart = 0, lastResetTime = 0, lastBeepTime = 0, lastDynamicCalc = 0, alarmStartTime = 0;
bool beepToggle = false, batteryDisconnected = false, engineKilled = false, alertActive = false, alarmRunning = false;
bool batteryNotifSent = false, engineOffNotifSent = false;
int pendingMotionLevel = 0;
float baseX = 0, baseY = 0, baseZ = 0;
float dynamicSmallThreshold = SMALL_MOTION_BASE, dynamicBigThreshold = BIG_MOTION_BASE;

// ================================================================
// BUZZER HELPERS
// ================================================================
void buzzerBegin() {
  ledcAttach(BUZZER_PIN, ALARM_FREQ, LEDC_RESOLUTION);
}

void buzzerTone(uint32_t freq) {
  ledcWriteTone(BUZZER_PIN, freq);
}

void buzzerStop() {
  ledcWriteTone(BUZZER_PIN, 0);
}

void startAlarm() {
  if (alarmRunning) return; // Don't restart if already beeping
  alarmRunning = true;
  alarmStartTime = millis();
  lastBeepTime = millis();
  beepToggle = false;
  Serial.println("[ALARM TRIGGERED IMMEDIATELY]");
}

void stopAlarm() {
  alarmRunning = false;
  buzzerStop();
  Serial.println("[ALARM STOPPED]");
}

void runAlarm() {
  if (!alarmRunning) return;

  // Auto-stop only after 60 seconds of beeping
  if (millis() - alarmStartTime >= ALARM_DURATION) {
    stopAlarm();
    return;
  }

  // Rapid Beeping
  if (millis() - lastBeepTime >= 150) {
    lastBeepTime = millis();
    beepToggle = !beepToggle;
    beepToggle ? buzzerTone(ALARM_FREQ) : buzzerStop();
  }
}

// ================================================================
// SYSTEM ACTIONS
// ================================================================
void updateLEDs() {
  digitalWrite(LED_BATTERY_PIN, batteryDisconnected ? LOW : HIGH);
  digitalWrite(LED_ENGINE_PIN,  engineKilled        ? LOW : HIGH);
}

void resetSystem() {
  stopAlarm(); // Force buzzer off immediately on reset/NO command
  digitalWrite(BATTERY_RELAY_PIN, HIGH);
  digitalWrite(ENGINE_RELAY_PIN,  HIGH);
  batteryDisconnected = false;
  engineKilled = false;
  alertActive = false;
  batteryNotifSent = false;
  engineOffNotifSent = false;
  pendingMotionLevel = 0;
  currentState = ARMED;
  lastResetTime = millis();
  updateLEDs();
  Serial.println("[SYSTEM RESET & RE-ARMED]");
}

void disconnectBattery(String reason) {
  Serial.println("[ACTION] Battery Disconnected: " + reason);
  digitalWrite(BATTERY_RELAY_PIN, LOW);
  batteryDisconnected = true;
  currentState = BATTERY_DISCONNECTED;
  updateLEDs();
}

void killEngine(String reason) {
  Serial.println("[ACTION] Engine Killed: " + reason);
  digitalWrite(ENGINE_RELAY_PIN, LOW);
  engineKilled = true;
  currentState = ENGINE_OFF;
  updateLEDs();
}

// ================================================================
// SENSOR LOGIC
// ================================================================
void updateDynamicThresholds() {
  sensors_event_t a, g, temp;
  float sumDelta = 0;
  for (int i = 0; i < NOISE_SAMPLES; i++) {
    mpu.getEvent(&a, &g, &temp);
    float delta = sqrt(pow(a.acceleration.x - baseX, 2) + pow(a.acceleration.y - baseY, 2) + pow(a.acceleration.z - baseZ, 2));
    sumDelta += delta;
    delay(10);
  }
  float avgNoise = sumDelta / NOISE_SAMPLES;
  dynamicSmallThreshold = max(SMALL_MOTION_BASE, avgNoise + SMALL_MOTION_MARGIN);
  dynamicBigThreshold   = max(BIG_MOTION_BASE,   avgNoise + BIG_MOTION_MARGIN);
}

void calibrateSensor() {
  Serial.println("[CALIBRATING] Do not move the device...");
  delay(2000);
  sensors_event_t a, g, temp;
  float sx = 0, sy = 0, sz = 0;
  for (int i = 0; i < 100; i++) {
    mpu.getEvent(&a, &g, &temp);
    sx += a.acceleration.x; sy += a.acceleration.y; sz += a.acceleration.z;
    delay(10);
  }
  baseX = sx / 100; baseY = sy / 100; baseZ = sz / 100;
  updateDynamicThresholds();
}

int detectMotionLevel() {
  sensors_event_t a, g, temp;
  mpu.getEvent(&a, &g, &temp);
  float delta = sqrt(pow(a.acceleration.x - baseX, 2) + pow(a.acceleration.y - baseY, 2) + pow(a.acceleration.z - baseZ, 2));
  if (delta > dynamicBigThreshold)   return 2;
  if (delta > dynamicSmallThreshold) return 1;
  return 0;
}

// ================================================================
// STATE HANDLERS
// ================================================================
void handleArmed() {
  if (millis() - lastResetTime < COOLDOWN_MS) return;

  if (millis() - lastDynamicCalc >= DYNAMIC_RECALC_MS) {
    lastDynamicCalc = millis();
    updateDynamicThresholds();
  }

  int motionLevel = detectMotionLevel();
  if (motionLevel > 0) {
    startAlarm(); // TRIGGER ALARM IMMEDIATELY ON MOTION
    alertActive = true;
    pendingMotionLevel = motionLevel;
    waitStart = millis();

    Serial.println("\n--- ALERT: MOTION DETECTED ---");
    Serial.println(motionLevel == 2 ? "CRITICAL: High movement!" : "WARNING: Slight movement.");
    Serial.println("Reply YES to act, NO to ignore.");
    
    currentState = WAITING_RESPONSE;
  }
}

void handleWaitingResponse() {
  // Auto engine kill if timeout reached
  if (millis() - waitStart > TIMEOUT_MS) {
    killEngine("Timeout - No response");
    return; 
  }

  if (Serial.available()) {
    String r = Serial.readStringUntil('\n'); r.trim(); r.toUpperCase();
    if (r == "YES") {
      // Alarm keeps running (up to 60s total) while we perform the action
      if (pendingMotionLevel == 2) killEngine("Owner confirmed High Motion");
      else disconnectBattery("Owner confirmed Slight Motion");
    } 
    else if (r == "NO") {
      resetSystem(); // This will stop the alarm immediately
    }
  }
}

void handleBatteryDisconnected() {
  if (!batteryNotifSent) {
    batteryNotifSent = true;
    Serial.println("Battery Cut. Reply YES to Kill Engine, NO to Reset.");
  }
  if (Serial.available()) {
    String r = Serial.readStringUntil('\n'); r.trim(); r.toUpperCase();
    if (r == "YES") killEngine("Escalated to Engine Kill");
    else if (r == "NO") resetSystem();
  }
}

void handleEngineOff() {
  if (!engineOffNotifSent) {
    engineOffNotifSent = true;
    Serial.println("Engine Cut. Reply NO to Reset/Re-arm.");
  }
  if (Serial.available()) {
    String r = Serial.readStringUntil('\n'); r.trim(); r.toUpperCase();
    if (r == "NO") resetSystem();
  }
}

// ================================================================
// MAIN SETUP & LOOP
// ================================================================
void setup() {
  Serial.begin(115200);
  pinMode(BATTERY_RELAY_PIN, OUTPUT);
  pinMode(ENGINE_RELAY_PIN,  OUTPUT);
  pinMode(LED_BATTERY_PIN,   OUTPUT);
  pinMode(LED_ENGINE_PIN,    OUTPUT);
  
  digitalWrite(BATTERY_RELAY_PIN, HIGH);
  digitalWrite(ENGINE_RELAY_PIN,  HIGH);

  buzzerBegin();
  Wire.begin(21, 22);
  if (!mpu.begin()) {
    Serial.println("MPU6050 Error!");
    while (1);
  }
  calibrateSensor();
  updateLEDs();
  lastResetTime = millis();
  Serial.println("[SYSTEM ARMED & MONITORING]");
}

void loop() {
  runAlarm(); // Always servicing the beep toggle and 60s timer
  
  switch (currentState) {
    case ARMED:                handleArmed();             break;
    case WAITING_RESPONSE:     handleWaitingResponse();   break;
    case BATTERY_DISCONNECTED: handleBatteryDisconnected(); break;
    case ENGINE_OFF:           handleEngineOff();          break;
  }
  delay(20);
}