//this program moves the servo to the right or left depending on if one of the
//ultrasonic sensors is measuring within a minimum tolerance
//the servo is meant to turn the motor to move away from a wall on one side
//to keep the bot from hitting walls on the right or left
#include <Servo.h>
#include "pitches.h"

//define I/O pins
#define echoR 7
#define trigR 6
#define echoL 5
#define trigL 4
#define servoPin 9
#define button 12
#define speaker 3

//polling rate
#define DELAY 10
//tolerance before turning is 5cm
#define TURN_TOL 5
//the servo turns 3 degrees per poll, if needed
#define TURN_AMT 3


long duration; // duration of sound wave travel for ultrasonic sensors
int distance; // distance measurement for ultrasonic sensors
int servo_pos = 90; //current position of the servo
bool mode = false; //whether buzzer should be playing
float lastTime = millis(); //last time that was polled. used for interrupting the song.
Servo servo; //the servo controlling the position of the motor

void tokyo_drift() {
  delay(100); // delay so button press can be finished
  // notes in the melody:
  int melody[] = {
    NOTE_AS4, NOTE_AS4, NOTE_AS4, NOTE_AS4, NOTE_AS4, NOTE_B4, NOTE_DS5, NOTE_AS4, NOTE_AS4, NOTE_AS4, NOTE_B4, NOTE_DS5,
  };
  // note durations: 4 = quarter note, 8 = eighth note, etc.:
  int noteDurations[] = {
    4, 4, 4, 4, 4, 4, 8, 4, 4, 4, 4, 8
  };
  int pauses[] = {
    500, 500, 500, 500, 333, 333, 333, 500, 500, 333, 333, 333
  };
  // iterate over the notes of the melody:
  for (int thisNote = 0; thisNote < 8; thisNote++) {
    // to calculate the note duration, take one second divided by the note type.
    //e.g. quarter note = 1000 / 4, eighth note = 1000/8, etc.
    int noteDuration = 1000 / noteDurations[thisNote];
    tone(speaker, melody[thisNote], noteDuration);
    // to distinguish the notes, set a minimum time between them.
    lastTime = millis();
    while ((millis() - lastTime) < pauses[thisNote]) {
      if (digitalRead(button) == LOW){//if button is pressed to stop music, stop immediately
        mode = false;
        noTone(speaker);
        return;
      }
      delay(10);
    }
    // stop the tone playing:
    noTone(speaker);
  }
}

void setup()
{
  //set output/input pins for each HC-SR04
  pinMode(echoL, INPUT);
  pinMode(echoR, INPUT);
  pinMode(trigL, OUTPUT);
  pinMode(trigR, OUTPUT);
  //set input button
  pinMode(button, INPUT_PULLUP);
  //begin serial communication
  Serial.begin(9600);

  //initialize the servo
  servo.attach(servoPin);
  servo.write(servo_pos);
}

//this method returns the distance of the ultrasonic sensor with
// trigger on pin [trigPin] and echo on pin [echoPin]
int poll_sensor(int trigPin, int echoPin) {
  // Clears the trigPin condition
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);
  // Sets the trigPin HIGH (ACTIVE) for 10 microseconds
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);
  // Reads the echoPin, returns the sound wave travel time in microseconds
  duration = pulseIn(echoPin, HIGH);
  // Calculating the distance
  distance = duration * 0.034 / 2; // Speed of sound wave divided by 2 (go and back)
  return distance;
}

//turn the servo left by TURN_AMT
void leftTurn() {
  if (servo_pos < TURN_AMT) {
    servo_pos = 0;
  }
  else {
    servo_pos -= TURN_AMT;
  }
}

//turn the servo right by TURN_AMT
void rightTurn() {
  if (servo_pos > 180 - TURN_AMT) {
    servo_pos = 180;
  }
  else {
    servo_pos += TURN_AMT;
  }
}

void loop() {
  //find distances from both ultrasonic sensors
  if (digitalRead(button) == LOW) {
    mode = true;
  }
  if (mode) {//if music should be playing, play music
    tokyo_drift();
  }
  int dL = poll_sensor(trigL, echoL);//distance on the left side
  int dR = poll_sensor(trigR, echoR);//distance on the right side
  //if the left side is reading closer
  if (dL < dR) {
    //if the left side is smaller than the turn tolerance
    if (dL < TURN_TOL) {
      //turn right
      rightTurn();
    }
  }
  //if the right side is reading closer
  if (dR < dL) {
    //if the right side is smaller than the turn tolerance
    if (dR < TURN_TOL) {
      //turn left
      leftTurn();
    }
  }
  //update the servo position
  servo.write(servo_pos);
  delay(DELAY);
}