//libraries for I2C communication and OLED display
#include <Wire.h>
#include <Adafruit_SSD1306.h>
#include <Adafruit_GFX.h>
#include <MPU6050_tockn.h>

//OLED display dimensions
#define WIDTH 128
#define HEIGHT 64

//create display object and MPU object
Adafruit_SSD1306 display(WIDTH, HEIGHT, &Wire, -1);
MPU6050 mpu(Wire);

void setup() {
//setup code runs once
display.begin(SSD1306_SWITCHCAPVCC, 0x3c);
display.clearDisplay();
display.setTextSize(1);
display.setTextColor(WHITE);

Wire.begin();
mpu.begin();
delay(1000);

}

void loop() {
//main code runs repeatedly
mpu.update();
float angleX = mpu.getAngleX();
float angleY = mpu.getAngleY();

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

drawLevel(angleX, angleY);
display.display();

}

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

int centerX = 68, centerY = 32;
int dotX = centerX + (int)(30 * cos(angleX * PI/180));
int dotY = centerY + (int)(30 * sin(angleY * PI/180));

display.fillCircle(dotX, dotY, 5, WHITE);

}