// Advance Robot Car Demo
// Cycles through the following routines:
//   StarWars();
//   PoliceCar();
//   Dance();
//   RingAroundTheRosy();
//
// c 2019 Enoch Hwang
//
/////////////////////////////////////////////////////////////
// Ultrasonic distance sensor stuff

// this function calculates and returns the distance
// as obtained from the ultrasonic sensor
int distance() {
  int trigPin = 11;
  int echoPin = 12;  
  pinMode(trigPin, OUTPUT);
  pinMode(echoPin, INPUT);

  // STEP 1
  // The next five lines will send out one ultrasound pulse
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);

  // STEP 2
  // The pulseIn command starts the timing and wait for the echo
  // when it hears the echo it will return the time in microseconds
  int duration = pulseIn(echoPin, HIGH, 5000); // the 5000 timeout is to limit reading to < 30 cm

  // STEP 3
  // Calculate the distance (in cm) based on the time from STEP 2 and the speed of sound
  // the calculated distance is returned by the function
  return duration/58.2;  // in cm
  //return duration/148;// in inches
} // end of function


/////////////////////////////////////////////////////////////
// LEDs stuff
int redPin = 13;
int greenPin = 4;
int whitePin = 3;
int yellowPin = 2;
int bluePin = 1;

void allLightsOn() {
  digitalWrite(redPin, HIGH);
  digitalWrite(greenPin, HIGH);
  digitalWrite(whitePin, HIGH);
  digitalWrite(yellowPin, HIGH);
  digitalWrite(bluePin, HIGH);
}

void allLightsOff() {
  digitalWrite(redPin, LOW);
  digitalWrite(greenPin, LOW);
  digitalWrite(whitePin, LOW);
  digitalWrite(yellowPin, LOW);
  digitalWrite(bluePin, LOW);
}

void sparkles() {
  digitalWrite(redPin, random(0,2));
  digitalWrite(greenPin, random(0,2));
  digitalWrite(whitePin, random(0,2));
  digitalWrite(yellowPin, random(0,2));
  digitalWrite(bluePin, random(0,2));
}

/////////////////////////////////////////////////////////////
// Buzzer stuff
int buzzerPin = 8;

int NOTE_C4 = 262;
int NOTE_D4 = 294;
int NOTE_E4 = 330;
int NOTE_F4 = 349;
int NOTE_FS4 = 370;
int NOTE_G4 = 392;
int NOTE_GS4 = 415;
int NOTE_A4 = 440;
int NOTE_AS4 = 466;
int NOTE_B4 = 494;
int NOTE_C5 = 523;
int NOTE_CS5 = 554;
int NOTE_D5 = 587;
int NOTE_E5 = 659;
int NOTE_F5 = 698;
int NOTE_G5 = 784;
int NOTE_B5 = 988;
int NOTE_C6 = 1047;
int NOTE_D6 = 1175;
int NOTE_E6 = 1319;
int NOTE_F6 = 1397;

void chirp(int n=0) {
  int frequency, duration, pause;
  if (n == 0) {
    n = random(2, 10)*2;
  }
  for (int i=0; i<n; i++) {
    frequency = random(40, 350)*7;
    duration = random(20, 70)*3;
    tone(buzzerPin, frequency, duration);
    delay(duration);
    noTone(buzzerPin);
  }
}

/////////////////////////////////////////////////////////////
// Motor stuff

// In order to control the motor speed all the pins must
// use one of the PWM ~ pins
// connections for motor 1
int in1Pin = 6;   // Input 1, must be one of the PWM ~ pins
int in2Pin = 5;   // Input 2, must be one of the PWM ~ pins
// connections for motor 2
int in3Pin = 9;   // Input 3, must be one of the PWM ~ pins
int in4Pin = 10;  // Input 4, must be one of the PWM ~ pins

void stopMotor() {
  analogWrite(in1Pin, 0);   // stop motor 1
  analogWrite(in2Pin, 0);
  analogWrite(in3Pin, 0);   // stop motor 2
  analogWrite(in4Pin, 0);
}

