#include <AccelStepper.h>

//Pins definieren
#define step1 4
#define dir1 5

#define step2 6
#define dir2 7

#define step3 8
#define dir3 9

#define step4 10
#define dir4 11

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

//winding up definitionen
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 relayPin22 = 22;
const int relayPin23 = 23;
//Heizpatrone Up down
const int relayPin24 = 24;

//relais für Heizpatrone
const int relayPin25 = 25;

//relais für Lüfter
const int relayPin31 = 31;


//Türsensor
const int pin_Trig=32;
const int pin_Echo=33;


//LED Farbkanäle
const int statusLED_G = 28;
const int statusLED_R = 29;
const int statusLED_B = 30;

//Hilfsvariablen
bool returnToStart = false;
bool stop=false;
bool lastButtonState = false;
bool lastButtonStateHand = false;


// 2 Pneumatik (relais pin 22 und 23) fahren runter wenn schmelzen fertig ist um extruder zu öffnen 1 relais schaltet um heizpatrone zu aktivieren
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(relayPin22, OUTPUT);
  pinMode(relayPin23, OUTPUT);
  pinMode(relayPin24, OUTPUT);
  //Heizpatrone
  pinMode(relayPin25, OUTPUT);
  //Lüfter
  pinMode(relayPin31, OUTPUT);
  //Status LED
  pinMode(statusLED_G, OUTPUT);
  pinMode(statusLED_R, OUTPUT);
  pinMode(statusLED_B, OUTPUT);


  //button als input deklarieren
  pinMode(26, INPUT_PULLUP); 
  pinMode(27, INPUT_PULLUP);

  //Türsensor
  pinMode(pin_Trig, OUTPUT);
  pinMode(pin_Echo, INPUT);

  //Handbetrieb Knopf
  pinMode(53, INPUT_PULLUP);
  //PneumatikExtruder1 an/aus
  pinMode(52, INPUT_PULLUP);
  //PneumatikExtruder2 an/aus
  pinMode(51, INPUT_PULLUP);
  //Heizpatrone an/aus
  pinMode(50, INPUT_PULLUP);
  //Pneumatik Heizpatrone an/aus
  pinMode(49,INPUT_PULLUP);
  //Stepper2 vorwärts
  pinMode(48, INPUT_PULLUP);
  //Stepper2 rückwärts
  pinMode(47, INPUT_PULLUP);
  //Stepper3 vorwärts
  pinMode(46, INPUT_PULLUP);
  //Stepper3 rückwärts
  pinMode(45, INPUT_PULLUP);
  //Stepper4 an/aus
  pinMode(44, INPUT_PULLUP);
  //Kalibrierungsfahrt stepper1
  pinMode(43,INPUT_PULLUP);



  //Erstes Stepperziel setzen
  stepper2.moveTo(-200);
  stepper3.moveTo(200);
}
void loop() {
  bool buttonState = digitalRead(26);
  bool buttonStateHand = digitalRead(53);
  // Start a new measurement:
  digitalWrite(pin_Trig, HIGH);
  delayMicroseconds(10);
  digitalWrite(pin_Trig, LOW);
  int duration = pulseIn(pin_Echo, HIGH);


  digitalWrite(statusLED_G,HIGH);
  digitalWrite(statusLED_R,HIGH);

  //Erst möglich die Automatik zu starten, wenn der Sensor eine Entfernung von weniger als 3cm erkennt
  if (buttonState == 0 && lastButtonState == 0&&(duration/58)<=3) {
      digitalWrite(statusLED_R,LOW);
      move();
  }
  if (buttonStateHand == 0 && lastButtonStateHand == 0) {
    digitalWrite(statusLED_R,LOW);
    digitalWrite(statusLED_G,LOW);
    digitalWrite(statusLED_B,HIGH);
    handbetrieb();
    digitalWrite(statusLED_B,LOW);
    lastButtonStateHand=1;
  }
  if(lastButtonStateHand==1){
    digitalWrite(statusLED_G,HIGH);
    digitalWrite(statusLED_R,HIGH);
    delay(500);
    lastButtonStateHand=0;
  }
}
void move(){
  int i = 0;
  digitalWrite(statusLED_G,HIGH);
  while(true){
    stepper2.run();
    stepper3.run();
    //Wenn Stopknopf gedrückt ist, dann alles stop und wieder auf 0 setzen
    if(digitalRead(27)==0){
      autoStop();
      return;
    }
    //wenn Filament geschmolzen wird, dann wird noch etwas Filament nachgeschoben
    if(stepper2.currentPosition()==-200 && stepper3.currentPosition()==200 && returnToStart == false){
      delay(3000);
      stepper2.moveTo(-210);
      stepper3.moveTo(210);
      //Heizpatrone fährt nach unten
      digitalWrite(relayPin24, HIGH);
      delay(500);
      digitalWrite(relayPin25,HIGH);
      delay(20);
      stop=false;
      returnToStart = true;
    }

    //Wenn Filament verschweißt ist, dann position 0 
    if (stepper2.currentPosition() == -210 && stepper3.currentPosition() == 210 && returnToStart == true) {
      delay(100);
      digitalWrite(relayPin25,LOW);
      digitalWrite(relayPin31,HIGH);
      delay(50);
      //Heizpatrone fährt zurück
      digitalWrite(relayPin24, LOW);
      //Extruder werden geöffnet
      digitalWrite(relayPin22, HIGH);
      digitalWrite(relayPin23, HIGH);
      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.run();

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

    //Test ob 2 Sekunden vergangen sind seit letztem Pos wechsel
    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==1){
      //Lüfter aus
      digitalWrite(relayPin31,LOW);
    }
    if(i==2){
      
      stepper4.stop();
      return;
    }
  }
}
void reset(){
  stepper2.moveTo(-200);
  stepper3.moveTo(200);
  //relais ausschalten
  digitalWrite(relayPin22, LOW);
  digitalWrite(relayPin23, LOW);
  digitalWrite(relayPin24, LOW);
  digitalWrite(relayPin25, LOW);
  //Status LED
  digitalWrite(statusLED_G,LOW);

}
void autoStop(){
  //Alles stop neue Startposition setzen und anschließend Relais zurücksetzen
  stepper1.stop();
  stepper2.stop();
  stepper3.stop();
  stepper4.stop();

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

  reset();
}
void handbetrieb(){
  delay(500);
  int statePE1=LOW;
  int statePE2=LOW;
  int stateHP=LOW;
  int statePH=LOW;
  int i=0;
  bool tof=0;
  bool fahren=0;
  while(true){


    if (digitalRead(53)==0){
      stepper2.setCurrentPosition(0);
      stepper3.setCurrentPosition(0);
      reset();
      return;
    }
//Pneumatik
    //Pneumatik Extruder1
    if(digitalRead(52)==LOW){
      statePE1=!statePE1;
      digitalWrite(relayPin22,statePE1);
      delay(100);
    }
    //Pneumatik Extruder2
    if(digitalRead(51)==LOW){
      statePE2=!statePE2;
      digitalWrite(relayPin23,statePE2);
      delay(100);
    }
     //Pneumatik Heizpatrone
    if(digitalRead(49)==LOW){
      statePH=!statePH;
      digitalWrite(relayPin25,statePH);
      delay(100);
    }
    //Heizpatrone
    if(digitalRead(50)==LOW){
      stateHP=!stateHP;
      digitalWrite(relayPin24,stateHP);
      delay(100);
    }
//Stepper2 vor/rück
    //Stepper2 vorwärts
    if(digitalRead(48)==LOW){
      stepper2.setSpeed(-10);
      stepper2.run();
    }
    //Stepper2 rückwärts
    else if(digitalRead(47)==LOW){
      stepper2.setSpeed(10);
      stepper2.run();
    }
    else{
      stepper2.stop();
    }
//Stepper3 vor/rück
    //Stepper3 vorwärts
    if(digitalRead(46)==LOW){
      stepper3.setSpeed(10);
      stepper3.run();
    }
    //Stepper3 rückwärts
    else if(digitalRead(45)==LOW){
      stepper3.setSpeed(-10);
      stepper3.run();
    }
    else{
      stepper3.stop();
    }
//Stepper4 an/aus
    if(digitalRead(44)==LOW){
      tof=!tof;
      if(tof==0){
        fahren=1;
      }
      else{
        fahren=0;
      }
      delay(100);
    }
    if(fahren==1){
      stepper4.run();
    }
    else if(fahren==0){
      stepper4.stop();
    }

//Stepper1 kalibrierungsfahrt

  if(digitalRead(43)==LOW){
    unsigned long currentmillis,previousmillis=0;

    while(true){
    currentmillis=millis();
    stepper1.run();
    stepper1.moveTo(positions[i]);
    if(stepper1.currentPosition()==positions[i]&&(currentmillis-previousmillis)>=1500){
      previousmillis=currentmillis;
      i++;
    }
    if(i==9){
      i=0;
      break;
    }
    }
  }
  }
}