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

MPU6050 mpu;
Servo servoRoll, servoPitch, servoYaw;

// Variabel sudut
double roll, pitch, yaw;
double rollSetpoint = 0, rollInput, rollOutput;
double pitchSetpoint = 0, pitchInput, pitchOutput;
double yawSetpoint = 0, yawInput, yawOutput;

// Parameter PID (sesuaikan nilai Kp, Ki, Kd)
double Kp = 2.0, Ki = 0.5, Kd = 1.0;
PID rollPID(&rollInput, &rollOutput, &rollSetpoint, Kp, Ki, Kd, DIRECT);
PID pitchPID(&pitchInput, &pitchOutput, &pitchSetpoint, Kp, Ki, Kd, DIRECT);
PID yawPID(&yawInput, &yawOutput, &yawSetpoint, Kp, Ki, Kd, DIRECT);

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

    // Inisialisasi Servo
    servoRoll.attach(4);
    servoPitch.attach(0);
    servoYaw.attach(2);

    // Inisialisasi PID
    rollPID.SetMode(AUTOMATIC);
    pitchPID.SetMode(AUTOMATIC);
    yawPID.SetMode(AUTOMATIC);
}

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

    // Konversi ke sudut (misal dengan complementary filter)
    rollInput = atan2(ay, az) * 180 / PI;
    pitchInput = atan2(-ax, sqrt(ay * ay + az * az)) * 180 / PI;
    yawInput = gx / 131.0;  // Yaw dari giroskop

    // Update PID
    rollPID.Compute();
    pitchPID.Compute();
    yawPID.Compute();

    // Kontrol servo
    servoRoll.write(90 + rollOutput);
    servoPitch.write(90 + pitchOutput);
    servoYaw.write(90 + yawOutput);

    // Debug
    Serial.print("Roll: "); Serial.print(rollInput);
    Serial.print(" | Pitch: "); Serial.print(pitchInput);
    Serial.print(" | Yaw: "); Serial.println(yawInput);

    delay(10);
}