void goForward(int motorSpeed = 255) {
  analogWrite(in1Pin, motorSpeed);  // one input > 0
  analogWrite(in2Pin, 0);           // the other = 0   
  analogWrite(in3Pin, motorSpeed);  // one input > 0
  analogWrite(in4Pin, 0);           // the other = 0  
}

void goBackward(int motorSpeed = 255) {
  analogWrite(in1Pin, 0);           // one input = 0
  analogWrite(in2Pin, motorSpeed);  // the other > 0   
  analogWrite(in3Pin, 0);           // one input = 0
  analogWrite(in4Pin, motorSpeed);  // the other > 0  
}

void turnLeft(int motorSpeed = 255) {
  analogWrite(in1Pin, 0);           // one input = 0
  analogWrite(in2Pin, motorSpeed);  // the other > 0
  analogWrite(in3Pin, motorSpeed);  // one input > 0
  analogWrite(in4Pin, 0);           // the other = 0
}

void turnRight(int motorSpeed = 255) {
  analogWrite(in1Pin, motorSpeed);  // one input > 0
  analogWrite(in2Pin, 0);           // the other = 0
  analogWrite(in3Pin, 0);           // one input = 0
  analogWrite(in4Pin, motorSpeed);  // the other > 0
}

void testMotors() {
  goForward();
  delay(1000);
  goBackward();
  delay(1000);
  turnLeft();
  delay(600);
  turnRight();
  delay(600);
  stopMotor();
}

/////////////////////////////////////////////////////////////
// Star Wars stuff

void StarWars() {
  // code to play the first 16 notes of the Star Wars Theme song
  int melody[] = {
    NOTE_C4, NOTE_G4, NOTE_F4, NOTE_E4, NOTE_D4, NOTE_C5, NOTE_G4, 
    NOTE_F4, NOTE_E4, NOTE_D4, NOTE_C5, NOTE_G4, 
    NOTE_F4, NOTE_E4, NOTE_F4, NOTE_D4
  };
  int noteDurations[] = {
    1000, 1000, 167, 167, 167, 1000, 500,
    167, 167, 167, 1000, 500,
    167, 167, 167, 2000
  };
  for (int thisNote = 0; thisNote < 16; thisNote++) {
    if (thisNote%2) allLightsOff();
    else allLightsOn();
    tone(buzzerPin, melody[thisNote], noteDurations[thisNote]);
    delay(noteDurations[thisNote]);
    noTone(buzzerPin);
  }
  noTone(buzzerPin);
}

/////////////////////////////////////////////////////////////
// Police car stuff

void flashRedBlue() {
  static unsigned long startTime = 0;
  static bool redOn = true;
  if (millis() - startTime > 200) { // flash leds every 200 milliseconds
    startTime = millis();
    digitalWrite(redPin, redOn);
    digitalWrite(bluePin, !redOn);
    redOn = !redOn; // flip value
  }
}

void siren() {
  static unsigned long startTime = 0;
  static int frequency = 650;  // starting frequency
  static bool goingUp = true;  // go up first
  if (millis() - startTime > 10) { // play note every 10 milliseconds
    startTime = millis(); 
    tone(buzzerPin, frequency);
    if (goingUp == true) {
      frequency++;            // increment frequency for going up
      if (frequency == 900) { // reached the top?
        goingUp = false;      // change direction
      }
    } else {                  // for going down
      frequency--;            // decrement frequency for going down
      if (frequency == 650) { // reached the bottom?
        goingUp = true; 
      }
    }
  }
}

void delayWithSiren(int wait) {
  for (int i=0; i<wait; i++) {
    flashRedBlue();
    siren();
    delay(1);
  }
}

