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

MPU6050 mpu;

const int ledPinForward = 13;
const int ledPinBackward = 12;
const int ledPinLeft = 11;
const int ledPinRight = 10;

void setup() {
  Serial.begin(9600);

  // Initialize MPU6050
  mpu.initialize();
  mpu.setDLPFMode(6); // Set Digital Low Pass Filter to reduce noise

  // Set LED pins as OUTPUT
  pinMode(ledPinForward, OUTPUT);
  pinMode(ledPinBackward, OUTPUT);
  pinMode(ledPinLeft, OUTPUT);
  pinMode(ledPinRight, OUTPUT);
}

void loop() {
  // Read gyroscope data
  int16_t gx, gy, gz;
  mpu.getRotation(&gx, &gy, &gz);

  // Detect hand gestures based on gyroscope data
  // You may need to adjust these thresholds based on your testing
  if (gx > 3000) { // Forward gesture
    digitalWrite(ledPinForward, HIGH);
    digitalWrite(ledPinBackward, LOW);
    digitalWrite(ledPinLeft, LOW);
    digitalWrite(ledPinRight, LOW);
  } else if (gx < -3000) { // Backward gesture
    digitalWrite(ledPinForward, LOW);
    digitalWrite(ledPinBackward, HIGH);
    digitalWrite(ledPinLeft, LOW);
    digitalWrite(ledPinRight, LOW);
  } else if (gy > 3000) { // Right gesture
    digitalWrite(ledPinForward, LOW);
    digitalWrite(ledPinBackward, LOW);
    digitalWrite(ledPinLeft, LOW);
    digitalWrite(ledPinRight, HIGH);
  } else if (gy < -3000) { // Left gesture
    digitalWrite(ledPinForward, LOW);
    digitalWrite(ledPinBackward, LOW);
    digitalWrite(ledPinLeft, HIGH);
    digitalWrite(ledPinRight, LOW);
  } else { // No gesture
    digitalWrite(ledPinForward, LOW);
    digitalWrite(ledPinBackward, LOW);
    digitalWrite(ledPinLeft, LOW);
    digitalWrite(ledPinRight, LOW);
  }

  delay(100); // Adjust delay as needed for responsiveness
}