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

// For the AD0 pin of the MPU-6050.
const int AD0pin = 18;  // You can keep the AD0 pin, but it's unnecessary with only one sensor.

const int mpuAddr = 0x68;  // I2C address

MPU6050 mpu(Wire);  // Single MPU6050 object
Servo servo;

unsigned long previousMillis;
const unsigned long interval = 1000;

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

  Wire.begin();  // default pins: SDA=21, SCL=22.

  pinMode(AD0pin, OUTPUT);
  digitalWrite(AD0pin, LOW);  // Set for I2C address 0x68

  // Initialize the MPU-6050 sensor.
  Serial.println(F("Initializing the sensor and calculating offsets. Do not move MPU-6050 sensor."));
  delay(500);

  servo.attach(17, 500, 2400);
  byte status = mpu.begin();
    

  Serial.print(F("Initializing MPU-6050, error = "));
  Serial.print(status);
  if(status == 0)
    Serial.print(F(" (no error)"));
  Serial.println();
  
  mpu.calcOffsets(true, true);  // Gyro and accelerometer
  Serial.println("Initialization done");

  // Set previousMillis to a fresh value after setup completes
  previousMillis = millis();
}

int pos, z;

void loop() 
{
  unsigned long currentMillis = millis();
  z = map(mpu.getGyroZ(), -250, 250, 0, 180);
  servo.write(z);
  // Update the sensor data
  mpu.update();

  if (currentMillis - previousMillis >= interval) {
    previousMillis = currentMillis;

    // Display sensor data
    Serial.print(F("T="));
    Serial.println(mpu.getTemp(), 0);
    Serial.print("Acceleration X: ");
    Serial.print(mpu.getAccX());
    Serial.print(",");
    Serial.print(mpu.getAccY());
    Serial.print(",");
    Serial.println(mpu.getAccZ());
    
    Serial.print("Rotation X: ");
    Serial.print(mpu.getGyroX(), 0);
    Serial.print(",");
    Serial.print(mpu.getGyroY(), 0);
    Serial.print(",");
    Serial.println(mpu.getGyroZ(), 0);
  
    
    Serial.println(F("======================================================================================\n"));
  }
}