// all the defines below are just defining notes, these notes come from a song that is copied from http://pasted.co/e525c1b2
#define  C0 16.35
#define Db0 17.32
#define D0  18.35
#define Eb0 19.45
#define E0  20.60
#define F0  21.83
#define Gb0 23.12
#define G0  24.50
#define Ab0 25.96
#define LA0 27.50
#define Bb0 29.14
#define B0  30.87
#define C1  32.70
#define Db1 34.65
#define D1  36.71
#define Eb1 38.89
#define E1  41.20
#define F1  43.65
#define Gb1 46.25
#define G1  49.00
#define Ab1 51.91
#define LA1 55.00
#define Bb1 58.27
#define B1  61.74
#define C2  65.41
#define Db2 69.30
#define D2  73.42
#define Eb2 77.78
#define E2  82.41
#define F2  87.31
#define Gb2 92.50
#define G2  98.00
#define Ab2 103.83
#define LA2 110.00
#define Bb2 116.54
#define B2  123.47
#define C3  130.81
#define Db3 138.59
#define D3  146.83
#define Eb3 155.56
#define E3  164.81
#define F3  174.61
#define Gb3 185.00
#define G3  196.00
#define Ab3 207.65
#define LA3 220.00
#define Bb3 233.08
#define B3  246.94
#define C4  261.63
#define Db4 277.18
#define D4  293.66
#define Eb4 311.13
#define E4  329.63
#define F4  349.23
#define Gb4 369.99
#define G4  392.00
#define Ab4 415.30
#define LA4 440.00
#define Bb4 466.16
#define B4  493.88
#define C5  523.25
#define Db5 554.37
#define D5  587.33
#define Eb5 622.25
#define E5  659.26
#define F5  698.46
#define Gb5 739.99
#define G5  783.99
#define Ab5 830.61
#define LA5 880.00
#define Bb5 932.33
#define B5  987.77
#define C6  1046.50
#define Db6 1108.73
#define D6  1174.66
#define Eb6 1244.51
#define E6  1318.51
#define F6  1396.91
#define Gb6 1479.98
#define G6  1567.98
#define Ab6 1661.22
#define LA6 1760.00
#define Bb6 1864.66
#define B6  1975.53
#define C7  2093.00
#define Db7 2217.46
#define D7  2349.32
#define Eb7 2489.02
#define E7  2637.02
#define F7  2793.83
#define Gb7 2959.96
#define G7  3135.96
#define Ab7 3322.44
#define LA7 3520.01
#define Bb7 3729.31
#define B7  3951.07
#define C8  4186.01
#define Db8 4434.92
#define D8  4698.64
#define Eb8 4978.03

// These are how long notes stay on
#define BPM 240   //  you can change this value changing all the others
#define H 2*Q //half 2/4
#define Q 60000/BPM //quarter 1/4
#define E Q/2   //eighth 1/8
#define S Q/4 // sixteenth 1/16
#define W 4*Q // whole 4/4


#include <Servo.h> // Include servo library
Servo ServoLeft; // Declares left and right servos objects
Servo ServoRight;

// define all pins used later

const int leftsensor = 11;
const int rightsensor = 10;

const int leftled = 5;

const int rightled = 7;

const int buzz = 8;

const int button = 9;

bool hasstopped = false;
// for ultrasonic sensor
//int timesturned = 0;
// for ultrasonic sensor
//int trigPin = 3;
// for ultrasonic sensor
//int echoPin = 4;
//second button for turning
const int button2 = 2;

void setup() {

  // initialize all pins used
  pinMode(leftsensor, INPUT);
  pinMode(rightsensor, INPUT);
  pinMode(leftled, OUTPUT);
  pinMode(rightled, OUTPUT);
  pinMode(buzz, OUTPUT);
  pinMode(button, INPUT_PULLUP);
  pinMode(button2, INPUT_PULLUP);
  //used for debugging not for when robot is running
  Serial.begin(9600);

}