void PoliceCar() {
  int d = distance();
  if (d > 0 && d < 20) {  // check for object less than 20
    // stop, backup, and then turn left or right
    stopMotor();
    delayWithSiren(1000);
    goBackward(150);
    delayWithSiren(600);
    stopMotor();
    delayWithSiren(1000);
    // randomly generate a number between 0 and 1 inclusive
    if (random(0,2) == 0) {  //test if the number is a 0
       turnLeft(150);   // turn left if it is a 0
    } else {
       turnRight(150);  // turn right if it is a 1
    }
    delayWithSiren(500);
    stopMotor(); 
    delayWithSiren(1000);
  } else {
    goForward(255);          // continue to go forward
  }
  delayWithSiren(1);   // one millisecond each time round the loop
}

/////////////////////////////////////////////////////////////
// Dance stuff

void Dance() {
  tone(buzzerPin, NOTE_C4, 200);
  digitalWrite(greenPin, HIGH);
  turnRight(130);
  delay(300);
  digitalWrite(greenPin, LOW);
  stopMotor();
  delay(450);
  noTone(buzzerPin);
  tone(buzzerPin, NOTE_A4, 730);
  digitalWrite(bluePin, HIGH);
  turnRight(130);
  delay(500);
  digitalWrite(bluePin, LOW);
  stopMotor();
  delay(250);
  noTone(buzzerPin);
  tone(buzzerPin, NOTE_F4, 200);
  digitalWrite(yellowPin, HIGH);
  turnRight(130);
  delay(300);
  digitalWrite(yellowPin, LOW);
  stopMotor();
  delay(450);
  noTone(buzzerPin);
  tone(buzzerPin, NOTE_C4, 200);
  digitalWrite(greenPin, HIGH);
  turnLeft(130);
  delay(300);
  digitalWrite(greenPin, LOW);
  stopMotor();
  delay(450);
  noTone(buzzerPin);
  tone(buzzerPin, NOTE_A4, 730);
  digitalWrite(bluePin, HIGH);
  turnLeft(130);
  delay(500);
  digitalWrite(bluePin, LOW);
  stopMotor();
  delay(250);
  noTone(buzzerPin);
  tone(buzzerPin, NOTE_F4, 200);
  digitalWrite(yellowPin, HIGH);
  turnLeft(130);
  delay(300);
  digitalWrite(yellowPin, LOW);
  stopMotor();
  delay(450);
  noTone(buzzerPin);
  tone(buzzerPin, NOTE_C4, 200);
  digitalWrite(greenPin, HIGH);
  turnRight(130);
  delay(300);
  digitalWrite(greenPin, LOW);
  stopMotor();
  delay(450);
  noTone(buzzerPin);
  tone(buzzerPin, NOTE_E4, 1400);
  allLightsOn();
  turnLeft(130);
  delay(1350);
  allLightsOff();    
  stopMotor();
  delay(150);
  noTone(buzzerPin);

  delay(500);

  tone(buzzerPin, NOTE_C4, 200);
  digitalWrite(yellowPin, HIGH);
  turnRight(130);
  delay(300);
  digitalWrite(yellowPin, LOW);
  stopMotor();
  delay(450);
  noTone(buzzerPin);
  tone(buzzerPin, NOTE_G4, 730);
  digitalWrite(redPin, HIGH);
  turnRight(130);
  delay(500);
  digitalWrite(redPin, LOW);
  stopMotor();
  delay(250);
  noTone(buzzerPin);
  tone(buzzerPin, NOTE_E4, 200);
  digitalWrite(greenPin, HIGH);
  turnRight(130);
  delay(300);
  digitalWrite(greenPin, LOW);
  stopMotor();
  delay(450);
  noTone(buzzerPin);
  tone(buzzerPin, NOTE_C4, 200);
  digitalWrite(yellowPin, HIGH);
  turnLeft(130);
  delay(300);
  digitalWrite(yellowPin, LOW);
  stopMotor();
  delay(450);
  noTone(buzzerPin);
  tone(buzzerPin, NOTE_G4, 730);
  digitalWrite(redPin, HIGH);
  turnLeft(130);
  delay(500);
  digitalWrite(redPin, LOW);
  stopMotor();
  delay(250);
  noTone(buzzerPin);
  tone(buzzerPin, NOTE_E4, 200);
  digitalWrite(greenPin, HIGH);
  turnLeft(130);
  delay(300);
  digitalWrite(greenPin, LOW);
  stopMotor();
  delay(450);
  noTone(buzzerPin);
  tone(buzzerPin, NOTE_C4, 200);
  digitalWrite(yellowPin, HIGH);
  turnRight(130);
  delay(300);
  digitalWrite(yellowPin, LOW);
  stopMotor();
  delay(450);
  noTone(buzzerPin);
  tone(buzzerPin, NOTE_F4, 1400);
  allLightsOn();
  turnLeft(130);
  delay(1350);
  allLightsOff();
  stopMotor();
  delay(150);
  noTone(buzzerPin);

  delay(500);

///////
  
  tone(buzzerPin, NOTE_C5, 375);
  digitalWrite(whitePin, HIGH);
  goForward(130);
  delay(375);
  noTone(buzzerPin);
  tone(buzzerPin, NOTE_A4, 375);
  stopMotor();
  delay(225);
  digitalWrite(whitePin, LOW);
  stopMotor();
  delay(150);
  noTone(buzzerPin);
  tone(buzzerPin, NOTE_F4, 730);
  digitalWrite(greenPin, HIGH);
  digitalWrite(yellowPin, HIGH);
  goForward(130);
  delay(300);
  stopMotor();
  delay(200);
  digitalWrite(greenPin, LOW);
  digitalWrite(yellowPin, LOW);
  stopMotor();
  delay(250);
  noTone(buzzerPin);
  tone(buzzerPin, NOTE_A4, 730);
  digitalWrite(redPin, HIGH);
  digitalWrite(bluePin, HIGH);
  goForward(130);
  delay(300);
  stopMotor();
  delay(200);
  digitalWrite(redPin, LOW);
  digitalWrite(bluePin, LOW);
  stopMotor();
  delay(250);
  noTone(buzzerPin);
  tone(buzzerPin, NOTE_G4, 1400);
  allLightsOn();
  goBackward(130);
  delay(975);
  stopMotor();
  delay(375);
  allLightsOff();
  stopMotor();
  delay(150);
  noTone(buzzerPin);
  
  tone(buzzerPin, NOTE_AS4, 375);
  digitalWrite(whitePin, HIGH);
  goForward(130);
  delay(375);
  noTone(buzzerPin);
  tone(buzzerPin, NOTE_G4, 375);
  stopMotor();
  delay(225);
  digitalWrite(whitePin, LOW);
  stopMotor();
  delay(150);
  noTone(buzzerPin);
  tone(buzzerPin, NOTE_E4, 730);
  digitalWrite(greenPin, HIGH);
  digitalWrite(yellowPin, HIGH);
  goForward(130);
  delay(300);
  stopMotor();
  delay(200);
  digitalWrite(greenPin, LOW);
  digitalWrite(yellowPin, LOW);
  stopMotor();
  delay(250);
  noTone(buzzerPin);
  tone(buzzerPin, NOTE_G4, 730);
  digitalWrite(redPin, HIGH);
  digitalWrite(bluePin, HIGH);
  goForward(130);
  delay(300);
  stopMotor();
  delay(200);
  digitalWrite(redPin, LOW);
  digitalWrite(bluePin, LOW);
  stopMotor();
  delay(250);
  noTone(buzzerPin);
  tone(buzzerPin, NOTE_F4, 1400);
  allLightsOn();
  goBackward(130);
  delay(975);
  stopMotor();
  delay(375);
  allLightsOff();
  stopMotor();
  delay(150);
  noTone(buzzerPin);
}

