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

// Create servo objects
Servo servoX;
Servo servoY;
Servo servoZ;

// MPU6050 object
MPU6050 mpu;

// Variable for sensor values
int16_t ax, ay, az; // Accelerometer readings
int16_t gx, gy, gz; // Gyroscope readings

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

  // Initialize I2C for ESP32
  Wire.begin(21, 22);  // SDA = 21, SCL = 22 (default I2C pins on ESP32)
  
  // Initialize MPU6050
  mpu.initialize();
  if (!mpu.testConnection()) {
    Serial.println("MPU6050 connection failed");
    while (1);
  }

  // Attach servos to GPIO pins on ESP32
  servoX.attach(5);   // PWM pin for Servo X
  servoY.attach(18);  // PWM pin for Servo Y
  servoZ.attach(19);  // PWM pin for Servo Z
}

void loop() {
  // Get raw accelerometer and gyroscope data
  mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);

  // Map accelerometer data (or gyroscope data) to servo angles (0 to 180 degrees)
  int angleX = map(ax, -17000, 17000, 0, 180);
  int angleY = map(ay, -17000, 17000, 0, 180);
  int angleZ = map(az, -17000, 17000, 0, 180);

  // Write angles to servos
  servoX.write(angleX);
  servoY.write(angleY);
  servoZ.write(angleZ);

  // Debugging output
  Serial.print("X Angle: "); Serial.print(angleX);
  Serial.print("\tY Angle: "); Serial.print(angleY);
  Serial.print("\tZ Angle: "); Serial.println(angleZ);
  // Serial.print("\tRX Angle: "); Serial.print(gx);
  // Serial.print("\tRY Angle: "); Serial.print(gy);
  // Serial.print("\tRZ Angle: "); Serial.println(gz);

  delay(100);  // Adjust delay for smooth operation
}