#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
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);

Servo myServo;

const int servoPin = 9;
const int buttonPin = 2;

// 段數與角度陣列
int angleSteps[] = {0, 45, 90, 135, 180};
const char* sectionLabels[] = {"1", "2", "3", "4", "5"};
const int numSteps = sizeof(angleSteps) / sizeof(angleSteps[0]);
int currentStep = 0;

bool lastButtonState = HIGH;

void setup() {
  // 初始化 OLED
  if (!display.begin(SSD1306_SWITCHCAPVCC, 0x3C)) {
    while (true); // OLED 初始化失敗時卡住
  }

  // 初始畫面
  display.clearDisplay();
  display.setTextSize(1);
  display.setTextColor(SSD1306_WHITE);
  display.setCursor(15, 20);
  display.println("Servo Motor");
  display.display();
  delay(3000); // 顯示3秒
  display.clearDisplay();
  display.display();

  // 初始化 Servo 與按鈕
  pinMode(buttonPin, INPUT_PULLUP);
  myServo.attach(servoPin);
  myServo.write(angleSteps[currentStep]);
  showAngle(currentStep);
}

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

  if (lastButtonState == HIGH && buttonState == LOW) 
  {
    currentStep = (currentStep + 1) % numSteps;
    int angle = angleSteps[currentStep];
    myServo.write(angle);
    showAngle(currentStep);
    delay(300); // 防彈跳
  }

  lastButtonState = buttonState;
}

// 顯示目前段數與角度
void showAngle(int stepIndex) 
{
  display.clearDisplay();
  display.setTextSize(1);
  display.setCursor(0, 10);
  display.print(sectionLabels[stepIndex]);
  display.print(": ");
  display.print(angleSteps[stepIndex]);
  display.println(" dgree");
  display.display();
}