// // Basic demo for accelerometer readings from Adafruit MPU6050

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

// Adafruit_MPU6050 mpu;

// int ax = 0;
// int ay = 0;
// int az = 0;

// // Definisi PID untuk kontrol keseimbangan
// #define KP 10.0
// #define KI 0.0
// #define KD 0.0


// // Deklarasi variabel untuk PID
// float error_sum_pitch = 0.0;
// float last_error_pitch = 0.0;

// float error_sum_roll = 0.0;
// float last_error_roll = 0.0;

// // Deklarasi variabel untuk menyimpan sudut roll dan pitch
// float roll = 0.0;
// float pitch = 0.0;

// float pid_output_roll;
// float pid_output_pitch;

// void setup(void) {
//   Serial.begin(115200);
//   while (!Serial)
//     delay(10); // will pause Zero, Leonardo, etc until serial console opens

//   Serial.println("Adafruit MPU6050 test!");

//   // Try to initialize!
//   if (!mpu.begin()) {
//     Serial.println("Failed to find MPU6050 chip");
//     while (1) {
//       delay(10);
//     }
//   }
//   Serial.println("MPU6050 Found!");

//   mpu.setAccelerometerRange(MPU6050_RANGE_8_G);
//   Serial.print("Accelerometer range set to: ");
//   switch (mpu.getAccelerometerRange()) {
//     case MPU6050_RANGE_2_G:
//       Serial.println("+-2G");
//       break;
//     case MPU6050_RANGE_4_G:
//       Serial.println("+-4G");
//       break;
//     case MPU6050_RANGE_8_G:
//       Serial.println("+-8G");
//       break;
//     case MPU6050_RANGE_16_G:
//       Serial.println("+-16G");
//       break;
//   }
//   mpu.setGyroRange(MPU6050_RANGE_500_DEG);
//   Serial.print("Gyro range set to: ");
//   switch (mpu.getGyroRange()) {
//     case MPU6050_RANGE_250_DEG:
//       Serial.println("+- 250 deg/s");
//       break;
//     case MPU6050_RANGE_500_DEG:
//       Serial.println("+- 500 deg/s");
//       break;
//     case MPU6050_RANGE_1000_DEG:
//       Serial.println("+- 1000 deg/s");
//       break;
//     case MPU6050_RANGE_2000_DEG:
//       Serial.println("+- 2000 deg/s");
//       break;
//   }

//   mpu.setFilterBandwidth(MPU6050_BAND_21_HZ);
//   Serial.print("Filter bandwidth set to: ");
//   switch (mpu.getFilterBandwidth()) {
//     case MPU6050_BAND_260_HZ:
//       Serial.println("260 Hz");
//       break;
//     case MPU6050_BAND_184_HZ:
//       Serial.println("184 Hz");
//       break;
//     case MPU6050_BAND_94_HZ:
//       Serial.println("94 Hz");
//       break;
//     case MPU6050_BAND_44_HZ:
//       Serial.println("44 Hz");
//       break;
//     case MPU6050_BAND_21_HZ:
//       Serial.println("21 Hz");
//       break;
//     case MPU6050_BAND_10_HZ:
//       Serial.println("10 Hz");
//       break;
//     case MPU6050_BAND_5_HZ:
//       Serial.println("5 Hz");
//       break;
//   }

//   Serial.println("");
//   delay(100);
// }

// /* Convert radians to servo position offset. */
// short radToServo(float rads) {
//   float val = (rads * 100) / 51 * 100;
//   return (int)val;
// }

// float degreesToRadians(float degrees) {
//   return degrees * (3.14159265358979323846 / 180.0);
// }

// void ReadMPU(void){
//   sensors_event_t a, g, temp;
//   mpu.getEvent(&a, &g, &temp);
//   ax = a.acceleration.x;
//   ay = a.acceleration.y;
//   az = a.acceleration.z;



//   // Hitung sudut kemiringan (roll dan pitch)
//   roll = degreesToRadians(atan2(ay, az) * 180 / PI);
//   pitch = degreesToRadians(atan2(-ax, sqrt(ay * ay + az * az)) * 180 / PI);
// }


// void PID(void){
//   // Hitung sinyal kontrol menggunakan PID pitch
//   float target_pitch = 0.0; // Target sudut pitch (dapat disesuaikan sesuai kebutuhan)
//   float errorPitch = target_pitch - pitch;
//   error_sum_pitch += errorPitch;
//   float d_error_pitch = errorPitch - last_error_pitch;
//   last_error_pitch = errorPitch;
//   pid_output_pitch = KP * errorPitch + KI * error_sum_pitch + KD * d_error_pitch;


