/*##### Motor Shield (L298N) #####*/

const int ENA = 5;
const int ENB = 6; 
/*
* IN1: HIGH; IN2: LOW --> Direction 1 
* IN1: LOW; IN2: HIGH --> Direction 2
* IN3: HIGH; IN4: LOW --> Direction 1
* IN3: LOW; IN4: HIGH --> Direction2
*/
const int IN1 = 11; 
const int IN2 = 10;
const int IN3 = 9;
const int IN4 = 8;

/*const int leftTrigger=0;
const int leftEcho=1;

//const int forTrigger =13;
//const int forEcho=12;

const int rightTopTrigger = 4;        
const int rightTopEcho =3;

const int rightBottomTrigger = 7;
const int rightBottomEcho = 2;
*/

#include <NewPing.h>

#define leftTrigger 0
#define leftEcho 1

#define rightTopTrigger 14
#define rightTopEcho 15

#define rightBottomTrigger 19
#define rightBottomEcho 18                   // **************************** DEFINE replaces the "const int" 
 
#define forTrigger 13
#define forEcho 12
#define MAX_DISTANCE 200

NewPing forSonar( forTrigger, forEcho, MAX_DISTANCE);

NewPing rightTopSonar( rightTopTrigger , rightTopEcho , MAX_DISTANCE);

NewPing rightBottomSonar( rightBottomTrigger , rightBottomEcho, MAX_DISTANCE);

NewPing leftSonar( leftTrigger , leftEcho , MAX_DISTANCE);

 int counter = 0;

void setup()
{
 pinMode(ENA, OUTPUT);
 pinMode(ENB, OUTPUT);
 pinMode(IN1, OUTPUT);
 pinMode(IN2, OUTPUT);
 pinMode(IN3, OUTPUT);
 pinMode(IN4, OUTPUT);
 pinMode(leftTrigger, OUTPUT);
 pinMode(leftEcho, INPUT);
 pinMode(forTrigger, OUTPUT);
 pinMode(forEcho, INPUT);
 pinMode(rightTopTrigger, OUTPUT);
 pinMode(rightTopEcho, INPUT);
  pinMode(rightBottomTrigger, OUTPUT);
 pinMode(rightBottomEcho, INPUT);
 // Enable Motor A, Motor B: Constant Speed
 digitalWrite(ENA, HIGH);
 digitalWrite(ENB, HIGH);
 // Serial communication
 Serial.begin(9600);
}
 
void loop()
{
stickToRight();
}

void stickToRight()
{
  if(getSonarDistance(rightTopSonar) - getSonarDistance(rightBottomSonar) >= 3)
  {
    turnRight1(10);
  }
    if(getSonarDistance(rightBottomSonar) - getSonarDistance(rightTopSonar) >= 3)
  {
    turnLeft1(10);
  }
  else
  {
    driveForward1(10);
  }
  
}

long microsecondsToCentimeters(long microseconds)  //********************************************************* Function : from microSec to CM 
{
  return microseconds / 29 / 2;
}

// digitalWrietTry
void driveForward1(int milliseconds)  //*********************************************************  Function : drive forward
{
  digitalWrite(IN1,HIGH);
   digitalWrite(IN2, LOW);
   digitalWrite(IN3, LOW);
   digitalWrite(IN4,HIGH);
 if (milliseconds > 0)
 delay(milliseconds);
}
 
void driveBackward1(int milliseconds) //********************************************************* Function : drive backward
{
   digitalWrite(IN1,LOW);
   digitalWrite(IN2, HIGH);
   digitalWrite(IN3, HIGH);
   digitalWrite(IN4,LOW);
 if(milliseconds > 0)
 delay(milliseconds);
}
 
void stopDrive1(int milliseconds)  //********************************************************* Function : stop   
{
   digitalWrite(IN1,LOW);
   digitalWrite(IN2, LOW);
   digitalWrite(IN3, LOW);
   digitalWrite(IN4,LOW);
 if(milliseconds > 0)
 delay(milliseconds); 
}

void turnLeft1(int milliSeconds) //********************************************************* Function : turn left
{
   digitalWrite(IN1,HIGH);
   digitalWrite(IN2, LOW);
   digitalWrite(IN3, HIGH);
   digitalWrite(IN4,LOW);
   if(milliSeconds > 0)
     delay(milliSeconds);   
}

void turnRight1(int milliSeconds) //********************************************************* Function : turn right
{
   digitalWrite(IN1, LOW);
   digitalWrite(IN2, HIGH);
   digitalWrite(IN3, LOW);
   digitalWrite(IN4, HIGH);
   if(milliSeconds > 0)
     delay(milliSeconds);   
}


int getSonarDistance(NewPing sonar)  //********************************************************* Function : recieves a specific Sonar and returns distance 
{
  delay(200);                                 
   unsigned int uS = sonar.ping_cm();
 //  Serial.print(uS);
 //  Serial.println("cm");
   return uS;
}
                                      

void funRun()          //****************************************** Function : just avoiding objcects and turns left
{

  while(getSonarDistance(forSonar) > 30 || getSonarDistance(forSonar) == 0 )
  {
    driveForward1(50);
  }
  while(getSonarDistance(forSonar) < 30)
   {
    driveBackward1(70);
    delay(50);
      turnLeft1(800);
   }
   }

void funRun2()      // ********************* Function: same as funRun , just if turned left 4 times, it turns Right. PROBLEM not always return to its initial spot- in maze it will be ok
{
  
  while(getSonarDistance(forSonar) > 30 || getSonarDistance(forSonar) == 0 )
  {
    driveForward1(50);
  }
  while(getSonarDistance(forSonar) < 30)
   {
    if(counter==4)
    {
      driveBackward1(70);
    delay(50);
      turnRight1(800);
      counter = 0;
    }
    else
    {
    driveBackward1(70);
    delay(50);
      turnLeft1(800);
      counter++;
   }
   }
}