#include <Arduino.h>

const int MOTOR_COUNT = 3; // ใช้ 3 มอเตอร์

const int EN[MOTOR_COUNT] = {13, 10, 7};  // ขาเปิดใช้งานมอเตอร์
const int STEP[MOTOR_COUNT] = {12, 9, 6}; // ขาขั้นตอน
const int DIR[MOTOR_COUNT] = {11, 8, 5};  // ขาทิศทาง

const int X_button = 52;
const int Xbutton = 53;
const int Y_button = 50;
const int Ybutton = 51;
const int Z_button = 48;
const int Zbutton = 49;

const int mode_10deg = 46;
const int mode_100deg = 47;

const int stepsPerRev = 400; // จำนวนขั้นตอนต่อรอบของมอเตอร์
int stepSize = 10; // ขนาดของ step เริ่มต้น
bool buttonPressed[6] = {false, false, false, false, false, false};

void setup() {
  Serial.begin(115200);
  Serial.println("Starting...");

  for (int i = 0; i < MOTOR_COUNT; i++) {
    pinMode(EN[i], OUTPUT);
    pinMode(STEP[i], OUTPUT);
    pinMode(DIR[i], OUTPUT);
    digitalWrite(EN[i], LOW);
  }

  pinMode(Xbutton, INPUT_PULLUP);
  pinMode(X_button, INPUT_PULLUP);
  pinMode(Ybutton, INPUT_PULLUP);
  pinMode(Y_button, INPUT_PULLUP);
  pinMode(Zbutton, INPUT_PULLUP);
  pinMode(Z_button, INPUT_PULLUP);
  pinMode(mode_10deg, INPUT_PULLUP);
  pinMode(mode_100deg, INPUT_PULLUP);

  selectMode();
}

void selectMode() {
  Serial.println("Select mode: Press 10 deg or 100 deg");
  while (true) {
    if (digitalRead(mode_10deg) == LOW) {
      stepSize = 10;
      Serial.println("Mode: 10 degrees selected");
      delay(500);
      break;
    }
    if (digitalRead(mode_100deg) == LOW) {
      stepSize = 100;
      Serial.println("Mode: 100 degrees selected");
      delay(500);
      break;
    }
  }
}

void loop() {
  checkButtonPress();
}

void checkButtonPress() {
  if (digitalRead(X_button) == LOW && !buttonPressed[0]) { moveMotor(0, 1); buttonPressed[0] = true; }
  if (digitalRead(Xbutton) == LOW && !buttonPressed[1]) { moveMotor(0, -1); buttonPressed[1] = true; }
  if (digitalRead(Y_button) == LOW && !buttonPressed[2]) { moveMotor(1, 1); buttonPressed[2] = true; }
  if (digitalRead(Ybutton) == LOW && !buttonPressed[3]) { moveMotor(1, -1); buttonPressed[3] = true; }
  if (digitalRead(Z_button) == LOW && !buttonPressed[4]) { moveMotor(2, 1); buttonPressed[4] = true; }
  if (digitalRead(Zbutton) == LOW && !buttonPressed[5]) { moveMotor(2, -1); buttonPressed[5] = true; }

  for (int i = 0; i < 6; i++) {
    if (digitalRead(X_button + i) == HIGH) {
      buttonPressed[i] = false;
    }
  }
}

void moveMotor(int motor, int direction) {
  digitalWrite(DIR[motor], (direction == 1) ? HIGH : LOW);
  int stepsToMove = (stepSize * stepsPerRev) / 360; // คำนวณจำนวนสเต็ปจากองศา
  Serial.print("Motor "); Serial.print(motor); Serial.print(" moving ");
  Serial.print(stepSize); Serial.println(" degrees");

  for (int i = 0; i < stepsToMove; i++) {
    digitalWrite(STEP[motor], HIGH);
    delayMicroseconds(500);
    digitalWrite(STEP[motor], LOW);
    delayMicroseconds(500);
  }
}