void dance2() {
  sparkles();
  turnRight(140);
  delay(500);
  sparkles();
  stopMotor();
  delay(250);
  sparkles();
  turnRight(140);
  delay(300);
  sparkles();
  stopMotor();
  delay(150);
  sparkles();
  turnRight(140);
  delay(300);
  stopMotor();
  delay(1000);
  sparkles();
  turnLeft(140);
  delay(500);
  sparkles();
  stopMotor();
  delay(250);
  sparkles();
  turnLeft(140);
  delay(300);
  sparkles();
  stopMotor();  
  delay(150);
  sparkles();
  turnLeft(140);
  delay(300);
  stopMotor();
  delay(1000);
  sparkles();
  turnRight(160);
  delay(1500);
  stopMotor();
  delay(1000);

  sparkles();
  goForward(140);
  delay(500);
  sparkles();
  stopMotor();  
  delay(250);
  sparkles();
  goForward(140);
  delay(300);
  sparkles();
  stopMotor();  
  delay(150);
  goForward(140);
  delay(300);
  stopMotor();  
  delay(1000);
  sparkles();
  goBackward(140);
  delay(500);
  sparkles();
  stopMotor();  
  delay(250);
  sparkles();
  goBackward(140);
  delay(300);
  sparkles();
  stopMotor();  
  delay(150);
  sparkles();
  goBackward(140);
  delay(300);
  stopMotor();  
  delay(1000);
  sparkles();
  turnLeft(160);
  delay(1500);
  stopMotor();  
  delay(1000);
 
  for(int i=250; i>0; i-=25) {
    sparkles();
    turnRight(240);
    delay(i);
    sparkles();
    turnLeft(240);
    delay(i);
  }
  sparkles();
  stopMotor(); 
  delay(1000);
  goForward(150);
  delay(300);
  sparkles();
  stopMotor();
  delay(10);
  goBackward(150); 
  delay(300);
  sparkles();
  stopMotor(); 
  allLightsOff();
  chirp();
}

