#include <NewPing.h>
#include <Servo.h>

#define MAX_DISTANCE 200
#define TRIG_PIN  4
#define ECHO_PIN  2

#define LEFT 0
#define CENTER 90
#define RIGHT 180
#define ENABLE1 3
#define INPUT1 9
#define INPUT2 8
#define ENABLE2 11
#define INPUT3 7
#define INPUT4 6

NewPing sonar(TRIG_PIN, ECHO_PIN, MAX_DISTANCE);
Servo ultrasonicServo;

float dangerThreshold = 40.0;

int currentPos = 0;

#define LEFT_FORWARD_RIGHT_FORWARD {HIGH, HIGH, LOW, HIGH, HIGH, LOW}
#define LEFT_OFF_RIGHT_FORWARD {LOW, HIGH, LOW, LOW, HIGH, LOW}
#define LEFT_FORWARD_RIGHT_OFF {HIGH, LOW, LOW, HIGH, LOW, LOW}
#define LEFT_REVERSE_RIGHT_FORWARD {HIGH, HIGH, HIGH, LOW, HIGH, LOW}
#define LEFT_FORWARD_RIGHT_REVERSE {HIGH, HIGH, LOW, HIGH, LOW, HIGH}
#define LEFT_REVERSE_RIGHT_REVERSE {HIGH, HIGH, HIGH, LOW, LOW, HIGH}
#define LEFT_OFF_RIGHT_OFF {HIGH, HIGH, LOW, LOW, LOW, LOW}
#define LEFT_FREEWHEEL_RIGHT_FREEWHEEL {LOW, LOW, LOW, LOW, LOW, LOW}

#define FORWARD LEFT_FORWARD_RIGHT_FORWARD
#define REVERSE LEFT_REVERSE_RIGHT_REVERSE
#define LEFT_TURN LEFT_OFF_RIGHT_FORWARD
#define RIGHT_TURN LEFT_FORWARD_RIGHT_OFF
#define ROTATE_LEFT LEFT_REVERSE_RIGHT_FORWARD
#define ROTATE_RIGHT LEFT_FORWARD_RIGHT_REVERSE
#define BRAKE LEFT_OFF_RIGHT_OFF
#define FREEWHEEL LEFT_FREEWHEEL_RIGHT_FREEWHEEL

#define SLOW 130;
#define MEDIUM 195;
#define FAST 255;

int throttle = MEDIUM;

void setup() {

pinMode(ENABLE1, OUTPUT);
pinMode(ENABLE2, OUTPUT);
pinMode(INPUT1, OUTPUT);
pinMode(INPUT2, OUTPUT);
pinMode(INPUT3, OUTPUT);
pinMode(INPUT4, OUTPUT);

ultrasonicServo.attach(5);

servo_position(CENTER);
}

void loop() {

float distanceForward = ping();

if (distanceForward > dangerThreshold)
{
drive_forward();
}
else
{
brake();

servo_position(LEFT);
float distanceLeft = ping();

servo_position(RIGHT);
float distanceRight = ping();

if (distanceLeft > distanceRight && distanceLeft > dangerThreshold)       //if left is less obstructed
{

rotate_left();
}
else if (distanceRight > distanceLeft && distanceRight > dangerThreshold) //if right is less obstructed
{

rotate_right();
}
else
{

u_turn();
}

servo_position(CENTER);
}
}

void freewheel(){
const int driveControl[] = FREEWHEEL;
drive(driveControl);
delay(25);

void brake(){
const int driveControl[] = BRAKE;
drive(driveControl);
delay(25);
}

void drive_forward(){
const int driveControl[] = FORWARD;
drive(driveControl);
}

void drive_backward(){
const int driveControl[] = REVERSE;
drive(driveControl);
}

void turn_left(){
const int driveControl[] = LEFT_TURN;
drive(driveControl);
delay(600);

}

void turn_right(){
const int driveControl[] = RIGHT_TURN;
drive(driveControl);
delay(600);

}

void rotate_left(){
const int driveControl[] = ROTATE_LEFT;
drive(driveControl);
delay(300);
}
void rotate_right(){
const int driveControl[] = ROTATE_RIGHT;
drive(driveControl);
delay(300);
}

void u_turn(){
const int driveControl[] = ROTATE_RIGHT;
drive(driveControl);
delay(600);
}

void drive(const int settings[6]){

if (settings[0] == HIGH)
analogWrite(ENABLE1, throttle);
else
digitalWrite(ENABLE1, LOW);

if (settings[1] == HIGH)
analogWrite(ENABLE2, throttle);
else
digitalWrite(ENABLE2, LOW);

digitalWrite(INPUT1, settings[2]);
digitalWrite(INPUT2, settings[3]);
digitalWrite(INPUT3, settings[4]);
digitalWrite(INPUT4, settings[5]);
}

void servo_position(int newPos){

if (newPos > currentPos){
for(int pos=currentPos; pos < newPos; pos += 1)
{
ultrasonicServo.write(pos);
delay(15);
}
currentPos = newPos;
}
else if (newPos < currentPos){
for(int pos=currentPos; pos > newPos; pos -= 1)
{
ultrasonicServo.write(pos);
delay(15);
}
currentPos = newPos;
}
}

float ping(){
delay(50);

unsigned int seconds = sonar.ping();

if (seconds == 0)
return MAX_DISTANCE;
else
return seconds / US_ROUNDTRIP_CM;
}