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

// Declare global constants and objects
const int OLED_ADDR = 0x3C;  // OLED display I2C address
Adafruit_SSD1306 display(128, 64, &Wire, -1);

MPU6050 mpu;

void setup() {
  // Initialize serial communication
  Serial.begin(9600);
  
  // Initialize OLED display
  display.begin(SSD1306_SWITCHCAPVCC, OLED_ADDR);

  // Initialize I2C bus and MPU6050 sensor
  Wire.begin();
  mpu.initialize();
}

void loop() {
  // Declare variables for sensor readings
  int16_t ax, ay, az;
  float roll, pitch;

  // Read accelerometer data from MPU6050 sensor
  mpu.getAcceleration(&ax, &ay, &az);

  // Compute roll and pitch angles in degrees
  roll = atan2(ay, az) * 180.0 / PI;
  pitch = atan2(-ax, sqrt(ay * ay + az * az)) * 180.0 / PI;

  // Clear OLED display
  display.clearDisplay();

  // Print roll angle on first row of OLED display
  display.setTextSize(1);
  display.setTextColor(SSD1306_WHITE);
  display.setCursor(0, 0);
  display.println("Roll: " + String(roll, 1) + " deg");

  // Print pitch angle on second row of OLED display
  display.setCursor(0, 10);
  display.println("Pitch: " + String(pitch, 1) + " deg");

  // Draw a simple 2D graphic based on roll and pitch angles
  drawGraphic(roll, pitch);

  // Display the updated content on the OLED display
  display.display();

  // Wait for some time before repeating the loop
  delay(100);
}

void drawGraphic(float roll, float pitch) {
  // Compute x and y coordinates of the '*' character
  int16_t x = map(roll, -90, 90, 1, 128);
  int16_t y = map(pitch, -90, 90, 1, 64);

  // Draw the '*' character at the computed x and y coordinates
  display.drawPixel(x, y, SSD1306_WHITE);
}