#include <Adafruit_MotorShield.h>
#include <Servo.h>
#include <Wire.h>

// Pin constants for the Ultrasonic sensor.
const int pingPinOut = 8;
const int pingPinIn = 8;
// Constant Definitions
const unsigned slowSpeed = 400;
const unsigned fastSpeed = 800;
const unsigned openSpace = 7;
const unsigned turnTime = 1000;

Adafruit_MotorShield AFMS = Adafruit_MotorShield();
// Declares the left motor on the Adafruit M3 and right motor on M4.
// Declares the Ultrasonic sensor as "myLooker".
Adafruit_DCMotor *lMotor = AFMS.getMotor(2);
Adafruit_DCMotor *rMotor = AFMS.getMotor(3);
Servo myLooker;


void setup()
{
	Serial.begin(9600);
	// Declares the servo motor that the ultrasonic sensor is on to pin 9.
	myLooker.attach(9);
AFMS.begin();
}

void loop()
{
        delay(2000);
	bool found = SolveMaze();

	// If the goal of the maze was found, the robot will do a happy dance.
	if (found == true)
	{
		goLeft(100);
		goRight(100);
		goLeft(100);
		goRight(100);
	}
}

bool SolveMaze()
{
	// Begins infinite loop to look for the maze goal.
	for (;;)
	{
		// Declares the step counter "i" and the character path array "path".
		unsigned i = 0;
		char path[50];

		// Assigns the variable "cm" to the distance to the wall that is calculated by the function "DistancePing".
		long cm = DistancePing();
		long x = cm;

		// Begins loop to move forward and put the letter "F" into the path array.
		// This continues for a preset distance.
		LookForward();
		path[i++] = 'F';
		goForward(slowSpeed);

		while (cm >= x - 15)
		{
			goForward(slowSpeed);
			cm = DistancePing();
		}

		// Determines if either the left or right direction is open.
		bool left = LookLeft();
		bool right = LookRight();

		// If Both are left and right are open the robot will determine if it is at the goal.
		if (left == true && right == true)
		{
			bool end = ChckForEnd();
			if (end == true)
			{
				// If it is at the goal it will begin to shorten the path array. 
				char sPath[50];
				char one, two, three;
				unsigned index;

				for (index = 0; index <= i; index++)
				{
					one = path[index];
					two = path[index + 1];
					three = path[index + 2];

					if (one == 'L' && two == 'B' && three == 'R')
						sPath[index] = 'B';
					else if (one == 'L' && two == 'B' && three == 'F')
						sPath[index] = 'R';
					else if (one == 'R' && two == 'B' && three == 'L')
						sPath[index] = 'B';
					else if (one == 'F' && two == 'B' && three == 'L')
						sPath[index] = 'R';
					else if (one == 'F' && two == 'B' && three == 'F')
						sPath[index] == 'B';
					else if (one == 'L' && two == 'B' && three == 'L')
						sPath[index] == 'F';
					else
						sPath[index] = path[index];
				}

				// The robot will then return to the start of the maze.
				for (unsigned a = index; a >= 0; a--)
				{
					char step;
					step = sPath[a];

					if (step == 'F')
						goBack(slowSpeed);
					else if (step == 'R')
						goLeft(slowSpeed);
					else if (step == 'L')
						goRight(slowSpeed);

				}

				// The robot will wait and begin its speed run through the maze.
				delay(5000);
				for (unsigned b = 0; b <= index; b++)
				{
					if (sPath[b] == 'F')
						goForward(fastSpeed);
					else if (sPath[b] == 'L')
						goLeft;
					else if (sPath[b] == 'R')
						goRight(fastSpeed);
					else
						break;
				}
				return true;
			}
		}

		// If the robot is not at the goal and the left path is open, it will go left.
		// Otherwise if right is open it will go right.
		// If neither are open, it will turn around and go back.
		if (left == true)
		{
			goLeft(slowSpeed);
			path[i++] = 'L';
		}
		else if (right == true)
		{
			goRight(slowSpeed);
			path[i++] = 'R';
		}
		else
		{
			goBack(slowSpeed);
			path[i++] = 'B';

		}
	}
}

