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

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

// Accelerometer
MPU6050 mpu;

// Cursor states
float cursorX = SCREEN_WIDTH / 2;
float cursorY = SCREEN_HEIGHT / 2;
float shadowX = SCREEN_WIDTH / 2;
float shadowY = SCREEN_HEIGHT / 2;

float sensitivity = 2.0;        // pixels per tilt
float springFactor = 0.9;       // main cursor spring
float shadowSpringFactor = 0.9; // shadow cursor spring

// Center
const float centerX = SCREEN_WIDTH / 2;
const float centerY = SCREEN_HEIGHT / 2;

// Shadow radius
const int shadowRadius = 12;

// Floating line config (for N,S,E,W)
const int lineOffset = 18;  // distance from shadow center
const int lineLength = 6;   // half-length of the floating bars

// Rotating ring state
float angle1 = 0;   // clockwise
float angle2 = 180; // counterclockwise
const int ringRadius1 = shadowRadius + 2;
const int ringRadius2 = shadowRadius + 4;

const int bracketSize = 45; // length of each bracket arc

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

  // Init OLED
  if (!display.begin(SSD1306_SWITCHCAPVCC, 0x3C)) {
    Serial.println(F("SSD1306 allocation failed"));
    for (;;);
  }
  display.clearDisplay();
  display.display();

  // Init MPU6050
  mpu.initialize();
  if (!mpu.testConnection()) {
    Serial.println("MPU6050 not connected!");
    while (1);
  }
}

void drawBracket(float cx, float cy, float radius, float angleDeg, int size) {
  // Draws a small arc (line segment) at given angle
  float rad1 = radians(angleDeg);
  float rad2 = radians(angleDeg + size);

  int x1 = cx + cos(rad1) * radius;
  int y1 = cy + sin(rad1) * radius;
  int x2 = cx + cos(rad2) * radius;
  int y2 = cy + sin(rad2) * radius;

  display.drawLine(x1, y1, x2, y2, SSD1306_WHITE);
}

void loop() {
  int16_t ax, ay, az;
  mpu.getAcceleration(&ax, &ay, &az);

  // Normalize (-1 to 1 approx)
  float axf = ax / 16384.0;
  float ayf = ay / 16384.0;

  // --- Main cursor (moves with tilt) ---
  cursorX += axf * sensitivity;
  cursorY += ayf * sensitivity;
  cursorX = (cursorX - centerX) * springFactor + centerX;
  cursorY = (cursorY - centerY) * springFactor + centerY;

  // --- Shadow cursor (mirrored) ---
  shadowX -= axf * sensitivity; // opposite direction
  shadowY -= ayf * sensitivity;
  shadowX = (shadowX - centerX) * shadowSpringFactor + centerX;
  shadowY = (shadowY - centerY) * shadowSpringFactor + centerY;

  // Clamp both
  cursorX = constrain(cursorX, 0, SCREEN_WIDTH - 1);
  cursorY = constrain(cursorY, 0, SCREEN_HEIGHT - 1);
  shadowX = constrain(shadowX, 0, SCREEN_WIDTH - 1);
  shadowY = constrain(shadowY, 0, SCREEN_HEIGHT - 1);

  // --- Rotate ring angles ---
  angle1 += 4; // clockwise
  angle2 -= 3; // counterclockwise
  if (angle1 >= 360) angle1 -= 360;
  if (angle2 < 0) angle2 += 360;

  // --- Draw ---
  display.clearDisplay();

  // Shadow cursor: big circle
  display.drawCircle(shadowX, shadowY, shadowRadius, SSD1306_WHITE);

  // Floating N,S,E,W bars
  display.drawLine(shadowX - lineLength, shadowY - lineOffset,
                   shadowX + lineLength, shadowY - lineOffset, SSD1306_WHITE); // N
  display.drawLine(shadowX - lineLength, shadowY + lineOffset,
                   shadowX + lineLength, shadowY + lineOffset, SSD1306_WHITE); // S
  display.drawLine(shadowX - lineOffset, shadowY - lineLength,
                   shadowX - lineOffset, shadowY + lineLength, SSD1306_WHITE); // W
  display.drawLine(shadowX + lineOffset, shadowY - lineLength,
                   shadowX + lineOffset, shadowY + lineLength, SSD1306_WHITE); // E

  // Connected diagonals (NE, NW, SE, SW)
  display.drawLine(shadowX, shadowY, shadowX + shadowRadius, shadowY - shadowRadius, SSD1306_WHITE); // NE
  display.drawLine(shadowX, shadowY, shadowX - shadowRadius, shadowY - shadowRadius, SSD1306_WHITE); // NW
  display.drawLine(shadowX, shadowY, shadowX + shadowRadius, shadowY + shadowRadius, SSD1306_WHITE); // SE
  display.drawLine(shadowX, shadowY, shadowX - shadowRadius, shadowY + shadowRadius, SSD1306_WHITE); // SW

  // --- Rotating Brackets ---
  for (int i = 0; i < 360; i += 90) {
    drawBracket(shadowX, shadowY, ringRadius1, angle1 + i, bracketSize);
    drawBracket(shadowX, shadowY, ringRadius2, angle2 + i, bracketSize);
  }

  // Main cursor: small filled dot
  display.fillCircle(cursorX, cursorY, 2, SSD1306_WHITE);

  // Center marker (small square)
  display.drawRect(centerX - 2, centerY - 2, 4, 4, SSD1306_WHITE);

  display.display();

  delay(30); // ~33 FPS
}