#include <LiquidCrystal.h>
#include <Adafruit_MotorShield.h>
#include <Ping.h>
#include <Wire.h>
struct MotorSpeed;
struct MotorSpeed{
  int Speed;
  int Direction;
  int delay;
};
//struct MotorSpeed
//{
//	int Speed;
//	int Direction;
//};

Ping pingRight = Ping(11,12,true);
Ping pingLeft = Ping(8,7,true);
//LiquidCrystal lcd(12,11,5,4,3,2);
Adafruit_MotorShield shield = Adafruit_MotorShield();
Adafruit_DCMotor* rightMotor = shield.getMotor(2);
Adafruit_DCMotor* leftMotor = shield.getMotor(3);

struct MotorSpeed GetMotorSpeed(int);

const int longWait = 500;
const int wait = 250;
unsigned long previousMillis = 0;

void setup()
{
	shield.begin();
	rightMotor->run(RELEASE);
	leftMotor->run(RELEASE);
	Serial.begin(9600);
	//lcd.begin(16,2);
	//lcd.print("Set Ping for Inc");
}

void loop()
{
	unsigned long currentMills = millis();

	int distanceRight = constrain(pingRight.Distance(),1,40);
	int distanceLeft = constrain(pingLeft.Distance(),1,40);
	Serial.print(distanceRight);
	Serial.print(",");
	Serial.println(distanceLeft);
	//lcd.setCursor(0,1);
	//lcd.print(distance);
	//lcd.print(" inch,");

	// each ping control the opposite Motor.
	// if ping right is getting to close to the wall, we will want to slow the left motor that way we will turn left
	// if ping left is getting to close to the wall, we will want to slow the right motor that way we will turn right
	// if both pings are close to the wall in the end we will start moving backwards <-- this might be an issue of going backward and forward.
	// how to solve this issue ?
	MotorSpeed motorLeftSpeed = GetMotorSpeed(distanceRight);
	MotorSpeed motorRightSpeed = GetMotorSpeed(distanceLeft);
	
	
	//lcd.print(motorSpeed.Speed);
	//lcd.print("     ");
	if(motorRightSpeed.Direction == BRAKE && motorLeftSpeed.Direction == BRAKE)
	{
		rightMotor->run(FORWARD);
		leftMotor->run(BACKWARD);
		rightMotor->setSpeed(150);
		leftMotor->setSpeed(150);
		delay(750);
	}
	else
	{
		rightMotor->run(motorRightSpeed.Direction);
		rightMotor->setSpeed(motorRightSpeed.Speed);
		leftMotor->run(motorLeftSpeed.Direction);
		leftMotor->setSpeed(motorLeftSpeed.Speed);
		int d = max(motorLeftSpeed.delay,motorRightSpeed.delay);
		delay(d);
	}
	
}


struct MotorSpeed GetMotorSpeed(int distance)
{
	struct MotorSpeed speed;
	if(distance >=21)
	{
		speed.Direction = FORWARD;
		speed.Speed = map(distance,21,40,201,255);
		speed.delay = 100;
	}
	else if(distance >=17)
	{
		speed.Direction = FORWARD;
		speed.Speed = map(distance,17,21,151,200);
		speed.delay = 200;
	}
	else if(distance >=13)
	{
		speed.Direction = FORWARD;
		speed.Speed = map(distance,13,16,101,150);
		speed.delay = 250;
	}
	else if(distance >=9)
	{
		speed.Direction = FORWARD;
		speed.Speed = map(distance,9,12,80,100);
		speed.delay = 350;
	}
	else if(distance >=6)
	{
		speed.Direction = BRAKE;
		speed.Speed = 0;
		speed.delay = 500;
	}
	else if(distance >=3)
	{
		speed.Direction = BACKWARD;
		speed.Speed = map(distance,3,7,60,125);
		speed.delay = 350;
	}
	else
	{
		speed.Direction = BACKWARD;
		speed.Speed = 200;
		speed.delay = 150;
	}
	return speed;
}
