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

// OLED Display Settings
#define SCREEN_WIDTH 128
#define SCREEN_HEIGHT 64
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, -1);

// MPU6050 Setup
MPU6050 mpu;

int16_t ax, ay, az; // Accelerometer readings
int16_t gx, gy, gz; // Gyroscope readings

// Dot position on OLED
float dotX = 64; // Start at center
float dotY = 32;

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

  // Initialize MPU6050
  mpu.initialize();
  if (mpu.testConnection()) {
    Serial.println("MPU6050 Connected");
  } else {
    Serial.println("MPU6050 Connection Failed");
    while (1);
  }

  // Initialize OLED Display
  if (!display.begin(SSD1306_PAGEADDR, 0x3C)) {
    Serial.println("OLED initialization failed!");
    while (1);
  }

  display.clearDisplay();
  display.display();

  // Display a welcome message
  display.setTextSize(1);
  display.setTextColor(SSD1306_WHITE);
  display.setCursor(20, 28);
  display.print("Virtual Joystick");
  display.display();
  delay(2000); // Show welcome message for 2 seconds
  display.clearDisplay();
}

void loop() {
  // Read accelerometer data
  mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);

  // Normalize accelerometer readings
  float xMovement = map(ax, -17000, 17000, -5, 5); // Sensitivity adjustment
  float yMovement = map(ay, -17000, 17000, -5, 5);

  // Add movement to dot position
  dotX += xMovement;
  dotY += yMovement;

  // Constrain dot position to OLED boundaries
  dotX = constrain(dotX, 0, SCREEN_WIDTH - 1);
  dotY = constrain(dotY, 0, SCREEN_HEIGHT - 1);

  // Clear display and draw new dot position
  display.clearDisplay();
  display.fillCircle(dotX, dotY, 3, SSD1306_WHITE); // Draw a circle as the dot
  display.display();

  delay(50); // Delay for smooth movement
}