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

#define SCREEN_WIDTH 128
#define SCREEN_HEIGHT 64
#define OLED_RESET -1
#define OLED_ADDRESS 0x3C // Alamat I2C untuk OLED

Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);
MPU6050 mpu;

void setup() {
  Serial.begin(9600);
  
  // Inisialisasi OLED
  display.begin(SSD1306_SWITCHCAPVCC, OLED_ADDRESS);
  display.clearDisplay();
  display.display();

  // Inisialisasi sensor
  mpu.initialize();
}

void loop() {
  // Membaca data dari MPU6050
  int16_t ax, ay, az;
  int16_t gx, gy, gz;
  mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
  float temperature = mpu.getTemperature() / 340.00 + 36.53; // Konversi suhu

  // Menghitung heading
  float heading = atan2(ay, ax) * 180 / PI;
  if (heading < 0) {
    heading += 360; // Pastikan heading positif
  }

  // Menentukan arah
  String direction;
  if (heading >= 337.5 || heading < 22.5) {
    direction = "Utara";
  } else if (heading >= 22.5 && heading < 67.5) {
    direction = "Utara-Timur";
  } else if (heading >= 67.5 && heading < 112.5) {
    direction = "Timur";
  } else if (heading >= 112.5 && heading < 157.5) {
    direction = "Selatan-Timur";
  } else if (heading >= 157.5 && heading < 202.5) {
    direction = "Selatan";
  } else if (heading >= 202.5 && heading < 247.5) {
    direction = "Selatan-Barat";
  } else if (heading >= 247.5 && heading < 292.5) {
    direction = "Barat";
  } else if (heading >= 292.5 && heading < 337.5) {
    direction = "Utara-Barat";
  }

  // Menampilkan data di OLED
  display.clearDisplay();
  display.setTextSize(1);
  display.setTextColor(SSD1306_WHITE);
  display.setCursor(0, 0);
  display.print("Heading: ");
  display.setTextSize(2);
  display.setCursor(0, 10);
  display.print(heading);
  display.print("°");
  display.setTextSize(1);
  display.setCursor(0, 40);
  display.print("Arah: ");
  display.print(direction);
  display.setCursor(0, 50);
  display.print("Suhu: ");
  display.print(temperature);
  display.print(" C");
  display.display();

  delay(10); // Update setiap detik
}