#include <Wire.h>

// =================================================
// PIN DEFINITIONS
// =================================================

#define PIN_BUZZER     25
#define PIN_BUTTON     26
#define PIN_LED_WARN   32
#define PIN_LED_ALERT  33
#define PIN_GAS_SENSOR 34

// =================================================
// MPU-6050 CONFIGURATION
// =================================================

const uint8_t MPU_ADDR = 0x68;

// =================================================
// 2-OF-3 THRESHOLDS
// =================================================

const float THRESHOLD_IMPACT_G    = 3.0;
const float THRESHOLD_TILT_DEG    = 45.0;
const float THRESHOLD_INACTIVITY  = 0.25;

const unsigned long TILT_HOLD_MS = 1500;

// =================================================
// GAS SENSOR
// =================================================

const int GAS_THRESHOLD = 1500;
const unsigned long GAS_WARMUP_MS = 5000;

// =================================================
// TIMING
// =================================================

const unsigned long COUNTDOWN_DURATION_MS = 10000;
const unsigned long DEBOUNCE_DELAY_MS     = 50;
const unsigned long SENSOR_INTERVAL_MS    = 50;

// =================================================
// SYSTEM STATES
// =================================================

enum SystemState {
  STATE_NORMAL,
  STATE_COUNTDOWN,
  STATE_ALERT_SENT,
  STATE_CANCELLED
};

SystemState currentState = STATE_NORMAL;

// =================================================
// IGNITION STATES
// =================================================

enum IgnitionState {
  IGNITION_WARMUP,
  IGNITION_CHECKING,
  IGNITION_READY,
  IGNITION_BLOCKED
};

IgnitionState ignitionState = IGNITION_WARMUP;

// =================================================
// GLOBAL VARIABLES
// =================================================

unsigned long systemBootTime = 0;
unsigned long countdownStartTime = 0;
unsigned long lastSensorRead = 0;
unsigned long stateTimestamp = 0;

int lastGasReading = 0;

int lastButtonState = HIGH;
int buttonState = HIGH;
unsigned long lastDebounceTime = 0;

float prevGForce = 1.0;

// =================================================
// 2-OF-3 VOTING VARIABLES
// =================================================

bool impactVote = false;
bool tiltVote = false;
bool inactivityVote = false;

int accidentVotes = 0;

unsigned long impactDetectedTime = 0;
unsigned long tiltStartTime = 0;

// =================================================
// CAMERA EVIDENCE VARIABLES
// =================================================

const unsigned long PRE_EVENT_EVIDENCE_MS  = 20000;
const unsigned long POST_EVENT_EVIDENCE_MS = 20000;

unsigned long accidentConfirmedTime = 0;

// =================================================
// MPU-6050 LOW LEVEL DRIVER
// =================================================

void initMPU6050() {
  Wire.begin();
  Wire.beginTransmission(MPU_ADDR);
  Wire.write(0x6B);
  Wire.write(0);
  Wire.endTransmission(true);
}

// =================================================
// READ MPU6050
// =================================================

void readMPU6050(float &totalG, float &tiltAngle) {

  Wire.beginTransmission(MPU_ADDR);
  Wire.write(0x3B);
  Wire.endTransmission(false);

  Wire.requestFrom((uint8_t)MPU_ADDR, (uint8_t)6, (uint8_t)true);

  if (Wire.available() >= 6) {

    int16_t rawX = (Wire.read() << 8) | Wire.read();
    int16_t rawY = (Wire.read() << 8) | Wire.read();
    int16_t rawZ = (Wire.read() << 8) | Wire.read();

    float ax = rawX / 16384.0;
    float ay = rawY / 16384.0;
    float az = rawZ / 16384.0;

    totalG = sqrt(ax * ax + ay * ay + az * az);

    float horizontalMag = sqrt(ax * ax + ay * ay);
    tiltAngle = atan2(horizontalMag, fabs(az)) * 180.0 / PI;

  } else {
    totalG = 1.0;
    tiltAngle = 0.0;
  }
}

// =================================================
// MQ-3 ALCOHOL CHECK
// =================================================

