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

MPU6050 mpu;

int width = 128;
int height = 64;
int reset = 1;

Adafruit_SSD1306 oled(width, height, &Wire, reset);

void setup() {
  
Serial.begin(9600);
Wire.begin();
mpu.initialize();

if (mpu.testConnection()) {
 Serial.println("MPU6050 connection successful");
} 
else 
 {
  Serial.println("MPU6050 connection failed");
  while(1);
 } 
if (!oled.begin(SSD1306_SWITCHCAPVCC, 0x3C)) 
{
  Serial.println("SSD1306 OLED connection failed");
  while(true);
}

oled.clearDisplay();
}

void loop() {
 
int ax, ay, az;
int gx, gy, gz;


mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
  
int circleX = map(ax, -17000, 17000, 0, width);  
int circleY = map(ay, -17000, 17000, 0, height); 
int radius = map(abs(az), 0, 17000, 5, 30);       

oled.clearDisplay();
oled.fillCircle(circleX, circleY, radius, WHITE);

oled.display();

Serial.print("a/g:\t");
Serial.print(ax); Serial.print('\t');
Serial.print(ay); Serial.print('\t');
Serial.print(az); Serial.print('\t');
Serial.print(gx); Serial.print('\t');
Serial.print(gy); Serial.print('\t');
Serial.print(gz); Serial.println();

delay(100);
}