#include "Wire.h"
#include <MPU6050_light.h>

MPU6050 mpu(Wire);

long timer = 0;
const int tiltThreshold = 30; // Nilai batas kemiringan
const int greenLED = 12; // Pin digital untuk lampu hijau
const int redLED = 13;   // Pin digital untuk lampu merah

void setup() {
  Serial.begin(9600);
  Wire.begin();
  
  byte status = mpu.begin();
  Serial.print(F("MPU6050 status: "));
  Serial.println(status);
  while(status != 0){ } // stop everything if could not connect to MPU6050
  
  Serial.println(F("Calculating offsets, do not move MPU6050"));
  delay(1000);
  mpu.calcOffsets(true, true); // gyro and accelero
  Serial.println("Done!\n");
  
  pinMode(greenLED, OUTPUT); // Atur pin sebagai output
  pinMode(redLED, OUTPUT);   // Atur pin sebagai output
}

void loop() {
  mpu.update();

  if(millis() - timer > 1000){ // print data every second
    Serial.print(F("ACCELERO  X: "));Serial.print(mpu.getAccX());
    Serial.print("\tY: ");Serial.print(mpu.getAccY());
    Serial.print("\t  Z: ");Serial.println(mpu.getAccZ());
  
    Serial.print(F("GYRO      X: "));Serial.print(mpu.getGyroX());
    Serial.print("\tY: ");Serial.print(mpu.getGyroY());
    Serial.print("\t  Z: ");Serial.println(mpu.getGyroZ());
  
    Serial.print(F("ACC ANGLE X: "));Serial.print(mpu.getAccAngleX());
    Serial.print("\tY: ");Serial.println(mpu.getAccAngleY());
    
    Serial.print(F("ANGLE     X: "));Serial.print(mpu.getAngleX());
    Serial.print("\tY: ");Serial.print(mpu.getAngleY());
    Serial.print("\t  Z: ");Serial.println(mpu.getAngleZ());
    Serial.println(F("=====================================================\n"));
    
    // Cek kemiringan melebihi threshold
    if(abs(mpu.getAngleX()) > tiltThreshold || abs(mpu.getAngleY()) > tiltThreshold){
        // Ubah lampu menjadi merah karena robot miring
        digitalWrite(greenLED, LOW); // Matikan lampu hijau
        digitalWrite(redLED, HIGH);  // Hidupkan lampu merah
        Serial.println("Robot miring!");
    } else {
        // Ubah lampu menjadi hijau karena robot kembali tegak
        digitalWrite(greenLED, HIGH); // Hidupkan lampu hijau
        digitalWrite(redLED, LOW);   // Matikan lampu merah
        Serial.println("Robot tidak miring lagi");
    }
    timer = millis();
  }
}