#include <Servo.h>
#include <Wire.h>
#include <MPU6050.h>

// Ultrasonic Sensor Pins
#define TRIG_PIN 2
#define ECHO_PIN 3

// Potentiometer and Buzzer Pins
#define POT_PIN A0             // Potentiometer pin at A0
#define BUZZER_PIN 5           // Buzzer pin at digital pin 5

// Relay and LED Pins
#define RELAY_PIN 9            // Relay pin at digital pin 9
#define RED_LED 13
#define SAFE_LED 4              // Green LED for safe operation
#define EMERGENCY_LED 5         // Red LED for emergency stop

Servo throttleServo;
Servo myServo;
MPU6050 mpu;

// Constants
const int maxPosition = 90;       // Servo position for maximum throttle
const int stopPosition = 0;       // Servo position for full stop
const int obstacleThreshold = 50; // Distance threshold for safe speed in cm
const int emergencyThreshold = 10; // Distance threshold for emergency stop in cm
const int alcoholThreshold = 400; // Threshold alcohol value

void setup() {
  // Initialize Serial Monitor
  Serial.begin(9600);

  // Setup pins
  pinMode(TRIG_PIN, OUTPUT);
  pinMode(ECHO_PIN, INPUT);
  pinMode(POT_PIN, INPUT);
  pinMode(BUZZER_PIN, OUTPUT);
  pinMode(RELAY_PIN, OUTPUT);
  pinMode(RED_LED, OUTPUT);
  pinMode(SAFE_LED, OUTPUT);
  pinMode(EMERGENCY_LED, OUTPUT);

  // Initialize servo motors
  throttleServo.attach(9);
  throttleServo.write(maxPosition); // Start at full speed
  myServo.attach(3);
  myServo.write(0);                 // Initial servo position at 0 degrees

  // Initialize LEDs
  digitalWrite(SAFE_LED, LOW);
  digitalWrite(EMERGENCY_LED, LOW);

  // Initialize MPU6050
  Wire.begin();
  mpu.initialize();

  if (mpu.testConnection()) {
    Serial.println("MPU6050 Connected");
  } else {
    Serial.println("MPU6050 Connection Failed");
    while (1);
  }
}

void loop() {
  int alcoholValue = analogRead(POT_PIN);    // Reading the alcohol level value
  Serial.print("Alcohol level = ");
  Serial.println(alcoholValue);

  int16_t ax, ay, az;
  mpu.getAcceleration(&ax, &ay, &az);

  // Normalize accelerometer values (optional, depends on sensitivity)
  float xAccel = ax / 16384.0;
  float yAccel = ay / 16384.0;
  float zAccel = az / 16384.0;

  // Check if the vehicle is stationary
  bool isStationary = (abs(xAccel) < 0.02) && (abs(yAccel) < 0.02) && (abs(zAccel) < 0.02);

  // Measure distance
  int distance = measureDistance();

  if (alcoholValue >= alcoholThreshold) {
    if (isStationary) { // Turn off engine if stationary
      digitalWrite(RELAY_PIN, LOW);
      digitalWrite(BUZZER_PIN, HIGH);
      tone(7, 1000, 500);
      digitalWrite(RED_LED, HIGH);
      myServo.write(90); // Lock the servo
      Serial.println("Alcohol detected! Engine locked");
    } else {
      // Alcohol detected while moving
      Serial.println("Alcohol detected! Drive Safe");
      digitalWrite(BUZZER_PIN, HIGH);
      digitalWrite(RED_LED, HIGH);

      // Adjust speed based on obstacles
      if (distance > 0 && distance <= emergencyThreshold) {
        Serial.println("Emergency stop! Obstacle too close.");
        digitalWrite(SAFE_LED, LOW);
        digitalWrite(EMERGENCY_LED, HIGH);
        graduallyAdjustSpeed(stopPosition); // Stop immediately
      } else if (distance > emergencyThreshold && distance <= obstacleThreshold) {
        int safeSpeedPosition = map(distance, emergencyThreshold + 1, obstacleThreshold, 30, 60);
        Serial.print("Obstacle detected! Reducing speed to safe level: ");
        Serial.println(safeSpeedPosition);
        digitalWrite(SAFE_LED, HIGH);
        digitalWrite(EMERGENCY_LED, LOW);
        graduallyAdjustSpeed(safeSpeedPosition);
      } else {
        Serial.println("No obstacle detected. Gradually stopping...");
        digitalWrite(SAFE_LED, LOW);
        digitalWrite(EMERGENCY_LED, LOW);
        graduallyAdjustSpeed(stopPosition);
      }
    }
  } else {
    digitalWrite(RELAY_PIN, HIGH); // Engine ON
    digitalWrite(BUZZER_PIN, LOW);
    digitalWrite(RED_LED, LOW);
    myServo.write(0); // Servo remains at 0 degrees
    Serial.println("No Alcohol detected!");
  }

  delay(500);
}

// Function to measure distance using Ultrasonic Sensor
int measureDistance() {
  // Send a 10us pulse to trigger
  digitalWrite(TRIG_PIN, LOW);
  delayMicroseconds(2);
  digitalWrite(TRIG_PIN, HIGH);
  delayMicroseconds(10);
  digitalWrite(TRIG_PIN, LOW);

  // Measure the duration of the echo
  long duration = pulseIn(ECHO_PIN, HIGH);

  // Convert to distance in cm
  int distance = duration * 0.034 / 2;

  Serial.print("Distance: ");
  Serial.print(distance);
  Serial.println(" cm");

  return distance;
}

// Function to adjust throttle gradually
void graduallyAdjustSpeed(int targetPosition) {
  int currentPosition = throttleServo.read(); // Get current position

  if (currentPosition > targetPosition) {
    for (int pos = currentPosition; pos >= targetPosition; pos -= 2) {
      throttleServo.write(pos);
      delay(100); // Adjust speed of reduction
    }
  } else {
    for (int pos = currentPosition; pos <= targetPosition; pos += 2) {
      throttleServo.write(pos);
      delay(100); // Adjust speed of increase
    }
  }
}