#include <SoftwareSerial.h>
#include <Servo.h>
#include <NewPing.h>
#include "pitches.h"
#include <avr/wdt.h>

//Create software serial object to communicate with HC-05
SoftwareSerial mySerial(3, 2); //HC-05 Tx & Rx is connected to Arduino #3 & #2

char Options=0;
#define TRIGGER_PIN  6  // Arduino pin tied to trigger pin on the ultrasonic sensor.
#define ECHO_PIN     7  // Arduino pin tied to echo pin on the ultrasonic sensor.
#define MAX_DISTANCE 20 // Maximum distance we want to ping for (in centimeters). Maximum sensor distance is rated at 400-500cm.
#define LED          8  // LED on pin 8
#define BUZZER_PIN   13 // Buzzer is on Pin 13
#define servoPin     11 // Servo on Pin 11 

//Motor Walking
const int motorPin1  = 9;  
const int motorPin2  = 10; 
//Motor Turning
const int motorPin3  = 4; 
const int motorPin4  = 12; 

long timeInMicro;
long distanceInCm;

int ledstate=LOW;
unsigned long currentMillis=0;
unsigned long previousMillis=0;
const unsigned long period = 500;

unsigned long preservo1=0;
int servo1state=HIGH;
const int on=5000;
const int off=1500;

// Creating Servo Object
Servo servo;
NewPing sonar(TRIGGER_PIN, ECHO_PIN, MAX_DISTANCE);

// Define a simple pattern for a robotic sound
int melody[] = {
  NOTE_C4, NOTE_E4, NOTE_G4, NOTE_C5, 
  NOTE_G4, NOTE_E4, NOTE_C4, 
  NOTE_C4, NOTE_E4, NOTE_G4, NOTE_C5, 
  NOTE_G4, NOTE_E4, NOTE_C4,
  REST, NOTE_D4, NOTE_F4, NOTE_A4, NOTE_D5, 
  NOTE_A4, NOTE_F4, NOTE_D4,
  NOTE_D4, NOTE_F4, NOTE_A4, NOTE_D5, 
  NOTE_A4, NOTE_F4, NOTE_D4
};

int durations[] = {
  8, 8, 8, 8, 
  8, 8, 8, 
  8, 8, 8, 8, 
  8, 8, 8, 
  16, 8, 8, 8, 8, 
  8, 8, 8, 
  8, 8, 8, 8, 
  8, 8, 8
};



void setup()
{
  wdt_disable();
  
  //Begin serial communication with Arduino and Arduino IDE (Serial Monitor)
  Serial.begin(9600);
  
  //Begin serial communication with Arduino and HC-05
  mySerial.begin(9600);

  Serial.println("Initializing...");
  Serial.println("The device started, now you can pair it with bluetooth!");

  pinMode(LED,OUTPUT);
  pinMode(motorPin1, OUTPUT);
  pinMode(motorPin2, OUTPUT);
  pinMode(motorPin3, OUTPUT);
  pinMode(motorPin4, OUTPUT);
  pinMode(ECHO_PIN,INPUT);
  pinMode(TRIGGER_PIN,OUTPUT);
  pinMode(BUZZER_PIN, OUTPUT);
  servo.write(83);
  servo.attach(servoPin);
}

void loop()
{
 if (mySerial.available()) {
  Options=mySerial.read();
  
  if (Options == 'F' || Options == 'B' || Options == 'L' || Options == 'R' || Options == 'H') {
      manual();
    }

  else if (Options == 'A') {
      automatic();
    }
  }
}

/********************************************* MANUAL MODE ********************************************/

void manual() {
  if (Options == 'F') {
    FORWARD();
    Serial.println("FORWARD");
  } 
  else if (Options == 'B') {
    //BACKWARD();
    Serial.println("BACKWARD");
  } 
  else if (Options == 'L') {
    LEFT();
    Serial.println("LEFT");
  } 
  else if (Options == 'R') {
    RIGHT();
    Serial.println("RIGHT");
  } 
  else if (Options == 'H') {
    STOP();
    Serial.println("STOP");
  }
}

/******************************************** AUTOMATIC MODE *******************************************/

void automatic() {

  while (true) {

     SERVO();
     blinkLED();

    if (Distance() <= 15 && Distance() != 0) {
      STOP();
      Serial.println("Obstacle detected, stopping!");
      delay(500);

      // Rotate left until a clear path is found
      while (true) {
        LEFT();
        
        int checkDistance = Distance();

        if (checkDistance > 15 || checkDistance == 0) {
          delay(1200);
          STOP();
          Serial.println("Clear path found, moving forward");
          break;
        }
      }
    } 
    else {
      FORWARD();
      Serial.println("Moving forward");
    }

    if(mySerial.read() == 'E'){
      break;
    }
  }
}

void RIGHT(){
    digitalWrite(motorPin1, LOW);
    digitalWrite(motorPin2, LOW);
    digitalWrite(motorPin3, HIGH);
    digitalWrite(motorPin4, LOW);
}

void BACKWARD(){
    digitalWrite(motorPin1, HIGH);
    digitalWrite(motorPin2, LOW);
    digitalWrite(motorPin3, LOW);
    digitalWrite(motorPin4, LOW);
}

void LEFT(){
    digitalWrite(motorPin1, LOW);
    digitalWrite(motorPin2, LOW);
    digitalWrite(motorPin3, LOW);
    digitalWrite(motorPin4, HIGH);
}

void FORWARD(){
    digitalWrite(motorPin1, LOW);
    digitalWrite(motorPin2, HIGH);
    digitalWrite(motorPin3, LOW);
    digitalWrite(motorPin4, LOW);
}

void STOP(){
    digitalWrite(motorPin1, LOW);
    digitalWrite(motorPin2, LOW);
    digitalWrite(motorPin3, LOW);
    digitalWrite(motorPin4, LOW);  
}

void SERVO(){
  if(servo1state==HIGH){
    if((millis()-preservo1) >= on){
      servo1state=LOW;
      preservo1=millis();
      servo.write(63);
      }
    }
    else{
      if((millis()-preservo1) >= off){
        preservo1=millis();
        servo1state=HIGH;
        servo.write(83);
      }
    }
  }

void blinkLED(){
  if(currentMillis - previousMillis >= period){
    previousMillis=currentMillis;

    if(ledstate == LOW){
      ledstate=HIGH;
    }
    else{
      ledstate=LOW;
    }

    digitalWrite(LED,ledstate);
  }
}

int Distance(){

  delay(50);                     // Wait 50ms between pings (about 20 pings/sec). 29ms should be the shortest delay between pings.
  distanceInCm=sonar.ping_cm();

  return distanceInCm;
}

void SONG(){
    int size = sizeof(durations) / sizeof(int);

  for (int note = 0; note < size; note++) {
    int duration = 1000 / durations[note];
    if (melody[note] == REST) {
      delay(duration);
    } else {
      tone(BUZZER_PIN, melody[note], duration);
    }

    int pauseBetweenNotes = duration * 1.30;
    delay(pauseBetweenNotes);

    noTone(BUZZER_PIN);
  }
  
  // Add a delay between each repetition of the sound pattern
  delay(1000);
}