#include <Wire.h>
#include <Servo.h>
#include <LiquidCrystal_I2C.h>

#define MPU6050_ADDR 0x68
#define GYRO_XOUT_H 0x43
#define TEMP_OUT_H  0x41

LiquidCrystal_I2C lcd(0x27, 16, 2);
Servo myservo;
const int servoPin = 9;

float angleX = 0;
float filteredRateX = 0;
const float alpha = 0.9;

int lastStep = -999;
unsigned long lastTime = 0;

void setup() {
  Serial.begin(9600);
  Wire.begin();

  Wire.beginTransmission(MPU6050_ADDR);
  Wire.write(0x6B); // Wake up sensor
  Wire.write(0);
  Wire.endTransmission();

  lcd.init();
  lcd.backlight();
  myservo.attach(servoPin);
  lastTime = millis();
}

void loop() {
  unsigned long currentTime = millis();
  float elapsedTime = (currentTime - lastTime) / 1000.0;
  lastTime = currentTime;

  // Baca data gyro X
  Wire.beginTransmission(MPU6050_ADDR);
  Wire.write(GYRO_XOUT_H);
  Wire.endTransmission(false);
  Wire.requestFrom(MPU6050_ADDR, 2, true);
  int16_t gyroX = (Wire.read() << 8) | Wire.read();
  float rateX = gyroX / 131.0;

  // Hitung sudut dengan filter
  filteredRateX = alpha * filteredRateX + (1 - alpha) * rateX;
  angleX += filteredRateX * elapsedTime;
  angleX = constrain(angleX, -90, 90);

  // Deteksi perubahan kelipatan 10 derajat
  int stepX = round(angleX / 10.0) * 10;

  if (stepX != lastStep) {
    int servoPos = map(stepX, -90, 90, 0, 180);
    servoPos = constrain(servoPos, 0, 180);
    myservo.write(servoPos);
    lastStep = stepX;

    Serial.print("AngleX: ");
    Serial.print(stepX);
    Serial.print("° -> Servo: ");
    Serial.println(servoPos);
  }

  // Baca suhu dari sensor
  Wire.beginTransmission(MPU6050_ADDR);
  Wire.write(TEMP_OUT_H);
  Wire.endTransmission(false);
  Wire.requestFrom(MPU6050_ADDR, 2, true);
  int16_t tempRaw = (Wire.read() << 8) | Wire.read();
  float temperature = (tempRaw / 340.0) + 36.53;

  // Tampilkan pada LCD
  lcd.setCursor(0, 0);
  lcd.print("X: ");
  lcd.print(angleX, 1);
  lcd.print((char)223);
  lcd.print("   ");

  lcd.setCursor(0, 1);
  lcd.print("T: ");
  lcd.print(temperature, 1);
  lcd.print((char)223);
  lcd.print("C   ");

  delay(100);
}