// Robot Dance
//
// 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();
}

/////////////////////////////////////////////////////////////
// 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();
}

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();
}

void loop() {
  int d = distance();
  if (d > 0 && d < 10) {  // check for object less than 10
    Dance();
  } else if (d > 0 && d < 20) {  // check for object less than 20
    sparkles();
    chirp(1);
    delay(50);
  } else {
    allLightsOff();
  }
}