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

constexpr int SCREEN_WIDTH = 128;
constexpr int SCREEN_HEIGHT = 64;
constexpr int OLED_RESET = -1;
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);

Servo myServo;

constexpr int SERVO_PIN = 9;
constexpr int START_ANGLE = 0;
constexpr int END_ANGLE = 180;
constexpr int WAIT_TIME = 1000;     // 1초 대기
constexpr int MOVE_DURATION = 6000; // 이동 시간 (6초)

constexpr int CENTER_X = 64;   // 축 중심 X
constexpr int CENTER_Y = 48;   // 축 중심 Y
constexpr int ARM_LENGTH = 30; // 암 길이
constexpr int ARM_WIDTH = 8;   // 암 밑변 길이
constexpr int BODY_WIDTH = 30;
constexpr int BODY_HEIGHT = 12;
constexpr int BODY_OFFSET_Y = 10;
constexpr int SHAFT_RADIUS = 5;

int currentAngle = START_ANGLE;
unsigned long lastUpdate = 0;
unsigned long stateStartTime = 0;
constexpr int steps = END_ANGLE - START_ANGLE;
constexpr int delayPerStep = MOVE_DURATION / steps;

// 상태머신
enum State {
    WAIT_AT_0,
    MOVE_TO_180,
    WAIT_AT_180,
    MOVE_TO_0
};
State state = WAIT_AT_0;

void setup() {
    myServo.attach(SERVO_PIN);
    myServo.write(currentAngle);
    display.begin(SSD1306_SWITCHCAPVCC, 0x3C);
    display.clearDisplay();
    display.display();
    drawServo(currentAngle);
    stateStartTime = millis();
}

void drawMotorBody() {
    display.fillRect(CENTER_X - BODY_WIDTH / 2, CENTER_Y + BODY_OFFSET_Y, BODY_WIDTH, BODY_HEIGHT, SSD1306_WHITE);
}

void drawShaft() {
    display.fillCircle(CENTER_X, CENTER_Y, SHAFT_RADIUS, SSD1306_WHITE);
}

void drawArm(int angle) {
    float rad = radians(angle + 180); // 0도는 왼쪽(180도) 방향에서 시작
    float width = ARM_WIDTH;
    float height = ARM_LENGTH;
    int tipX = CENTER_X + height * cos(rad);
    int tipY = CENTER_Y + height * sin(rad);
    float sideRad1 = rad + radians(90);
    float sideRad2 = rad - radians(90);
    int baseX1 = CENTER_X + (width / 2) * cos(sideRad1);
    int baseY1 = CENTER_Y + (width / 2) * sin(sideRad1);
    int baseX2 = CENTER_X + (width / 2) * cos(sideRad2);
    int baseY2 = CENTER_Y + (width / 2) * sin(sideRad2);
    display.fillTriangle(tipX, tipY, baseX1, baseY1, baseX2, baseY2, SSD1306_WHITE);
}

void drawAngleText(int angle) {
    display.setTextSize(1);
    display.setTextColor(SSD1306_WHITE);
    display.setCursor(0, 0);
    display.print("Angle: ");
    display.print(angle);
    display.print(" deg");
}

void drawServo(int angle) {
    display.clearDisplay();
    drawMotorBody();
    drawShaft();
    drawArm(angle);
    drawAngleText(angle);
    display.display();
}

void handleWaitAt0(unsigned long now) {
    if (now - stateStartTime >= WAIT_TIME) {
        state = MOVE_TO_180;
        lastUpdate = now;
    }
}

void handleMoveTo180(unsigned long now) {
    if (now - lastUpdate >= delayPerStep) {
        lastUpdate = now;
        if (currentAngle < END_ANGLE) {
            currentAngle++;
            myServo.write(currentAngle);
            drawServo(currentAngle);
        } else {
            state = WAIT_AT_180;
            stateStartTime = now;
        }
    }
}

void handleWaitAt180(unsigned long now) {
    if (now - stateStartTime >= WAIT_TIME) {
        state = MOVE_TO_0;
        lastUpdate = now;
    }
}

void handleMoveTo0(unsigned long now) {
    if (now - lastUpdate >= delayPerStep) {
        lastUpdate = now;
        if (currentAngle > START_ANGLE) {
            currentAngle--;
            myServo.write(currentAngle);
            drawServo(currentAngle);
        } else {
            state = WAIT_AT_0;
            stateStartTime = now;
        }
    }
}

void loop() {
    unsigned long now = millis();
    switch (state) {
        case WAIT_AT_0:
            handleWaitAt0(now);
            break;
        case MOVE_TO_180:
            handleMoveTo180(now);
            break;
        case WAIT_AT_180:
            handleWaitAt180(now);
            break;
        case MOVE_TO_0:
            handleMoveTo0(now);
            break;
    }
}