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

MPU6050 mpu6050(Wire);
Adafruit_SSD1306 display(128, 64, &Wire, -1);

const int SYMBOL_SIZE = 8;
float posX = 60, posY = 30;
float speedX = 0, speedY = 0;
int lastUpdate = 0;
int minDelay = 100;
int lastSerialOutput = 0;

void setup() {
  Serial.begin(9600);
  Wire.begin();
  mpu6050.begin();

  display.begin(SSD1306_SWITCHCAPVCC, 0x3C);
  display.clearDisplay();
  display.display();
}

void loop() {
  int currentTime = millis();
  float dt = (currentTime - lastUpdate) / 1000.0;
  lastUpdate = currentTime;

  mpu6050.update();

  float accelX = mpu6050.getAccX();
  float accelY = mpu6050.getAccY();

  // Compute speed from accelerometer
  speedX += accelX * dt * 2;
  speedY += accelY * dt * 2;

  // Apply speed to position
  posX += speedX * dt;
  posY += speedY * dt;

  // Keep symbol within bounds of screen
  if (posX < SYMBOL_SIZE/2) {
    posX = SYMBOL_SIZE/2;
    speedX = 0;
  }

  if (posX > display.width() - SYMBOL_SIZE/2) {
    posX = display.width() - SYMBOL_SIZE/2;
    speedX = 0;
  }

  if (posY < SYMBOL_SIZE/2) {
    posY = SYMBOL_SIZE/2;
    speedY = 0;
  }

  if (posY > display.height() - SYMBOL_SIZE/2) {
    posY = display.height() - SYMBOL_SIZE/2;
    speedY = 0;
  }

  // Clear display and draw symbol at new location
  // display.clearDisplay();
  // display.drawCircle(posX, posY, SYMBOL_SIZE/2, SSD1306_WHITE);
  // display.display();
  drawSprite(posX, posY);

  // Output coordinates to serial terminal
  if ((currentTime - lastSerialOutput) > minDelay) {
    Serial.print(posX);
    Serial.print(",");
    Serial.print(posY);
    Serial.println();
    lastSerialOutput = currentTime;
  }

  delay(1000);
}

void drawSprite(int x, int y) {
  display.clearDisplay();
  display.fillTriangle(x, y, x + SYMBOL_SIZE, y + SYMBOL_SIZE / 2, x + SYMBOL_SIZE, y - SYMBOL_SIZE / 2, WHITE);
  display.fillRect(x + SYMBOL_SIZE + 1, y - SYMBOL_SIZE / 2, SYMBOL_SIZE, SYMBOL_SIZE, WHITE);
  display.display();
}