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

MPU6050 mpu;
int16_t accX, accY, accZ;
int16_t  gyroX, gyroY, gyroZ;

Servo servoX, servoY, servoZ;

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

  servoX.attach(9);
  servoY.attach(10);
  servoZ.attach(11);

  Wire.begin();
  mpu.initialize();

  if (!mpu.testConnection()) {
    Serial.println("MPU6050 connection failed!");
    while (1);
  }
}

void loop() {
  // Vector3f accel = mpu.getAcceleration();
  // Vector3f gyro = mpu.getRotation();

  accX = mpu.getAccelerationX();
  accY = mpu.getAccelerationY();
  accZ = mpu.getAccelerationZ();

  gyroX = mpu.getRotationX();
  gyroY = mpu.getRotationY();
  gyroZ = mpu.getRotationZ();

  // Serial.print("Accelerometer: ");
  // Serial.print("X = "); Serial.print(accX);
  // Serial.print(" Y = "); Serial.print(accY);
  // Serial.print(" Z = "); Serial.println(accZ);

  // Serial.print("Gyroscope: ");
  // Serial.print("X = "); Serial.print(gyroX);
  // Serial.print(" Y = "); Serial.print(gyroY);
  // Serial.print(" Z = "); Serial.println(gyroZ);


  int angleX = map(gyroX, -2000, 2000, 0, 360);
  int angleY = map(gyroY, -2000, 2000, 0, 360);
  int angleZ = map(gyroZ, -2000, 2000, 0, 360);

  // Limit the servo angles to avoid excessive movements
  angleX = constrain(angleX, 0, 360);
  angleY = constrain(angleY, 0, 360);
  angleZ = constrain(angleZ, 0, 360);

  // Move the servos to the calculated angles
  // servoX.write(angleX);
  // servoY.write(angleY);
  // servoZ.write(angleZ);

  // Serial.print("Servo Angles: ");
  Serial.print("X = "); Serial.print(angleX);
  Serial.print(" Y = "); Serial.print(angleY);
  Serial.print(" Z = "); Serial.println(angleZ);


  delay(150);
}