//GNG2101 - D3.1 Folding Fellows
//code by Gray Berneche

//output pins
// Pin 0,1: Motor 1
int m0 = 0;
int m1 = 1;
// Pin 2,3: Motor 2
int m2 = 2;
int m3 = 3;
// Pin 4,5: Motor 3
int m4 = 4;
int m5 = 5;
// Pin 6: PWM Enable 
int PWM_en = 6;
// Pin 7: LED Enable
int LED_en = 7;
// Pin 8,9: LED Select
int LED_sel0 = 8;
int LED_sel1 = 9;

//input pins
// Pin 11: Push Button
int btn = 11;
// Pin 12: Lock Switch
int lock = 12;
// Pin 13: AUX button
int aux = 13;

//PWM speed (0-255)
int left_speed = 255;
int right_speed = 255;
int middle_speed = 255;

//Motor time delay
int left_time = 1000;
int right_time = 1000;
int middle_time = 1000;
int post_time = 200;

//input reading
int lockState = digitalRead(lock);
int buttonState = digitalRead(btn);
int auxState = digitalRead(aux);

//lockout control
unsigned long motorStartTime = 0;
bool done = false;
bool done_left, done_right, done_middle = false;

void setBlue(){
  digitalWrite(LED_sel0, HIGH);
  digitalWrite(LED_sel1, HIGH);
}
void setRed(){
  digitalWrite(LED_sel0, HIGH);
  digitalWrite(LED_sel1, LOW);
}
void setYellow(){
  digitalWrite(LED_sel0, LOW);
  digitalWrite(LED_sel1, HIGH);
}
void setGreen(){
  digitalWrite(LED_sel0, LOW);
  digitalWrite(LED_sel1, LOW);
}

void setup() {
    //PIN MODES
    pinMode(m0, OUTPUT);
    pinMode(m1, OUTPUT);
    pinMode(m2, OUTPUT);
    pinMode(m3, OUTPUT);
    pinMode(m4, OUTPUT);
    pinMode(m5, OUTPUT);
    pinMode(PWM_en, OUTPUT);
    pinMode(LED_en, OUTPUT);
    pinMode(LED_sel0, OUTPUT);
    pinMode(LED_sel1, OUTPUT);

    pinMode(btn, INPUT);
    pinMode(lock, INPUT);
    
    //INITIAL STATE
    analogWrite(PWM_en, 0);

    digitalWrite(m0, LOW);
    digitalWrite(m1, LOW);
    digitalWrite(m2, LOW);
    digitalWrite(m3, LOW);
    digitalWrite(m4, LOW);
    digitalWrite(m5, LOW);       

    digitalWrite(LED_en, LOW);
    setBlue();
  }

void loop() {

    //check lock
    lockState = digitalRead(lock);

    if (lockState == HIGH) {
      //turn on light
      digitalWrite(LED_en, HIGH);
      setBlue();
      done = false;
      done_left = false;
      done_right = false;
      done_middle = false;
      
      //check button
      buttonState = digitalRead(btn);
      auxState = digitalRead(aux);

      //start sequence when a button is pressed
      if(buttonState == HIGH || auxState == HIGH) {
        startSequence();
      }

    } else {
      //turn off light
      digitalWrite(LED_en, LOW);      
    }
}

void startSequence() {

  //Lets Roll

  //LEFT
      //setup
      setRed();
      analogWrite(PWM_en, left_speed);
      digitalWrite(m0, HIGH);
      digitalWrite(m1, LOW);
      motorStartTime = millis();
      done = false;
      done_left = false;

      //loop
      while (digitalRead(lock) == HIGH && !done) {
        if (((millis() - motorStartTime) >= left_time) && !done_left) {
          digitalWrite(LED_en, LOW);
          delay(post_time);
          digitalWrite(LED_en, HIGH);
          digitalWrite(m0, LOW);
          digitalWrite(m1, HIGH);
          done_left = true;
        }
        if ((millis() - motorStartTime) >= (2*left_time)) {
          motorStartTime = 0;
          done = true; 
        }
      }

      //ending
      analogWrite(PWM_en, 0);
      digitalWrite(m0, LOW);
      digitalWrite(m1, LOW);

  //RIGHT
      //setup
      setYellow();
      analogWrite(PWM_en, right_speed);
      digitalWrite(m2, HIGH);
      digitalWrite(m3, LOW);
      motorStartTime = millis();
      done = false;
      done_right = false;

      //loop
      while (digitalRead(lock) == HIGH && !done) {
        if (((millis() - motorStartTime) >= right_time) && !done_right) {
          digitalWrite(LED_en, LOW);
          delay(post_time);
          digitalWrite(LED_en, HIGH);
          digitalWrite(m2, LOW);
          digitalWrite(m3, HIGH);
          done_right = true;
        }
        if ((millis() - motorStartTime) >= (2*right_time)) {
          motorStartTime = 0;
          done = true; 
        }
      }

      //ending
      analogWrite(PWM_en, 0);
      digitalWrite(m2, LOW);
      digitalWrite(m3, LOW);

  //MIDDLE
      //setup
      setGreen();
      analogWrite(PWM_en, middle_speed);
      digitalWrite(m4, HIGH);
      digitalWrite(m5, LOW);
      motorStartTime = millis();
      done = false;
      done_middle = false;

      while (digitalRead(lock) == HIGH && !done) {
        if (((millis() - motorStartTime) >= middle_time) && !done_middle) {
          digitalWrite(LED_en, LOW);
          delay(post_time);
          digitalWrite(LED_en, HIGH);
          digitalWrite(m4, LOW);
          digitalWrite(m5, HIGH);
          done_middle = true;
        }
        if ((millis() - motorStartTime) >= (2*middle_time)) {
          motorStartTime = 0;
          done = true; 
        }
      }

  //POST FOLD

      //light show
      done = false;
      int count = 0;
      while ((digitalRead(lock) == HIGH) && (count < 3)) {
          count++;
          digitalWrite(LED_sel0, HIGH);
          digitalWrite(LED_sel1, HIGH);
          delay(500);
          digitalWrite(LED_sel0, HIGH);
          digitalWrite(LED_sel1, LOW);
          delay(500);
          digitalWrite(LED_sel0, LOW);
          digitalWrite(LED_sel1, HIGH);
          delay(500);
          digitalWrite(LED_sel0, LOW);
          digitalWrite(LED_sel1, LOW);
          delay(500);
          digitalWrite(LED_sel0, HIGH);
          digitalWrite(LED_sel1, HIGH);  
          //if (count=3) { done = true; }
        
      }
}