void handleIgnitionCheck() {

  unsigned long elapsed = millis() - systemBootTime;

  switch (ignitionState) {

    case IGNITION_WARMUP:
      if (elapsed > GAS_WARMUP_MS) {
        ignitionState = IGNITION_CHECKING;
        Serial.println("[IGNITION] Gas sensor warm-up complete.");
      }
      break;

    case IGNITION_CHECKING:
      lastGasReading = analogRead(PIN_GAS_SENSOR);
      Serial.printf("[IGNITION] Gas sensor reading: %d (threshold %d)\n", lastGasReading, GAS_THRESHOLD);

      if (lastGasReading > GAS_THRESHOLD) {
        ignitionState = IGNITION_BLOCKED;
        Serial.println("[IGNITION] ALCOHOL LEVEL HIGH — ride blocked. Advisory alert only.");
      } else {
        ignitionState = IGNITION_READY;
        Serial.println("[IGNITION] Sobriety check passed. Ignition allowed.");
      }
      break;

    case IGNITION_READY:
    case IGNITION_BLOCKED:
      lastGasReading = analogRead(PIN_GAS_SENSOR);
      bool overThreshold = lastGasReading > GAS_THRESHOLD;

      if (overThreshold && ignitionState == IGNITION_READY) {
        ignitionState = IGNITION_BLOCKED;
        Serial.println("[IGNITION] Gas reading rose above threshold — now BLOCKED.");
      } else if (!overThreshold && ignitionState == IGNITION_BLOCKED) {
        ignitionState = IGNITION_READY;
        Serial.println("[IGNITION] Gas reading cleared — ignition now allowed.");
      }
      break;
  }
}

// =================================================
// 2-OF-3 VOTING ENGINE
// =================================================

void evaluateVoting(float currentG, float tiltAngle) {

  unsigned long now = millis();

  // ---------------- VOTE 1 — IMPACT ----------------
  if (currentG >= THRESHOLD_IMPACT_G) {
    impactVote = true;
    impactDetectedTime = now;
    Serial.println();
    Serial.println("[VOTE 1] IMPACT DETECTED");
    Serial.print("Acceleration: ");
    Serial.print(currentG);
    Serial.println(" g");
  }

  if (impactVote && now - impactDetectedTime > 2000) {
    impactVote = false;
  }

  // ---------------- VOTE 2 — TILT ----------------
  if (tiltAngle >= THRESHOLD_TILT_DEG) {
    if (tiltStartTime == 0) {
      tiltStartTime = now;
    }
    if (now - tiltStartTime >= TILT_HOLD_MS) {
      if (!tiltVote) {
        Serial.println("[VOTE 2] SUSTAINED TILT DETECTED");
        Serial.print("Tilt: ");
        Serial.print(tiltAngle);
        Serial.println(" degrees");
      }
      tiltVote = true;
    }
  } else {
    tiltStartTime = 0;
    tiltVote = false;
  }

  // ---------------- VOTE 3 — INACTIVITY ----------------
  float motionVariance = fabs(currentG - 1.0);
  inactivityVote = (motionVariance < THRESHOLD_INACTIVITY);

  // ---------------- COUNT VOTES ----------------
  accidentVotes = 0;
  if (impactVote) accidentVotes++;
  if (tiltVote) accidentVotes++;
  if (inactivityVote && (impactVote || tiltVote)) accidentVotes++;

  // ---------------- 2-OF-3 DECISION ----------------
  if (accidentVotes >= 2) {

    Serial.println();
    Serial.println("==============================================");
    Serial.println("           2-OF-3 VOTING LOGIC");
    Serial.println("==============================================");
    Serial.print("Impact Vote     : "); Serial.println(impactVote ? "YES" : "NO");
    Serial.print("Tilt Vote       : "); Serial.println(tiltVote ? "YES" : "NO");
    Serial.print("Inactivity Vote : "); Serial.println(inactivityVote ? "YES" : "NO");
    Serial.print("TOTAL VOTES     : "); Serial.print(accidentVotes); Serial.println("/3");
    Serial.println();
    Serial.println(">>> 2-OF-3 CONDITION SATISFIED <<<");
    Serial.println(">>> ACCIDENT SUSPECTED <<<");

    currentState = STATE_COUNTDOWN;
    countdownStartTime = millis();
  }
}

// =================================================
// BUZZER + WARNING LED  (FIXED: real audible tone)
// =================================================