//     // Hitung sinyal kontrol menggunakan PID roll
//   float target_roll = 0.0; // Target sudut pitch (dapat disesuaikan sesuai kebutuhan)
//   float error_roll = target_roll - roll;
//   error_sum_roll += error_roll;
//   float d_error_roll = error_roll - last_error_roll;
//   last_error_roll = error_roll;
//   pid_output_roll = KP * error_roll + KI * error_sum_roll + KD * d_error_roll;
// }


// void MoveLeg(void){
//   int kakiDepanKanan;
//   int kakiDepanKiri;
//   int kakiBelakangKanan;
//   int kakiBelakangKiri;

//   kakiDepanKanan = radToServo(max(pid_output_roll, pid_output_pitch));
//   kakiDepanKiri = radToServo(max(pid_output_roll, -pid_output_pitch));
//   kakiBelakangKanan = radToServo(max(-pid_output_roll, pid_output_pitch));
//   kakiBelakangKiri = radToServo(max(-pid_output_roll, -pid_output_pitch));

//   Serial.print("kanan : ");
//   Serial.println(kakiDepanKanan);
//   Serial.print("kiri : ");
//   Serial.println(kakiDepanKiri);
//   Serial.print("kanan belakang : ");
//   Serial.println(kakiBelakangKanan);
//   Serial.print("kiri belakang : ");
//   Serial.println(kakiBelakangKiri);


//   Serial.println("");
// }



// void loop() {
//   MoveLeg();



//   // Calculate Pitch & Roll from accelerometer (deg)
//   // pitch = -(atan2(ax, sqrt(ay * ay + az * az)) * 180.0) / M_PI;
//   // roll  = (atan2(ay, az) * 180.0) / M_PI;







//   delay(500);
// }


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

Adafruit_MPU6050 mpu;

const float Kp = 1.0;  // Proportional
const float Ki = 0.1;  // integral
const float Kd = 0.0;  // Derivative
const float alpha = 0.5; // Complementary filter coefficient

float prevRoll = 0.0;
float prevPitch = 0.0;

// Constants for PID control
const float setPoint = 0.0;  // Desired roll angle (in degrees)

void setup() {
  Serial.begin(115200);
  while (!Serial) delay(10); // wait for Serial Monitor to open
  Serial.println("Adafruit MPU6050 test!");

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

  mpu.setAccelerometerRange(MPU6050_RANGE_8_G);
  mpu.setGyroRange(MPU6050_RANGE_500_DEG);
  mpu.setFilterBandwidth(MPU6050_BAND_21_HZ);

  Serial.println("MPU6050 initialized successfully!");
  Serial.println("");
  delay(100);
}

void loop() {
  sensors_event_t accel;
  sensors_event_t gyro;
  mpu.getAccelerometerSensor()->getEvent(&accel);
  mpu.getGyroSensor()->getEvent(&gyro);

  // Calculate roll and pitch angles using complementary filter
  float rollAccel = degreesToRadians(atan2(accel.acceleration.y, accel.acceleration.z) * 180.0 / PI);
  float pitchAccel = degreesToRadians(atan2(-accel.acceleration.x, sqrt(accel.acceleration.y * accel.acceleration.y + accel.acceleration.z * accel.acceleration.z)) * 180 / PI);

  float rollGyro = prevRoll + gyro.gyro.z * 0.01; // Integration over time
  float roll = alpha * rollGyro + (1 - alpha) * rollAccel;
  prevRoll = roll;

  float pitchGyro = prevPitch + gyro.gyro.y * 0.01; // Integration over time
  float pitch = alpha * pitchGyro + (1 - alpha) * pitchAccel;
  prevPitch = pitch;




  float previousErrorRoll_RF = 0.0;
  float integralRoll_RF = 0.0;
  float previousErrorPitch_RF = 0.0;
  float integralPitch_RF = 0.0;


  float errorRoll_RF = setPoint - roll;
  integralRoll_RF += errorRoll_RF;
  float derivativeRoll_RF = errorRoll_RF - previousErrorRoll_RF;
  float pidOutputRoll_RF = radServo(Kp * errorRoll_RF + Ki * integralRoll_RF + Kd * derivativeRoll_RF);
  previousErrorRoll_RF = previousErrorRoll_RF;


  float errorPitch_RF = setPoint - pitch;
  integralPitch_RF += errorPitch_RF;
  float derivativePitch_RF = errorPitch_RF - previousErrorPitch_RF;
  float pidOutputPitch_RF = radServo(Kp * errorPitch_RF + Ki * integralPitch_RF + Kd * derivativePitch_RF);
  previousErrorPitch_RF = errorPitch_RF;

  // Serial.print("rollAccel");
  Serial.println(rollAccel);
  // Serial.print("roll");
  Serial.println(pidOutputRoll_RF);
}

/* Convert radians to servo position offset. */
short radServo(float rads) {
  float val = (rads * 100) / 51 * 100;
  return (int)val;
}

float degreesToRadians(float degrees) {
  return degrees * (3.14159265358979323846 / 180.0);
}