#include <Adafruit_MPU6050.h>
#include <Adafruit_Sensor.h>
#include <Wire.h>
#include <Servo.h>  // Include the Servo library

Adafruit_MPU6050 mpu;
Servo myServo;  // Create a Servo object

void setup(void) {
  Serial.begin(115200);
  while (!Serial)
    delay(10); // will pause Zero, Leonardo, etc until serial console opens

  Serial.println("Adafruit MPU6050 test!");

  // Initialize MPU6050
  if (!mpu.begin()) {
    Serial.println("Failed to find MPU6050 chip");
    while (1) {
      delay(10);
    }
  }
  Serial.println("MPU6050 Found!");

  // Initialize the Servo on pin 9
  myServo.attach(9);

  // Set up accelerometer and gyro ranges
  mpu.setAccelerometerRange(MPU6050_RANGE_8_G);
  mpu.setGyroRange(MPU6050_RANGE_500_DEG);
  mpu.setFilterBandwidth(MPU6050_BAND_21_HZ);
  delay(100);
}

void loop() {
  // Get accelerometer data
  sensors_event_t a, g, temp;
  mpu.getEvent(&a, &g, &temp);

  // Print accelerometer data
  Serial.print("Acceleration X: ");
  Serial.print(a.acceleration.x);
  Serial.print(", Y: ");
  Serial.print(a.acceleration.y);
  Serial.print(", Z: ");
  Serial.print(a.acceleration.z);
  Serial.println(" m/s^2");

  // Map the accelerometer's X-axis data to servo angle (0-180 degrees)
  int angle = map(a.acceleration.x, -10, 10, 0, 180);  // Adjust the range (-10 to 10 based on your tilt)
  angle = constrain(angle, 0, 180);  // Make sure the angle stays between 0 and 180

  // Move the servo to the mapped angle
  myServo.write(angle);

  // Print the servo angle
  Serial.print("Servo Angle: ");
  Serial.println(angle);

  delay(100); // Wait for 100 ms before taking the next reading
}