void handleBuzzerFeedback(unsigned long elapsedMs) {

  unsigned long remaining =
    (COUNTDOWN_DURATION_MS > elapsedMs)
    ? (COUNTDOWN_DURATION_MS - elapsedMs)
    : 0;

  unsigned long period;
  int toneFreq;

  if (remaining > 6000) {
    period = 1000;
    toneFreq = 1500;
  } else if (remaining > 3000) {
    period = 500;
    toneFreq = 2000;
  } else {
    period = 150;
    toneFreq = 2800;
  }

  bool onPhase = (millis() / (period / 2)) % 2 == 0;

  if (onPhase) {
    tone(PIN_BUZZER, toneFreq);
    digitalWrite(PIN_LED_WARN, HIGH);
  } else {
    noTone(PIN_BUZZER);
    digitalWrite(PIN_LED_WARN, LOW);
  }
}

// =================================================
// TURN OFF INDICATORS (FIXED: noTone instead of digitalWrite)
// =================================================

void silenceIndicators() {
  noTone(PIN_BUZZER);
  digitalWrite(PIN_LED_WARN, LOW);
  digitalWrite(PIN_LED_ALERT, LOW);
}

// =================================================
// NEW: CONFIRMED-ACCIDENT SIREN
// =================================================

void playConfirmationSiren() {
  for (int i = 0; i < 4; i++) {
    tone(PIN_BUZZER, 2200);
    delay(120);
    tone(PIN_BUZZER, 1200);
    delay(120);
  }
  noTone(PIN_BUZZER);
}

// =================================================
// CAMERA — PRE ACCIDENT EVIDENCE
// =================================================

void capturePreAccidentEvidence() {
  Serial.println();
  Serial.println("----------------------------------------------");
  Serial.println("[CAMERA] PRE-ACCIDENT EVIDENCE");
  Serial.println("20-second rolling video buffer retained.");
  Serial.println("✓ Pre-accident footage preserved.");
  Serial.println("✓ Evidence buffer locked.");
}

// =================================================
// CAMERA — ACCIDENT MOMENT
// =================================================

void captureAccidentEvidence() {
  Serial.println();
  Serial.println("[CAMERA] ACCIDENT EVENT CAPTURE");
  Serial.println("Camera activated...");
  delay(500);
  Serial.println("Capturing accident moment...");
  delay(500);
  Serial.println("✓ Accident frame captured.");
}

// =================================================
// CAMERA — POST ACCIDENT EVIDENCE
// =================================================

void capturePostAccidentEvidence() {
  Serial.println();
  Serial.println("[CAMERA] POST-ACCIDENT EVIDENCE");
  Serial.println("Recording post-accident footage...");
  delay(1000);
  Serial.println("✓ 20-second post-event recording saved.");
  Serial.println("✓ Complete evidence package created.");
  Serial.println("----------------------------------------------");
}

// =================================================
// COMPLETE CAMERA EVIDENCE SEQUENCE
// =================================================

void captureCameraEvidence() {
  Serial.println();
  Serial.println("=================================================");
  Serial.println("             CAMERA EVIDENCE SYSTEM");
  Serial.println("=================================================");
  capturePreAccidentEvidence();
  captureAccidentEvidence();
  capturePostAccidentEvidence();
}

// =================================================
// SIMULATED GPS + GSM ALERT
// =================================================

void sendSimulatedAlert() {

  digitalWrite(PIN_LED_ALERT, HIGH);

  const char* lat = "12.9716";
  const char* lon = "77.5946";

  Serial.println();
  Serial.println("=================================================");
  Serial.println("           GPS + GSM EMERGENCY ALERT");
  Serial.println("=================================================");

  Serial.println("[GPS] Acquiring location...");
  delay(700);
  Serial.println("[GPS] Location acquired.");
  Serial.printf("GPS LOCATION: Lat %s, Lon %s\n", lat, lon);
  Serial.printf("MAP LINK: https://maps.google.com/?q=%s,%s\n", lat, lon);

  Serial.println();
  Serial.println("[GSM] Checking network...");
  delay(700);
  Serial.println("[GSM] Network available.");
  Serial.println("[GSM] Sending emergency SMS...");
  delay(1000);
  Serial.println("[GSM] SMS Delivery SUCCESS (SIMULATED)");

  Serial.println();
  Serial.println("SMS CONTENT:");
  Serial.println("RAKSHA KAVACHA ALERT!");
  Serial.println("Accident confirmed.");
  Serial.printf("Location: %s, %s\n", lat, lon);
  Serial.println("Evidence: PRE + ACCIDENT + POST captured.");
  Serial.println("Dispatch: [108 / +91-XXXXX-XXXXX]");
  Serial.println("=================================================");
}

