#include <AccelStepper.h>
#include <Wire.h>
#include <U8g2lib.h>

// Cấu hình màn hình OLED U8g2
U8G2_SSD1306_128X64_NONAME_F_HW_I2C u8g2(U8G2_R0, U8X8_PIN_NONE);

// Cấu hình động cơ bước
#define EN_PIN 8
#define X_DIR_PIN 5
#define X_STEP_PIN 2
#define Y_DIR_PIN 6
#define Y_STEP_PIN 3

AccelStepper stepperX(AccelStepper::DRIVER, X_STEP_PIN, X_DIR_PIN);
AccelStepper stepperY(AccelStepper::DRIVER, Y_STEP_PIN, Y_DIR_PIN);

// Cấu hình rotary encoder
#define ENCODER_CLK 9
#define ENCODER_DT 10
#define ENCODER_SW 11

int menuIndex = 0;       // Vị trí menu đang chọn
int m1_rpm = 20;         // Tốc độ động cơ 1 (RPM)
int m2_rpm = 40;         // Tốc độ động cơ 2 (RPM)
bool editing = false;    // Đang chỉnh sửa giá trị hay không
bool startFlag = false;  // Cờ kiểm tra khi nhấn "Start"
unsigned long lastBlink = 0;
bool cursorVisible = true;

// Tính số bước cho mỗi vòng
const int stepsPerRevolution = 200;
const int microstep = 1;
const int stepsPerFullRev = stepsPerRevolution * microstep;

void setup() {
    pinMode(EN_PIN, OUTPUT);
    digitalWrite(EN_PIN, LOW);

    stepperX.setMaxSpeed(500000);
    stepperX.setAcceleration(1000);
    stepperY.setMaxSpeed(2000000);
    stepperY.setAcceleration(1000);

    pinMode(ENCODER_CLK, INPUT);
    pinMode(ENCODER_DT, INPUT);
    pinMode(ENCODER_SW, INPUT_PULLUP);

    u8g2.begin();
    updateMenu();
}

void loop() {
    readEncoder();
    blinkCursor();  // Nháy con trỏ khi chỉnh sửa

    // Kiểm tra nếu startFlag = true thì chạy động cơ
    if (startFlag) {
        runMotors();
        startFlag = false;
    }
}

// Đọc tín hiệu từ rotary encoder
void readEncoder() {
    static int lastState = digitalRead(ENCODER_CLK);
    int newState = digitalRead(ENCODER_CLK);

    if (digitalRead(ENCODER_SW) == LOW) {
        delay(200);
        if (menuIndex == 2) { 
            startFlag = true; // Khi nhấn Start thì đặt cờ chạy động cơ
        } else {
            editing = !editing; // Chế độ chỉnh sửa
        }
        updateMenu();
    }

    if (editing) {
        if (newState != lastState && newState == LOW) {
            if (digitalRead(ENCODER_DT) != newState) {
                if (menuIndex == 0) m1_rpm++;
                if (menuIndex == 1) m2_rpm++;
            } else {
                if (menuIndex == 0 && m1_rpm > 1) m1_rpm--;
                if (menuIndex == 1 && m2_rpm > 1) m2_rpm--;
            }
            updateMenu();
        }
    } else {
        if (newState != lastState && newState == LOW) {
            if (digitalRead(ENCODER_DT) != newState) {
                menuIndex++;
            } else {
                menuIndex--;
            }
            if (menuIndex > 2) menuIndex = 0;
            if (menuIndex < 0) menuIndex = 2;
            updateMenu();
        }
    }
    lastState = newState;
}

// Nháy con trỏ khi đang chỉnh sửa
void blinkCursor() {
    if (editing) {
        if (millis() - lastBlink > 500) {
            cursorVisible = !cursorVisible;
            updateMenu();
            lastBlink = millis();
        }
    }
}

// Cập nhật menu trên màn hình OLED
void updateMenu() {
    u8g2.clearBuffer();
    u8g2.setFont(u8g2_font_10x20_tf); // Font chữ lớn hơn

    u8g2.setCursor(10, 20);
    u8g2.print("M1: "); u8g2.print(m1_rpm); u8g2.print(" RPM");
    if (menuIndex == 0) {
        if (!editing || cursorVisible) u8g2.print(" <");
    }

    u8g2.setCursor(10, 40);
    u8g2.print("M2: "); u8g2.print(m2_rpm); u8g2.print(" RPM");
    if (menuIndex == 1) {
        if (!editing || cursorVisible) u8g2.print(" <");
    }

    u8g2.setCursor(10, 60);
    u8g2.print("Start");
    if (menuIndex == 2) u8g2.print(" <");

    u8g2.sendBuffer();
}

// Chạy động cơ theo số vòng/phút đã cài đặt
void runMotors() {
    float m1_speed = (m1_rpm * stepsPerFullRev) / 60.0;
    float m2_speed = (m2_rpm * stepsPerFullRev) / 60.0;
    stepperX.setMaxSpeed(m1_speed);
    stepperY.setMaxSpeed(m2_speed);

    stepperX.move(10*stepsPerFullRev);
    stepperY.move(10*stepsPerFullRev); 

    while (stepperX.distanceToGo() != 0 || stepperY.distanceToGo() != 0) {
        stepperX.run();
        stepperY.run();
    }
}