#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   // Green LED — battery status
#define LED_ENGINE_PIN     33   // Red LED   — engine status
#define MOTOR_SENSE_PIN    34   // Input: HIGH = motor running (e.g. ignition line)

// ================================================================
// SETTINGS
// ================================================================
#define MOTION_THRESHOLD     8.0
#define MOTION_CONFIRM_COUNT 5
#define TIMEOUT_MS           180000  // 3 minutes
#define COOLDOWN_MS          5000

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

// ================================================================
// STATES
// ================================================================
enum State {
  ARMED,
  WAITING_BATTERY,     // motor OFF path — ask to disconnect battery
  WAITING_ENGINE,      // motor ON  path — ask to kill engine directly
  BATTERY_DISCONNECTED,
  ENGINE_OFF
};
State currentState = ARMED;

Adafruit_MPU6050 mpu;

unsigned long waitStart     = 0;
unsigned long lastCountdown = 0;
unsigned long lastResetTime = 0;
unsigned long lastBeepTime  = 0;
bool beepToggle = false;

bool batteryDisconnected = false;
bool engineKilled        = false;
bool alertActive         = false;
bool smsSent             = false;
bool alarmOn             = false;

int   motionHitCount = 0;
float baseX = 0, baseY = 0, baseZ = 0;

// ================================================================
// FORWARD DECLARATIONS
// ================================================================
void handleArmed();
void handleWaitingBattery();
void handleWaitingEngine();
void handleBatteryDisconnected();
void handleEngineOff();
void disconnectBattery(String reason);
void killEngine(String reason);
void resetSystem();
bool detectMotion();
bool isMotorRunning();
void calibrateSensor();
void runAlarm();
void stopAlarm();
void updateLEDs();

// ================================================================
// MOTOR SENSE — reads ignition/battery line
// ================================================================
bool isMotorRunning() {
  return digitalRead(MOTOR_SENSE_PIN) == HIGH;
}

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

// ================================================================
// ALARM — non-blocking
// ================================================================
void runAlarm() {
  unsigned long now = millis();
  if (now - lastBeepTime >= 150) {
    lastBeepTime = now;
    beepToggle = !beepToggle;
    beepToggle ? tone(BUZZER_PIN, 2500) : noTone(BUZZER_PIN);
  }
}

void stopAlarm() {
  alarmOn    = false;
  beepToggle = false;
  noTone(BUZZER_PIN);
}

// ================================================================
// CALIBRATION
// ================================================================
void calibrateSensor() {
  Serial.println("[CALIBRATING] Keep device still...");
  delay(5000);

  sensors_event_t a, g, temp;
  float sumX = 0, sumY = 0, sumZ = 0;

  for (int i = 0; i < 200; i++) {
    mpu.getEvent(&a, &g, &temp);
    sumX += a.acceleration.x;
    sumY += a.acceleration.y;
    sumZ += a.acceleration.z;
    delay(20);
  }

  baseX = sumX / 200;
  baseY = sumY / 200;
  baseZ = sumZ / 200;

  Serial.println("[CALIBRATION DONE]");
  Serial.print("  Base X: "); Serial.println(baseX);
  Serial.print("  Base Y: "); Serial.println(baseY);
  Serial.print("  Base Z: "); Serial.println(baseZ);
}

// ================================================================
// SETUP
// ================================================================
void setup() {
  Serial.begin(115200);
  delay(1000);

  pinMode(BATTERY_RELAY_PIN, OUTPUT);
  pinMode(ENGINE_RELAY_PIN,  OUTPUT);
  pinMode(BUZZER_PIN,        OUTPUT);
  pinMode(LED_BATTERY_PIN,   OUTPUT);
  pinMode(LED_ENGINE_PIN,    OUTPUT);
  pinMode(MOTOR_SENSE_PIN,   INPUT);

  digitalWrite(BATTERY_RELAY_PIN, HIGH);
  digitalWrite(ENGINE_RELAY_PIN,  HIGH);
  noTone(BUZZER_PIN);

  updateLEDs();

  Wire.begin(21, 22);

  if (!mpu.begin()) {
    Serial.println("[ERROR] MPU6050 not found! Check SDA=21, SCL=22.");
    while (1) delay(10);
  }

  mpu.setAccelerometerRange(MPU6050_RANGE_2_G);
  mpu.setGyroRange(MPU6050_RANGE_250_DEG);
  mpu.setFilterBandwidth(MPU6050_BAND_21_HZ);

  calibrateSensor();

  Serial.println("========================================");
  Serial.println(" Motorcycle Anti-Theft System — READY");
  Serial.println(" Green LED ON  = Battery connected");
  Serial.println(" Red   LED ON  = Engine allowed");
  Serial.println("========================================");
  Serial.println(" Commands:");
  Serial.println("   MOTION — manually trigger alert");
  Serial.println("   STATUS — show current state");
  Serial.println("   YES    — confirm action");
  Serial.println("   NO     — false alarm / reset");
  Serial.println("========================================");
}

