#include <AccelStepper.h>

#define step1 4
#define dir1 5

#define step2 6
#define dir2 7

#define step3 8
#define dir3 9

#define step4 10
#define dir4 11

AccelStepper stepper1(1, step1, dir1);
AccelStepper stepper2(1, step2, dir2);
AccelStepper stepper3(1, step3, dir3);
AccelStepper stepper4(1, step4, dir4);

int positions[] = {0, 50, 100, 150, 200, 150, 100, 50, 0};
int currentPosIndex = 0;
unsigned long previousMillis = 0;
const long interval = 2000; // 3 Sekunden

//Pin des Relais für die Extruder festlegen
const int relayPin3 = 3;
//relais für Heizpatrone
const int relayPin12 = 12;


bool returnToStart = false;
bool stop=false;
bool lastButtonState = false;


void setup() {
  stepper1.setMaxSpeed(200);
  stepper1.setAcceleration(200);
  stepper1.moveTo(positions[currentPosIndex]);
 
  stepper2.setMaxSpeed(300);
  stepper2.setSpeed(100);
 
  stepper3.setMaxSpeed(300);
  stepper3.setSpeed(100);

  stepper4.setMaxSpeed(100);
  
  //Pins an den Relais angeschlossen sind als Output deklarieren
  pinMode(relayPin3, OUTPUT);
  pinMode(relayPin12, OUTPUT);

  //button als input deklarieren
  pinMode(2, INPUT_PULLUP); 
  pinMode(13, INPUT_PULLUP);

  //Erstes Stepperziel setzen
  stepper2.moveTo(-200);
  stepper3.moveTo(200);
  stepper4.moveTo(8000);
}

void loop() {
  bool buttonState = digitalRead(2);
  
 
  if (buttonState == 0 && lastButtonState == 0) {
    move();
  }
  
  
}
void move(){
  int i = 0;

  while(true){
    stepper2.run();
    stepper3.run();
    //Wenn Notknopf gedrückt ist, dann alles stop und wieder auf 0 setzen
    if(digitalRead(13)==0){
      emStop();
      return;
    }
    //wenn Filament geschmolzen wird, dann wird noch etwas Filament nachgeschoben
    if(stepper2.currentPosition()==-200 && stepper3.currentPosition()==200 && returnToStart == false){
      delay(3000);
      stepper2.moveTo(-220);
      stepper3.moveTo(220);
      //Heizpatrone fährt nach unten
      digitalWrite(relayPin12, HIGH);
      delay(500);
      stop=false;
      returnToStart = true;
    }

    //Wenn Filament verschweißt ist, dann position 0 
    if (stepper2.currentPosition() == -220 && stepper3.currentPosition() == 220 && returnToStart == true) {
      delay(2000);
      //Heizpatrone fährt zurück
      digitalWrite(relayPin12, LOW);
      //Extruder werden geöffnet
      digitalWrite(relayPin3, HIGH);
      //Neue Position wird gesetzt auf 0 gesetzt
      digitalWrite(relayPin12,LOW);
      stepper2.setCurrentPosition(0);
      stepper3.setCurrentPosition(0);  
      stop=true;    
    } 

    //Wenn die motoren auf Position 0 sind dann starte den Aufwickel-Prozess und resette die Werte
    if(stop==true && stepper2.currentPosition()==0&& stepper3.currentPosition()==0){
      stop==false;
      returnToStart=false;
      windingUp();
      reset();
      return;
    }
    
   
  }
}
void windingUp(){
  int i=0;

  while(true){
    unsigned long currentMillis = millis();

    stepper1.run();
    stepper4.setSpeed(50);
    stepper4.runSpeed();

    //Wenn Notknopf gedrückt ist, dann alles stop und wieder auf 0 setzen
    if(digitalRead(13)==0){
      emStop();
      return;
    }

    // Check if 2 seconds have passed
    if (currentMillis - previousMillis >= interval) {
      // Save the last time you moved the stepper
      previousMillis = currentMillis;

      // Zur nächsten Position fahren
      currentPosIndex++;
      if (currentPosIndex >= 9) {
      currentPosIndex = 0;
      i++;
      }
      stepper1.moveTo(positions[currentPosIndex]);
    }
    if(i==2){
      stepper4.stop();
      return;
    }
  }
}
void reset(){
  stepper2.moveTo(-200);
  stepper3.moveTo(200);
  stepper4.setCurrentPosition(0);
  stepper4.moveTo(8000);
  digitalWrite(relayPin3, LOW);
}
void emStop(){
  stepper1.stop();
  stepper2.stop();
  stepper3.stop();
  stepper4.stop();

  stepper1.setCurrentPosition(0);
  stepper2.setCurrentPosition(0);
  stepper3.setCurrentPosition(0);
  stepper4.setCurrentPosition(0);

  reset();
}