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

MPU6050 mpu;

int frontLeftLed = 4;
int frontRightLed = 3;
int rearLeftLed = 6;
int rearRightLed = 5;

float yawThreshold = 5.0;
unsigned long turnDuration = 1000;

int16_t ax, ay, az;
int16_t gx, gy, gz;

float accelX, accelY, accelZ;
float gyroX, gyroY, gyroZ;

unsigned long turnStartTime = 0;

void setup() {
  Serial.begin(9600);
  Wire.begin();
  mpu.initialize();
  
  pinMode(frontLeftLed, OUTPUT);
  pinMode(frontRightLed, OUTPUT);
  pinMode(rearLeftLed, OUTPUT);
  pinMode(rearRightLed, OUTPUT);
  
  if (!mpu.testConnection()) {
    Serial.println("MPU6050 connection failed!");
    while (1);
  }

  Serial.println("MPU6050 Initialized Successfully.");
}

void loop() {
  mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
  
  accelX = (float)ax / 16384.0;
  accelY = (float)ay / 16384.0;
  accelZ = (float)az / 16384.0;

  gyroX = (float)gx / 131.0;
  gyroY = (float)gy / 131.0;
  gyroZ = (float)gz / 131.0;

  Serial.print("Accelerometer (g): X: "); Serial.print(accelX);
  Serial.print(" Y: "); Serial.print(accelY);
  Serial.print(" Z: "); Serial.println(accelZ);
  Serial.print("Gyroscope (°/s): X: "); Serial.print(gyroX);
  Serial.print(" Y: "); Serial.print(gyroY);
  Serial.print(" Z: "); Serial.println(gyroZ);
  
  if (abs(gyroZ) > yawThreshold) {
    if (gyroZ > 0) {
      Serial.println("Turning Right");
      digitalWrite(frontLeftLed, LOW);
      digitalWrite(frontRightLed, HIGH);
      digitalWrite(rearLeftLed, LOW);
      digitalWrite(rearRightLed, HIGH);
      
      if (turnStartTime == 0) {
        turnStartTime = millis();
      }
    } else {
      Serial.println("Turning Left");
      digitalWrite(frontLeftLed, HIGH);
      digitalWrite(frontRightLed, LOW);
      digitalWrite(rearLeftLed, HIGH);
      digitalWrite(rearRightLed, LOW);
      
      if (turnStartTime == 0) {
        turnStartTime = millis();
      }
    }
  } else {
    if (turnStartTime != 0 && millis() - turnStartTime > turnDuration) {
      Serial.println("Turn completed.");
      turnStartTime = 0;
    }

    digitalWrite(frontLeftLed, LOW);
    digitalWrite(frontRightLed, LOW);
    digitalWrite(rearLeftLed, LOW);
    digitalWrite(rearRightLed, LOW);
  }
  
  delay(1000);
}