#include <Wire.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>
#include <Stepper.h>
#include <ESP32Servo.h>

#define SCREEN_WIDTH 128
#define SCREEN_HEIGHT 64
#define SCREEN_ADDRESS 0x3C
#define BUTTON_PIN_1 2
#define BUTTON_PIN_2 3

// OLED setup
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, -1);

// Stepper motor setup
const int stepsPerRevolution = 200;  // Sesuaikan dengan spesifikasi motor stepper
Stepper stepper(stepsPerRevolution, 14, 27, 26, 25); // Pins: EN, Step, Dir, MS1

// Servo motor setup
Servo servoMotor;
int servoPosition = 90;  // Posisi awal servo

int currentMenu = 0; // 0: Main menu, 1: Stepper Speed, 2: Servo Position

void displayMainMenu() {
  display.clearDisplay();
  display.setTextSize(1);
  display.setTextColor(SSD1306_WHITE);
  display.setCursor(0, 0);
  display.println(F("1. Stepper Speed"));
  display.println(F("2. Servo Position"));
  display.display();
}

void setup() {
  Serial.begin(115200);
  pinMode(BUTTON_PIN_1, INPUT_PULLUP);
  pinMode(BUTTON_PIN_2, INPUT_PULLUP);
  
  #define OLED_RESET    -1
  #define SSD1306_I2C_ADDRESS 0x3C
  if (!display.begin(SSD1306_I2C_ADDRESS, OLED_RESET)) {
    Serial.println(F("SSD1306 allocation failed"));
    for(;;);
  }

  display.display();
  delay(2000);
  display.clearDisplay();

  stepper.setSpeed(500);  // Set kecepatan awal motor stepper
  servoMotor.attach(13);  // Hubungkan servo ke pin 13

  display.println(F("Press button to control motors"));
  display.display();
}

void updateStepperSpeed() {
  int speed = analogRead(A0);  // Baca nilai dari potensiometer sebagai input kecepatan
  stepper.setSpeed(map(speed, 0, 1023, 0, 1000));
  display.clearDisplay();
  display.println(F("Stepper Speed Set"));
  display.display();
  delay(1000);
}

void updateServoPosition() {
  servoPosition = map(analogRead(A0), 0, 1023, 0, 180);
  servoMotor.write(servoPosition);
  display.clearDisplay();
  display.println(F("Servo Position Set"));
  display.display();
  delay(1000);
}

void loop() {
  if(digitalRead(BUTTON_PIN_1) == LOW) {
    // Tampilkan menu pengaturan pada OLED
    if (currentMenu == 0) {
      displayMainMenu();
      currentMenu = 1;
      while(digitalRead(BUTTON_PIN_1) == LOW);
      return;
    }

    // Ubah nilai sesuai dengan pemilihan menu
    if(currentMenu == 1) {
      updateStepperSpeed();
      currentMenu = 0;
    } else if(currentMenu == 2) {
      updateServoPosition();
      currentMenu = 0;
    }
  }

  if(digitalRead(BUTTON_PIN_2) == LOW) {
    // Pindah antar menu
    if(currentMenu == 0) {
      currentMenu = 1;
      displayMainMenu();
      while(digitalRead(BUTTON_PIN_2) == LOW);
    } else {
      currentMenu = 2;
      displayMainMenu();
      while(digitalRead(BUTTON_PIN_2) == LOW);
    }
  }
}