#include <Servo.h>  // Include servo library for the funnels, see https://www.arduino.cc/en/Reference/Servo
Servo myservo1;  // create servo object to control a servo
Servo myservo2;  // create servo object to control a servo
int pos = 0;  // Variable to store the servo position

long randNumber; // for using random numbers, see https://www.arduino.cc/reference/en/language/functions/random-numbers/randomseed/

#define EN        8  // enable pin is 8

//Direction pin
#define X_DIR     5 //X axis stepper motor
#define Y_DIR     6 //Y axis stepper motor
#define Z_DIR     7 //Z axis stepper motor

//Step pin
#define X_STP     2 //X axis stepper control
#define Y_STP     3 //Y axis stepper control
#define Z_STP     4 //Z axis stepper control

#define SWITCH1Pin  A5

//DRV8825
int delayTime=60; // Delay between each phase (uS)
int stps=6400; // Steps to move microsteps 1/32 (200*32 = 6400)


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 tube 2

 pinMode(EN, OUTPUT);

 digitalWrite(EN, LOW);

 pinMode(SWITCH1Pin, INPUT_PULLUP);

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


void loop(){

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

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

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

void randoM(){
randNumber = random(0, 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.

 for(int i =0; i<8; ++i){
     step(false, Y_DIR, Y_STP, stps);
 }

 delay(2000);
 
 for (pos = 0; pos <= 90; 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
 }
 
 for (pos = 90; 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); //go back to starting position
 }

 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);
 }

 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)
 }
 
 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);  //go back to starting position
 }

delay(2000);
}

void stage4(){

 for(int i =0; i<33; ++i){
     step(false, Y_DIR, Y_STP, stps);  
 }
//turn motor tube 2

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

  for(int i =0; i<33; ++i){
     step(true, Y_DIR, Y_STP, stps);  //go back to starting position
  }
 delay(2000);
}