#include <Servo.h>

Servo myservo;
Servo pullServo;

int maxWait = 30000;
unsigned long currMillis;
unsigned long prevMillis = 0;
unsigned long buttonMillis;
unsigned long prevButtonMillis;
int interval = 1000;
unsigned long startPullMillis;
int pull;
int prevPull;
int state = 1;
int prevState;
int buttonPin = 10;
int buttonVar = 0;
int prevButtonVar = 0;
int debounce = 100;
int speedMin = 2;
int pos = 105;
int spinCount = 0;
int prevSpinCount;
int spin = 0;
int speedVar = 10;
int directionVar = 1;
int pauseHalfwayVar;
int pauseMillis;
int spinPin = 11;
int spinButton;
int prevSpinButton;
unsigned long spinMillis;
unsigned long prevSpinMillis = 0;
int waitAtEndMillis;
int waitHalfwayMillis;
unsigned long waitHalfwayStart;
unsigned long waitAtEndStart;
int on = 1;
float pullVar;
float stopHalfwayVar = 0;
int pastHalfway = 0;


void setup(){
  myservo.attach(9);
  pullServo.attach(5);
  pinMode(buttonPin, INPUT);
  pinMode(spinPin, INPUT);
  Serial.begin(9600);
  pullServo.write(0);
}

void loop(){
  pullVar = 55;// random(100);

  buttonVar = digitalRead(buttonPin);
  spinButton = digitalRead(spinPin);
  currMillis = millis();
  ///////////////////////////PAUSE STATE (pause via pause button)
  if(state == 0){
    speedVar = 0;
  }

  /////////////////////////RUN STATE
  if(state == 1){

    if(spin == 16){
      if(pastHalfway == 0){
        state = 2;
      }
    }

    if(spin >= 32){
      directionVar *= -1;
      state = 3;
    }
  }
  //////////////////////////PAUSE HALFWAY STATE
  if(state == 2){
    if(prevState == 1){
      speedVar = 0;
      waitHalfwayStart = currMillis;
      waitHalfwayMillis = random(maxWait);
      if(pullVar > 50){
      pull = 1;
      }
    }
    if(currMillis - waitHalfwayStart > waitHalfwayMillis){
      pastHalfway = 1;
      state = 1;
      speedVar = random(25) + speedMin;
      if(pullVar > 50){
        pull = 1;
      }
    }
  } 

  ///////////////////////PAUSE AT END STATE
  if(state == 3){
    speedVar = 0;
    if(prevState == 1){

      if(pullVar > 50){
        pull = 1;
      }
      waitAtEndMillis = random(maxWait);
      waitAtEndStart = currMillis;
    }
    if(currMillis - waitAtEndStart > waitAtEndMillis){
      state = 1;
      pastHalfway = 0;
      spin = 0;
      speedVar = random(25) + speedMin;
         if(pullVar > 50){
       pull = 1;
       }
       
    }
  }


  ///////////////////////////////PAUSE BUTTON
  if(buttonVar == 1){
    if(prevButtonVar == 0){
      buttonMillis = currMillis;
      if(buttonMillis - prevButtonMillis > debounce){
        prevButtonMillis = buttonMillis;
        if(state == 0){
          state = 1;
          speedVar = 10;
        }
        else{
          if(state == 1){
            state = 0;
          }
        }
      }
    }
  } 
  
//////////////////////////////PULL SERVO
   if(pull == 1){
   if(prevPull == 0){
   startPullMillis = currMillis;
   pullServo.write(150);
   }
   if(currMillis - startPullMillis > 1000){
   pullServo.write(0);
   pull = 0;
   }
   }
   
   
  /////////////////////////////SPIN COUNTER
  if(spinButton == 1){
    if(prevSpinButton == 0){
      spinMillis = currMillis;
      if(spinMillis - prevSpinMillis > debounce){
        prevSpinMillis = spinMillis;
        spin += 1;
      }
    }
  }

  Serial.println(pull);
  //Serial.println(waitHalfwayMillis);
  pos = 90 + (speedVar * directionVar);
  myservo.write(pos);
  prevSpinButton = spinButton;
  prevButtonVar = buttonVar;
  prevState = state;
  prevPull = pull;
}