/*************************************************
 * Public Constants
 *************************************************/

#define NOTE_B0  31
#define NOTE_C1  33
#define NOTE_CS1 35
#define NOTE_D1  37
#define NOTE_DS1 39
#define NOTE_E1  41
#define NOTE_F1  44
#define NOTE_FS1 46
#define NOTE_G1  49
#define NOTE_GS1 52
#define NOTE_A1  55
#define NOTE_AS1 58
#define NOTE_B1  62
#define NOTE_C2  65
#define NOTE_CS2 69
#define NOTE_D2  73
#define NOTE_DS2 78
#define NOTE_E2  82
#define NOTE_F2  87
#define NOTE_FS2 93
#define NOTE_G2  98
#define NOTE_GS2 104
#define NOTE_A2  110
#define NOTE_AS2 117
#define NOTE_B2  123
#define NOTE_C3  131
#define NOTE_CS3 139
#define NOTE_D3  147
#define NOTE_DS3 156
#define NOTE_E3  165
#define NOTE_F3  175
#define NOTE_FS3 185
#define NOTE_G3  196
#define NOTE_GS3 208
#define NOTE_A3  220
#define NOTE_AS3 233
#define NOTE_B3  247
#define NOTE_C4  262
#define NOTE_CS4 277
#define NOTE_D4  294
#define NOTE_DS4 311
#define NOTE_E4  330
#define NOTE_F4  349
#define NOTE_FS4 370
#define NOTE_G4  392
#define NOTE_GS4 415
#define NOTE_A4  440
#define NOTE_AS4 466
#define NOTE_B4  494
#define NOTE_C5  523
#define NOTE_CS5 554
#define NOTE_D5  587
#define NOTE_DS5 622
#define NOTE_E5  659
#define NOTE_F5  698
#define NOTE_FS5 740
#define NOTE_G5  784
#define NOTE_GS5 831
#define NOTE_A5  880
#define NOTE_AS5 932
#define NOTE_B5  988
#define NOTE_C6  1047
#define NOTE_CS6 1109
#define NOTE_D6  1175
#define NOTE_DS6 1245
#define NOTE_E6  1319
#define NOTE_F6  1397
#define NOTE_FS6 1480
#define NOTE_G6  1568
#define NOTE_GS6 1661
#define NOTE_A6  1760
#define NOTE_AS6 1865
#define NOTE_B6  1976
#define NOTE_C7  2093
#define NOTE_CS7 2217
#define NOTE_D7  2349
#define NOTE_DS7 2489
#define NOTE_E7  2637
#define NOTE_F7  2794
#define NOTE_FS7 2960
#define NOTE_G7  3136
#define NOTE_GS7 3322
#define NOTE_A7  3520
#define NOTE_AS7 3729
#define NOTE_B7  3951
#define NOTE_C8  4186
#define NOTE_CS8 4435
#define NOTE_D8  4699
#define NOTE_DS8 4978

#include <AFMotor.h> // Enables the Motor library
#include <Servo.h> // Enables the Servo library
//#include "pitches.h"

// notes in the melody:
int melody[] = {
  NOTE_C4, NOTE_G3, NOTE_G3, NOTE_A3, NOTE_G3, 0, NOTE_B3, NOTE_C4
};

// note durations: 4 = quarter note, 8 = eighth note, etc.:
int noteDurations[] = {
  4, 8, 8, 4, 4, 4, 4, 4
};
long randNumber;
//Servo PingServo;
AF_DCMotor motor1(1); // Motor 1 is connected to the port 1 on the motor shield
AF_DCMotor motor2(2); // Motor 2 is connected to the port 2 on the motor shield
AF_DCMotor motor3(3); // Motor 3 is connected to the port 1 on the motor shield
AF_DCMotor motor4(4); // Motor 4 is connected to the port 2 on the motor shield
int minSafeDist = 15 ; // Minimum distance for ping sensor to know when to turn
//int pingPin = 30; // Ping sensor is connected to port A0
int trigPin = 30;
int echoPin = 31;
float pingTime;
float targetDistance;
float speedOfSound = 776.5;
int centerDist, leftDist, rightDist, backDist; // Define variables center, left, right and back distance
long duration, inches, cm; // Define variables for Ping sensor

int trigPinL = 34;
int echoPinL = 35;
float pingTimeL;
float targetDistanceL;
long inchesL;

int beepPin = 36;

void beep(unsigned char delayms){
  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(beepPin, melody[thisNote], noteDuration);

    // to distinguish the notes, set a minimum time between them.
    // the note's duration + 30% seems to work well:
    int pauseBetweenNotes = noteDuration * 1.30;
    delay(pauseBetweenNotes);
    // stop the tone playing:
    noTone(beepPin);
  }
}  


void setup() {
//PingServo.attach(10); // Servo is attached to pin 10 in the motor shield
//PingServo.write(90); // Center the Ping sensor (puts it at 90 degrees)
pinMode(trigPin, OUTPUT);
pinMode(echoPin, INPUT);
pinMode(beepPin, OUTPUT);
motor1.setSpeed(200); // Sets the speed of the first motor (At 0, the motors are turned off. 255 is the fastest setting that you are able to use, I used 215 to not push the motors too hard.)
motor2.setSpeed(200); // Sets the speed of the second motor (At 0, the motors are turned off. 255 is the fastest setting that you are able to use, I used 215 to not push the motors too hard.)
motor3.setSpeed(200); // Sets the speed of the first motor (At 0, the motors are turned off. 255 is the fastest setting that you are able to use, I used 215 to not push the motors too hard.)
motor4.setSpeed(200);
Serial.begin(9600); // Enables Serial monitor for debugging purposes
Serial.println("Serial test!"); // Test the Serial communication

}