void loop() {
  //Start the loop when he button is pressed
  if (digitalRead(button) == LOW) {
    while (true) {

      ServoLeft.attach(12); // Attach servo signal to pin 12 from servoleft object
      ServoRight.attach(13);// Attach servo signal to pin 13 from servoright object

      int leftdetect = digitalRead(leftsensor);
      int rightdetect = digitalRead(rightsensor);

      if (leftdetect == 0 && rightdetect == 0) {
        ServoLeft.writeMicroseconds(1600);
        ServoRight.writeMicroseconds(1350);
        straightleds();

      }

      if (leftdetect == 1 && rightdetect == 0) {
        ServoLeft.writeMicroseconds(1700);
        ServoRight.writeMicroseconds(1600);
        leftledonly();

      }

      if (leftdetect == 0 && rightdetect == 1) {
        ServoLeft.writeMicroseconds(1300);
        ServoRight.writeMicroseconds(1400);
        rightledonly();

      }


      if (leftdetect == 1 && rightdetect == 1) {
        ServoLeft.writeMicroseconds(1650);
        ServoRight.writeMicroseconds(1350);
        straightleds();


      }

      int phototransistorvalue = rcTime(6);

      if (phototransistorvalue < 500 && hasstopped == false) {
        left(600);
        forward(1400);
        right(400);
        forward(1150);
        left(400);
        ServoRight.detach();
        ServoLeft.detach();
        straightleds();
        // play song, the song is copied from http://pasted.co/e525c1b2

        song();


        ServoLeft.attach(12);
        ServoRight.attach(13);

        forward(400);

        ServoLeft.detach();
        ServoRight.detach();
        ServoLeft.attach(12);
        ServoRight.attach(13);

        hasstopped = true;
      }
      phototransistorvalue = rcTime(6);
      // take readings from phototransitor by charging and discharging capacitor, since the hasstopped boolean is true, shining light will not stop robot and will instead cause robot to turn
      if (phototransistorvalue < 500 && hasstopped == true) {
        while (phototransistorvalue < 500 && hasstopped == true) {
          phototransistorvalue = rcTime(6);
          ServoLeft.writeMicroseconds(1400);
          ServoRight.writeMicroseconds(1400);
          delay(10);
          if (digitalRead(button) == LOW) {

            break;
          }
        }

        ServoLeft.writeMicroseconds(1500);
        ServoRight.writeMicroseconds(1500);
      }

      if (digitalRead(button2) == LOW) {
        while (digitalRead(button2) == LOW) {

          ServoLeft.writeMicroseconds(1400);
          ServoRight.writeMicroseconds(1400);
          delay(10);

          if (digitalRead(button) == LOW) {
            exit;

          }

        }

        ServoLeft.writeMicroseconds(1500);
        ServoRight.writeMicroseconds(1500);
      }

    }
  }
}


long rcTime(int pin) { // rcTime measures decay at pin, this is used to take readings from the phototransistor using a capacitor, for more sensitivity to light, for more accuracy, the phototransistor isn't triggered by leds
  // this was taken from BOE-booklet
  pinMode(pin, OUTPUT); // Charge capacitor
  digitalWrite(pin, HIGH); // ..by setting pin ouput-high
  delay(5); // ..for 5 ms
  pinMode(pin, INPUT); // Set pin to input
  digitalWrite(pin, LOW); // ..with no pullup
  long time = micros(); // Mark the time
  while (digitalRead(pin)); // Wait for voltage < threshold
  time = micros() - time; // Calculate decay time
  return time; // Returns decay time
}

void forward(int timeinms) {

  ServoLeft.writeMicroseconds(1700);
  ServoRight.writeMicroseconds(1300);
  delay (timeinms);
  straightleds();
}

void left(int timeinms) {

  ServoLeft.writeMicroseconds(1300);
  ServoRight.writeMicroseconds(1300);
  delay (timeinms);
  leftledonly();
}
void right(int timeinms) {

  ServoLeft.writeMicroseconds(1700);
  ServoRight.writeMicroseconds(1700);
  delay(timeinms);
  rightledonly();
}
void straightleds() {

  digitalWrite(leftled, HIGH);
  digitalWrite(rightled, HIGH);
}
void leftledonly() {

  digitalWrite(leftled, HIGH);
  digitalWrite(rightled, LOW);
}
void rightledonly() {

  digitalWrite(leftled, LOW);
  digitalWrite(rightled, HIGH);
}
tones
void hightone() {
  tone(8, Eb8);
  delay(10);
  noTone(8);
}