/*-------------- g o F o r w a r d (  ) ----------------------
Purpose: To move the robot in the forward direction.
*/
void goForward(int speed)
{
        delay(1000);
	lMotor->setSpeed(speed);
	rMotor->setSpeed(speed);
	lMotor->run(FORWARD);
	rMotor->run(FORWARD);
	delay(turnTime);
	lMotor->run(RELEASE);
	rMotor->run(RELEASE);
}

/*-------------- g o L e f t (  ) -------------------------
Purpose: To turn the robot left.
*/
void goLeft(int speed)
{
        delay(1000);
	lMotor->setSpeed(speed);
	rMotor->setSpeed(speed);
	lMotor->run(BACKWARD);
	rMotor->run(FORWARD);
	delay(turnTime);
	lMotor->run(RELEASE);
	rMotor->run(RELEASE);
}

/*---------------------- g o R i g h t (  ) ---------------------
Purpose: To turn the robot right
*/
void goRight(int speed)
{
        delay(1000);
	lMotor->setSpeed(speed);
	rMotor->setSpeed(speed);
	lMotor->run(FORWARD);
	rMotor->run(BACKWARD);
	delay(turnTime);
	lMotor->run(RELEASE);
	rMotor->run(RELEASE);
}

/*---------------------- g o B a c k (  ) ----------------------
Purpose: to turn the robot 180 degrees
*/
void goBack(int speed)
{
        delay(1000);
	lMotor->setSpeed(speed);
	rMotor->setSpeed(speed);
	lMotor->run(FORWARD);
	rMotor->run(BACKWARD);
	delay(turnTime);
	delay(turnTime);
	lMotor->run(RELEASE);
	rMotor->run(RELEASE);
}

/*-------------- D i s t a n c e P i n g (  ) ---------------------------
Purpose: To determine the distance to the wall using the ultrasonic sensor.
*/
long DistancePing()
{
	long duration, cm;

	// Pings a test pulse, then the measured one.
	pinMode(pingPinOut, OUTPUT);
	digitalWrite(pingPinOut, LOW);
	delayMicroseconds(2);
	digitalWrite(pingPinOut, HIGH);
	delayMicroseconds(10);
	digitalWrite(pingPinOut, LOW);

	// Read in the time it takes for the signal to travel and return.
	pinMode(pingPinIn, INPUT);
	duration = pulseIn(pingPinIn, HIGH);

	return (cm / 29 / 2);
}

/*----------------------- L o o k F o r w a r d (  ) -------------------------
Purpose: Turns the servo so that the sensor is facing forward. 
*/
void LookForward()
{
        delay(1000);
	int x = 90;
	myLooker.write(x);
}

/*------------------------ L o o k L e f t (  ) ----------------------------
Purpose: Determines if the left side of the robot is open.
*/
bool LookLeft()
{
        delay(1000);
	long cM;

	myLooker.write(0);
	cM = DistancePing();

	if (cM >= openSpace)
		return true;
	else
		return false;
}

/*--------------------- L o o k R i g h t (  )------------------------------
Purpose: Determines if the right side of the robot is open.
*/
bool LookRight()
{
        delay(1000);
	long centiM;

	myLooker.write(180);
	centiM = DistancePing();

	if (centiM >= openSpace)
		return true;
	else
		return false;
}

/*----------------------- C h c k F o r E n d (  ) ---------------------------
Purpose: Determines if the robot is at the goal of the maze.
*/
bool ChckForEnd()
{
        delay(1000);
	long leftCenti, rightCenti;

	myLooker.write(135);
	rightCenti = DistancePing();
	myLooker.write(45);
	leftCenti = DistancePing();

	if (rightCenti >= openSpace && leftCenti >= openSpace)
		return true;
	else
		return false;
}