/* hw3*/
#include <Stepper.h>
#define tri 2
#define ech 3

#define lSw 13
#define lLED 12
#define lButton 11

#define rSw 5
#define rLED 4
#define rButton 0

int distance_cm = 0;
int duration = 0;

const int stepsPerRev = 200;1
Stepper hw3Motor(stepsPerRev,7,8,9,10);

boolean lSwStat = true;
boolean rSwStat = true;
boolean lBtStat = true;
boolean rBtStat = true;
boolean UltrSenStat = true; 

void ccw(){
  hw3Motor.step(-1);
}

void cw(){
  hw3Motor.step(1);
}
void motorStop(){
  digitalWrite(7,LOW);
  digitalWrite(8,LOW);
  digitalWrite(9,LOW);
  digitalWrite(10,LOW);
}
void setup(){

  pinMode(lLED, OUTPUT);
  
  pinMode(rLED, OUTPUT);

  pinMode(lSw, INPUT);

  pinMode(rSw, INPUT);

  pinMode(lButton, INPUT_PULLUP);

  pinMode(rButton, INPUT_PULLUP);

  pinMode(ech, INPUT);

  pinMode(tri, OUTPUT);

  hw3Motor.setSpeed(60);

  Serial.begin(9600);


  pinMode(tri, OUTPUT);
  pinMode(ech, INPUT);
}
void loop(){


//Left switch, green LED on, motor stops
Serial.println("stage1");

lSwCheck:
lSwStat = digitalRead(lSw);
    if(!lSwStat == false){
      digitalWrite(lLED, HIGH);
      motorStop();
    
      delay(200);
      goto lBtCheck;
    } else {digitalWrite(lLED, LOW);
     ccw();
   goto lSwCheck;
   }


//Green button pressed, green LED off, motor CW
lBtCheck:
  Serial.println("stage2");
  delay(200);
lBtStat = digitalRead(lButton);
Serial.println(lBtStat);
      while (!lBtStat == true){
      digitalWrite(lLED, LOW);
      cw();
      rSwStat = digitalRead(rSw);
      if (!rSwStat == false) {
        goto rSwCheck;
      }
      }
  goto lBtCheck;


//Right switch, red LED on, motor stops
rSwCheck:
rSwStat = digitalRead(rSw);
     while(!rSwStat == false){
       digitalWrite(rLED, HIGH);
      motorStop(); 
      goto rBtCheck;
    
      }
     
//Right button, ultrasonic sensor, red LED off, motor starts   
rBtCheck: 
Serial.println("stage3");
lBtStat = digitalRead(lButton);
Serial.println(lBtStat);
 rBtStat= digitalRead(rButton);

 
 // Ultrasonic sensor:
 // Start a new measurement:
  digitalWrite(tri, HIGH);
  delayMicroseconds(10);
  digitalWrite(tri, LOW);

 // Read the result:
  duration = pulseIn(ech, HIGH);
  distance_cm = 0.017 * duration;
  Serial.print("distance = ");
  Serial.println(distance_cm);
  
  // If right button pressed or ultrasonic sensor > 20 cm, LED off and motor starts CCW
  while (!rBtStat == true ||  ((distance_cm) > 20)){
              digitalWrite(rLED,LOW);
            delay(500);
  goto lSwCheck;
    }
    goto rBtCheck;
}