#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);

// LED pins (tránh chân TX/RX)
#define LED_RED    4
#define LED_GREEN  5
#define LED_BLUE   3
#define LED_YELLOW 2

void scanI2C() {
  byte error, address;
  int nDevices = 0;
  Serial.println("Quet I2C de tim dia chi thiet bi...");
  for (address = 1; address < 127; address++) {
    Wire.beginTransmission(address);
    error = Wire.endTransmission();
    if (error == 0) {
      Serial.print("Thiet bi tim thay tai dia chi 0x");
      if (address < 16) Serial.print("0");
      Serial.println(address, HEX);
      nDevices++;
    } else if (error == 4) {
      Serial.print("Loi tai dia chi 0x");
      if (address < 16) Serial.print("0");
      Serial.println(address, HEX);
    }
  }
  if (nDevices == 0) Serial.println("Khong tim thay thiet bi I2C!");
  else Serial.println("Quet I2C hoan tat.");
}

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

  // Khởi tạo I2C (dùng chân mặc định của ESP32 hoặc thay đổi nếu cần)
  Wire.begin(D9,D10); // ESP32: SDA = 21, SCL = 22 (mặc định)
  // Wire.begin(10, 9); // Nếu dùng chân tùy chỉnh
  Serial.println("I2C da duoc khoi tao");
  scanI2C();

  // Khởi tạo LED
  pinMode(LED_RED, OUTPUT);
  pinMode(LED_GREEN, OUTPUT);
  pinMode(LED_BLUE, OUTPUT);
  pinMode(LED_YELLOW, OUTPUT);

  // Khởi tạo màn hình OLED
  if (!display.begin(SSD1306_SWITCHCAPVCC, 0x3C)) {
    Serial.println("Không thể khởi tạo màn hình OLED. Vui lòng kiểm tra kết nối và địa chỉ I2C.");
    while (1); // Treo chương trình
  }

  display.clearDisplay();

  // Vẽ nền trắng
  display.fillRect(0, 0, SCREEN_WIDTH, SCREEN_HEIGHT, SSD1306_WHITE);

  // Vẽ ngôi sao ở giữa
  drawFilledStar(SCREEN_WIDTH / 2, SCREEN_HEIGHT / 2, 20, 10, SSD1306_BLACK);

  display.display();
}

void loop() {
  digitalWrite(LED_RED, HIGH);
  delay(200);
  digitalWrite(LED_RED, LOW);

  digitalWrite(LED_GREEN, HIGH);
  delay(200);
  digitalWrite(LED_GREEN, LOW);

  digitalWrite(LED_BLUE, HIGH);
  delay(200);
  digitalWrite(LED_BLUE, LOW);

  digitalWrite(LED_YELLOW, HIGH);
  delay(200);
  digitalWrite(LED_YELLOW, LOW);
}

void drawFilledStar(int x0, int y0, int outerRadius, int innerRadius, uint16_t color) {
  float angle = -PI / 2;
  float step = PI / 5;

  int x[10], y[10];

  for (int i = 0; i < 10; i++) {
    int r = (i % 2 == 0) ? outerRadius : innerRadius;
    x[i] = x0 + cos(angle) * r;
    y[i] = y0 + sin(angle) * r;
    angle += step;
  }

  for (int i = 0; i < 10; i++) {
    int next = (i + 1) % 10;
    display.fillTriangle(x0, y0, x[i], y[i], x[next], y[next], color);
  }
}