// =================================================
// CANCEL BUTTON
// =================================================

bool isCancelButtonPressed() {

  int reading = digitalRead(PIN_BUTTON);

  if (reading != lastButtonState) {
    lastDebounceTime = millis();
  }

  if ((millis() - lastDebounceTime) > DEBOUNCE_DELAY_MS) {
    if (reading != buttonState) {
      buttonState = reading;
      if (buttonState == LOW) {
        lastButtonState = reading;
        return true;
      }
    }
  }

  lastButtonState = reading;
  return false;
}

// =================================================
// SETUP
// =================================================

void setup() {

  Serial.begin(115200);
  while (!Serial && millis() < 2000);

  pinMode(PIN_BUZZER, OUTPUT);
  pinMode(PIN_LED_WARN, OUTPUT);
  pinMode(PIN_LED_ALERT, OUTPUT);
  pinMode(PIN_BUTTON, INPUT_PULLUP);

  silenceIndicators();
  initMPU6050();
  systemBootTime = millis();

  Serial.println();
  Serial.println("=================================================");
  Serial.println("              RAKSHA KAVACHA");
  Serial.println("=================================================");
  Serial.println("[SYSTEM READY] ESP32 initialized.");
  Serial.println("[SYSTEM READY] MPU6050 initialized.");
  Serial.println("[SYSTEM READY] 2-of-3 accident voting active.");
  Serial.println("[SYSTEM READY] MQ-3 sobriety check running.");
  Serial.println("[SYSTEM READY] Camera evidence simulation ready.");
  Serial.println("[SYSTEM READY] Buzzer audio feedback active.");
}

// =================================================
// MAIN LOOP
// =================================================

void loop() {

  unsigned long currentMillis = millis();

  handleIgnitionCheck();

  if (currentMillis - lastSensorRead >= SENSOR_INTERVAL_MS) {

    lastSensorRead = currentMillis;

    float currentG;
    float tiltAngle;

    readMPU6050(currentG, tiltAngle);

    switch (currentState) {

      case STATE_NORMAL:
        evaluateVoting(currentG, tiltAngle);
        prevGForce = currentG;
        break;

      case STATE_COUNTDOWN: {

        unsigned long elapsed = currentMillis - countdownStartTime;

        if (elapsed % 1000 < SENSOR_INTERVAL_MS) {
          unsigned long remainingSec = (COUNTDOWN_DURATION_MS - elapsed) / 1000 + 1;
          Serial.printf("[COUNTDOWN] Alert dispatch in: %lu s | Press button to CANCEL\n", remainingSec);
        }

        handleBuzzerFeedback(elapsed);

        if (isCancelButtonPressed()) {
          Serial.println();
          Serial.println("[OVERRIDE] Alert cancelled by rider.");
          silenceIndicators();
          currentState = STATE_CANCELLED;
          stateTimestamp = currentMillis;
          break;
        }

        if (elapsed >= COUNTDOWN_DURATION_MS) {

          silenceIndicators();

          Serial.println();
          Serial.println(">>> ACCIDENT CONFIRMED <<<");
          Serial.println(">>> NO RIDER CANCELLATION <<<");

          playConfirmationSiren();   // audible siren at confirmation

          captureCameraEvidence();
          sendSimulatedAlert();

          currentState = STATE_ALERT_SENT;
          stateTimestamp = currentMillis;
        }

        break;
      }

      case STATE_CANCELLED:
        if (currentMillis - stateTimestamp > 3000) {
          Serial.println("[RESET] Resuming normal monitoring.");
          currentState = STATE_NORMAL;
        }
        break;

      case STATE_ALERT_SENT:
        if (isCancelButtonPressed()) {
          Serial.println("[RESET] Manual post-alert reset.");
          silenceIndicators();
          currentState = STATE_NORMAL;
        }
        break;
    }
  }
}