#include <Servo.h>
Servo SonarServo;
const int pingPin = 25;
const int SafeDist = 20;              //cm

int ServoAngle1[] = {
  1200,1500,1750,1500};          //define angles in microseconds for object avoidance sweep
int ServoAngle2[] = {
  0,45,90,135,180,135,90,45,0};       //define angles for distance sweep 
int MyArray1[9] = {
  0};                                 //distance values saved into array

int mspeed;
int i;
int q;
#define DIR1 29 // motor 1
#define PwM1 12
#define DIR2 28 // motor 2   
#define PwM2 11
#define DIR3 27 // motor 3   
#define PwM3 10
#define DIR4 26 // motor 4
#define PwM4 9

long duration, inches, cm, m;

void setup()
{
  Serial.begin(9600);
  pinMode( DIR1, OUTPUT);
  pinMode( PwM1, OUTPUT);
  pinMode( DIR2, OUTPUT);
  pinMode( PwM2, OUTPUT);
  pinMode( DIR3, OUTPUT);
  pinMode( PwM3, OUTPUT);
  pinMode( DIR4, OUTPUT);
  pinMode( PwM4, OUTPUT);       //set all pins attatched to motor driver to output
  mspeed = 75;                 //motor speed
  SonarServo.attach(8);         
  delay(5000);
}

void loop(){                        //sends rover5 forward whilst performaing object avoidance sweep 

   Serial.print("motor forward");
  forward();
  

  for(int x = 0; x < 4; x++){                          
    SonarServo.writeMicroseconds(ServoAngle1[x]);    // write servo in microseconds through angles in array
    //Serial.print(ServoAngle1[x]);
    // Serial.print(" Degrees-");
    delay(250);                                      //allow servo to reach position
    SonarScan();                                     //scan for objects
    //Serial.print(cm);
    // Serial.print("cm   ");
    delay(200);                                      //wait a bit more
    if (cm < SafeDist && cm > 0){                    //min sensor range is 3cm however a bad reading of 0cm is sometimes recorded 
      Serial.println("stop!");
      Stop();
      back();
      delay(500);
      Stop();
      DistanceSweep();                              //scan and determine best direction to travel next
    }
  }
}

void DistanceSweep(){

  for(int a = 0; a < 8; a++){             
    SonarServo.write(ServoAngle2[a]);     //Write servo through values saved in array
    delay(300);                           //give servo enough time to reach position
    SonarScan();                          //scan for distance measurements
    delay(300);                           //wait long enough for sound to return before moving to next position
    MyArray1[a] = cm;                     //save reading to array
  }

  q = getIndexOfMaximumValue(MyArray1, 9);        //prints maximum distance and index in array
  Serial.print("max distance is at index --");
  Serial.print(q);
  Serial.print(" with value of - ");
  Serial.println(MyArray1[q]);
  Serial.println(" ");

  if (q == 0){                          //Still pretty rough!
    right();                             
    Serial.println("full right");
    delay(2000);
  }
  else if (q== 1){
    right();
    Serial.println("Partial right");
    delay(1000);
  }
  else if (q == 2){
    Serial.println("straight ahead");
    loop();
  }
  else if (q== 3){
    left();
    Serial.println("Partial left");
    delay(1000);
  }
  else if (q==4){
    left();
    Serial.println("Full left");
    delay(2000);
  }
  else if (q == 5){
    right();
    Serial.println("Full right");
    delay(2000);
  }
  else if (q== 6){
    right();
    Serial.println("Partial right");
    delay(1000);
  }
  else if (q == 7){
    loop();
    Serial.println("straight");
  }
  else if (q== 8){
    left();
    Serial.println("Partial left");
    delay(1000);
  }
}

void SonarScan()
{
  pinMode(pingPin, OUTPUT);
  digitalWrite(pingPin, LOW);
  delayMicroseconds(2);
  digitalWrite(pingPin, HIGH);
  delayMicroseconds(5);
  digitalWrite(pingPin, LOW);
  pinMode(pingPin, INPUT);
  duration = pulseIn(pingPin, HIGH);

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

int getIndexOfMaximumValue(int* array, int size){              //determines the maximum value of an array and its index value 
  int maxIndex = 0;
  int max = array[maxIndex];
  for (int i=1; i<size; i++){
    if (max<array[i]){
      max = array[i];
      maxIndex = i;
    }
  }
  return maxIndex;
} 

void forward(){
  digitalWrite(DIR1, LOW);        //back left 
  analogWrite(PwM1, mspeed);          //back left
  digitalWrite(DIR3, LOW);        //front left
  analogWrite(PwM3, mspeed);          //front left
  digitalWrite(DIR2, LOW);      //BR
  analogWrite(PwM2, mspeed);         //BR
  digitalWrite(DIR4, HIGH);
  analogWrite(PwM4, mspeed);
}

void back(){
  digitalWrite(DIR1, HIGH);       //BL
  analogWrite(PwM1, mspeed);        //BL
  digitalWrite(DIR3, HIGH);       //FL
  analogWrite(PwM3, mspeed);        //FL
  digitalWrite(DIR2, HIGH);      //BR
  analogWrite(PwM2,mspeed);        //BR
  digitalWrite(DIR4, LOW);
  analogWrite(PwM4, mspeed);
}

void left(){
  digitalWrite(DIR1, LOW);      //BL
  analogWrite(PwM1, 100);        //BL
  digitalWrite(DIR3, LOW);      //FL
  analogWrite(PwM3, 100);        //FL
  digitalWrite(DIR2, HIGH);     //BR
  analogWrite(PwM2,100);        //BR
  digitalWrite(DIR4, LOW);      //FR    For some reason the wires for this motor came reversed 
  analogWrite(PwM4, 100);       //FR
}

void right(){
  digitalWrite(DIR1, HIGH);       //BL
  analogWrite(PwM1, 100);        //BL
  digitalWrite(DIR3, HIGH);       //FL
  analogWrite(PwM3, 100);        //FL
  digitalWrite(DIR2, LOW);       //BR
  analogWrite(PwM2,100);         //BR
  digitalWrite(DIR4, HIGH);      //FR   For some reason the wires for this motor came reversed 
  analogWrite(PwM4, 100);        //FR
}
void Stop(){
  analogWrite(PwM1, 0);        
  analogWrite(PwM3, 0);        

  analogWrite(PwM2,0);        
  analogWrite(PwM4, 0);
}