#define STEP_X 2
#define DIR_X 5
#define STEP_Y 3
#define DIR_Y 6
#define STEP_Z 4
#define DIR_Z 7
#define LIMIT_X 8
#define LIMIT_Y 9
#define LIMIT_Z 10
#define STOP_BTN 11
#define BTN_1MM 12
#define BTN_10MM 13
#define BTN_X_PLUS 14
#define BTN_X_MINUS 15
#define BTN_Y_PLUS 16
#define BTN_Y_MINUS 17
#define BTN_Z_PLUS 18
#define BTN_Z_MINUS 19

int stepSize = 3000; // Default step size (300 steps)
bool stopFlag = false;

void setup() {
    Serial.begin(115200);
    Serial.println("Setup complete. Waiting for commands...");
    pinMode(STEP_X, OUTPUT);
    pinMode(DIR_X, OUTPUT);
    pinMode(STEP_Y, OUTPUT);
    pinMode(DIR_Y, OUTPUT);
    pinMode(STEP_Z, OUTPUT);
    pinMode(DIR_Z, OUTPUT);
    pinMode(LIMIT_X, INPUT_PULLUP);
    pinMode(LIMIT_Y, INPUT_PULLUP);
    pinMode(LIMIT_Z, INPUT_PULLUP);
    pinMode(STOP_BTN, INPUT_PULLUP);
    pinMode(BTN_1MM, INPUT_PULLUP);
    pinMode(BTN_10MM, INPUT_PULLUP);
    pinMode(BTN_X_PLUS, INPUT_PULLUP);
    pinMode(BTN_X_MINUS, INPUT_PULLUP);
    pinMode(BTN_Y_PLUS, INPUT_PULLUP);
    pinMode(BTN_Y_MINUS, INPUT_PULLUP);
    pinMode(BTN_Z_PLUS, INPUT_PULLUP);
    pinMode(BTN_Z_MINUS, INPUT_PULLUP);
}

void loop() {
    if (Serial.available() > 0) {
        String command = Serial.readStringUntil('\n');
        command.trim();
        Serial.print("Received command: ");
        Serial.println(command);
        if (command == "G28") {
            homeAllAxes();
        }
    }

    if (digitalRead(BTN_1MM) == LOW) stepSize = 46;  // 10 degrees
    if (digitalRead(BTN_10MM) == LOW) stepSize = 892; // 100 degrees

    if (digitalRead(STOP_BTN) == LOW) {
        stopFlag = true;
        Serial.println("STOP button pressed. Stopping all motors.");
        return;
    } else {
        stopFlag = false;
    }

    if (!stopFlag) {
        if (digitalRead(BTN_X_PLUS) == LOW) moveMotor(STEP_X, DIR_X, LOW, stepSize, LIMIT_X);
        if (digitalRead(BTN_X_MINUS) == LOW) moveMotor(STEP_X, DIR_X, HIGH, stepSize, LIMIT_X);
        if (digitalRead(BTN_Y_PLUS) == LOW) moveMotor(STEP_Y, DIR_Y, LOW, stepSize, LIMIT_Y);
        if (digitalRead(BTN_Y_MINUS) == LOW) moveMotor(STEP_Y, DIR_Y, HIGH, stepSize, LIMIT_Y);
        if (digitalRead(BTN_Z_PLUS) == LOW) moveMotor(STEP_Z, DIR_Z, LOW, stepSize, LIMIT_Z);
        if (digitalRead(BTN_Z_MINUS) == LOW) moveMotor(STEP_Z, DIR_Z, HIGH, stepSize, LIMIT_Z);
    }
}

void moveMotor(int stepPin, int dirPin, int direction, int steps, int limitSwitch) {
    Serial.print("Moving motor: ");
    Serial.println(stepPin);
    digitalWrite(dirPin, direction);
    for (int i = 0; i < steps; i++) {
        if (digitalRead(limitSwitch) == LOW || digitalRead(STOP_BTN) == LOW) {
            stopFlag = true;
            Serial.println("Limit switch triggered or STOP button pressed. Stopping.");
            return;
        }
        digitalWrite(stepPin, HIGH);
        delayMicroseconds(500);
        digitalWrite(stepPin, LOW);
        delayMicroseconds(500);
    }
    Serial.println("Motor movement complete.");
}

void homeAllAxes() {
    Serial.println("Homing X axis...");
    while (digitalRead(LIMIT_X) != LOW) {
        moveMotor(STEP_X, DIR_X, LOW, 1, LIMIT_X);
    }
    Serial.println("X axis homed.");

    Serial.println("Homing Y axis...");
    while (digitalRead(LIMIT_Y) != LOW) {
        moveMotor(STEP_Y, DIR_Y, LOW, 1, LIMIT_Y);
    }
    Serial.println("Y axis homed.");

    Serial.println("Homing Z axis...");
    while (digitalRead(LIMIT_Z) != LOW) {
        moveMotor(STEP_Z, DIR_Z, LOW, 1, LIMIT_Z);
    }
    Serial.println("Z axis homed.");
}