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

// Membuat objek servo untuk akselerasi dan rotasi
Servo servoAccelX;  // Servo untuk akselerasi pada sumbu X
Servo servoAccelY;  // Servo untuk akselerasi pada sumbu Y
Servo servoAccelZ;  // Servo untuk akselerasi pada sumbu Z
Servo servoRotX;    // Servo untuk rotasi pada sumbu X
Servo servoRotY;    // Servo untuk rotasi pada sumbu Y
Servo servoRotZ;    // Servo untuk rotasi pada sumbu Z

// Alamat I2C untuk MPU6050 dan LCD
const int MPU_ADDR = 0x68;
LiquidCrystal_I2C lcd(0x27, 16, 2); // Alamat I2C untuk LCD 16x2

// Variabel untuk menyimpan data akselerometer, gyroscope, dan suhu
int16_t accelX, accelY, accelZ;
int16_t gyroX, gyroY, gyroZ;
int16_t tempRaw;  // Variabel untuk menyimpan suhu mentah
float temperature; // Variabel untuk menyimpan suhu yang sudah dikonversi

void setup() {
  Wire.begin(); // Memulai komunikasi I2C
  lcd.init();   // Inisialisasi LCD
  lcd.backlight(); // Menyalakan lampu latar LCD

  Wire.beginTransmission(MPU_ADDR);
  Wire.write(0x6B); // Mengatur register Power Management 1
  Wire.write(0);    // Mengaktifkan sensor MPU6050
  Wire.endTransmission(true);

  // Menghubungkan servo ke pin Arduino
  servoAccelX.attach(9);   // Servo untuk akselerasi sumbu X di pin 9
  servoAccelY.attach(10);  // Servo untuk akselerasi sumbu Y di pin 10
  servoAccelZ.attach(13);  // Servo untuk akselerasi sumbu Z di pin 11
  servoRotX.attach(11);    // Servo untuk rotasi sumbu X di pin 12
  servoRotY.attach(12);    // Servo untuk rotasi sumbu Y di pin 13
  servoRotZ.attach(8);     // Servo untuk rotasi sumbu Z di pin 8

  // Menampilkan pesan awal di LCD
  lcd.setCursor(0, 0);
  lcd.print("Servo System Init");
  delay(1000);
  lcd.clear();
}

void loop() {
  // Membaca data akselerometer
  Wire.beginTransmission(MPU_ADDR);
  Wire.write(0x3B); // Register akselerometer
  Wire.endTransmission(false);
  Wire.requestFrom(MPU_ADDR, 6, true); // Meminta 6 byte data akselerometer
  accelX = Wire.read() << 8 | Wire.read();
  accelY = Wire.read() << 8 | Wire.read();
  accelZ = Wire.read() << 8 | Wire.read();

  // Membaca data gyroscope
  Wire.beginTransmission(MPU_ADDR);
  Wire.write(0x43); // Register gyroscope
  Wire.endTransmission(false);
  Wire.requestFrom(MPU_ADDR, 6, true); // Meminta 6 byte data gyroscope
  gyroX = Wire.read() << 8 | Wire.read();
  gyroY = Wire.read() << 8 | Wire.read();
  gyroZ = Wire.read() << 8 | Wire.read();

  // Membaca data suhu dari MPU6050
  Wire.beginTransmission(MPU_ADDR);
  Wire.write(0x41); // Register suhu
  Wire.endTransmission(false);
  Wire.requestFrom(MPU_ADDR, 2, true);  // Meminta 2 byte data suhu
  tempRaw = Wire.read() << 8 | Wire.read(); // Membaca suhu mentah
  temperature = (tempRaw / 340.0) + 36.53;  // Mengonversi ke Celcius

  // Menghitung posisi servo berdasarkan akselerasi
  int servoAccelXPos = map(accelX, -17000, 17000, 0, 180); // Memetakan nilai akselerasi X untuk servo
  int servoAccelYPos = map(accelY, -17000, 17000, 0, 180); // Memetakan nilai akselerasi Y untuk servo
  int servoAccelZPos = map(accelZ, -17000, 17000, 0, 180); // Memetakan nilai akselerasi Z untuk servo

  // Menghitung posisi servo berdasarkan rotasi (gyroscope)
  int servoRotXPos = map(gyroX, -32768, 32767, 0, 180); // Memetakan nilai rotasi X untuk servo
  int servoRotYPos = map(gyroY, -32768, 32767, 0, 180); // Memetakan nilai rotasi Y untuk servo
  int servoRotZPos = map(gyroZ, -32768, 32767, 0, 180); // Memetakan nilai rotasi Z untuk servo

  // Mengatur posisi servo untuk akselerasi
  servoAccelX.write(servoAccelXPos);
  servoAccelY.write(servoAccelYPos);
  servoAccelZ.write(servoAccelZPos);

  // Mengatur posisi servo untuk rotasi
  servoRotX.write(servoRotXPos);
  servoRotY.write(servoRotYPos);
  servoRotZ.write(servoRotZPos);

  // Menampilkan posisi servo akselerasi di LCD
  lcd.setCursor(0, 0);
  lcd.print("Acc X:"); lcd.print(servoAccelXPos);
  lcd.print(" Y:"); lcd.print(servoAccelYPos);

  lcd.setCursor(0, 1);
  lcd.print("Z:"); lcd.print(servoAccelZPos);

  delay(1000); // Delay agar posisi akselerasi terlihat

  // Menampilkan posisi servo rotasi di LCD
  lcd.clear();
  lcd.setCursor(0, 0);
  lcd.print("Rot X:"); lcd.print(servoRotXPos);
  lcd.print(" Y:"); lcd.print(servoRotYPos);

  lcd.setCursor(0, 1);
  lcd.print("Z:"); lcd.print(servoRotZPos);

  delay(1000); // Delay agar posisi rotasi terlihat

  // Menampilkan suhu internal dari MPU6050
  lcd.clear();
  lcd.setCursor(0, 0);
  lcd.print("Temp:"); lcd.print(temperature);
  lcd.print(" C");

  delay(1000); // Delay agar suhu terlihat
}