// Include servo library for the fusels
// see https://www.arduino.cc/en/Reference/Servo
#include <Servo.h>

Servo myservo1;  // create servo object to control a servo
Servo myservo2;  // create servo object to control a servo

// for using random numberssee
// https://www.arduino.cc/reference/en/language/functions/random-numbers/randomseed/

long randNumber;

#define EN        8  

//Direction pin
#define X_DIR     5 
#define Y_DIR     6
#define Z_DIR     7

//Step pin
#define X_STP     2
#define Y_STP     3 
#define Z_STP     4 

#define SWITCH1Pin  A5

//DRV8825
int delayTime=60; //Delay between each pause (uS)
int stps=6400;// Steps to move microsteps 1/32 (200*32 = 6400)
int pos = 180;    // variable to store the servo position

void step(boolean dir, byte dirPin, byte stepperPin, int steps)

{

  digitalWrite(dirPin, dir);

  for (int i = 0; i < steps; i++) {

    digitalWrite(stepperPin, HIGH);

    delayMicroseconds(delayTime); 

    digitalWrite(stepperPin, LOW); 

    delayMicroseconds(delayTime); 

  }

}

void setup(){

    Serial.begin(9600);
    randomSeed(analogRead(0));
  // if analog input pin 0 is unconnected, random analog
  // noise will cause the call to randomSeed() to generate
  // different seed numbers each time the sketch runs.
  // randomSeed() will then shuffle the random function.
  
  pinMode(Y_DIR, OUTPUT); pinMode(Y_STP, OUTPUT); // Motor for Stage

  pinMode(Z_DIR, OUTPUT); pinMode(Z_STP, OUTPUT); // Motor for tube 1

  pinMode(X_DIR, OUTPUT); pinMode(X_STP, OUTPUT); // Motor for thube 2

  pinMode(EN, OUTPUT);

  digitalWrite(EN, LOW);

  pinMode(SWITCH1Pin, INPUT_PULLUP);

  // servo part in the setup
  myservo1.attach(10);  // attaches the servo on pin 9 to the servo object
  myservo2.attach(9);  // attaches the servo on pin 10 to the servo object

  for (pos = 0; pos <= 90; pos += 1) {
    myservo1.write(pos);
    myservo2.write(pos);
  }
}

void loop(){

int currentButtonState = digitalRead(SWITCH1Pin);
if (currentButtonState == LOW)  {
  randoM();

if (randNumber == 0){
  stage1();
//0= position 1
  
}
if (randNumber == 1){
  stage2();
//10= position 2
  
}
if (randNumber == 2){
  stage3();
//24=position 3

}
if (randNumber == 3){
  stage4();
//34=position 4 
 
    }
  }
}

void randoM(){
 randNumber = random(1, 4);
 Serial.println(randNumber);
  
}

void stage1(){
//position 1. Don't move the stage), use motor connected to the Z-dir to release some topping, wait some ms.
      step(false, Z_DIR, Z_STP, 1600); //Z, Clockwise
 delay(2000);     
}

void stage2(){
//position 2. Move the stage 8 steps, release topping 2 (turn servo), wait some ms and return home.
// false 0-34 steps from motor
//true 34-0 steps towards motor

  for(int i =0; i<8; ++i){
      step(false, Y_DIR, Y_STP, stps); //true equals back from motor
  }

  delay(2000);
  
  for (pos = 0; pos <= 180; pos += 1) { // goes from 0 degrees to 180 degrees
    // in steps of 1 degree
    myservo1.write(pos);              // tell servo to go to position in variable 'pos'
    delay(5);                       // waits 5ms for the servo to reach the position
  }

  delay(5000);
  
  for (pos = 180; pos >= 0; pos -= 1) { // goes from 180 degrees to 0 degrees
    myservo1.write(pos);              // tell servo to go to position in variable 'pos'
    delay(5);                       // waits 5ms for the servo to reach the position
  }

  delay(2000);
  
  for(int i =0; i<8; ++i){
      step(true, Y_DIR, Y_STP, stps); //true equals back towards 
  }

  delay(2000);
  
}

void stage3(){
//position 3. Move the stage 23 steps, release topping 3 (turn servo), wait some ms and return home.
    
  
  for(int i =0; i<23; ++i){
      step(false, Y_DIR, Y_STP, stps); //true equals back from motor
  }

  delay(2000);

  for (pos = 0; pos <= 180; pos += 1) { // goes from 0 degrees to 180 degrees, in steps of 1 degree
    myservo2.write(pos);              // tell servo to go to position in variable 'pos'
    delay(5);                       // waits 5ms between each degree for the servo to reach the position (speed of the shaft)
  }

  delay(5000);
  
  for (pos = 180; pos >= 0; pos -= 1) { // goes from 180 degrees to 0 degrees
    myservo2.write(pos);              // tell servo to go to position in variable 'pos'
    delay(5);                       // waits 5ms between each degree for the servo to reach the position (speed of the shaft)
  }

 delay(2000);

  for(int i =0; i<23; ++i){
      step(true, Y_DIR, Y_STP, stps); //true equals back towards motor 
  }


 delay(2000);
}

void stage4(){

  for(int i =0; i<33; ++i){
      step(false, Y_DIR, Y_STP, stps); //true equals back towards motor  
  }
  
 delay(2000);

//turn motor tube 2

  step(false, X_DIR, X_STP, 3200); //Z, Clockwise  
    
 delay(2000); 

   for(int i =0; i<33; ++i){
      step(true, Y_DIR, Y_STP, stps); //true equals back towards motor
   }
 
  delay(2000);
}