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

#define SCREEN_WIDTH 128
#define SCREEN_HEIGHT 64
#define OLED_RESET    -1  // 預設使用 -1 表示無 Reset 腳位
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);

Servo myServo;

const int servoPin = 9;
const int buttonPin = 2;
bool toggleState = false;
bool lastButtonState = HIGH;

void setup() {
  // 初始化 OLED
  if (!display.begin(SSD1306_SWITCHCAPVCC, 0x3C)) {
    Serial.println(F("OLED Initialization failed"));
    while (true);
  }

  display.clearDisplay();
  display.setTextSize(1);
  display.setTextColor(SSD1306_WHITE);
  display.setCursor(0, 0);
  display.println("Servo Control");
  display.display();

  // 初始化 Servo 與按鈕
  myServo.attach(servoPin);
  pinMode(buttonPin, INPUT_PULLUP);
  myServo.write(0); // 初始位置
  delay(1000);
}

void loop() {
  bool currentButtonState = digitalRead(buttonPin);

  if (lastButtonState == HIGH && currentButtonState == LOW) {
    toggleState = !toggleState;

    if (toggleState) {
      myServo.write(90);
      showStatus("open", 90);
    } else {
      myServo.write(0);
      showStatus("close", 0);
    }

    delay(300); // 防彈跳
  }

  lastButtonState = currentButtonState;
}

void showStatus(String status, int angle) {
  display.clearDisplay();
  display.setCursor(0, 0);
  display.setTextSize(1);
  display.println("Demonstration");
  display.setCursor(0, 20);
  display.print("state: ");
  display.println(status);
  display.setCursor(0, 35);
  display.print("angle: ");
  display.print(angle);
  display.println(" degree");
  display.display();
}