const int stepPin1 = 4;
const int stepPin2 = 5;
const int stepPin3 = 6;
const int stepPin4 = 7;
const int dirPin = 3;
const int enablePin = 2;
const int buttonPin = 53;  // Pin pro tlačítko spojující s GND při stisku
const int buttonPin2 = 13; // Další pin pro tlačítko, případně pro jinou funkci
const int stepsPerRevolution = 200;  // Předpokládaný počet kroků na otáčku

bool STOPKA;
int currentMotor = 1;  // Proměnná pro sledování aktuálně ovládaného motoru
int lastValue[4] = {0, 0, 0, 0};  // Hodnoty pro každý motor

void setup() {
  pinMode(stepPin1, OUTPUT);
  pinMode(stepPin2, OUTPUT);
  pinMode(stepPin3, OUTPUT);
  pinMode(stepPin4, OUTPUT);
  pinMode(dirPin, OUTPUT);
  pinMode(enablePin, OUTPUT);
  pinMode(buttonPin, INPUT_PULLUP);  // Nastavení pinu tlačítka s interním pull-up rezistorem
  pinMode(buttonPin2, INPUT_PULLUP);  // Stejně tak pro druhé tlačítko
  digitalWrite(enablePin, HIGH);  // Motor zastaven
  Serial.begin(9600);
  STOPKA = true;
}

void loop() {
  if (STOPKA) {
    handleStopkaMode();  // Zpracování režimu STOPKA
  } else {
    handleNormalMode();  // Běžný režim ovládání motorů
  }
}

void handleStopkaMode() {
  int motorPins[4] = {stepPin1, stepPin2, stepPin3, stepPin4};
  for (int i = 0; i < 4; i++) {
    rotateMotorUntilButton(motorPins[i]);
    retractMotor(motorPins[i], 150);
  }
  STOPKA = false;  // Po dokončení inicializace všech motorů vypneme STOPKA režim
}

void rotateMotorUntilButton(int motorPin) {
  const unsigned long debounceDelay = 50;  // Debounce čas v milisekundách
  unsigned long lastDebounceTime = 0;
  int lastButtonState = HIGH;
  int buttonState = HIGH;

  digitalWrite(enablePin, LOW);  // Aktivace motoru
  digitalWrite(dirPin, HIGH);  // Nastavení směru rotace

  while (true) {
    int reading = digitalRead(buttonPin);

    if (reading != lastButtonState) {
      lastDebounceTime = millis();
    }

    if ((millis() - lastDebounceTime) > debounceDelay) {
      if (reading != buttonState) {
        buttonState = reading;
        if (buttonState == LOW) {
          break;  // Tlačítko bylo stisknuto
        }
      }
    }
    lastButtonState = reading;

    digitalWrite(motorPin, HIGH);
    delayMicroseconds(1000);
    digitalWrite(motorPin, LOW);
    delayMicroseconds(1000);
  }

  digitalWrite(enablePin, HIGH);  // Deaktivace motoru po dokončení
}

void retractMotor(int motorPin, int steps) {
  digitalWrite(dirPin, LOW);  // Obrácení směru
  for (int i = 0; i < steps; i++) {
    digitalWrite(motorPin, HIGH);
    delayMicroseconds(1000);
    digitalWrite(motorPin, LOW);
    delayMicroseconds(1000);
  }
}

void handleNormalMode() {
  if (digitalRead(buttonPin2) == LOW) {  // Přepínání mezi motory
    int previousMotor = currentMotor;
    currentMotor = (currentMotor % 4) + 1;  // Cyklické přepínání 1-4
    lastValue[previousMotor - 1] = analogRead(A0);  // Uložení aktuální hodnoty potenciometru pro původní motor
    delay(500);  // Debounce a zamezení rychlého přepínání
  }

  int motorPins[4] = {stepPin1, stepPin2, stepPin3, stepPin4};
  int motorPin = motorPins[currentMotor - 1];  // Výběr aktuálního motoru

  int potValue = analogRead(A0);
  int mappedValue = map(potValue, 0, 1023, 0, 2000);
  
  if (mappedValue != lastValue[currentMotor - 1]) {
    int steps = abs(mappedValue - lastValue[currentMotor - 1]);
    digitalWrite(enablePin, LOW);
    digitalWrite(dirPin, (mappedValue > lastValue[currentMotor - 1]) ? HIGH : LOW);
    for (int i = 0; i < steps; i++) {
      digitalWrite(motorPin, HIGH);
      delayMicroseconds(1000);
      digitalWrite(motorPin, LOW);
      delayMicroseconds(1000);
    }
    lastValue[currentMotor - 1] = mappedValue;
    digitalWrite(enablePin, HIGH);
  }
}