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

#define SCREEN_WIDTH 128
#define SCREEN_HEIGHT 64

Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, -1);

MPU6050 mpu6050(Wire);

void setup() {

  display.begin(SSD1306_SWITCHCAPVCC, 0x3C);
  display.clearDisplay();
  display.setTextSize(1);
  display.setTextColor(WHITE);
  
  Wire.begin();
  mpu6050.begin();

  delay(1000); // wait for the sensor to stabilize
}

void loop(){
  mpu6050.update();
  float angleX = mpu6050.getAngleX();
  float angleY = mpu6050.getAngleY();

  display.clearDisplay();
  display.setCursor(0, 0);
  display.print("X: ");
  display.print(angleX);
  display.print(" deg");
  display.setCursor(0, 10);
  display.print("Y: ");
  display.print(angleY);
  display.print(" deg");

  drawLevel(angleX, angleY);

  display.display();
  delay(50);
}

void drawLevel(float angleX, float angleY){
  display.drawLine(34, 32, 102, 32, WHITE);
  display.drawLine(68, 2, 68, 62, WHITE);

  int centerX = 68;
  int centerY = 32;

  int ballX = centerX + (int)(30 * cos(angleX * PI / 180));
  int ballY = centerY + (int)(30 * sin(angleY * PI / 180));
  
  display.fillCircle(ballX, ballY, 3, WHITE);
}