// ================================================================
// LOOP
// ================================================================
void loop() {
  if (alarmOn) runAlarm();

  switch (currentState) {
    case ARMED:                handleArmed();               break;
    case WAITING_BATTERY:      handleWaitingBattery();      break;
    case WAITING_ENGINE:       handleWaitingEngine();       break;
    case BATTERY_DISCONNECTED: handleBatteryDisconnected(); break;
    case ENGINE_OFF:           handleEngineOff();           break;
  }

  delay(50);
}

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

  if (Serial.available()) {
    String cmd = Serial.readStringUntil('\n');
    cmd.trim();
    cmd.toUpperCase();

    if (cmd == "MOTION") {
      Serial.println("[SIM] Manual motion trigger.");
      // Fall through to the motion-detected block below
      motionHitCount = MOTION_CONFIRM_COUNT;
    }

    if (cmd == "STATUS") {
      String states[] = {
        "ARMED", "WAITING_BATTERY", "WAITING_ENGINE",
        "BATTERY_DISCONNECTED", "ENGINE_OFF"
      };
      Serial.println("[STATUS] State               : " + states[currentState]);
      Serial.println("[STATUS] Motor running       : " + String(isMotorRunning() ? "YES" : "NO"));
      Serial.println("[STATUS] Battery disconnected: " + String(batteryDisconnected ? "YES" : "NO"));
      Serial.println("[STATUS] Engine killed       : " + String(engineKilled        ? "YES" : "NO"));
      Serial.println("[STATUS] Alarm on            : " + String(alarmOn             ? "YES" : "NO"));
      Serial.println("[STATUS] GPS                 : " + SIM_LOCATION);
      return;
    }
  }

  if (detectMotion()) {
    Serial.println("[ALERT] Motion detected!");
    alertActive = true;
    alarmOn     = true;
    smsSent     = false;
    waitStart   = millis();

    if (isMotorRunning()) {
      // Motor is ON — skip battery prompt, go straight to engine kill prompt
      Serial.println("========================================");
      Serial.println("[SMS SENT TO OWNER]");
      Serial.println("  THEFT DETECTED! Motor is running.");
      Serial.println("  " + SIM_LOCATION);
      Serial.println("  Reply YES to kill engine.");
      Serial.println("  Reply NO  if this is a false alarm.");
      Serial.println("========================================");
      currentState = WAITING_ENGINE;
    } else {
      // Motor is OFF — ask about battery disconnect
      Serial.println("========================================");
      Serial.println("[SMS SENT TO OWNER]");
      Serial.println("  THEFT DETECTED! Motor is off.");
      Serial.println("  " + SIM_LOCATION);
      Serial.println("  Reply YES to disconnect battery.");
      Serial.println("  Reply NO  if this is a false alarm.");
      Serial.println("========================================");
      currentState = WAITING_BATTERY;
    }
  }
}

// ================================================================
// STATE: WAITING_BATTERY — motor was OFF when motion detected
// YES = disconnect battery. NO = false alarm reset.
// ================================================================
void handleWaitingBattery() {
  unsigned long elapsed = millis() - waitStart;

  if (elapsed - lastCountdown > 5000) {
    lastCountdown = elapsed;
    int remaining = (TIMEOUT_MS - (int)elapsed) / 1000;
    if (remaining > 0) {
      Serial.println("[WAITING] " + String(remaining) + "s left. Reply YES or NO.");
    }
  }

  if (Serial.available()) {
    String reply = Serial.readStringUntil('\n');
    reply.trim();
    reply.toUpperCase();
    Serial.println("[INPUT] " + reply);

    if (reply == "YES") {
      disconnectBattery("Owner confirmed — motor was off");
      return;
    }
    if (reply == "NO") {
      Serial.println("[SAFE] False alarm. Resetting system.");
      Serial.println("[SMS SENT] All clear. System reset.");
      resetSystem();
      return;
    }
  }

  if (elapsed > TIMEOUT_MS) {
    Serial.println("[FAIL-SAFE] No response. Auto battery disconnect.");
    disconnectBattery("Timeout — no owner response");
  }
}

