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

#define ACCEL_MS  1000
#define GYRO_MS   500
#define LED_MS    2000

MPU6050 mpu;
uint64_t prevMillisAccel = 0;
uint64_t prevMillisGyro = 0;
uint64_t prevMillisLED = 0;

void setup() {
  Serial.begin(115200);
  Wire.begin();
  mpu.initialize();
  pinMode(2, OUTPUT);
}

void loop() {
  uint64_t currentMillis = millis();

  if (currentMillis - prevMillisAccel >= ACCEL_MS) {
    prevMillisAccel = currentMillis;
    int16_t ax, ay, az;
    mpu.getAcceleration(&ax, &ay, &az);
    float ax_g = ax / 16384.0;
    float ay_g = ay / 16384.0;
    float az_g = az / 16384.0;

    Serial.print("Accelerometer: ");
    Serial.print(ax_g); 
    Serial.print(",");
    Serial.print(ay_g); 
    Serial.print(",");
    Serial.println(az_g);
  }

  if (currentMillis - prevMillisGyro >= GYRO_MS) {
    prevMillisGyro = currentMillis;
    int16_t gx, gy, gz;
    mpu.getRotation(&gx, &gy, &gz);
    float gx_dps = gx / 131.0;
    float gy_dps = gy / 131.0;
    float gz_dps = gz / 131.0;

    Serial.print("Gyroscope: ");
    Serial.print(gx_dps);
    Serial.print(",");
    Serial.print(gy_dps);
    Serial.print(",");
    Serial.println(gz_dps);
  }

  // Menghidupkan dan mematikan LED setiap 2 detik
  if (currentMillis - prevMillisLED >= LED_MS) {
    prevMillisLED = currentMillis;
    digitalWrite(2, !digitalRead(2));
  }
}