void lowtone() {
  tone(8, C0);
  delay(10);
  noTone(8);
}
void song(){
  tone(8, LA3, Q);
        delay(1 + Q); //minimum delay of 1ms required to separate tones
        tone(8, LA3, Q);


        delay(1 + Q);
        tone(8, LA3, Q);
        delay(1 + Q);
        tone(8, F3, E + S);
        delay(1 + E + S);
        tone(8, C4, S);
        delay(1 + S);

        tone(8, LA3, Q);
        delay(1 + Q);
        tone(8, F3, E + S);
        delay(1 + E + S);
        tone(8, C4, S);
        delay(1 + S);
        tone(8, LA3, H);
        delay(1 + H);

        tone(8, E4, Q);
        delay(1 + Q);
        tone(8, E4, Q);
        delay(1 + Q);
        tone(8, E4, Q);
        delay(1 + Q);
        tone(8, F4, E + S);
        delay(1 + E + S);
        tone(8, C4, S);
        delay(1 + S);

        tone(8, Ab3, Q);
        delay(1 + Q);
        tone(8, F3, E + S);
        delay(1 + E + S);
        tone(8, C4, S);
        delay(1 + S);
        tone(8, LA3, H);
        delay(1 + H);

        tone(8, LA4, Q);
        delay(1 + Q);
        tone(8, LA3, E + S);
        delay(1 + E + S);
        tone(8, LA3, S);
        delay(1 + S);
        tone(8, LA4, Q);
        delay(1 + Q);
        tone(8, Ab4, E + S);
        delay(1 + E + S);
        tone(8, G4, S);
        delay(1 + S);

        tone(8, Gb4, S);
        delay(1 + S);
        tone(8, E4, S);
        delay(1 + S);
        tone(8, F4, E);
        delay(1 + E);
        delay(1 + E); //PAUSE
        tone(8, Bb3, E);
        delay(1 + E);
        tone(8, Eb4, Q);
        delay(1 + Q);
        tone(8, D4, E + S);
        delay(1 + E + S);
        tone(8, Db4, S);
        delay(1 + S);

        tone(8, C4, S);
        delay(1 + S);
        tone(8, B3, S);
        delay(1 + S);
        tone(8, C4, E);
        delay(1 + E);
        delay(1 + E); //PAUSE QUASI FINE RIGA
        tone(8, F3, E);
        delay(1 + E);
        tone(8, Ab3, Q);
        delay(1 + Q);
        tone(8, F3, E + S);
        delay(1 + E + S);
        tone(8, LA3, S);
        delay(1 + S);

        tone(8, C4, Q);
        delay(1 + Q);
        tone(8, LA3, E + S);
        delay(1 + E + S);
        tone(8, C4, S);
        delay(1 + S);
        tone(8, E4, H);
        delay(1 + H);

        tone(8, LA4, Q);
        delay(1 + Q);
        tone(8, LA3, E + S);
        delay(1 + E + S);
        tone(8, LA3, S);
        delay(1 + S);
        tone(8, LA4, Q);
        delay(1 + Q);
        tone(8, Ab4, E + S);
        delay(1 + E + S);
        tone(8, G4, S);
        delay(1 + S);

        tone(8, Gb4, S);
        delay(1 + S);
        tone(8, E4, S);
        delay(1 + S);
        tone(8, F4, E);
        delay(1 + E);
        delay(1 + E); //PAUSE
        tone(8, Bb3, E);
        delay(1 + E);
        tone(8, Eb4, Q);
        delay(1 + Q);
        tone(8, D4, E + S);
        delay(1 + E + S);
        tone(8, Db4, S);
        delay(1 + S);

        tone(8, C4, S);
        delay(1 + S);
        tone(8, B3, S);
        delay(1 + S);
        tone(8, C4, E);
        delay(1 + E);
        delay(1 + E); //PAUSE QUASI FINE RIGA
        tone(8, F3, E);
        delay(1 + E);
        tone(8, Ab3, Q);
        delay(1 + Q);
        tone(8, F3, E + S);
        delay(1 + E + S);
        tone(8, C4, S);
        delay(1 + S);

        tone(8, LA3, Q);
        delay(1 + Q);
        tone(8, F3, E + S);
        delay(1 + E + S);
        tone(8, C4, S);
        delay(1 + S);
        tone(8, LA3, H);
        delay(1 + H);

        delay(2 * H);
  }

/*All code below was for the sonic distance sensor and was intended to be used for going around the pylon
but the sensor stopped functioning properly the day before the exam and began giving false readings, we then decided
to just use the photoresistor for the stop and the turn
*/


void doStuff()
{
  //Configure pins and create a variable
  long duration, inches, cm;
  pinMode(trigPin, OUTPUT);
  //Send out a pulse (soundwave) and measure in microseconds how long it takes to return
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);
  pinMode(echoPin, INPUT);
  duration = pulseIn(echoPin, HIGH);
  //convert microseconds to inches and centimeters
  inches = microsecondsToInches(duration);
  cm = microsecondsToCentimeters(duration);
//Print out distance to object in inches and centimeters in the serial monitor
  Serial.print(inches);
  Serial.print("in, ");
  Serial.print(cm);
  Serial.print("cm");
  Serial.println();
//If the object is closer than 10cm, go forward and make a 90 degree turn, turn a maximum of 5 times
  if (cm <= 10 && timesturned <= 4) {
    left(600);
    forward(1000);

    timesturned ++;
  }
  else {

  }
}
//converting microseconds to centimeters and inches using math
long microsecondsToInches(long microseconds)
{
  return microseconds / 74 / 2;
}

long microsecondsToCentimeters(long microseconds)
{
  return microseconds / 29 / 2;
}