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

MPU6050 mpu;

int motorPins[] = {13, 14, 27, 12};

void setup() {
  Serial.begin(115200);
  Wire.begin();
  mpu.initialize();

  for (int i = 0; i < 4; i++) {
    pinMode(motorPins[i], OUTPUT);
  }

  if (!mpu.testConnection()) {
    Serial.println("MPU6050 tidak terhubung!");
    while (1);
  }
}

void loop() {
  int16_t ax, ay, az, gx, gy, gz;
  mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);

  float pitch = atan2(ay, az) * 180 / PI;
  float roll = atan2(-ax, az) * 180 / PI;

  int motorSpeed1 = constrain(map(pitch - roll, -180, 180, 0, 255), 0, 255); // Kiri depan
  int motorSpeed2 = constrain(map(pitch + roll, -180, 180, 0, 255), 0, 255); // Kanan depan
  int motorSpeed3 = constrain(map(-pitch - roll, -180, 180, 0, 255), 0, 255); // Kiri belakang
  int motorSpeed4 = constrain(map(-pitch + roll, -180, 180, 0, 255), 0, 255); // Kanan belakang

  analogWrite(motorPins[0], motorSpeed1);
  analogWrite(motorPins[1], motorSpeed2);
  analogWrite(motorPins[2], motorSpeed3);
  analogWrite(motorPins[3], motorSpeed4);

  Serial.print("Pitch: "); Serial.print(pitch);
  Serial.print(" Roll: "); Serial.print(roll);
  Serial.print(" | Motor Speeds: ");
  Serial.print(motorSpeed1); Serial.print(" ");
  Serial.print(motorSpeed2); Serial.print(" ");
  Serial.print(motorSpeed3); Serial.print(" ");
  Serial.print(motorSpeed4);

  if (motorSpeed1 > motorSpeed3 && motorSpeed2 > motorSpeed4 && motorSpeed1 > 50 && motorSpeed2 > 50) {
    Serial.println(" | Miring ke Depan");
  } else if (motorSpeed3 > motorSpeed1 && motorSpeed4 > motorSpeed2 && motorSpeed3 > 50 && motorSpeed4 > 50) {
    Serial.println(" | Miring ke Belakang");
  } else if (motorSpeed1 > motorSpeed2 && motorSpeed3 > motorSpeed4 && motorSpeed1 > 50 && motorSpeed3 > 50) {
    Serial.println(" | Miring ke Kiri");
  } else if (motorSpeed2 > motorSpeed1 && motorSpeed4 > motorSpeed3 && motorSpeed2 > 50 && motorSpeed4 > 50) {
    Serial.println(" | Miring ke Kanan");
  } else {
    Serial.println(" | Melayang Stabil (Hover)");
  }


  delay(100);
}



// Kondisi "Miring ke Depan": Jika motor depan (motorSpeed1 dan motorSpeed2) lebih cepat dari motor belakang (motorSpeed3 dan motorSpeed4), itu berarti drone condong ke depan.
// Kondisi "Miring ke Belakang": Jika motor belakang lebih cepat dari motor depan, berarti drone condong ke belakang.
// Kondisi "Miring ke Kiri": Jika motor kiri (motorSpeed1 dan motorSpeed3) lebih cepat dari motor kanan (motorSpeed2 dan motorSpeed4), berarti drone condong ke kiri.
// Kondisi "Miring ke Kanan": Jika motor kanan lebih cepat dari motor kiri, berarti drone condong ke kanan.
// Kondisi "Melayang Stabil": Jika semua motor berada pada kecepatan rendah (di bawah threshold yang ditentukan), drone dianggap stabil.

// Threshold Kecepatan
// Dalam kondisi penentuan arah, terdapat threshold kecepatan motor (misalnya, > 50) 
// untuk memastikan bahwa perubahan arah benar-benar signifikan. Ini membantu mencegah 
// pembacaan palsu yang mungkin disebabkan oleh fluktuasi kecil dalam data sensor atau kecepatan motor.