#include <Adafruit_MPU6050.h>
#include <Adafruit_Sensor.h>
#include <Adafruit_BusIO_Register.h>
#include <Wire.h>
#include <Servo.h>

Servo servoY;
Servo servoZ;
Adafruit_MPU6050 srituhobby;

void setup(void) {
  Serial.begin(115200);
  
  // Attach servo for Y-axis to pin 3
  servoY.attach(3);
  
  // Attach servo for Z-axis to pin 5 (adjust pin as needed)
  servoZ.attach(5);

  Wire.begin();
  srituhobby.begin();
  
  // Set initial positions for servos
  servoY.write(0);
  servoZ.write(0);

  srituhobby.setAccelerometerRange(MPU6050_RANGE_8_G); // 2_G, 4_G, 8_G, 16_G
  srituhobby.setGyroRange(MPU6050_RANGE_500_DEG);       // 250, 500, 1000, 2000
  srituhobby.setFilterBandwidth(MPU6050_BAND_21_HZ);

  delay(100);
}

void loop() {
  /* Get new sensor events with the readings */
  sensors_event_t a, g, temp;
  srituhobby.getEvent(&a, &g, &temp);

  // Convertendo os valores do acelerômetro para aceleração da gravidade (g)
  float accel_x_g = a.acceleration.x / 9.81; // 1 g é aproximadamente 9.81 m/s^2
  float accel_y_g = a.acceleration.y / 9.81;
  float accel_z_g = a.acceleration.z / 9.81;

  // Convertendo os valores do giroscópio para graus por segundo
  float gyro_x_deg = g.gyro.x;
  float gyro_y_deg = g.gyro.y; 
  float gyro_z_deg = g.gyro.z;

  // Mapeia o valor do giroscópio (Y) para a posição do servoY
  int servoY_position = map(gyro_y_deg, -4, 4, 0, 180);
  servoY.write(servoY_position);
  Serial.print("Servo Y: ");
  Serial.println(servoY_position);

  // Mapeia o valor do giroscópio (Z) para a posição do servoZ
  int servoZ_position = map(gyro_z_deg, -4, 4, 0, 180);
  servoZ.write(servoZ_position);
  Serial.print("Servo Z: ");
  Serial.println(servoZ_position);

  // Exibindo os valores convertidos
  Serial.print("Acelerômetro (X): ");
  Serial.print(accel_x_g);
  Serial.print(" g\t");

  Serial.print("Acelerômetro (Y): ");
  Serial.print(accel_y_g);
  Serial.print(" g\t");

  Serial.print("Acelerômetro (Z): ");
  Serial.print(accel_z_g);
  Serial.print(" g\t");

  Serial.print("Giroscópio (X): ");
  Serial.print(gyro_x_deg);
  Serial.print(" graus/s\t");

  Serial.print("Giroscópio (Y): ");
  Serial.print(gyro_y_deg);
  Serial.print(" graus/s\t");

  Serial.print("Giroscópio (Z): ");
  Serial.print(gyro_z_deg);
  Serial.print(" graus/s\t");

  delay(100);
}