// ================================================================
// STATE: WAITING_ENGINE — motor WAS running when motion detected
// YES = kill engine immediately. NO = false alarm reset.
// ================================================================
void handleWaitingEngine() {
  unsigned long elapsed = millis() - waitStart;

  if (elapsed - lastCountdown > 5000) {
    lastCountdown = elapsed;
    int remaining = (TIMEOUT_MS - (int)elapsed) / 1000;
    if (remaining > 0) {
      Serial.println("[WAITING] " + String(remaining) + "s left. Reply YES or NO.");
    }
  }

  if (Serial.available()) {
    String reply = Serial.readStringUntil('\n');
    reply.trim();
    reply.toUpperCase();
    Serial.println("[INPUT] " + reply);

    if (reply == "YES") {
      // Kill battery first then engine (both layers)
      disconnectBattery("Motor running — owner confirmed theft");
      killEngine("Motor running — owner triggered engine kill");
      return;
    }
    if (reply == "NO") {
      Serial.println("[SAFE] False alarm. Resetting system.");
      Serial.println("[SMS SENT] All clear. System reset.");
      resetSystem();
      return;
    }
  }

  if (elapsed > TIMEOUT_MS) {
    Serial.println("[FAIL-SAFE] No response. Auto engine kill.");
    disconnectBattery("Timeout — motor was running");
    killEngine("Timeout — auto engine kill");
  }
}

// ================================================================
// STATE: BATTERY_DISCONNECTED (Layer 1 active, motor was off)
// Thief may now try to hot-wire. YES = also kill engine. NO = reset.
// ================================================================
void handleBatteryDisconnected() {
  if (Serial.available()) {
    String cmd = Serial.readStringUntil('\n');
    cmd.trim();
    cmd.toUpperCase();
    Serial.println("[INPUT] " + cmd);

    if (cmd == "YES") {
      Serial.println("[LAYER 2] Engine kill command received.");
      killEngine("Owner triggered Layer 2 — hot-wire attempt");
      return;
    }
    if (cmd == "NO") {
      Serial.println("[RESET] Stopping alarm and reconnecting battery.");
      resetSystem();
    }
  }
}

// ================================================================
// STATE: ENGINE_OFF (Both layers active)
// NO = full reset.
// ================================================================
void handleEngineOff() {
  if (Serial.available()) {
    String cmd = Serial.readStringUntil('\n');
    cmd.trim();
    cmd.toUpperCase();
    Serial.println("[INPUT] " + cmd);

    if (cmd == "NO") {
      Serial.println("[RESET] Restoring all systems.");
      resetSystem();
    }
  }
}

// ================================================================
// LAYER 1 — DISCONNECT BATTERY
// ================================================================
void disconnectBattery(String reason) {
  Serial.println("[LAYER 1] BATTERY DISCONNECTED — " + reason);
  Serial.println("[SMS SENT] Battery disconnected. " + SIM_LOCATION);

  if (currentState != ENGINE_OFF) {
    // Only send the hot-wire prompt if engine kill hasn't already happened
    Serial.println("[SMS SENT] Reply YES if you suspect hot-wiring, NO to reset.");
  }

  digitalWrite(BATTERY_RELAY_PIN, LOW);
  batteryDisconnected = true;

  // Only change state if we're not already jumping straight to ENGINE_OFF
  if (currentState != ENGINE_OFF) {
    currentState = BATTERY_DISCONNECTED;
  }

  updateLEDs();
}

// ================================================================
// LAYER 2 — KILL ENGINE
// ================================================================
void killEngine(String reason) {
  Serial.println("[LAYER 2] ENGINE KILLED — " + reason);
  Serial.println("[SMS SENT] Engine disabled. " + SIM_LOCATION);
  Serial.println("[SMS SENT] Reply NO to reset system when recovered.");

  digitalWrite(ENGINE_RELAY_PIN, LOW);
  engineKilled = true;
  currentState = ENGINE_OFF;
  updateLEDs();
}

// ================================================================
// RESET SYSTEM
// ================================================================
void resetSystem() {
  digitalWrite(BATTERY_RELAY_PIN, HIGH);
  digitalWrite(ENGINE_RELAY_PIN,  HIGH);
  stopAlarm();

  batteryDisconnected = false;
  engineKilled        = false;
  alertActive         = false;
  smsSent             = false;
  motionHitCount      = 0;
  lastCountdown       = 0;
  lastResetTime       = millis();
  currentState        = ARMED;

  updateLEDs();

  Serial.println("[SYSTEM ARMED] Both layers restored. Monitoring resumed.");
  Serial.println("  Commands: MOTION | STATUS");
}

// ================================================================
// MOTION DETECTION
// ================================================================
bool detectMotion() {
  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 > MOTION_THRESHOLD) {
    motionHitCount++;
  } else {
    motionHitCount = 0;
  }

  return motionHitCount >= MOTION_CONFIRM_COUNT;
}