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

MPU6050 mpu;

// Define pins for LEDs and Buzzer
const int leftLED = 8;
const int rightLED = 9;
const int brakeLED = 10;
const int triggerPin = 11; // Ultrasonic Trigger Pin
const int echoPin = 12;     // Ultrasonic Echo Pin
const int buzzerPin = 7;    // Buzzer Pin

void setup() {
    Serial.begin(115200);
    Wire.begin();

    // Initialize MPU6050
    mpu.initialize();
    
    // Check MPU6050 connection
    if (mpu.testConnection()) {
        Serial.println("MPU6050 is connected!");
    } else {
        Serial.println("MPU6050 connection failed!");
    }

    // Setup pins
    pinMode(leftLED, OUTPUT);
    pinMode(rightLED, OUTPUT);
    pinMode(brakeLED, OUTPUT);
    pinMode(buzzerPin, OUTPUT);
    pinMode(triggerPin, OUTPUT);
    pinMode(echoPin, INPUT);
}

void loop() {
    // Read motion data from MPU6050
    int16_t ax, ay, az, gx, gy, gz;
    mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);

    // Print accelerometer values for debugging
    Serial.print("Accel: ");
    Serial.print("X: "); Serial.print(ax);
    Serial.print(" Y: "); Serial.print(ay);
    Serial.print(" Z: "); Serial.println(az);

    // Check tilt for left or right signals
    if (ax > 2000) { // Adjust this threshold as necessary
        digitalWrite(leftLED, HIGH);
        digitalWrite(rightLED, LOW);
        digitalWrite(brakeLED, LOW);
    } 
    else if (ax < -2000) { // Adjust this threshold as necessary
        digitalWrite(rightLED, HIGH);
        digitalWrite(leftLED, LOW);
        digitalWrite(brakeLED, LOW);
    } 
    else {
        digitalWrite(leftLED, LOW);
        digitalWrite(rightLED, LOW);
    }

    // Check for brake condition (assuming low speed or abrupt stop)
    if (ay > 2000) { // Adjust this threshold for braking
        digitalWrite(brakeLED, HIGH);
        digitalWrite(leftLED, LOW);
        digitalWrite(rightLED, LOW);
        tone(buzzerPin, 1000); // Activate buzzer
    } else {
        digitalWrite(brakeLED, LOW);
        noTone(buzzerPin); // Deactivate buzzer
    }

    // Ultrasonic Sensor Logic
    long duration, distance;
    
    // Trigger the ultrasonic sensor
    digitalWrite(triggerPin, LOW);
    delayMicroseconds(2);
    digitalWrite(triggerPin, HIGH);
    delayMicroseconds(10);
    digitalWrite(triggerPin, LOW);
    
    // Read the echo
    duration = pulseIn(echoPin, HIGH);
    distance = (duration * 0.034) / 2; // Calculate distance in cm

    // Check for nearby objects (adjust threshold as necessary)
    if (distance < 20) { // Example threshold
        tone(buzzerPin, 1000); // Activate buzzer for nearby object
    } else {
        noTone(buzzerPin); // Deactivate buzzer
    }

    // Add a delay for better readability
    delay(100);
}