#include <Wire.h>
#include <Adafruit_MPU6050.h>
#include <Adafruit_Sensor.h>
#include <ESP32Servo.h>

// Create MPU6050 object
Adafruit_MPU6050 mpu;

// Servo motor object and pin setup
Servo myServo;
const int servoPin = 18; // Pin for the servo motor

// Servo motor angle limits for each axis
const int servoXMin = 0;
const int servoXMax = 59;

const int servoYMin = 60;
const int servoYMax = 119;

const int servoZMin = 120;
const int servoZMax = 180;

// Variables to store the previous servo position
int previousServoAngle = 90; // Initialize servo at the center position

void setup() {
  Serial.begin(115200);
  myServo.attach(servoPin);
  myServo.write(previousServoAngle); // Initialize servo to center position

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

  Serial.println("MPU6050 initialized!");

  // Optional: Set sensor configurations
  mpu.setAccelerometerRange(MPU6050_RANGE_2_G);
  mpu.setGyroRange(MPU6050_RANGE_250_DEG);
  mpu.setFilterBandwidth(MPU6050_BAND_21_HZ);
}

void loop() {
  // Variable to hold sensor data
  sensors_event_t accel, gyro, temp;
  mpu.getEvent(&accel, &gyro, &temp);

  // Print gyroscope data (rotation)
  Serial.print("Gyro X: "); Serial.print(gyro.gyro.x);
  Serial.print(" Gyro Y: "); Serial.print(gyro.gyro.y);
  Serial.print(" Gyro Z: "); Serial.println(gyro.gyro.z);

  int servoAngle = previousServoAngle;

  // Map the gyroscope rotation values to corresponding servo angles

  // X-axis rotation corresponds to 0-59 degrees
  if (abs(gyro.gyro.x) > 0.5) {  // Threshold to prevent noise, you can adjust this
    servoAngle = map(gyro.gyro.x * 100, -25000, 25000, servoXMin, servoXMax);  // Scale gyro values to degrees
  }
  // Y-axis rotation corresponds to 60-119 degrees
  else if (abs(gyro.gyro.y) > 0.5) {  
    servoAngle = map(gyro.gyro.y * 100, -25000, 25000, servoYMin, servoYMax);
  }
  // Z-axis rotation corresponds to 120-180 degrees
  else if (abs(gyro.gyro.z) > 0.5) {  
    servoAngle = map(gyro.gyro.z * 100, -25000, 25000, servoZMin, servoZMax);
  }

  // Constrain the servo angle to the defined limits
  servoAngle = constrain(servoAngle, 0, 180);

  // Update the servo position if it has changed
  if (servoAngle != previousServoAngle) {
    myServo.write(servoAngle);
    previousServoAngle = servoAngle;
    Serial.print("Servo moving to angle: ");
    Serial.println(servoAngle);
  }

  // Delay for stability
  delay(100);
}















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

// Servo sg90;
// int servo_pin = 9;
// MPU6050 sensor;
// int16_t ax, ay, az;
// int16_t gx, gy, gz;

// void setup() {

//   sg90.attach(servo_pin);
//   Wire.begin();
//   Serial.begin(9600);
//   Serial.println("Initializing the sensor");
//   sensor.initialize();
//   Serial.println(sensor.testConnection() ? "Successfully Connected" : "Connection failed");
//   delay(1000);
//   Serial.println("Taking Values from the sensor");
//   delay(1000);
// }

// void loop() {
//   sensor.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
//   ax = map(ax, -17000, 17000, 0, 180);
//   Serial.println(ax);
//   sg90.write(ax);
//   delay(200);
// }