#include <Servo.h>

// create servo objects
Servo rotaryServo;             // Servo to rotate the marbles

// define Varibles

int fwdPin = 4;                // The number of the Solenoid Active LED pin
int stpPin = 6;                // The number of the Solenoid pin
int revPin = 8;
int strtStpPin = 11;           // The number of the Start Stop pin
int pressState1 = LOW;         // pressState 1 is used to debounce StartStop button, it is what the button is when first checked
int pressState2 = LOW;         // pressState 2 is used to debounce StartStop button, it is what the button is on the second check
int pressStateLast = HIGH;     // pressStateLast is used to store what the button push was last time it was checked to make sure it doesn't repeat 
int pressStateCounter = 0;     // PressStateCounter is used to count the button pushes for different modes 0=0ff, 1=run forward, 2=off, 3=run reverse
int deBug = 0;
void setup() {

 // Initialize the Solenoid Active LED pin, the Solenoid pin as an outputs and the Start Stop pin as an Input
  pinMode(fwdPin, OUTPUT);
  pinMode(stpPin, OUTPUT); 
  pinMode(revPin, OUTPUT); 
  pinMode(strtStpPin, INPUT);
  
// start serial communication
  Serial.begin(9600);
}

void loop() {

  // Read in the Start Stop Button, debounce signal and increment the Press State Mode
if (digitalRead (strtStpPin) == LOW and pressStateLast == HIGH)
      { pressState1 = digitalRead(strtStpPin);
        delay(5); //change to 25 or 10 if problem with button press needing to be too long
        pressState2 = digitalRead(strtStpPin);
      
        if(pressState1 == pressState2) 
      { pressStateCounter = pressStateCounter + 1;
        pressStateLast = pressState2;
      }
      }
 if (pressStateCounter > 3)
      {
        pressStateCounter = 0; //Set pressStateCounter back to 0 if it is greater than 3
      }
 if (digitalRead (strtStpPin) == HIGH and pressStateLast == LOW) 
      {
        pressStateLast = HIGH;
      }
 

Serial.println( digitalRead (strtStpPin));
 
 if (pressStateCounter == 1) 
      {
      digitalWrite(fwdPin, HIGH);
      digitalWrite(revPin, LOW);
      digitalWrite(stpPin, LOW);
      }
 if (pressStateCounter == 3) 
      {
      digitalWrite(fwdPin, LOW);
      digitalWrite(revPin, HIGH);
      digitalWrite(stpPin, LOW);
      }  

if (pressStateCounter == 0 or pressStateCounter == 2) 
      {
      digitalWrite(fwdPin, LOW);
      digitalWrite(revPin, LOW);
      digitalWrite(stpPin, HIGH);
      }

 }