/*##### Motor Shield (L298N) #####*/
 /* To import NEWPING library go to - https://bitbucket.org/teckel12/arduino-new-ping/downloads/NewPing_v1.8.zip */
 
const int tDelay = 370; // without tires was 370         with tires:not turning good, do not use.
                        // 896 after checks, could be 897/895/ more or less
 
// constanst to turn 90 degrees with delay
 
const int ENA = 5;      // right side
const int ENB = 6;      // left side
/*
* 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;        //RIGHT SIDE
const int IN2 = 10;        //RIGHT SIDE
const int IN3 = 9;         //LEFT SIDE
const int IN4 = 8;         //LEFT SIDE
 
/*const int leftTrigger=0;
const int leftEcho=1;
 
//const int forTrigger =13;
//const int forEcho=12;
 
const int rightTrigger = 4;        
const int rightEcho =3;
 
const int rightBottomTrigger = 7;
const int rightBottomEcho = 2;
*/
 
#include <NewPing.h>
 
 
#define rightTrigger 14
#define rightEcho 15
 
#define leftTrigger 19
#define leftEcho 18                   // **************************** DEFINE replaces the "const int"
 
#define forTrigger 13
#define forEcho 12
#define MAX_DISTANCE 200
 
NewPing forSonar( forTrigger, forEcho, MAX_DISTANCE);
 
NewPing rightSonar( rightTrigger , rightEcho , 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(forTrigger, OUTPUT);
 pinMode(forEcho, INPUT);
 pinMode(rightTrigger, OUTPUT);
 pinMode(rightEcho, INPUT);
  pinMode(leftTrigger, OUTPUT);
 pinMode(leftEcho, INPUT);
 // Enable Motor A, Motor B: Constant Speed
 analogWrite(ENA, 60);                              // change back to 80
 analogWrite(ENB, 60);                              // change back to 80
 // Serial communication
 Serial.begin(9600);
}
 
 
/* if(rightOpen())
        turnRight()
   elseif(frontOpen())
          goForward()
    else turnLeft()
 
 
 
 
 
 
 
 */
void loop()                       // *********** TODO --  PERFECT turn right, => right now it is too much - decrease angle, decrease curve
{  
 // testTurn();
  //turnTillWall();
 inspiration();                                          
}
 
void uTurn()
{
  analogWrite(ENA, 100);
  analogWrite(ENB, 100);
 
  digitalWrite(IN1,HIGH);
  digitalWrite(IN2, LOW);
  digitalWrite(IN3, HIGH);
  digitalWrite(IN4,LOW);
  delay(650);  
}
 
 
 
 
 
 
 
 
 
 
 
void testTurn()
{
  for(int i = 0; i < 4 ; i++)
  {
    turnRight(tDelay);
    stopDrive1(500);
  }
  stopDrive1(10000);
}
 
void stickToRight()
{
  if(getSonarDistance(rightSonar) - getSonarDistance(leftSonar) >= 3)
  {
    turnRight1(10);
  }
    if(getSonarDistance(leftSonar) - getSonarDistance(rightSonar) >= 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
{
 
   analogWrite(ENA, 60);
   analogWrite(ENB, 60);
   
   digitalWrite(IN1,HIGH);
   digitalWrite(IN2, LOW);
   digitalWrite(IN3, LOW);
   digitalWrite(IN4,HIGH);
 if (milliseconds > 0)
 delay(milliseconds);
}
 
void driveBackward1(int milliseconds) //********************************************************* Function : drive backward
{
   analogWrite(ENA, 60);
   analogWrite(ENB, 60);
   
   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
{
  analogWrite(ENA, 155);                              
  analogWrite(ENB, 0);
 
  digitalWrite(IN1,HIGH);
  digitalWrite(IN2, LOW);
  digitalWrite(IN3, LOW);
  digitalWrite(IN4,HIGH);
   if(milliSeconds > 0)
     delay(milliSeconds);  
}
 
 
void turnRight(int milliSeconds)
{
  analogWrite(ENA, 0);  //was 0                            
  analogWrite(ENB, 155); //was 155
 
  digitalWrite(IN1,HIGH);
  digitalWrite(IN2, LOW);
  digitalWrite(IN3, LOW);
  digitalWrite(IN4,HIGH);
  if(milliSeconds > 0)
    delay(milliSeconds);  
}
void turnRight1(int milliSeconds) //********************************************************* Function : turn right
{
  analogWrite(ENA, 80);                              // change back to 150
  analogWrite(ENB, 80);
 
  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(100);                                  
  unsigned int distance = sonar.ping_cm();
  //Serial.print(distance);
  //Serial.println("cm");
  if(distance !=0)
  return distance;
  else
  return 1000; // Comes in place of the "0" so it wont disturb the loop that check "smaller than..."
 
 
 
}
                                     
 
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++;
   }
   
}
}
 
void checkSpanish()
{
  if(getSonarDistance(forSonar) >8)
  {
    if(getSonarDistance(rightSonar) > 7 &&  getSonarDistance(rightSonar) <13  )
    {
      analogWrite(ENA , 60);    // 120
      analogWrite(ENB , 90);  // 150
      driveForward1(50);
    }
 
    if(getSonarDistance(rightSonar) >= 13)
    {
      analogWrite(ENA , 150);  //255
      analogWrite(ENB , 10);   // 60
      driveForward1(100);
    }
 
    if(getSonarDistance(rightSonar) <= 7 )
    {
      analogWrite(ENA , 10);    //60
      analogWrite(ENB , 150);    //255
      driveForward1(100);
    }
  }
 
  if( getSonarDistance(rightSonar) <=20 && getSonarDistance(rightSonar)  > 20 && getSonarDistance(forSonar) <=8)
    turnRight1(700);
 
  if(getSonarDistance(rightSonar) > 20 && getSonarDistance(rightSonar)  > 20 && getSonarDistance(forSonar) <=8)
    turnRight1(700);
 
  if(getSonarDistance(rightSonar) <=20 && getSonarDistance(rightSonar)  > 20 && getSonarDistance(forSonar) <=8)      
    turnLeft1(800);
 
  if( getSonarDistance(rightSonar) <=20 && getSonarDistance(rightSonar)  <= 20 && getSonarDistance(forSonar) <=8)      
    turnRight1(1200); // rotate 180
 
}
 
void inspiration()
{
  if(getSonarDistance(rightSonar) >25)
  {
      turnRight(tDelay); // 90 degrees
  }
  else if(getSonarDistance(forSonar) > 20)
  {
    driveForward1(20);       // previous version 100
  }
  else if(getSonarDistance(leftSonar) >30)
  {
    turnLeft1(tDelay);
    stopDrive1(500);// 90 degrees
  }
  else
  {
   uTurn(); //tDelay*2
  }
}
 
 void turnTillWall()
 {
 
  if(getSonarDistance(rightSonar) > 6 )
  {
    while(getSonarDistance(rightSonar) > 6 )
    {
      turnRight(105);         // 8 times = full turn (90 degrees)
    }
  }
  else if(getSonarDistance(forSonar) > 20)
  {
    driveForward1(50);       // previous version 100
  }
  else if(getSonarDistance(leftSonar) >20)
  {
    turnLeft1(tDelay);
    stopDrive1(500);// 90 degrees
  }
  else
  {
   turnRight(1350); //tDelay*2
  }
 }
 /* void correctParallel()
  {
    for(int i = 0 ; i < 5; i++)
    {
      if(rightBottomSonar - toprigthSonar == i)
      turnLeft(i*100);   // just a measure to turn need to check this
      else if(topright - rightBottom == i)
      turnRight(i*100); // same
    }
  }
*/