/////////////////////////////////////////////////////////////
// Ring Around The Rosy stuff

void RingAroundTheRosy() {
  int melody[] = {
    NOTE_G4, NOTE_G4, NOTE_E4, NOTE_A4, NOTE_G4, NOTE_E4, 
    NOTE_G4, NOTE_G4, NOTE_E4, NOTE_A4, NOTE_G4, NOTE_E4,
    NOTE_G4, NOTE_E4, NOTE_G4, NOTE_E4,
    NOTE_F4, NOTE_G4, NOTE_G4, NOTE_C4,
  
    NOTE_A4, NOTE_A4, NOTE_FS4, NOTE_B4, NOTE_A4, NOTE_FS4, 
    NOTE_A4, NOTE_A4, NOTE_FS4, NOTE_B4, NOTE_A4, NOTE_FS4, 
    NOTE_A4, NOTE_FS4, NOTE_A4, NOTE_FS4,
    NOTE_G4, NOTE_A4, NOTE_A4, NOTE_D4,
  
    NOTE_B4, NOTE_B4, NOTE_GS4, NOTE_CS5, NOTE_B4, NOTE_GS4,
    NOTE_B4, NOTE_B4, NOTE_GS4, NOTE_CS5, NOTE_B4, NOTE_GS4,
    NOTE_B4, NOTE_GS4, NOTE_B4, NOTE_GS4, 
    NOTE_A4, NOTE_B4, NOTE_B4, NOTE_E4
  };
  
  int noteDurations[] = {
    375, 125, 375, 125, 500, 500,
    375, 125, 375, 125, 500, 500,
    500, 500, 500, 375,
    125, 500, 500, 1000,
  
    300, 100, 300, 100, 400, 400,
    300, 100, 300, 100, 400, 400,
    400, 400, 400, 300,
    100, 400, 400, 1000,
  
    225, 75, 225, 75, 300, 300,
    225, 75, 225, 75, 300, 300,
    300, 300, 300, 225,
    75, 300, 300, 1000
  };

  // spin left
  analogWrite(in1Pin, 0);
  analogWrite(in2Pin, 0);
  analogWrite(in3Pin, 200);
  analogWrite(in4Pin, 0);
  for (int thisNote = 0; thisNote < 20; thisNote++) {
    tone(buzzerPin, melody[thisNote], noteDurations[thisNote]);
    delay(noteDurations[thisNote]);
    noTone(buzzerPin);
    sparkles();
  }
  stopMotor();
  allLightsOff();
  delay(1000);

  // spin right
  analogWrite(in1Pin, 255);
  analogWrite(in2Pin, 0);
  analogWrite(in3Pin, 0);
  analogWrite(in4Pin, 0);
  for (int thisNote = 20; thisNote < 40; thisNote++) {
    tone(buzzerPin, melody[thisNote], noteDurations[thisNote]);
    delay(noteDurations[thisNote]);
    noTone(buzzerPin);
    sparkles();
  }
  stopMotor();
  allLightsOff();
  delay(1000);
  
  // spin left
  analogWrite(in1Pin, 0);
  analogWrite(in2Pin, 180);
  analogWrite(in3Pin, 180);
  analogWrite(in4Pin, 0);
  for (int thisNote = 40; thisNote < 60; thisNote++) {
    tone(buzzerPin, melody[thisNote], noteDurations[thisNote]);
    delay(noteDurations[thisNote]);
    noTone(buzzerPin);
    sparkles();
  }
  allLightsOff();
  stopMotor();
  delay(1000);

  // all fall down
  int randNotes[] = {
    NOTE_D4, NOTE_E4, NOTE_F4, NOTE_A4, NOTE_D5, NOTE_F5, NOTE_G5, NOTE_B5, NOTE_C6, NOTE_D6, NOTE_E6, NOTE_F6
  };

  int timeCount = 0;
  int motorMode = 1;
  for (int i = 1000; i > 200; i = i - 5) {
    tone(buzzerPin, i);
    // must have a stopMotor delay of at least 200ms after each turn
    // otherwise the Arduino resets because of over current draw
    if (timeCount == 0) {
      if (motorMode == 0) {         // stop motor
        stopMotor();
        timeCount = 10;
        motorMode = 1;
      } else if (motorMode == 1) {  // turn left
        turnLeft(180);
        timeCount = 30;
        motorMode = 2;
      } else if (motorMode == 2) {  // stop motor
        stopMotor();
        timeCount = 10;
        motorMode = 3;
      } else if (motorMode == 3) {  // turn right
        turnRight(180);
        timeCount = 30;
        motorMode = 0;
      }
    }
    timeCount--;
    delay(20);
  }
  
  stopMotor();
  for (int i=0; i<15; i++) {
    tone(buzzerPin, randNotes[random(0, 14)]);
    sparkles();
    delay(100);
  }
  noTone(buzzerPin);
  allLightsOff();
}

void setup() {
  // motor stuff
  pinMode(in1Pin, OUTPUT);
  pinMode(in2Pin, OUTPUT);
  pinMode(in3Pin, OUTPUT);
  pinMode(in4Pin, OUTPUT);
  // LED stuff
  pinMode(redPin, OUTPUT);
  pinMode(greenPin, OUTPUT);
  pinMode(whitePin, OUTPUT);
  pinMode(yellowPin, OUTPUT);
  pinMode(bluePin, OUTPUT);
  // buzzer pin
  pinMode(buzzerPin, OUTPUT);

  allLightsOn();
  chirp(2);
  delay(1000);
  allLightsOff();
//  testMotors();
}

void loop() {
  int d = distance();
  if (d > 0 && d < 10) {  // check for object less than 10
    StarWars();
    delay(1500);
    unsigned long startTime = millis();
    while (millis() - startTime < 15000) { // run for 15 seconds
      PoliceCar();
    }
    noTone(buzzerPin);
    allLightsOff();
    stopMotor();
    delay(1500);
    // Dance();
    // delay(1500);
    RingAroundTheRosy();
  } else if (d > 0 && d < 20) {  // check for object less than 20
    sparkles();
    chirp(1);
    delay(50);
  } else {
    allLightsOff();
  }
}