// Ring Around The Rosy
//
// 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();
}

/////////////////////////////////////////////////////////////
// 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 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();
  
  int randomNotes[] = {
    NOTE_D4, NOTE_E4, NOTE_F4, NOTE_A4, NOTE_D5, NOTE_F5, NOTE_G5, NOTE_B5, NOTE_C6, NOTE_D6, NOTE_E6, NOTE_F6
  };
  for (int i=0; i<15; i++) {
    tone(buzzerPin, randomNotes[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();
}

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