void AllStop() {
motor1.run(RELEASE); // Turns off motor 1
motor2.run(RELEASE); // Turns off motor 2
motor3.run(RELEASE); // Turns off motor 3
motor4.run(RELEASE);
}
void AllForward() { // Makes the robot go forward
motor1.run(FORWARD); // Motor 1 goes forward
motor2.run(FORWARD); // Motor 2 goes forward
//delay(100);
//if(inches >= minSafeDist && inchesL >= minSafeDist){
  motor3.run(FORWARD); // Motor 3 goes forward
  motor4.run(FORWARD); // Motor 4 goes forward
//}
Serial.println("Going forward"); // Prints a line in the serial monitor
}
void turnRight() { // Makes the robot go right
motor2.run(BACKWARD); // Turns off motor 2
motor1.run(FORWARD); // Motor 1 goes forward
delay(1600); // Time required to turn right (1.6 seconds)
Serial.println("Motors going Right"); // Prints a line in the serial monitor
}
void GoBack(){ // Makes the robot go back
motor2.run(BACKWARD); // Motor 2 goes back
motor1.run(BACKWARD); // Motor 1 goes back
delay(1600); // Time Required to go back (1.6 seconds)
Serial.println("Backward"); // Prints a line in the serial monitor
}
void turnLeft() { // Makes the robot go Left
motor2.run(FORWARD); // Motor 2 goes forward
motor1.run(BACKWARD); // turns off motor 1
delay(1600); //Time Required to turn left (1.6)Seconds
Serial.println("Motors going Left");// Prints a line in the serial monitor
}

// Starts the loop to decide what to do
void loop()
{
LookAhead();
Serial.print(inches);
Serial.println(" inches"); // Prints a line in the serial monitor
Serial.println(inches);
if(inches >= minSafeDist || inchesL >= minSafeDist) /* If the inches in front of an object is greater than or equal to the minimum safe distance (11 inches), react*/
{
AllForward(); // All wheels forward
//delay(110); // Wait 0.11 seconds
}else if (inches < minSafeDist) // If not:
{
  Serial.print("Turn right");
  beep(200);
  AllStop();
  //randNumber = random(10, 20);
  //if(randNumber < 10.) {
  //  turnLeft();
  //  }
  //else {
    turnRight();
  //}
  //turnLeft();
} /*else if (inchesL < minSafeDist) {
  Serial.print("Turn right");
  AllStop();
  turnRight();
}*/
/*{
AllStop(); // Stop all motors
LookAround(); // Check your surroundings for best route
if(rightDist > leftDist) // If the right distance is greater than the left distance , turn right
{
turnRight();
}else if (leftDist > rightDist) // If the left distance is greater than the right distance , turn left
{
turnLeft();
}else if (leftDist&&rightDist<minSafeDist) // If the left and right distance is smaller than the min safe distance (11 inch) go back
{
GoBack();
}
}*/
}

unsigned long ping() {
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2000);
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(15);
  digitalWrite(trigPin, LOW);
  pingTime = pulseIn(echoPin, HIGH);
  pingTime = pingTime/1000000.;
  pingTime = pingTime/3600.;
  targetDistance = speedOfSound * pingTime;
  targetDistance = targetDistance/2;
  inches = targetDistance*63360; //Convert target distance to inches from mile;
  Serial.print("targetDistance");
}

unsigned long pingL() {
  digitalWrite(trigPinL, LOW);
  delayMicroseconds(2000);
  digitalWrite(trigPinL, HIGH);
  delayMicroseconds(15);
  digitalWrite(trigPinL, LOW);
  pingTimeL = pulseIn(echoPinL, HIGH);
  pingTimeL = pingTimeL/1000000.;
  pingTimeL = pingTimeL/3600.;
  targetDistanceL = speedOfSound * pingTimeL;
  targetDistanceL = targetDistanceL/2;
  inchesL = targetDistanceL*63360; //Convert target distance to inches from mile;
  Serial.print("targetDistanceL");
  Serial.print(inchesL);
}

long microsecondsToInches(long microseconds) // converts time to a distance
{
return microseconds / 74 / 2;
}
long microsecondsToCentimeters(long microseconds) // converts time to a distance
{
return microseconds / 29 / 2;
}

void LookAhead() {
//PingServo.write(90);// angle to look forward
delay(175); // wait 0.175 seconds
ping();
//if(inches < minSafeDist) {AllStop();}
//pingL();
//if(inchesL < minSafeDist) {AllStop();}
}

void LookAround(){
//PingServo.write(180); // 180° angle
//delay(320); // wait 0.32 seconds
ping();
pingL();
//rightDist = inches; //get the right distance
//PingServo.write(0); // look to the other side
//delay(620); // wait 0.62 seconds
//ping();
//leftDist = inches; // get the left distance
//PingServo.write(90); // 90° angle
//delay(275); // wait 0.275 seconds

// Prints a line in the serial monitor
//Serial.print("RightDist: ");
//Serial.println(rightDist);
//Serial.print("LeftDist: ");
//Serial.println(leftDist);
//Serial.print("CenterDist: ");
//Serial.println(centerDist);
}