#include <Servo.h>
/* 
Amanda Haraga, #300370622
Midterm prep assignment
V0.8 // NEARING COMPLETION, LACKING SOME COMMENTING AND CLEANING

Component table
Button A: pin 2
Button B: pin 12
Button C: pin 8
Button D: pin 7

Amber LED: pin 5
Blue LED: pin 6
Red LED: pin 9
Green LED: pin 10

Servo PMW: pin 11 
*/

Servo myservo; // Creates a Servo variable named myservo
unsigned long time; // used for time loops
int Exercise = 4; // Change number to run different exercises

void setup() 
{
  // put your setup code here, to run once:

  // Set up all LED pins
  pinMode(5, OUTPUT);
  pinMode(6, OUTPUT);
  pinMode(9, OUTPUT);
  pinMode(10, OUTPUT);
  
  // Servo set up
  myservo.attach(11);

  // Push button set up
  pinMode(2, INPUT);
  pinMode(12, INPUT);
  pinMode(8, INPUT);
  pinMode(7, INPUT);

  Serial.begin(9600); // 9600 bps serial monitor
}

void loop() 
{
  // put your main code here, to run repeatedly:

  // Turn all LED's off
  digitalWrite(5, HIGH); // Amber LED
  digitalWrite(6, HIGH); // Blue LED
  digitalWrite(9, HIGH); // Red LED
  digitalWrite(10, HIGH); // Green LED


  // EXERCISE 1
  Serial.println("Exercise 1: Start");
  while(Exercise == 1)
  {

    // Test every stationary light

    //AMBER
    digitalWrite(5, LOW); // light on
    delay(2000); // delay 2s
    digitalWrite(5, HIGH); // light off
    delay(150); // delay 150ms

    //BLUE
    digitalWrite(6, LOW);
    delay(2000);
    digitalWrite(6, HIGH);
    delay(150);

    //RED
    digitalWrite(9, LOW);
    delay(2000);
    digitalWrite(9, HIGH);
    delay(150);

    //GREEN
    digitalWrite(10, LOW);
    delay(2000);
    digitalWrite(10, HIGH);
    delay(150);

    // Test sequential blinking

    //AMBER
    digitalWrite(5, LOW);
    delay(300);
    digitalWrite(5, HIGH);
    delay(150);

    //BLUE
    digitalWrite(6, LOW);
    delay(300);
    digitalWrite(6, HIGH);
    delay(150);

    //RED
    digitalWrite(9, LOW);
    delay(300);
    digitalWrite(9, HIGH);
    delay(150);

    //GREEN
    digitalWrite(10, LOW);
    delay(300);
    digitalWrite(10, HIGH);
    delay(150);

    // Servo Movement
    myservo.write(0); // move servo to 0 degrees
    delay(1000);
    myservo.write(45); // move servo to 45 degrees
    delay(1000);
    myservo.write(0); // move servo to 0 degrees
    delay(1000);
    
    while (true) // Loop that prints the pin number of the pressed button (Only goes once!!)
    {
      if(digitalRead(2) != LOW) // if A is pressed
      {
        Serial.println("2"); // print pin number
        Exercise = 2;  // change exercise
        break; // break infinite loop
      }
      if(digitalRead(12) != LOW) // if B is pressed
      {
        Serial.println("12");
        Exercise = 2;
        break;
      }
      if(digitalRead(8) != LOW) // if C is pressed
      {
        Serial.println("8");
        Exercise = 2;
        break;
      }
      if(digitalRead(7) != LOW) // if D is pressed
      {
        Serial.println("7");
        Exercise = 2;
        break;
      }
    }

  }
  Serial.println("Exercise 1: Over");

  // EXERCISE 2
  Serial.println("Exercise 2: Start");
  while(Exercise == 2)
  {
    if(digitalRead(2) != LOW) // Blink all LEDS while pressed
    {
      digitalWrite(5, LOW);
      digitalWrite(6, LOW);
      digitalWrite(9, LOW);
      digitalWrite(10, LOW);
      delay(200);
      digitalWrite(5, HIGH);
      digitalWrite(6, HIGH);
      digitalWrite(9, HIGH);
      digitalWrite(10, HIGH);
      delay(150);
    }

    if(digitalRead(12) != LOW) // Turn all LEDS on while pressed
    {
      digitalWrite(5, LOW);
      digitalWrite(6, LOW);
      digitalWrite(9, LOW);
      digitalWrite(10, LOW);
    }

    if(digitalRead(8) != LOW) // Turn all LEDS off while pressed
    {
      digitalWrite(5, HIGH);
      digitalWrite(6, HIGH);
      digitalWrite(9, HIGH);
      digitalWrite(10, HIGH);
    }

    if(digitalRead(7) != LOW) // Rotate servo between 45 and 135 degrees while pressed
    {
      myservo.write(45);
      delay(500);
      myservo.write(135);
      delay(500);
    }
  }
  Serial.println("Exercise 2: Over");

  // EXERCISE 3
  Serial.println("Exercise 3: Start");
  while (Exercise == 3)
  {
    myservo.write(0); // starts servo at 0

    if(digitalRead(2) != LOW)
    {
      digitalWrite(5, LOW);
      myservo.write(45);
      delay(3000);
      digitalWrite(5, HIGH);
      myservo.write(0);
    }

    if(digitalRead(12) != LOW)
    {
      digitalWrite(6, LOW);
      myservo.write(90);
      delay(3000);
      digitalWrite(6, HIGH);
      myservo.write(0);
    }

    if(digitalRead(8) != LOW)
    {
      digitalWrite(10, LOW);
      myservo.write(135);
      delay(3000);
      digitalWrite(10, HIGH);
      myservo.write(0);
    }

    if(digitalRead(7) != LOW)
    {
      digitalWrite(9, LOW);
      myservo.write(180);
      delay(3000);
      digitalWrite(9, HIGH);
      myservo.write(0);
    }
  }
  Serial.println("Exercise 3: Over");


  // EXERCISE 4
  Serial.println("Exercise 4: Start");
  while(Exercise == 4)
  {
    if(digitalRead(2) != LOW)
    {
      time = millis();
      while(millis() - time <= 2000)
      {
        digitalWrite(5, LOW);
        delay(300);
        digitalWrite(5, HIGH);
        delay(150);
      }
      time = millis();
      while(millis() - time <= 2000)
      {
        digitalWrite(6, LOW);
        delay(300);
        digitalWrite(6, HIGH);
        delay(150);
      }
      time = millis();
      while(millis() - time <= 2000)
      {
        digitalWrite(9, LOW);
        delay(300);
        digitalWrite(9, HIGH);
        delay(150);
      }
      time = millis();
      while(millis() - time <= 2000)
      {
        digitalWrite(10, LOW);
        delay(300);
        digitalWrite(10, HIGH);
        delay(150);
      }
    }

    if(digitalRead(12) != LOW)
    {
      digitalWrite(5, LOW);
      digitalWrite(9, LOW);
      delay(300);
      digitalWrite(5, HIGH);
      digitalWrite(9, HIGH);
      delay(150);


      digitalWrite(6, LOW);
      digitalWrite(10, LOW);
      delay(300);
      digitalWrite(6, HIGH);
      digitalWrite(10, HIGH);
      delay(150);
    }

    if(digitalRead(8) != LOW)
    {
      myservo.write(0);
      delay(1000);
      myservo.write(30);
      delay(1000);
      myservo.write(60);
      delay(1000);
      myservo.write(120);
      delay(1000);
    }

    if(digitalRead(7) != LOW)
    {
      time = millis();
      while(millis() - time <= 4000)
      {
        digitalWrite(6, LOW);
        digitalWrite(10, LOW);
        myservo.write(30);
        delay(300);
        digitalWrite(6, HIGH);
        digitalWrite(10, HIGH);
        myservo.write(120);
        delay(400);
      }

      time = millis();
      while(millis() - time <= 4000)
      {
        digitalWrite(5, LOW);
        digitalWrite(9, LOW);
        myservo.write(45);
        delay(300);
        digitalWrite(5, HIGH);
        digitalWrite(9, HIGH);
        myservo.write(90);
        delay(400);
      }
    }
  }
  Serial.println("Exercise 4: Over");

  // EXERCISE 5

}