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

#define SCREEN_WIDTH 128
#define SCREEN_HEIGHT 64
#define OLED_RESET -1

Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);

void setup() {
  // Inisialisasi I2C
  Wire.begin(22, 21);  // SDA = pin 22, SCL = pin 21

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

  // Hapus konten layar
  display.clearDisplay();

  // Tampilkan kotak awal di tengah layar
  drawRotatedBox(SCREEN_WIDTH / 2, SCREEN_HEIGHT / 2, 40, 20, 0, WHITE);
}

void loop() {
  static float angle = 0; // Sudut rotasi
  float angularVelocity = 0.05; // Kecepatan rotasi
  
  // Hapus kotak sebelumnya
  display.clearDisplay();

  // Hitung posisi pusat kotak
  int centerX = SCREEN_WIDTH / 2;
  int centerY = SCREEN_HEIGHT / 2;

  // Hitung sudut rotasi
  angle += angularVelocity;

  // Gambar kotak yang berputar
  drawRotatedBox(centerX, centerY, 40, 20, angle, WHITE);

  // Tampilkan di layar
  display.display();

  // Delay untuk efek animasi
  delay(10);
}

// Fungsi untuk menggambar kotak yang berputar
void drawRotatedBox(int centerX, int centerY, int width, int height, float angle, uint16_t color) {
  // Hitung titik-titik sudut kotak yang berputar
  float angleRad = angle * PI / 180;
  int x1 = centerX + width / 2 * cos(angleRad) - height / 2 * sin(angleRad);
  int y1 = centerY + width / 2 * sin(angleRad) + height / 2 * cos(angleRad);
  int x2 = centerX - width / 2 * cos(angleRad) - height / 2 * sin(angleRad);
  int y2 = centerY - width / 2 * sin(angleRad) + height / 2 * cos(angleRad);
  int x3 = centerX - width / 2 * cos(angleRad) + height / 2 * sin(angleRad);
  int y3 = centerY - width / 2 * sin(angleRad) - height / 2 * cos(angleRad);
  int x4 = centerX + width / 2 * cos(angleRad) + height / 2 * sin(angleRad);
  int y4 = centerY + width / 2 * sin(angleRad) - height / 2 * cos(angleRad);

  // Gambar kotak yang berputar
  display.drawLine(x1, y1, x2, y2, color);
  display.drawLine(x2, y2, x3, y3, color);
  display.drawLine(x3, y3, x4, y4, color);
  display.drawLine(x4, y4, x1, y1, color);
}