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

#define SCREEN_WIDTH 128
#define SCREEN_HEIGHT 64
#define OLED_RESET 4
#define SCREEN_ADDRESS 0x3D
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);

Adafruit_MPU6050 mpu;

int currentx = 0;
int currenty = 0;
int currentz = 0;

void setup() {
  Serial.begin(9600);
  Wire.begin();
  mpu.begin();
  display.begin(SSD1306_SWITCHCAPVCC, 0x3C);
  display.clearDisplay();
  display.setTextSize(2);
  display.setTextColor(WHITE);
}

sensors_event_t event;

void loop() {
  mpu.getAccelerometerSensor()->getEvent(&event);
  int x = event.acceleration.x;
  int y = event.acceleration.y;
  int z = event.acceleration.z;

  currentx = currentx + x;
  currentx = max(-64, min(64, currentx));
  currenty = currenty + y;
  currenty = max(-32, min(32, currenty));
  currentz = currentz + z;
  currentz = max(-32, min(32, currentz));


  display.clearDisplay();
  display.drawLine(currentx + 64, currenty - currentz/2 + 30, currentx + 64, currenty + currentz/2 + 30, WHITE);
  display.drawLine(currentx - currentz/2 + 64, currenty + 30, currentx + currentz/2 + 64, currenty + 30, WHITE);
  display.drawRect(currentx - currentz/2 + 64, currenty - currentz/2 + 30, currentz, currentz, WHITE);
  display.display();
  delay(500);
}