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

// Define servo pins (use PWM-capable pins on ESP32)
#define PITCH_PIN 13
#define YAW_PIN 14
#define ROLL_PIN 15

// Define I2C pins for ESP32 (default pins, can be changed)
#define SDA_PIN 21
#define SCL_PIN 22

// Servo objects
Servo pitchServo;
Servo yawServo;
Servo rollServo;

// MPU6050 object
MPU6050 mpu;

// Variables for MPU6050 data
int16_t accelX, accelY, accelZ;
int16_t gyroX, gyroY, gyroZ;

// Map function with limit to prevent jitter
int mapServo(int value) {
  value = constrain(value, -20000, 20000); // Limit input range
  return map(value, -20000, 20000, 30, 150); // Map to servo-safe range
}

void setup() {
  Serial.begin(115200);  // ESP32 handles higher baud rates well
  Wire.begin(SDA_PIN, SCL_PIN);  // Initialize I2C with custom pins

  mpu.initialize();
  if (mpu.testConnection()) {
    Serial.println("MPU6050 Connected!");
  } else {
    Serial.println("MPU6050 Connection Failed!");
    while (1); // Stop execution if the sensor fails
  }

  // Attach servos to their respective pins
  pitchServo.attach(PITCH_PIN, 500, 2400); // ESP32 PWM range tuning
  yawServo.attach(YAW_PIN, 500, 2400);
  rollServo.attach(ROLL_PIN, 500, 2400);

  // Initial neutral position
  pitchServo.write(90);
  yawServo.write(90);
  rollServo.write(90);
}

void loop() {
  // Read MPU6050 data
  mpu.getMotion6(&accelX, &accelY, &accelZ, &gyroX, &gyroY, &gyroZ);

  // Smoothing for stability
  static int prevPitch = 90, prevYaw = 90, prevRoll = 90;
  int targetPitch = mapServo(gyroX);
  int targetYaw = mapServo(gyroY);
  int targetRoll = mapServo(gyroZ);

  // Apply smoothing
  prevPitch = (prevPitch * 0.8) + (targetPitch * 0.2);
  prevYaw = (prevYaw * 0.8) + (targetYaw * 0.2);
  prevRoll = (prevRoll * 0.8) + (targetRoll * 0.2);

  // Move servos
  pitchServo.write(prevPitch);
  yawServo.write(prevYaw);
  rollServo.write(prevRoll);

  // Debug info
  Serial.print("Pitch: "); Serial.print(prevPitch);
  Serial.print(" | Yaw: "); Serial.print(prevYaw);
  Serial.print(" | Roll: "); Serial.println(prevRoll);

  delay(20);  // Faster loop for smoother response
}