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

Servo servo;
Adafruit_MPU6050 mpu;

int accumulatedAngle = 90;
unsigned long previousTime = 0;
int sensitivityFactor = 2.0;

void setup(void) {
  // Initialize Serial Communication for debugging
  Serial.begin(115200);
  // Initialize I2C communication
  Wire.begin();

  // Attach the servo to pin D5 (which corresponds to GPIO 14 on the Wemos D1 Mini)
  servo.attach(D5);
  servo.write(accumulatedAngle);       // Set servo to the initial position

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

  // Configure the gyroscope range
  mpu.setGyroRange(MPU6050_RANGE_500_DEG); // Set to 500 degrees/sec range
  // Set the bandwidth for noise filtering
  mpu.setFilterBandwidth(MPU6050_BAND_21_HZ);

  // Record the initial time
  previousTime = millis();
}

void loop() {
  // Create events for accelerometer, gyroscope, and temperature readings
  sensors_event_t a, g, temp;
  // Get the new sensor data
  mpu.getEvent(&a, &g, &temp);

  // Calculate the elapsed time in seconds
  unsigned long currentTime = millis();
  float deltaTime = (currentTime - previousTime) / 1000.0; // Convert milliseconds to seconds
  previousTime = currentTime;

  // Get the Y-axis rotation rate and apply the sensitivity factor
  int gyroRate = g.gyro.y * sensitivityFactor; // Amplify the gyroscope rate for more sensitivity
  float angleChange = gyroRate * deltaTime;      // Change in angle over the elapsed time

  // Accumulate the angle change
  accumulatedAngle += angleChange;
  
  // Constrain the angle to be within 0 to 180 degrees
  accumulatedAngle = constrain(accumulatedAngle, 0, 180);

  // Write the new angle to the servo
  servo.write(accumulatedAngle);

  // Output the angle and gyro rate for debugging
  Serial.print("Gyro Y-axis Rate: ");
  Serial.print(gyroRate);
  Serial.print(" | Accumulated Angle: ");
  Serial.println(accumulatedAngle);

  // Small delay to make the output more readable
  delay(10);
}